From 71952b4294c41988bb4945d3f23d1678018fee28 Mon Sep 17 00:00:00 2001 From: Brandon Sutherland Date: Fri, 15 Dec 2023 10:41:44 -0700 Subject: [PATCH] Reformatted with new clang-format rules --- boards/airbourne/airbourne_board.cpp | 247 ++++---------- boards/airbourne/airbourne_board.h | 20 +- boards/airbourne/main.cpp | 172 +++++----- boards/breezy/breezy_board.cpp | 251 ++++---------- boards/breezy/breezy_board.h | 31 +- boards/breezy/flash.h | 9 +- boards/breezy/main.cpp | 5 +- comms/mavlink/mavlink.cpp | 494 +++++++++++++-------------- comms/mavlink/mavlink.h | 94 +++-- include/board.h | 16 +- include/comm_manager.h | 53 +-- include/command_manager.h | 20 +- include/controller.h | 12 +- include/estimator.h | 23 +- include/interface/comm_link.h | 94 +++-- include/mixer.h | 99 +++--- include/param.h | 10 +- include/rc.h | 4 +- include/rosflight.h | 8 +- include/sensors.h | 13 +- include/state_manager.h | 20 +- include/util.h | 17 +- lib/turbomath/turbomath.cpp | 326 ++++++++---------- lib/turbomath/turbomath.h | 63 ++-- src/comm_manager.cpp | 458 +++++++++++-------------- src/command_manager.cpp | 160 ++++----- src/controller.cpp | 153 ++++----- src/estimator.cpp | 81 ++--- src/mixer.cpp | 129 +++---- src/param.cpp | 54 ++- src/rc.cpp | 246 ++++++------- src/rosflight.cpp | 30 +- src/sensors.cpp | 381 +++++++++------------ src/state_manager.cpp | 354 +++++++++---------- test/command_manager_test.cpp | 18 +- test/common.cpp | 26 +- test/common.h | 63 ++-- test/estimator_test.cpp | 63 ++-- test/state_machine_test.cpp | 42 +-- test/test_board.cpp | 171 +++------- test/test_board.h | 20 +- test/turbotrig_test.cpp | 150 ++++---- 42 files changed, 1990 insertions(+), 2710 deletions(-) diff --git a/boards/airbourne/airbourne_board.cpp b/boards/airbourne/airbourne_board.cpp index 34f29171..48bc19e9 100644 --- a/boards/airbourne/airbourne_board.cpp +++ b/boards/airbourne/airbourne_board.cpp @@ -55,86 +55,64 @@ void AirbourneBoard::init_board() void AirbourneBoard::board_reset(bool bootloader) { - (void)bootloader; + (void) bootloader; NVIC_SystemReset(); } // clock -uint32_t AirbourneBoard::clock_millis() -{ - return millis(); -} +uint32_t AirbourneBoard::clock_millis() { return millis(); } -uint64_t AirbourneBoard::clock_micros() -{ - return micros(); -} +uint64_t AirbourneBoard::clock_micros() { return micros(); } -void AirbourneBoard::clock_delay(uint32_t milliseconds) -{ - delay(milliseconds); -} +void AirbourneBoard::clock_delay(uint32_t milliseconds) { delay(milliseconds); } // serial void AirbourneBoard::serial_init(uint32_t baud_rate, uint32_t dev) { vcp_.init(); - switch (dev) - { - case SERIAL_DEVICE_UART3: - uart3_.init(&uart_config[UART3], baud_rate); - current_serial_ = &uart3_; - secondary_serial_device_ = SERIAL_DEVICE_UART3; - break; - default: - current_serial_ = &vcp_; - secondary_serial_device_ = SERIAL_DEVICE_VCP; + switch (dev) { + case SERIAL_DEVICE_UART3: + uart3_.init(&uart_config[UART3], baud_rate); + current_serial_ = &uart3_; + secondary_serial_device_ = SERIAL_DEVICE_UART3; + break; + default: + current_serial_ = &vcp_; + secondary_serial_device_ = SERIAL_DEVICE_VCP; } } -void AirbourneBoard::serial_write(const uint8_t *src, size_t len) +void AirbourneBoard::serial_write(const uint8_t * src, size_t len) { current_serial_->write(src, len); } uint16_t AirbourneBoard::serial_bytes_available() { - if (vcp_.connected() || secondary_serial_device_ == SERIAL_DEVICE_VCP) - { + if (vcp_.connected() || secondary_serial_device_ == SERIAL_DEVICE_VCP) { current_serial_ = &vcp_; - } - else - { - switch (secondary_serial_device_) - { - case SERIAL_DEVICE_UART3: - current_serial_ = &uart3_; - break; - default: - // no secondary serial device - break; + } else { + switch (secondary_serial_device_) { + case SERIAL_DEVICE_UART3: + current_serial_ = &uart3_; + break; + default: + // no secondary serial device + break; } } return current_serial_->rx_bytes_waiting(); } -uint8_t AirbourneBoard::serial_read() -{ - return current_serial_->read_byte(); -} +uint8_t AirbourneBoard::serial_read() { return current_serial_->read_byte(); } -void AirbourneBoard::serial_flush() -{ - current_serial_->flush(); -} +void AirbourneBoard::serial_flush() { current_serial_->flush(); } // sensors void AirbourneBoard::sensors_init() { - while (millis() < 50) - { - } // wait for sensors to boot up + while (millis() < 50) {} // wait for sensors to boot up imu_.init(&spi1_); baro_.init(&int_i2c_); @@ -146,17 +124,12 @@ void AirbourneBoard::sensors_init() battery_monitor_.init(battery_monitor_config, &battery_adc_, 0, 0); } -uint16_t AirbourneBoard::num_sensor_errors() -{ - return ext_i2c_.num_errors(); -} +uint16_t AirbourneBoard::num_sensor_errors() { return ext_i2c_.num_errors(); } -bool AirbourneBoard::new_imu_data() -{ - return imu_.new_data(); -} +bool AirbourneBoard::new_imu_data() { return imu_.new_data(); } -bool AirbourneBoard::imu_read(float accel[3], float *temperature, float gyro[3], uint64_t *time_us) +bool AirbourneBoard::imu_read(float accel[3], float * temperature, float gyro[3], + uint64_t * time_us) { float read_accel[3], read_gyro[3]; imu_.read(read_accel, read_gyro, temperature, time_us); @@ -172,10 +145,7 @@ bool AirbourneBoard::imu_read(float accel[3], float *temperature, float gyro[3], return true; } -void AirbourneBoard::imu_not_responding_error() -{ - sensors_init(); -} +void AirbourneBoard::imu_not_responding_error() { sensors_init(); } bool AirbourneBoard::mag_present() { @@ -183,10 +153,7 @@ bool AirbourneBoard::mag_present() return mag_.present(); } -void AirbourneBoard::mag_update() -{ - mag_.update(); -} +void AirbourneBoard::mag_update() { mag_.update(); } void AirbourneBoard::mag_read(float mag[3]) { @@ -199,49 +166,31 @@ bool AirbourneBoard::baro_present() return baro_.present(); } -void AirbourneBoard::baro_update() -{ - baro_.update(); -} +void AirbourneBoard::baro_update() { baro_.update(); } -void AirbourneBoard::baro_read(float *pressure, float *temperature) +void AirbourneBoard::baro_read(float * pressure, float * temperature) { baro_.update(); baro_.read(pressure, temperature); } -bool AirbourneBoard::diff_pressure_present() -{ - return airspeed_.present(); -} +bool AirbourneBoard::diff_pressure_present() { return airspeed_.present(); } -void AirbourneBoard::diff_pressure_update() -{ - airspeed_.update(); -} +void AirbourneBoard::diff_pressure_update() { airspeed_.update(); } -void AirbourneBoard::diff_pressure_read(float *diff_pressure, float *temperature) +void AirbourneBoard::diff_pressure_read(float * diff_pressure, float * temperature) { - (void)diff_pressure; - (void)temperature; + (void) diff_pressure; + (void) temperature; airspeed_.update(); airspeed_.read(diff_pressure, temperature); } -bool AirbourneBoard::sonar_present() -{ - return sonar_.present(); -} +bool AirbourneBoard::sonar_present() { return sonar_.present(); } -void AirbourneBoard::sonar_update() -{ - sonar_.update(); -} +void AirbourneBoard::sonar_update() { sonar_.update(); } -float AirbourneBoard::sonar_read() -{ - return sonar_.read(); -} +float AirbourneBoard::sonar_read() { return sonar_.read(); } bool AirbourneBoard::gnss_present() { @@ -249,10 +198,7 @@ bool AirbourneBoard::gnss_present() return gnss_.present(); } void AirbourneBoard::gnss_update() {} -bool AirbourneBoard::gnss_has_new_data() -{ - return this->gnss_.new_data(); -} +bool AirbourneBoard::gnss_has_new_data() { return this->gnss_.new_data(); } // This method translates the UBLOX driver interface into the ROSFlight interface // If not gnss_has_new_data(), then this may return 0's for ECEF position data, // ECEF velocity data, or both @@ -278,8 +224,7 @@ GNSSData AirbourneBoard::gnss_read() gnss.v_acc = gnss_pvt.v_acc; // Does not include ECEF position data if the timestamp doesn't match // See UBLOX::new_data() for reasoning - if (gnss.time_of_week == pos_ecef.time_of_week) - { + if (gnss.time_of_week == pos_ecef.time_of_week) { gnss.ecef.x = pos_ecef.x; gnss.ecef.y = pos_ecef.y; gnss.ecef.z = pos_ecef.z; @@ -287,8 +232,7 @@ GNSSData AirbourneBoard::gnss_read() } // Does not include ECEF position data if the timestamp doesn't match // See UBLOX::new_data() for reasoning - if (gnss.time_of_week == vel_ecef.time_of_week) - { + if (gnss.time_of_week == vel_ecef.time_of_week) { gnss.ecef.vx = vel_ecef.vx; gnss.ecef.vy = vel_ecef.vy; gnss.ecef.vz = vel_ecef.vz; @@ -364,31 +308,26 @@ void AirbourneBoard::battery_current_set_multiplier(double multiplier) // PWM void AirbourneBoard::rc_init(rc_type_t rc_type) { - switch (rc_type) - { - case RC_TYPE_SBUS: - uart1_.init(&uart_config[UART1], 100000, UART::MODE_8E2); - inv_pin_.init(SBUS_INV_GPIO, SBUS_INV_PIN, GPIO::OUTPUT); - rc_sbus_.init(&inv_pin_, &uart1_); - rc_ = &rc_sbus_; - break; - case RC_TYPE_PPM: - default: - rc_ppm_.init(&pwm_config[RC_PPM_PIN]); - rc_ = &rc_ppm_; - break; + switch (rc_type) { + case RC_TYPE_SBUS: + uart1_.init(&uart_config[UART1], 100000, UART::MODE_8E2); + inv_pin_.init(SBUS_INV_GPIO, SBUS_INV_PIN, GPIO::OUTPUT); + rc_sbus_.init(&inv_pin_, &uart1_); + rc_ = &rc_sbus_; + break; + case RC_TYPE_PPM: + default: + rc_ppm_.init(&pwm_config[RC_PPM_PIN]); + rc_ = &rc_ppm_; + break; } } -float AirbourneBoard::rc_read(uint8_t channel) -{ - return rc_->read(channel); -} +float AirbourneBoard::rc_read(uint8_t channel) { return rc_->read(channel); } void AirbourneBoard::pwm_init(uint32_t refresh_rate, uint16_t idle_pwm) { - for (int i = 0; i < PWM_NUM_OUTPUTS; i++) - { + for (int i = 0; i < PWM_NUM_OUTPUTS; i++) { esc_out_[i].init(&pwm_config[i], refresh_rate, 2000, 1000); esc_out_[i].writeUs(idle_pwm); } @@ -396,92 +335,56 @@ void AirbourneBoard::pwm_init(uint32_t refresh_rate, uint16_t idle_pwm) void AirbourneBoard::pwm_disable() { - for (int i = 0; i < PWM_NUM_OUTPUTS; i++) - { - esc_out_[i].disable(); - } + for (int i = 0; i < PWM_NUM_OUTPUTS; i++) { esc_out_[i].disable(); } } void AirbourneBoard::pwm_write(uint8_t channel, float value) { - if (channel < PWM_NUM_OUTPUTS) - { - esc_out_[channel].write(value); - } + if (channel < PWM_NUM_OUTPUTS) { esc_out_[channel].write(value); } } -bool AirbourneBoard::rc_lost() -{ - return rc_->lost(); -} +bool AirbourneBoard::rc_lost() { return rc_->lost(); } // non-volatile memory -void AirbourneBoard::memory_init() -{ - return flash_.init(&spi3_); -} +void AirbourneBoard::memory_init() { return flash_.init(&spi3_); } -bool AirbourneBoard::memory_read(void *data, size_t len) +bool AirbourneBoard::memory_read(void * data, size_t len) { return flash_.read_config(reinterpret_cast(data), len); } -bool AirbourneBoard::memory_write(const void *data, size_t len) +bool AirbourneBoard::memory_write(const void * data, size_t len) { return flash_.write_config(reinterpret_cast(data), len); } // LED -void AirbourneBoard::led0_on() -{ - led1_.on(); -} +void AirbourneBoard::led0_on() { led1_.on(); } -void AirbourneBoard::led0_off() -{ - led1_.off(); -} +void AirbourneBoard::led0_off() { led1_.off(); } -void AirbourneBoard::led0_toggle() -{ - led1_.toggle(); -} +void AirbourneBoard::led0_toggle() { led1_.toggle(); } -void AirbourneBoard::led1_on() -{ - led2_.on(); -} +void AirbourneBoard::led1_on() { led2_.on(); } -void AirbourneBoard::led1_off() -{ - led2_.off(); -} +void AirbourneBoard::led1_off() { led2_.off(); } -void AirbourneBoard::led1_toggle() -{ - led2_.toggle(); -} +void AirbourneBoard::led1_toggle() { led2_.toggle(); } // Backup memory -void AirbourneBoard::backup_memory_init() -{ - backup_sram_init(); -} +void AirbourneBoard::backup_memory_init() { backup_sram_init(); } -bool AirbourneBoard::backup_memory_read(void *dest, size_t len) +bool AirbourneBoard::backup_memory_read(void * dest, size_t len) { backup_sram_read(dest, len); return true; //!< @todo backup_sram_read() has no return value } -void AirbourneBoard::backup_memory_write(const void *src, size_t len) +void AirbourneBoard::backup_memory_write(const void * src, size_t len) { backup_sram_write(src, len); } -void AirbourneBoard::backup_memory_clear(size_t len) -{ - backup_sram_clear(len); -} +void AirbourneBoard::backup_memory_clear(size_t len) { backup_sram_clear(len); } } // namespace rosflight_firmware diff --git a/boards/airbourne/airbourne_board.h b/boards/airbourne/airbourne_board.h index 7b09c1f2..6cb15be5 100644 --- a/boards/airbourne/airbourne_board.h +++ b/boards/airbourne/airbourne_board.h @@ -70,7 +70,7 @@ class AirbourneBoard : public Board VCP vcp_; UART uart1_; UART uart3_; - Serial *current_serial_; // A pointer to the serial stream currently in use. + Serial * current_serial_; // A pointer to the serial stream currently in use. I2C int_i2c_; I2C ext_i2c_; SPI spi1_; @@ -98,7 +98,7 @@ class AirbourneBoard : public Board }; SerialDevice secondary_serial_device_ = SERIAL_DEVICE_VCP; - RC_BASE *rc_ = nullptr; + RC_BASE * rc_ = nullptr; std::function imu_callback_; @@ -132,7 +132,7 @@ class AirbourneBoard : public Board // serial void serial_init(uint32_t baud_rate, uint32_t dev) override; - void serial_write(const uint8_t *src, size_t len) override; + void serial_write(const uint8_t * src, size_t len) override; uint16_t serial_bytes_available() override; uint8_t serial_read() override; void serial_flush() override; @@ -142,7 +142,7 @@ class AirbourneBoard : public Board uint16_t num_sensor_errors() override; bool new_imu_data() override; - bool imu_read(float accel[3], float *temperature, float gyro[3], uint64_t *time_us) override; + bool imu_read(float accel[3], float * temperature, float gyro[3], uint64_t * time_us) override; void imu_not_responding_error() override; bool mag_present() override; @@ -151,11 +151,11 @@ class AirbourneBoard : public Board bool baro_present() override; void baro_update() override; - void baro_read(float *pressure, float *temperature) override; + void baro_read(float * pressure, float * temperature) override; bool diff_pressure_present() override; void diff_pressure_update() override; - void diff_pressure_read(float *diff_pressure, float *temperature) override; + void diff_pressure_read(float * diff_pressure, float * temperature) override; bool sonar_present() override; void sonar_update() override; @@ -188,8 +188,8 @@ class AirbourneBoard : public Board // non-volatile memory void memory_init() override; - bool memory_read(void *dest, size_t len) override; - bool memory_write(const void *src, size_t len) override; + bool memory_read(void * dest, size_t len) override; + bool memory_write(const void * src, size_t len) override; // LEDs void led0_on() override; @@ -202,8 +202,8 @@ class AirbourneBoard : public Board // Backup Data void backup_memory_init() override; - bool backup_memory_read(void *dest, size_t len) override; - void backup_memory_write(const void *src, size_t len) override; + bool backup_memory_read(void * dest, size_t len) override; + void backup_memory_write(const void * src, size_t len) override; void backup_memory_clear(size_t len) override; }; diff --git a/boards/airbourne/main.cpp b/boards/airbourne/main.cpp index 74b3ef5c..0e08b821 100644 --- a/boards/airbourne/main.cpp +++ b/boards/airbourne/main.cpp @@ -37,105 +37,92 @@ #include "rosflight.h" -rosflight_firmware::ROSflight* rosflight_ptr = nullptr; // used to access firmware in case of a hard fault -void write_backup_data(const rosflight_firmware::StateManager::BackupData::DebugInfo& debug) +rosflight_firmware::ROSflight * rosflight_ptr = + nullptr; // used to access firmware in case of a hard fault +void write_backup_data(const rosflight_firmware::StateManager::BackupData::DebugInfo & debug) { - if (rosflight_ptr != nullptr) - { - rosflight_ptr->state_manager_.write_backup_data(debug); - } + if (rosflight_ptr != nullptr) { rosflight_ptr->state_manager_.write_backup_data(debug); } } -extern "C" -{ - /* The prototype shows it is a naked function - in effect this is just an +extern "C" { +/* The prototype shows it is a naked function - in effect this is just an assembly function. */ - void HardFault_Handler(void) __attribute__((naked)); +void HardFault_Handler(void) __attribute__((naked)); - /* The fault handler implementation calls a function called +/* The fault handler implementation calls a function called prvGetRegistersFromStack(). */ - void HardFault_Handler(void) - { - __asm volatile( - " tst lr, #4 \n" - " ite eq \n" - " mrseq r0, msp \n" - " mrsne r0, psp \n" - " ldr r1, [r0, #24] \n" - " ldr r2, handler2_address_const \n" - " bx r2 \n" - " handler2_address_const: .word prvGetRegistersFromStack \n"); - } - - void prvGetRegistersFromStack(uint32_t* pulFaultStackAddress) - { - /* These are volatile to try and prevent the compiler/linker optimising them +void HardFault_Handler(void) +{ + __asm volatile(" tst lr, #4 \n" + " ite eq \n" + " mrseq r0, msp \n" + " mrsne r0, psp \n" + " ldr r1, [r0, #24] \n" + " ldr r2, handler2_address_const \n" + " bx r2 \n" + " handler2_address_const: .word prvGetRegistersFromStack \n"); +} + +void prvGetRegistersFromStack(uint32_t * pulFaultStackAddress) +{ + /* These are volatile to try and prevent the compiler/linker optimising them away as the variables never actually get used. If the debugger won't show the values of the variables, make them global my moving their declaration outside of this function. */ - volatile uint32_t r0; - volatile uint32_t r1; - volatile uint32_t r2; - volatile uint32_t r3; - volatile uint32_t r12; - volatile uint32_t lr; /* Link register. */ - volatile uint32_t pc; /* Program counter. */ - volatile uint32_t psr; /* Program status register. */ - - r0 = pulFaultStackAddress[0]; - r1 = pulFaultStackAddress[1]; - r2 = pulFaultStackAddress[2]; - r3 = pulFaultStackAddress[3]; - - r12 = pulFaultStackAddress[4]; - lr = pulFaultStackAddress[5]; - pc = pulFaultStackAddress[6]; - psr = pulFaultStackAddress[7]; - - /* When the following line is hit, the variables contain the register values. */ - - // save crash information to backup SRAM - rosflight_firmware::StateManager::BackupData::DebugInfo debug = {r0, r1, r2, r3, r12, lr, pc, psr}; - write_backup_data(debug); - - // reboot - NVIC_SystemReset(); - } - - void NMI_Handler() - { - while (1) - { - } - } - - void MemManage_Handler() - { - while (1) - { - } - } - - void BusFault_Handler() - { - while (1) - { - } - } - - void UsageFault_Handler() - { - while (1) - { - } - } - - void WWDG_IRQHandler() - { - while (1) - { - } - } + volatile uint32_t r0; + volatile uint32_t r1; + volatile uint32_t r2; + volatile uint32_t r3; + volatile uint32_t r12; + volatile uint32_t lr; /* Link register. */ + volatile uint32_t pc; /* Program counter. */ + volatile uint32_t psr; /* Program status register. */ + + r0 = pulFaultStackAddress[0]; + r1 = pulFaultStackAddress[1]; + r2 = pulFaultStackAddress[2]; + r3 = pulFaultStackAddress[3]; + + r12 = pulFaultStackAddress[4]; + lr = pulFaultStackAddress[5]; + pc = pulFaultStackAddress[6]; + psr = pulFaultStackAddress[7]; + + /* When the following line is hit, the variables contain the register values. */ + + // save crash information to backup SRAM + rosflight_firmware::StateManager::BackupData::DebugInfo debug = {r0, r1, r2, r3, + r12, lr, pc, psr}; + write_backup_data(debug); + + // reboot + NVIC_SystemReset(); +} + +void NMI_Handler() +{ + while (1) {} +} + +void MemManage_Handler() +{ + while (1) {} +} + +void BusFault_Handler() +{ + while (1) {} +} + +void UsageFault_Handler() +{ + while (1) {} +} + +void WWDG_IRQHandler() +{ + while (1) {} +} } // extern "C" int main(void) @@ -149,10 +136,7 @@ int main(void) board.init_board(); firmware.init(); - while (true) - { - firmware.run(); - } + while (true) { firmware.run(); } return 0; } diff --git a/boards/breezy/breezy_board.cpp b/boards/breezy/breezy_board.cpp index 9931e8f9..413c501d 100644 --- a/boards/breezy/breezy_board.cpp +++ b/boards/breezy/breezy_board.cpp @@ -32,12 +32,11 @@ #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wold-style-cast" -extern "C" -{ +extern "C" { #include "flash.h" #include - extern void SetSysClock(bool overclock); +extern void SetSysClock(bool overclock); } #include "breezy_board.h" @@ -54,58 +53,34 @@ void BreezyBoard::init_board() _board_revision = 2; } -void BreezyBoard::board_reset(bool bootloader) -{ - systemReset(bootloader); -} +void BreezyBoard::board_reset(bool bootloader) { systemReset(bootloader); } // clock -uint32_t BreezyBoard::clock_millis() -{ - return millis(); -} +uint32_t BreezyBoard::clock_millis() { return millis(); } -uint64_t BreezyBoard::clock_micros() -{ - return micros(); -} +uint64_t BreezyBoard::clock_micros() { return micros(); } -void BreezyBoard::clock_delay(uint32_t milliseconds) -{ - delay(milliseconds); -} +void BreezyBoard::clock_delay(uint32_t milliseconds) { delay(milliseconds); } // serial void BreezyBoard::serial_init(uint32_t baud_rate, uint32_t dev) { - (void)dev; + (void) dev; Serial1 = uartOpen(USART1, NULL, baud_rate, MODE_RXTX); } -void BreezyBoard::serial_write(const uint8_t *src, size_t len) +void BreezyBoard::serial_write(const uint8_t * src, size_t len) { - for (size_t i = 0; i < len; i++) - { - serialWrite(Serial1, src[i]); - } + for (size_t i = 0; i < len; i++) { serialWrite(Serial1, src[i]); } } -uint16_t BreezyBoard::serial_bytes_available() -{ - return serialTotalBytesWaiting(Serial1); -} +uint16_t BreezyBoard::serial_bytes_available() { return serialTotalBytesWaiting(Serial1); } -uint8_t BreezyBoard::serial_read() -{ - return serialRead(Serial1); -} +uint8_t BreezyBoard::serial_read() { return serialRead(Serial1); } -void BreezyBoard::serial_flush() -{ - return; -} +void BreezyBoard::serial_flush() { return; } // sensors @@ -118,8 +93,7 @@ void BreezyBoard::sensors_init() ; i2cWrite(0, 0, 0); - if (bmp280_init()) - baro_type = BARO_BMP280; + if (bmp280_init()) baro_type = BARO_BMP280; else if (ms5611_init()) baro_type = BARO_MS5611; @@ -133,17 +107,11 @@ void BreezyBoard::sensors_init() _accel_scale = 9.80665f / acc1G; } -uint16_t BreezyBoard::num_sensor_errors() -{ - return i2cGetErrorCounter(); -} +uint16_t BreezyBoard::num_sensor_errors() { return i2cGetErrorCounter(); } -bool BreezyBoard::new_imu_data() -{ - return mpu6050_new_data(); -} +bool BreezyBoard::new_imu_data() { return mpu6050_new_data(); } -bool BreezyBoard::imu_read(float accel[3], float *temperature, float gyro[3], uint64_t *time_us) +bool BreezyBoard::imu_read(float accel[3], float * temperature, float gyro[3], uint64_t * time_us) { volatile int16_t gyro_raw[3], accel_raw[3]; volatile int16_t raw_temp; @@ -157,13 +125,11 @@ bool BreezyBoard::imu_read(float accel[3], float *temperature, float gyro[3], ui gyro[1] = -gyro_raw[1] * _gyro_scale; gyro[2] = -gyro_raw[2] * _gyro_scale; - (*temperature) = (float)raw_temp / 340.0f + 36.53f; + (*temperature) = (float) raw_temp / 340.0f + 36.53f; - if (accel[0] == 0 && accel[1] == 0 && accel[2] == 0) - { + if (accel[0] == 0 && accel[1] == 0 && accel[2] == 0) { return false; - } - else + } else return true; } @@ -180,38 +146,27 @@ void BreezyBoard::mag_read(float mag[3]) hmc5883l_async_read(mag); } -bool BreezyBoard::mag_present() -{ - return hmc5883l_present(); -} +bool BreezyBoard::mag_present() { return hmc5883l_present(); } -void BreezyBoard::mag_update() -{ - hmc5883l_request_async_update(); -} +void BreezyBoard::mag_update() { hmc5883l_request_async_update(); } void BreezyBoard::baro_update() { - if (baro_type == BARO_BMP280) - bmp280_async_update(); + if (baro_type == BARO_BMP280) bmp280_async_update(); else if (baro_type == BARO_MS5611) ms5611_async_update(); - else - { + else { bmp280_async_update(); ms5611_async_update(); } } -void BreezyBoard::baro_read(float *pressure, float *temperature) +void BreezyBoard::baro_read(float * pressure, float * temperature) { - if (baro_type == BARO_BMP280) - { + if (baro_type == BARO_BMP280) { bmp280_async_update(); bmp280_async_read(pressure, temperature); - } - else if (baro_type == BARO_MS5611) - { + } else if (baro_type == BARO_MS5611) { ms5611_async_update(); ms5611_async_read(pressure, temperature); } @@ -219,19 +174,14 @@ void BreezyBoard::baro_read(float *pressure, float *temperature) bool BreezyBoard::baro_present() { - if (baro_type == BARO_BMP280) - return bmp280_present(); + if (baro_type == BARO_BMP280) return bmp280_present(); else if (baro_type == BARO_MS5611) return ms5611_present(); - else - { - if (bmp280_present()) - { + else { + if (bmp280_present()) { baro_type = BARO_BMP280; return true; - } - else if (ms5611_present()) - { + } else if (ms5611_present()) { baro_type = BARO_MS5611; return true; } @@ -239,17 +189,11 @@ bool BreezyBoard::baro_present() return false; } -bool BreezyBoard::diff_pressure_present() -{ - return ms4525_present(); -} +bool BreezyBoard::diff_pressure_present() { return ms4525_present(); } -void BreezyBoard::diff_pressure_update() -{ - return ms4525_async_update(); -} +void BreezyBoard::diff_pressure_update() { return ms4525_async_update(); } -void BreezyBoard::diff_pressure_read(float *diff_pressure, float *temperature) +void BreezyBoard::diff_pressure_read(float * diff_pressure, float * temperature) { ms4525_async_update(); ms4525_async_read(diff_pressure, temperature); @@ -257,27 +201,21 @@ void BreezyBoard::diff_pressure_read(float *diff_pressure, float *temperature) void BreezyBoard::sonar_update() { - if (sonar_type == SONAR_I2C || sonar_type == SONAR_NONE) - mb1242_async_update(); + if (sonar_type == SONAR_I2C || sonar_type == SONAR_NONE) mb1242_async_update(); // We don't need to actively update the pwm sonar } bool BreezyBoard::sonar_present() { - if (sonar_type == SONAR_I2C) - return mb1242_present(); + if (sonar_type == SONAR_I2C) return mb1242_present(); else if (sonar_type == SONAR_PWM) return sonarPresent(); - else - { - if (mb1242_present()) - { + else { + if (mb1242_present()) { sonar_type = SONAR_I2C; return true; - } - else if (sonarPresent()) - { + } else if (sonarPresent()) { sonar_type = SONAR_PWM; return true; } @@ -287,52 +225,29 @@ bool BreezyBoard::sonar_present() float BreezyBoard::sonar_read() { - if (sonar_type == SONAR_I2C) - { + if (sonar_type == SONAR_I2C) { mb1242_async_update(); return mb1242_async_read(); - } - else if (sonar_type == SONAR_PWM) + } else if (sonar_type == SONAR_PWM) return sonarRead(6); else return 0.0f; } -uint16_t num_sensor_errors() -{ - return i2cGetErrorCounter(); -} +uint16_t num_sensor_errors() { return i2cGetErrorCounter(); } -bool BreezyBoard::battery_voltage_present() const -{ - return false; -} -float BreezyBoard::battery_voltage_read() const -{ - return 0; -} -void BreezyBoard::battery_voltage_set_multiplier(double multiplier) -{ - (void)multiplier; -} +bool BreezyBoard::battery_voltage_present() const { return false; } +float BreezyBoard::battery_voltage_read() const { return 0; } +void BreezyBoard::battery_voltage_set_multiplier(double multiplier) { (void) multiplier; } -bool BreezyBoard::battery_current_present() const -{ - return false; -} -float BreezyBoard::battery_current_read() const -{ - return 0; -} -void BreezyBoard::battery_current_set_multiplier(double multiplier) -{ - (void)multiplier; -} +bool BreezyBoard::battery_current_present() const { return false; } +float BreezyBoard::battery_current_read() const { return 0; } +void BreezyBoard::battery_current_set_multiplier(double multiplier) { (void) multiplier; } // PWM void BreezyBoard::rc_init(rc_type_t rc_type) { - (void)rc_type; // TODO SBUS is not supported on F1 + (void) rc_type; // TODO SBUS is not supported on F1 pwmInit(true, false, false, pwm_refresh_rate_, pwm_idle_pwm_); } @@ -350,87 +265,45 @@ void BreezyBoard::pwm_disable() pwmInit(true, false, false, pwm_refresh_rate_, pwm_idle_pwm_); } -float BreezyBoard::rc_read(uint8_t channel) -{ - return (float)(pwmRead(channel) - 1000) / 1000.0; -} +float BreezyBoard::rc_read(uint8_t channel) { return (float) (pwmRead(channel) - 1000) / 1000.0; } void BreezyBoard::pwm_write(uint8_t channel, float value) { pwmWriteMotor(channel, static_cast(value * 1000) + 1000); } -bool BreezyBoard::rc_lost() -{ - return ((millis() - pwmLastUpdate()) > 40); -} +bool BreezyBoard::rc_lost() { return ((millis() - pwmLastUpdate()) > 40); } // non-volatile memory -void BreezyBoard::memory_init() -{ - initEEPROM(); -} +void BreezyBoard::memory_init() { initEEPROM(); } -bool BreezyBoard::memory_read(void *dest, size_t len) -{ - return readEEPROM(dest, len); -} +bool BreezyBoard::memory_read(void * dest, size_t len) { return readEEPROM(dest, len); } -bool BreezyBoard::memory_write(const void *src, size_t len) -{ - return writeEEPROM(src, len); -} +bool BreezyBoard::memory_write(const void * src, size_t len) { return writeEEPROM(src, len); } // GNSS is not supported on breezy boards -GNSSData BreezyBoard::gnss_read() -{ - return {}; -} +GNSSData BreezyBoard::gnss_read() { return {}; } // GNSS is not supported on breezy boards -GNSSFull BreezyBoard::gnss_full_read() -{ - return {}; -} +GNSSFull BreezyBoard::gnss_full_read() { return {}; } // GNSS is not supported on breezy boards -bool BreezyBoard::gnss_has_new_data() -{ - return false; -} +bool BreezyBoard::gnss_has_new_data() { return false; } // LED -void BreezyBoard::led0_on() -{ - LED0_ON; -} +void BreezyBoard::led0_on() { LED0_ON; } -void BreezyBoard::led0_off() -{ - LED0_OFF; -} +void BreezyBoard::led0_off() { LED0_OFF; } -void BreezyBoard::led0_toggle() -{ - LED0_TOGGLE; -} +void BreezyBoard::led0_toggle() { LED0_TOGGLE; } -void BreezyBoard::led1_on() -{ - LED1_ON; -} +void BreezyBoard::led1_on() { LED1_ON; } -void BreezyBoard::led1_off() -{ - LED1_OFF; -} +void BreezyBoard::led1_off() { LED1_OFF; } -void BreezyBoard::led1_toggle() -{ - LED1_TOGGLE; -} +void BreezyBoard::led1_toggle() { LED1_TOGGLE; } } // namespace rosflight_firmware diff --git a/boards/breezy/breezy_board.h b/boards/breezy/breezy_board.h index 7376100c..3171f08c 100644 --- a/boards/breezy/breezy_board.h +++ b/boards/breezy/breezy_board.h @@ -36,8 +36,7 @@ #include #include -extern "C" -{ +extern "C" { #include } @@ -49,7 +48,7 @@ namespace rosflight_firmware class BreezyBoard : public Board { private: - serialPort_t *Serial1; + serialPort_t * Serial1; int _board_revision = 2; @@ -92,7 +91,7 @@ class BreezyBoard : public Board // serial void serial_init(uint32_t baud_rate, uint32_t dev) override; - void serial_write(const uint8_t *src, size_t len) override; + void serial_write(const uint8_t * src, size_t len) override; uint16_t serial_bytes_available() override; uint8_t serial_read() override; void serial_flush() override; @@ -102,7 +101,7 @@ class BreezyBoard : public Board uint16_t num_sensor_errors() override; bool new_imu_data() override; - bool imu_read(float accel[3], float *temperature, float gyro[3], uint64_t *time_us) override; + bool imu_read(float accel[3], float * temperature, float gyro[3], uint64_t * time_us) override; void imu_not_responding_error() override; bool mag_present() override; @@ -111,11 +110,11 @@ class BreezyBoard : public Board bool baro_present() override; void baro_update() override; - void baro_read(float *pressure, float *temperature) override; + void baro_read(float * pressure, float * temperature) override; bool diff_pressure_present() override; void diff_pressure_update() override; - void diff_pressure_read(float *diff_pressure, float *temperature) override; + void diff_pressure_read(float * diff_pressure, float * temperature) override; bool sonar_present() override; void sonar_update() override; @@ -148,8 +147,8 @@ class BreezyBoard : public Board // non-volatile memory void memory_init() override; - bool memory_read(void *dest, size_t len) override; - bool memory_write(const void *src, size_t len) override; + bool memory_read(void * dest, size_t len) override; + bool memory_write(const void * src, size_t len) override; // LEDs void led0_on() override; @@ -162,18 +161,18 @@ class BreezyBoard : public Board // Backup Data void backup_memory_init() override {} - bool backup_memory_read(void *dest, size_t len) override + bool backup_memory_read(void * dest, size_t len) override { - (void)dest; - (void)len; + (void) dest; + (void) len; return false; } - void backup_memory_write(const void *src, size_t len) override + void backup_memory_write(const void * src, size_t len) override { - (void)src; - (void)len; + (void) src; + (void) len; } - void backup_memory_clear(size_t len) override { (void)len; } + void backup_memory_clear(size_t len) override { (void) len; } }; } // namespace rosflight_firmware diff --git a/boards/breezy/flash.h b/boards/breezy/flash.h index 39251795..072ee9b8 100644 --- a/boards/breezy/flash.h +++ b/boards/breezy/flash.h @@ -44,7 +44,7 @@ #define FLASH_PAGE_COUNT 128 #endif -#define FLASH_PAGE_SIZE ((uint16_t)0x400) +#define FLASH_PAGE_SIZE ((uint16_t) 0x400) #define NUM_PAGES 3 // if sizeof(_params) is over this number, compile-time error will occur. so, need to add another // page to config data. @@ -54,7 +54,8 @@ // static const uint8_t EEPROM_CONF_VERSION = 76; // static uint32_t enabledSensors = 0; // static void resetConf(void); -static const uint32_t FLASH_WRITE_ADDR = 0x08000000 + (FLASH_PAGE_SIZE * (FLASH_PAGE_COUNT - (CONFIG_SIZE / 1024))); +static const uint32_t FLASH_WRITE_ADDR = + 0x08000000 + (FLASH_PAGE_SIZE * (FLASH_PAGE_COUNT - (CONFIG_SIZE / 1024))); /** * @brief Initialize Flash @@ -67,7 +68,7 @@ void initEEPROM(void); * @param len The number of bytes to copy * @returns true if the read was successful, false otherwise */ -bool readEEPROM(void *dest, size_t len); +bool readEEPROM(void * dest, size_t len); /** * @brief Write data to Flash @@ -75,4 +76,4 @@ bool readEEPROM(void *dest, size_t len); * @param len The number of bytes to copy * @returns true if the write was successful, false otherwise */ -bool writeEEPROM(const void *src, size_t len); +bool writeEEPROM(const void * src, size_t len); diff --git a/boards/breezy/main.cpp b/boards/breezy/main.cpp index 0f6835c7..195138f8 100644 --- a/boards/breezy/main.cpp +++ b/boards/breezy/main.cpp @@ -43,9 +43,6 @@ int main() firmware.init(); - while (true) - { - firmware.run(); - } + while (true) { firmware.run(); } return 0; } diff --git a/comms/mavlink/mavlink.cpp b/comms/mavlink/mavlink.cpp index 308a5ad4..1db2b803 100644 --- a/comms/mavlink/mavlink.cpp +++ b/comms/mavlink/mavlink.cpp @@ -36,7 +36,9 @@ namespace rosflight_firmware { -Mavlink::Mavlink(Board &board) : board_(board) {} +Mavlink::Mavlink(Board & board) + : board_(board) +{} void Mavlink::init(uint32_t baud_rate, uint32_t dev) { @@ -46,22 +48,20 @@ void Mavlink::init(uint32_t baud_rate, uint32_t dev) void Mavlink::receive(void) { - while (board_.serial_bytes_available()) - { + while (board_.serial_bytes_available()) { if (mavlink_parse_char(MAVLINK_COMM_0, board_.serial_read(), &in_buf_, &status_)) handle_mavlink_message(); } } -void Mavlink::send_attitude_quaternion(uint8_t system_id, - uint64_t timestamp_us, - const turbomath::Quaternion &attitude, - const turbomath::Vector &angular_velocity) +void Mavlink::send_attitude_quaternion(uint8_t system_id, uint64_t timestamp_us, + const turbomath::Quaternion & attitude, + const turbomath::Vector & angular_velocity) { mavlink_message_t msg; - mavlink_msg_attitude_quaternion_pack(system_id, compid_, &msg, timestamp_us / 1000, attitude.w, attitude.x, - attitude.y, attitude.z, angular_velocity.x, angular_velocity.y, - angular_velocity.z); + mavlink_msg_attitude_quaternion_pack(system_id, compid_, &msg, timestamp_us / 1000, attitude.w, + attitude.x, attitude.y, attitude.z, angular_velocity.x, + angular_velocity.y, angular_velocity.z); send_message(msg); } @@ -75,41 +75,40 @@ void Mavlink::send_baro(uint8_t system_id, float altitude, float pressure, float void Mavlink::send_command_ack(uint8_t system_id, Command command, bool success) { ROSFLIGHT_CMD rosflight_cmd = ROSFLIGHT_CMD_ENUM_END; - switch (command) - { - case CommLinkInterface::Command::COMMAND_READ_PARAMS: - rosflight_cmd = ROSFLIGHT_CMD_READ_PARAMS; - break; - case CommLinkInterface::Command::COMMAND_WRITE_PARAMS: - rosflight_cmd = ROSFLIGHT_CMD_WRITE_PARAMS; - break; - case CommLinkInterface::Command::COMMAND_SET_PARAM_DEFAULTS: - rosflight_cmd = ROSFLIGHT_CMD_SET_PARAM_DEFAULTS; - break; - case CommLinkInterface::Command::COMMAND_ACCEL_CALIBRATION: - rosflight_cmd = ROSFLIGHT_CMD_ACCEL_CALIBRATION; - break; - case CommLinkInterface::Command::COMMAND_GYRO_CALIBRATION: - rosflight_cmd = ROSFLIGHT_CMD_GYRO_CALIBRATION; - break; - case CommLinkInterface::Command::COMMAND_BARO_CALIBRATION: - rosflight_cmd = ROSFLIGHT_CMD_BARO_CALIBRATION; - break; - case CommLinkInterface::Command::COMMAND_AIRSPEED_CALIBRATION: - rosflight_cmd = ROSFLIGHT_CMD_AIRSPEED_CALIBRATION; - break; - case CommLinkInterface::Command::COMMAND_RC_CALIBRATION: - rosflight_cmd = ROSFLIGHT_CMD_RC_CALIBRATION; - break; - case CommLinkInterface::Command::COMMAND_REBOOT: - rosflight_cmd = ROSFLIGHT_CMD_REBOOT; - break; - case CommLinkInterface::Command::COMMAND_REBOOT_TO_BOOTLOADER: - rosflight_cmd = ROSFLIGHT_CMD_REBOOT_TO_BOOTLOADER; - break; - case CommLinkInterface::Command::COMMAND_SEND_VERSION: - rosflight_cmd = ROSFLIGHT_CMD_SEND_VERSION; - break; + switch (command) { + case CommLinkInterface::Command::COMMAND_READ_PARAMS: + rosflight_cmd = ROSFLIGHT_CMD_READ_PARAMS; + break; + case CommLinkInterface::Command::COMMAND_WRITE_PARAMS: + rosflight_cmd = ROSFLIGHT_CMD_WRITE_PARAMS; + break; + case CommLinkInterface::Command::COMMAND_SET_PARAM_DEFAULTS: + rosflight_cmd = ROSFLIGHT_CMD_SET_PARAM_DEFAULTS; + break; + case CommLinkInterface::Command::COMMAND_ACCEL_CALIBRATION: + rosflight_cmd = ROSFLIGHT_CMD_ACCEL_CALIBRATION; + break; + case CommLinkInterface::Command::COMMAND_GYRO_CALIBRATION: + rosflight_cmd = ROSFLIGHT_CMD_GYRO_CALIBRATION; + break; + case CommLinkInterface::Command::COMMAND_BARO_CALIBRATION: + rosflight_cmd = ROSFLIGHT_CMD_BARO_CALIBRATION; + break; + case CommLinkInterface::Command::COMMAND_AIRSPEED_CALIBRATION: + rosflight_cmd = ROSFLIGHT_CMD_AIRSPEED_CALIBRATION; + break; + case CommLinkInterface::Command::COMMAND_RC_CALIBRATION: + rosflight_cmd = ROSFLIGHT_CMD_RC_CALIBRATION; + break; + case CommLinkInterface::Command::COMMAND_REBOOT: + rosflight_cmd = ROSFLIGHT_CMD_REBOOT; + break; + case CommLinkInterface::Command::COMMAND_REBOOT_TO_BOOTLOADER: + rosflight_cmd = ROSFLIGHT_CMD_REBOOT_TO_BOOTLOADER; + break; + case CommLinkInterface::Command::COMMAND_SEND_VERSION: + rosflight_cmd = ROSFLIGHT_CMD_SEND_VERSION; + break; } mavlink_message_t msg; @@ -118,7 +117,8 @@ void Mavlink::send_command_ack(uint8_t system_id, Command command, bool success) send_message(msg); } -void Mavlink::send_diff_pressure(uint8_t system_id, float velocity, float pressure, float temperature) +void Mavlink::send_diff_pressure(uint8_t system_id, float velocity, float pressure, + float temperature) { mavlink_message_t msg; mavlink_msg_diff_pressure_pack(system_id, compid_, &msg, velocity, pressure, temperature); @@ -128,33 +128,31 @@ void Mavlink::send_diff_pressure(uint8_t system_id, float velocity, float pressu void Mavlink::send_heartbeat(uint8_t system_id, bool fixed_wing) { mavlink_message_t msg; - mavlink_msg_heartbeat_pack(system_id, compid_, &msg, fixed_wing ? MAV_TYPE_FIXED_WING : MAV_TYPE_QUADROTOR, 0, 0, 0, - 0); + mavlink_msg_heartbeat_pack(system_id, compid_, &msg, + fixed_wing ? MAV_TYPE_FIXED_WING : MAV_TYPE_QUADROTOR, 0, 0, 0, 0); send_message(msg); } -void Mavlink::send_imu(uint8_t system_id, - uint64_t timestamp_us, - const turbomath::Vector &accel, - const turbomath::Vector &gyro, - float temperature) +void Mavlink::send_imu(uint8_t system_id, uint64_t timestamp_us, const turbomath::Vector & accel, + const turbomath::Vector & gyro, float temperature) { mavlink_message_t msg; - mavlink_msg_small_imu_pack(system_id, compid_, &msg, timestamp_us, accel.x, accel.y, accel.z, gyro.x, gyro.y, gyro.z, - temperature); + mavlink_msg_small_imu_pack(system_id, compid_, &msg, timestamp_us, accel.x, accel.y, accel.z, + gyro.x, gyro.y, gyro.z, temperature); send_message(msg); } -void Mavlink::send_gnss(uint8_t system_id, const GNSSData &data) +void Mavlink::send_gnss(uint8_t system_id, const GNSSData & data) { mavlink_message_t msg; - mavlink_msg_rosflight_gnss_pack(system_id, compid_, &msg, data.time_of_week, data.fix_type, data.time, data.nanos, - data.lat, data.lon, data.height, data.vel_n, data.vel_e, data.vel_d, data.h_acc, - data.v_acc, data.ecef.x, data.ecef.y, data.ecef.z, data.ecef.p_acc, data.ecef.vx, - data.ecef.vy, data.ecef.vz, data.ecef.s_acc, data.rosflight_timestamp); + mavlink_msg_rosflight_gnss_pack( + system_id, compid_, &msg, data.time_of_week, data.fix_type, data.time, data.nanos, data.lat, + data.lon, data.height, data.vel_n, data.vel_e, data.vel_d, data.h_acc, data.v_acc, data.ecef.x, + data.ecef.y, data.ecef.z, data.ecef.p_acc, data.ecef.vx, data.ecef.vy, data.ecef.vz, + data.ecef.s_acc, data.rosflight_timestamp); send_message(msg); } -void Mavlink::send_gnss_full(uint8_t system_id, const GNSSFull &full) +void Mavlink::send_gnss_full(uint8_t system_id, const GNSSFull & full) { mavlink_message_t msg; mavlink_rosflight_gnss_full_t data = {}; @@ -189,45 +187,47 @@ void Mavlink::send_gnss_full(uint8_t system_id, const GNSSFull &full) send_message(msg); } -void Mavlink::send_log_message(uint8_t system_id, LogSeverity severity, const char *text) +void Mavlink::send_log_message(uint8_t system_id, LogSeverity severity, const char * text) { MAV_SEVERITY mavlink_severity = MAV_SEVERITY_ENUM_END; - switch (severity) - { - case CommLinkInterface::LogSeverity::LOG_INFO: - mavlink_severity = MAV_SEVERITY_INFO; - break; - case CommLinkInterface::LogSeverity::LOG_WARNING: - mavlink_severity = MAV_SEVERITY_WARNING; - break; - case CommLinkInterface::LogSeverity::LOG_ERROR: - mavlink_severity = MAV_SEVERITY_ERROR; - break; - case CommLinkInterface::LogSeverity::LOG_CRITICAL: - mavlink_severity = MAV_SEVERITY_CRITICAL; - break; + switch (severity) { + case CommLinkInterface::LogSeverity::LOG_INFO: + mavlink_severity = MAV_SEVERITY_INFO; + break; + case CommLinkInterface::LogSeverity::LOG_WARNING: + mavlink_severity = MAV_SEVERITY_WARNING; + break; + case CommLinkInterface::LogSeverity::LOG_ERROR: + mavlink_severity = MAV_SEVERITY_ERROR; + break; + case CommLinkInterface::LogSeverity::LOG_CRITICAL: + mavlink_severity = MAV_SEVERITY_CRITICAL; + break; } mavlink_message_t msg; - mavlink_msg_statustext_pack(system_id, compid_, &msg, static_cast(mavlink_severity), text); + mavlink_msg_statustext_pack(system_id, compid_, &msg, static_cast(mavlink_severity), + text); send_message(msg); } -void Mavlink::send_mag(uint8_t system_id, const turbomath::Vector &mag) +void Mavlink::send_mag(uint8_t system_id, const turbomath::Vector & mag) { mavlink_message_t msg; mavlink_msg_small_mag_pack(system_id, compid_, &msg, mag.x, mag.y, mag.z); send_message(msg); } -void Mavlink::send_named_value_int(uint8_t system_id, uint32_t timestamp_ms, const char *const name, int32_t value) +void Mavlink::send_named_value_int(uint8_t system_id, uint32_t timestamp_ms, + const char * const name, int32_t value) { mavlink_message_t msg; mavlink_msg_named_value_int_pack(system_id, compid_, &msg, timestamp_ms, name, value); send_message(msg); } -void Mavlink::send_named_value_float(uint8_t system_id, uint32_t timestamp_ms, const char *const name, float value) +void Mavlink::send_named_value_float(uint8_t system_id, uint32_t timestamp_ms, + const char * const name, float value) { mavlink_message_t msg; mavlink_msg_named_value_float_pack(system_id, compid_, &msg, timestamp_ms, name, value); @@ -241,65 +241,54 @@ void Mavlink::send_output_raw(uint8_t system_id, uint32_t timestamp_ms, const fl send_message(msg); } -void Mavlink::send_param_value_int(uint8_t system_id, - uint16_t index, - const char *const name, - int32_t value, - uint16_t param_count) +void Mavlink::send_param_value_int(uint8_t system_id, uint16_t index, const char * const name, + int32_t value, uint16_t param_count) { mavlink_param_union_t param; param.param_int32 = value; mavlink_message_t msg; - mavlink_msg_param_value_pack(system_id, 0, &msg, name, param.param_float, MAV_PARAM_TYPE_INT32, param_count, index); + mavlink_msg_param_value_pack(system_id, 0, &msg, name, param.param_float, MAV_PARAM_TYPE_INT32, + param_count, index); send_message(msg); } -void Mavlink::send_param_value_float(uint8_t system_id, - uint16_t index, - const char *const name, - float value, - uint16_t param_count) +void Mavlink::send_param_value_float(uint8_t system_id, uint16_t index, const char * const name, + float value, uint16_t param_count) { mavlink_message_t msg; - mavlink_msg_param_value_pack(system_id, 0, &msg, name, value, MAV_PARAM_TYPE_REAL32, param_count, index); + mavlink_msg_param_value_pack(system_id, 0, &msg, name, value, MAV_PARAM_TYPE_REAL32, param_count, + index); send_message(msg); } void Mavlink::send_rc_raw(uint8_t system_id, uint32_t timestamp_ms, const uint16_t channels[8]) { mavlink_message_t msg; - mavlink_msg_rc_channels_pack(system_id, compid_, &msg, timestamp_ms, 0, channels[0], channels[1], channels[2], - channels[3], channels[4], channels[5], channels[6], channels[7], 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0); + mavlink_msg_rc_channels_pack(system_id, compid_, &msg, timestamp_ms, 0, channels[0], channels[1], + channels[2], channels[3], channels[4], channels[5], channels[6], + channels[7], 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0); send_message(msg); } void Mavlink::send_sonar(uint8_t system_id, - /* TODO enum type*/ uint8_t type, - float range, - float max_range, + /* TODO enum type*/ uint8_t type, float range, float max_range, float min_range) { - (void)type; + (void) type; mavlink_message_t msg; - mavlink_msg_small_range_pack(system_id, compid_, &msg, /* TODO */ ROSFLIGHT_RANGE_SONAR, range, max_range, min_range); + mavlink_msg_small_range_pack(system_id, compid_, &msg, /* TODO */ ROSFLIGHT_RANGE_SONAR, range, + max_range, min_range); send_message(msg); } -void Mavlink::send_status(uint8_t system_id, - bool armed, - bool failsafe, - bool rc_override, - bool offboard, - uint8_t error_code, - uint8_t control_mode, - int16_t num_errors, - int16_t loop_time_us) +void Mavlink::send_status(uint8_t system_id, bool armed, bool failsafe, bool rc_override, + bool offboard, uint8_t error_code, uint8_t control_mode, + int16_t num_errors, int16_t loop_time_us) { mavlink_message_t msg; - mavlink_msg_rosflight_status_pack(system_id, compid_, &msg, armed, failsafe, rc_override, offboard, error_code, - control_mode, num_errors, loop_time_us); + mavlink_msg_rosflight_status_pack(system_id, compid_, &msg, armed, failsafe, rc_override, + offboard, error_code, control_mode, num_errors, loop_time_us); send_message(msg); } @@ -310,18 +299,18 @@ void Mavlink::send_timesync(uint8_t system_id, int64_t tc1, int64_t ts1) send_message(msg); } -void Mavlink::send_version(uint8_t system_id, const char *const version) +void Mavlink::send_version(uint8_t system_id, const char * const version) { mavlink_message_t msg; mavlink_msg_rosflight_version_pack(system_id, compid_, &msg, version); send_message(msg); } -void Mavlink::send_error_data(uint8_t system_id, const StateManager::BackupData &error_data) +void Mavlink::send_error_data(uint8_t system_id, const StateManager::BackupData & error_data) { mavlink_message_t msg; bool rearm = (error_data.arm_flag == StateManager::BackupData::ARM_MAGIC); - mavlink_msg_rosflight_hard_error_pack(system_id, compid_, &msg, error_data.error_code, error_data.debug.pc, - error_data.reset_count, rearm); + mavlink_msg_rosflight_hard_error_pack(system_id, compid_, &msg, error_data.error_code, + error_data.debug.pc, error_data.reset_count, rearm); send_message(msg); } void Mavlink::send_battery_status(uint8_t system_id, float voltage, float current) @@ -331,26 +320,24 @@ void Mavlink::send_battery_status(uint8_t system_id, float voltage, float curren send_message(msg); } -void Mavlink::send_message(const mavlink_message_t &msg) +void Mavlink::send_message(const mavlink_message_t & msg) { - if (initialized_) - { + if (initialized_) { uint8_t data[MAVLINK_MAX_PACKET_LEN]; uint16_t len = mavlink_msg_to_send_buffer(data, &msg); board_.serial_write(data, len); } } -void Mavlink::handle_msg_param_request_list(const mavlink_message_t *const msg) +void Mavlink::handle_msg_param_request_list(const mavlink_message_t * const msg) { mavlink_param_request_list_t list; mavlink_msg_param_request_list_decode(msg, &list); - if (listener_ != nullptr) - listener_->param_request_list_callback(list.target_system); + if (listener_ != nullptr) listener_->param_request_list_callback(list.target_system); } -void Mavlink::handle_msg_param_request_read(const mavlink_message_t *const msg) +void Mavlink::handle_msg_param_request_read(const mavlink_message_t * const msg) { mavlink_param_request_read_t read; mavlink_msg_param_request_read_decode(msg, &read); @@ -359,7 +346,7 @@ void Mavlink::handle_msg_param_request_read(const mavlink_message_t *const msg) listener_->param_request_read_callback(read.target_system, read.param_id, read.param_index); } -void Mavlink::handle_msg_param_set(const mavlink_message_t *const msg) +void Mavlink::handle_msg_param_set(const mavlink_message_t * const msg) { mavlink_param_set_t set; mavlink_msg_param_set_decode(msg, &set); @@ -368,139 +355,132 @@ void Mavlink::handle_msg_param_set(const mavlink_message_t *const msg) param.param_float = set.param_value; param.type = set.param_type; - switch (param.type) - { - case MAV_PARAM_TYPE_INT32: - if (listener_ != nullptr) - listener_->param_set_int_callback(set.target_system, set.param_id, param.param_int32); - break; - case MAV_PARAM_TYPE_REAL32: - if (listener_ != nullptr) - listener_->param_set_float_callback(set.target_system, set.param_id, param.param_float); - break; - default: - // unsupported parameter type - break; + switch (param.type) { + case MAV_PARAM_TYPE_INT32: + if (listener_ != nullptr) + listener_->param_set_int_callback(set.target_system, set.param_id, param.param_int32); + break; + case MAV_PARAM_TYPE_REAL32: + if (listener_ != nullptr) + listener_->param_set_float_callback(set.target_system, set.param_id, param.param_float); + break; + default: + // unsupported parameter type + break; } } -void Mavlink::handle_msg_rosflight_cmd(const mavlink_message_t *const msg) +void Mavlink::handle_msg_rosflight_cmd(const mavlink_message_t * const msg) { mavlink_rosflight_cmd_t cmd; mavlink_msg_rosflight_cmd_decode(msg, &cmd); CommLinkInterface::Command command; - switch (cmd.command) - { - case ROSFLIGHT_CMD_READ_PARAMS: - command = CommLinkInterface::Command::COMMAND_READ_PARAMS; - break; - case ROSFLIGHT_CMD_WRITE_PARAMS: - command = CommLinkInterface::Command::COMMAND_WRITE_PARAMS; - break; - case ROSFLIGHT_CMD_SET_PARAM_DEFAULTS: - command = CommLinkInterface::Command::COMMAND_SET_PARAM_DEFAULTS; - break; - case ROSFLIGHT_CMD_ACCEL_CALIBRATION: - command = CommLinkInterface::Command::COMMAND_ACCEL_CALIBRATION; - break; - case ROSFLIGHT_CMD_GYRO_CALIBRATION: - command = CommLinkInterface::Command::COMMAND_GYRO_CALIBRATION; - break; - case ROSFLIGHT_CMD_BARO_CALIBRATION: - command = CommLinkInterface::Command::COMMAND_BARO_CALIBRATION; - break; - case ROSFLIGHT_CMD_AIRSPEED_CALIBRATION: - command = CommLinkInterface::Command::COMMAND_AIRSPEED_CALIBRATION; - break; - case ROSFLIGHT_CMD_RC_CALIBRATION: - command = CommLinkInterface::Command::COMMAND_RC_CALIBRATION; - break; - case ROSFLIGHT_CMD_REBOOT: - command = CommLinkInterface::Command::COMMAND_REBOOT; - break; - case ROSFLIGHT_CMD_REBOOT_TO_BOOTLOADER: - command = CommLinkInterface::Command::COMMAND_REBOOT_TO_BOOTLOADER; - break; - case ROSFLIGHT_CMD_SEND_VERSION: - command = CommLinkInterface::Command::COMMAND_SEND_VERSION; - break; - default: // unsupported command; report failure then return without calling command callback - mavlink_message_t out_msg; - mavlink_msg_rosflight_cmd_ack_pack(msg->sysid, compid_, &out_msg, cmd.command, ROSFLIGHT_CMD_FAILED); - send_message(out_msg); - // log(LogSeverity::LOG_ERROR, "Unsupported ROSFLIGHT CMD %d", command); - return; + switch (cmd.command) { + case ROSFLIGHT_CMD_READ_PARAMS: + command = CommLinkInterface::Command::COMMAND_READ_PARAMS; + break; + case ROSFLIGHT_CMD_WRITE_PARAMS: + command = CommLinkInterface::Command::COMMAND_WRITE_PARAMS; + break; + case ROSFLIGHT_CMD_SET_PARAM_DEFAULTS: + command = CommLinkInterface::Command::COMMAND_SET_PARAM_DEFAULTS; + break; + case ROSFLIGHT_CMD_ACCEL_CALIBRATION: + command = CommLinkInterface::Command::COMMAND_ACCEL_CALIBRATION; + break; + case ROSFLIGHT_CMD_GYRO_CALIBRATION: + command = CommLinkInterface::Command::COMMAND_GYRO_CALIBRATION; + break; + case ROSFLIGHT_CMD_BARO_CALIBRATION: + command = CommLinkInterface::Command::COMMAND_BARO_CALIBRATION; + break; + case ROSFLIGHT_CMD_AIRSPEED_CALIBRATION: + command = CommLinkInterface::Command::COMMAND_AIRSPEED_CALIBRATION; + break; + case ROSFLIGHT_CMD_RC_CALIBRATION: + command = CommLinkInterface::Command::COMMAND_RC_CALIBRATION; + break; + case ROSFLIGHT_CMD_REBOOT: + command = CommLinkInterface::Command::COMMAND_REBOOT; + break; + case ROSFLIGHT_CMD_REBOOT_TO_BOOTLOADER: + command = CommLinkInterface::Command::COMMAND_REBOOT_TO_BOOTLOADER; + break; + case ROSFLIGHT_CMD_SEND_VERSION: + command = CommLinkInterface::Command::COMMAND_SEND_VERSION; + break; + default: // unsupported command; report failure then return without calling command callback + mavlink_message_t out_msg; + mavlink_msg_rosflight_cmd_ack_pack(msg->sysid, compid_, &out_msg, cmd.command, + ROSFLIGHT_CMD_FAILED); + send_message(out_msg); + // log(LogSeverity::LOG_ERROR, "Unsupported ROSFLIGHT CMD %d", command); + return; } - if (listener_ != nullptr) - listener_->command_callback(command); + if (listener_ != nullptr) listener_->command_callback(command); } -void Mavlink::handle_msg_rosflight_aux_cmd(const mavlink_message_t *const msg) +void Mavlink::handle_msg_rosflight_aux_cmd(const mavlink_message_t * const msg) { mavlink_rosflight_aux_cmd_t cmd; mavlink_msg_rosflight_aux_cmd_decode(msg, &cmd); CommLinkInterface::AuxCommand command; // Repack mavlink message into CommLinkInterface::AuxCommand - for (int i = 0; i < 14; i++) - { - switch (cmd.type_array[i]) - { - case DISABLED: - command.cmd_array[i].type = CommLinkInterface::AuxCommand::Type::DISABLED; - break; - case SERVO: - command.cmd_array[i].type = CommLinkInterface::AuxCommand::Type::SERVO; - break; - case MOTOR: - command.cmd_array[i].type = CommLinkInterface::AuxCommand::Type::MOTOR; - break; - default: - // Invalid channel mode; log an error and return with calling callback - // log(CommLinkInterface::LogSeverity::LOG_ERROR, "Unsupported AUX_CMD_CHANNEL_MODE %d", - // cmd.type_array[i]); - return; + for (int i = 0; i < 14; i++) { + switch (cmd.type_array[i]) { + case DISABLED: + command.cmd_array[i].type = CommLinkInterface::AuxCommand::Type::DISABLED; + break; + case SERVO: + command.cmd_array[i].type = CommLinkInterface::AuxCommand::Type::SERVO; + break; + case MOTOR: + command.cmd_array[i].type = CommLinkInterface::AuxCommand::Type::MOTOR; + break; + default: + // Invalid channel mode; log an error and return with calling callback + // log(CommLinkInterface::LogSeverity::LOG_ERROR, "Unsupported AUX_CMD_CHANNEL_MODE %d", + // cmd.type_array[i]); + return; } command.cmd_array[i].value = cmd.aux_cmd_array[i]; } // call callback after all channels have been repacked - if (listener_ != nullptr) - listener_->aux_command_callback(command); + if (listener_ != nullptr) listener_->aux_command_callback(command); } -void Mavlink::handle_msg_timesync(const mavlink_message_t *const msg) +void Mavlink::handle_msg_timesync(const mavlink_message_t * const msg) { mavlink_timesync_t tsync; mavlink_msg_timesync_decode(msg, &tsync); - if (listener_ != nullptr) - listener_->timesync_callback(tsync.tc1, tsync.ts1); + if (listener_ != nullptr) listener_->timesync_callback(tsync.tc1, tsync.ts1); } -void Mavlink::handle_msg_offboard_control(const mavlink_message_t *const msg) +void Mavlink::handle_msg_offboard_control(const mavlink_message_t * const msg) { mavlink_offboard_control_t ctrl; mavlink_msg_offboard_control_decode(msg, &ctrl); CommLinkInterface::OffboardControl control; - switch (ctrl.mode) - { - case MODE_PASS_THROUGH: - control.mode = CommLinkInterface::OffboardControl::Mode::PASS_THROUGH; - break; - case MODE_ROLLRATE_PITCHRATE_YAWRATE_THROTTLE: - control.mode = CommLinkInterface::OffboardControl::Mode::ROLLRATE_PITCHRATE_YAWRATE_THROTTLE; - break; - case MODE_ROLL_PITCH_YAWRATE_THROTTLE: - control.mode = CommLinkInterface::OffboardControl::Mode::ROLL_PITCH_YAWRATE_THROTTLE; - break; - default: - // invalid mode; ignore message and return without calling callback - return; + switch (ctrl.mode) { + case MODE_PASS_THROUGH: + control.mode = CommLinkInterface::OffboardControl::Mode::PASS_THROUGH; + break; + case MODE_ROLLRATE_PITCHRATE_YAWRATE_THROTTLE: + control.mode = CommLinkInterface::OffboardControl::Mode::ROLLRATE_PITCHRATE_YAWRATE_THROTTLE; + break; + case MODE_ROLL_PITCH_YAWRATE_THROTTLE: + control.mode = CommLinkInterface::OffboardControl::Mode::ROLL_PITCH_YAWRATE_THROTTLE; + break; + default: + // invalid mode; ignore message and return without calling callback + return; } control.x.value = ctrl.x; @@ -513,11 +493,10 @@ void Mavlink::handle_msg_offboard_control(const mavlink_message_t *const msg) control.z.valid = !(ctrl.ignore & IGNORE_VALUE3); control.F.valid = !(ctrl.ignore & IGNORE_VALUE4); - if (listener_ != nullptr) - listener_->offboard_control_callback(control); + if (listener_ != nullptr) listener_->offboard_control_callback(control); } -void Mavlink::handle_msg_external_attitude(const mavlink_message_t *const msg) +void Mavlink::handle_msg_external_attitude(const mavlink_message_t * const msg) { mavlink_external_attitude_t q_msg; mavlink_msg_external_attitude_decode(msg, &q_msg); @@ -528,52 +507,49 @@ void Mavlink::handle_msg_external_attitude(const mavlink_message_t *const msg) q_extatt.y = q_msg.qy; q_extatt.z = q_msg.qz; - if (listener_ != nullptr) - listener_->external_attitude_callback(q_extatt); + if (listener_ != nullptr) listener_->external_attitude_callback(q_extatt); } -void Mavlink::handle_msg_heartbeat(const mavlink_message_t *const msg) +void Mavlink::handle_msg_heartbeat(const mavlink_message_t * const msg) { // none of the information from the heartbeat is used - (void)msg; + (void) msg; - if (listener_ != nullptr) - listener_->heartbeat_callback(); + if (listener_ != nullptr) listener_->heartbeat_callback(); } void Mavlink::handle_mavlink_message() { - switch (in_buf_.msgid) - { - case MAVLINK_MSG_ID_OFFBOARD_CONTROL: - handle_msg_offboard_control(&in_buf_); - break; - case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: - handle_msg_param_request_list(&in_buf_); - break; - case MAVLINK_MSG_ID_PARAM_REQUEST_READ: - handle_msg_param_request_read(&in_buf_); - break; - case MAVLINK_MSG_ID_PARAM_SET: - handle_msg_param_set(&in_buf_); - break; - case MAVLINK_MSG_ID_ROSFLIGHT_CMD: - handle_msg_rosflight_cmd(&in_buf_); - break; - case MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD: - handle_msg_rosflight_aux_cmd(&in_buf_); - break; - case MAVLINK_MSG_ID_TIMESYNC: - handle_msg_timesync(&in_buf_); - break; - case MAVLINK_MSG_ID_EXTERNAL_ATTITUDE: - handle_msg_external_attitude(&in_buf_); - break; - case MAVLINK_MSG_ID_HEARTBEAT: - handle_msg_heartbeat(&in_buf_); - break; - default: - break; + switch (in_buf_.msgid) { + case MAVLINK_MSG_ID_OFFBOARD_CONTROL: + handle_msg_offboard_control(&in_buf_); + break; + case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: + handle_msg_param_request_list(&in_buf_); + break; + case MAVLINK_MSG_ID_PARAM_REQUEST_READ: + handle_msg_param_request_read(&in_buf_); + break; + case MAVLINK_MSG_ID_PARAM_SET: + handle_msg_param_set(&in_buf_); + break; + case MAVLINK_MSG_ID_ROSFLIGHT_CMD: + handle_msg_rosflight_cmd(&in_buf_); + break; + case MAVLINK_MSG_ID_ROSFLIGHT_AUX_CMD: + handle_msg_rosflight_aux_cmd(&in_buf_); + break; + case MAVLINK_MSG_ID_TIMESYNC: + handle_msg_timesync(&in_buf_); + break; + case MAVLINK_MSG_ID_EXTERNAL_ATTITUDE: + handle_msg_external_attitude(&in_buf_); + break; + case MAVLINK_MSG_ID_HEARTBEAT: + handle_msg_heartbeat(&in_buf_); + break; + default: + break; } } diff --git a/comms/mavlink/mavlink.h b/comms/mavlink/mavlink.h index 951609a5..f2e017f3 100644 --- a/comms/mavlink/mavlink.h +++ b/comms/mavlink/mavlink.h @@ -53,84 +53,70 @@ class Board; class Mavlink : public CommLinkInterface { public: - Mavlink(Board &board); + Mavlink(Board & board); void init(uint32_t baud_rate, uint32_t dev) override; void receive() override; - void send_attitude_quaternion(uint8_t system_id, - uint64_t timestamp_us, - const turbomath::Quaternion &attitude, - const turbomath::Vector &angular_velocity) override; + void send_attitude_quaternion(uint8_t system_id, uint64_t timestamp_us, + const turbomath::Quaternion & attitude, + const turbomath::Vector & angular_velocity) override; void send_baro(uint8_t system_id, float altitude, float pressure, float temperature) override; void send_command_ack(uint8_t system_id, Command command, bool success) override; - void send_diff_pressure(uint8_t system_id, float velocity, float pressure, float temperature) override; + void send_diff_pressure(uint8_t system_id, float velocity, float pressure, + float temperature) override; void send_heartbeat(uint8_t system_id, bool fixed_wing) override; - void send_imu(uint8_t system_id, - uint64_t timestamp_us, - const turbomath::Vector &accel, - const turbomath::Vector &gyro, - float temperature) override; - void send_log_message(uint8_t system_id, LogSeverity severity, const char *text) override; - void send_mag(uint8_t system_id, const turbomath::Vector &mag) override; - void send_named_value_int(uint8_t system_id, uint32_t timestamp_ms, const char *const name, int32_t value) override; - void send_named_value_float(uint8_t system_id, uint32_t timestamp_ms, const char *const name, float value) override; - void send_output_raw(uint8_t system_id, uint32_t timestamp_ms, const float raw_outputs[14]) override; - void send_param_value_int(uint8_t system_id, - uint16_t index, - const char *const name, - int32_t value, - uint16_t param_count) override; - void send_param_value_float(uint8_t system_id, - uint16_t index, - const char *const name, - float value, - uint16_t param_count) override; + void send_imu(uint8_t system_id, uint64_t timestamp_us, const turbomath::Vector & accel, + const turbomath::Vector & gyro, float temperature) override; + void send_log_message(uint8_t system_id, LogSeverity severity, const char * text) override; + void send_mag(uint8_t system_id, const turbomath::Vector & mag) override; + void send_named_value_int(uint8_t system_id, uint32_t timestamp_ms, const char * const name, + int32_t value) override; + void send_named_value_float(uint8_t system_id, uint32_t timestamp_ms, const char * const name, + float value) override; + void send_output_raw(uint8_t system_id, uint32_t timestamp_ms, + const float raw_outputs[14]) override; + void send_param_value_int(uint8_t system_id, uint16_t index, const char * const name, + int32_t value, uint16_t param_count) override; + void send_param_value_float(uint8_t system_id, uint16_t index, const char * const name, + float value, uint16_t param_count) override; void send_rc_raw(uint8_t system_id, uint32_t timestamp_ms, const uint16_t channels[8]) override; void send_sonar(uint8_t system_id, - /* TODO enum type*/ uint8_t type, - float range, - float max_range, + /* TODO enum type*/ uint8_t type, float range, float max_range, float min_range) override; - void send_status(uint8_t system_id, - bool armed, - bool failsafe, - bool rc_override, - bool offboard, - uint8_t error_code, - uint8_t control_mode, - int16_t num_errors, + void send_status(uint8_t system_id, bool armed, bool failsafe, bool rc_override, bool offboard, + uint8_t error_code, uint8_t control_mode, int16_t num_errors, int16_t loop_time_us) override; void send_timesync(uint8_t system_id, int64_t tc1, int64_t ts1) override; - void send_version(uint8_t system_id, const char *const version) override; - void send_gnss(uint8_t system_id, const GNSSData &data) override; - void send_gnss_full(uint8_t system_id, const GNSSFull &full) override; - void send_error_data(uint8_t system_id, const StateManager::BackupData &error_data) override; + void send_version(uint8_t system_id, const char * const version) override; + void send_gnss(uint8_t system_id, const GNSSData & data) override; + void send_gnss_full(uint8_t system_id, const GNSSFull & full) override; + void send_error_data(uint8_t system_id, const StateManager::BackupData & error_data) override; void send_battery_status(uint8_t system_id, float voltage, float current) override; - inline void set_listener(ListenerInterface *listener) override { listener_ = listener; } + inline void set_listener(ListenerInterface * listener) override { listener_ = listener; } private: - void send_message(const mavlink_message_t &msg); + void send_message(const mavlink_message_t & msg); - void handle_msg_param_request_list(const mavlink_message_t *const msg); - void handle_msg_param_request_read(const mavlink_message_t *const msg); - void handle_msg_param_set(const mavlink_message_t *const msg); - void handle_msg_offboard_control(const mavlink_message_t *const msg); - void handle_msg_external_attitude(const mavlink_message_t *const msg); - void handle_msg_rosflight_cmd(const mavlink_message_t *const msg); - void handle_msg_rosflight_aux_cmd(const mavlink_message_t *const msg); - void handle_msg_timesync(const mavlink_message_t *const msg); - void handle_msg_heartbeat(const mavlink_message_t *const msg); + void handle_msg_param_request_list(const mavlink_message_t * const msg); + void handle_msg_param_request_read(const mavlink_message_t * const msg); + void handle_msg_param_set(const mavlink_message_t * const msg); + void handle_msg_offboard_control(const mavlink_message_t * const msg); + void handle_msg_external_attitude(const mavlink_message_t * const msg); + void handle_msg_rosflight_cmd(const mavlink_message_t * const msg); + void handle_msg_rosflight_aux_cmd(const mavlink_message_t * const msg); + void handle_msg_timesync(const mavlink_message_t * const msg); + void handle_msg_heartbeat(const mavlink_message_t * const msg); void handle_mavlink_message(); - Board &board_; + Board & board_; uint32_t compid_ = 250; mavlink_message_t in_buf_; mavlink_status_t status_; bool initialized_ = false; - ListenerInterface *listener_ = nullptr; + ListenerInterface * listener_ = nullptr; }; } // namespace rosflight_firmware diff --git a/include/board.h b/include/board.h index 64e0c455..6d645a74 100644 --- a/include/board.h +++ b/include/board.h @@ -61,7 +61,7 @@ class Board // serial virtual void serial_init(uint32_t baud_rate, uint32_t dev) = 0; - virtual void serial_write(const uint8_t *src, size_t len) = 0; + virtual void serial_write(const uint8_t * src, size_t len) = 0; virtual uint16_t serial_bytes_available() = 0; virtual uint8_t serial_read() = 0; virtual void serial_flush() = 0; @@ -71,7 +71,7 @@ class Board virtual uint16_t num_sensor_errors() = 0; virtual bool new_imu_data() = 0; - virtual bool imu_read(float accel[3], float *temperature, float gyro[3], uint64_t *time) = 0; + virtual bool imu_read(float accel[3], float * temperature, float gyro[3], uint64_t * time) = 0; virtual void imu_not_responding_error() = 0; virtual bool mag_present() = 0; @@ -80,11 +80,11 @@ class Board virtual bool baro_present() = 0; virtual void baro_update() = 0; - virtual void baro_read(float *pressure, float *temperature) = 0; + virtual void baro_read(float * pressure, float * temperature) = 0; virtual bool diff_pressure_present() = 0; virtual void diff_pressure_update() = 0; - virtual void diff_pressure_read(float *diff_pressure, float *temperature) = 0; + virtual void diff_pressure_read(float * diff_pressure, float * temperature) = 0; virtual bool sonar_present() = 0; virtual void sonar_update() = 0; @@ -117,8 +117,8 @@ class Board // non-volatile memory virtual void memory_init() = 0; - virtual bool memory_read(void *dest, size_t len) = 0; - virtual bool memory_write(const void *src, size_t len) = 0; + virtual bool memory_read(void * dest, size_t len) = 0; + virtual bool memory_write(const void * src, size_t len) = 0; // LEDs virtual void led0_on() = 0; @@ -131,8 +131,8 @@ class Board // Backup memory virtual void backup_memory_init() = 0; - virtual bool backup_memory_read(void *dest, size_t len) = 0; - virtual void backup_memory_write(const void *src, size_t len) = 0; + virtual bool backup_memory_read(void * dest, size_t len) = 0; + virtual void backup_memory_write(const void * src, size_t len) = 0; virtual void backup_memory_clear(size_t len) = 0; }; diff --git a/include/comm_manager.h b/include/comm_manager.h index c84ffcf0..ddb2736b 100644 --- a/include/comm_manager.h +++ b/include/comm_manager.h @@ -80,8 +80,8 @@ class CommManager : public CommLinkInterface::ListenerInterface, public ParamLis uint8_t sysid_; uint64_t offboard_control_time_; - ROSflight& RF_; - CommLinkInterface& comm_link_; + ROSflight & RF_; + CommLinkInterface & comm_link_; uint8_t send_params_index_; bool initialized_ = false; bool connected_ = false; @@ -102,7 +102,7 @@ class CommManager : public CommLinkInterface::ListenerInterface, public ParamLis size_t size() const { return length_; } bool empty() const { return length_ == 0; } bool full() const { return length_ == LOG_BUF_SIZE; } - const LogMessage& oldest() const { return buffer_[oldest_]; } + const LogMessage & oldest() const { return buffer_[oldest_]; } void pop(); private: @@ -132,14 +132,17 @@ class CommManager : public CommLinkInterface::ListenerInterface, public ParamLis void update_system_id(uint16_t param_id); void param_request_list_callback(uint8_t target_system) override; - void param_request_read_callback(uint8_t target_system, const char* const param_name, int16_t param_index) override; - void param_set_int_callback(uint8_t target_system, const char* const param_name, int32_t param_value) override; - void param_set_float_callback(uint8_t target_system, const char* const param_name, float param_value) override; + void param_request_read_callback(uint8_t target_system, const char * const param_name, + int16_t param_index) override; + void param_set_int_callback(uint8_t target_system, const char * const param_name, + int32_t param_value) override; + void param_set_float_callback(uint8_t target_system, const char * const param_name, + float param_value) override; void command_callback(CommLinkInterface::Command command) override; void timesync_callback(int64_t tc1, int64_t ts1) override; - void offboard_control_callback(const CommLinkInterface::OffboardControl& control) override; - void aux_command_callback(const CommLinkInterface::AuxCommand& command) override; - void external_attitude_callback(const turbomath::Quaternion& q) override; + void offboard_control_callback(const CommLinkInterface::OffboardControl & control) override; + void aux_command_callback(const CommLinkInterface::AuxCommand & command) override; + void external_attitude_callback(const turbomath::Quaternion & q) override; void heartbeat_callback() override; void send_heartbeat(void); @@ -158,26 +161,32 @@ class CommManager : public CommLinkInterface::ListenerInterface, public ParamLis void send_low_priority(void); // Debugging Utils - void send_named_value_int(const char* const name, int32_t value); + void send_named_value_int(const char * const name, int32_t value); // void send_named_command_struct(const char *const name, control_t command_struct); void send_next_param(void); - Stream streams_[STREAM_COUNT] = { - Stream(0, [this] { this->send_heartbeat(); }), Stream(0, [this] { this->send_status(); }), - Stream(0, [this] { this->send_attitude(); }), Stream(0, [this] { this->send_imu(); }), - Stream(0, [this] { this->send_diff_pressure(); }), Stream(0, [this] { this->send_baro(); }), - Stream(0, [this] { this->send_sonar(); }), Stream(0, [this] { this->send_mag(); }), - Stream(0, [this] { this->send_battery_status(); }), Stream(0, [this] { this->send_output_raw(); }), - Stream(0, [this] { this->send_gnss(); }), Stream(0, [this] { this->send_gnss_full(); }), - Stream(0, [this] { this->send_rc_raw(); }), Stream(20000, [this] { this->send_low_priority(); })}; + Stream streams_[STREAM_COUNT] = {Stream(0, [this] { this->send_heartbeat(); }), + Stream(0, [this] { this->send_status(); }), + Stream(0, [this] { this->send_attitude(); }), + Stream(0, [this] { this->send_imu(); }), + Stream(0, [this] { this->send_diff_pressure(); }), + Stream(0, [this] { this->send_baro(); }), + Stream(0, [this] { this->send_sonar(); }), + Stream(0, [this] { this->send_mag(); }), + Stream(0, [this] { this->send_battery_status(); }), + Stream(0, [this] { this->send_output_raw(); }), + Stream(0, [this] { this->send_gnss(); }), + Stream(0, [this] { this->send_gnss_full(); }), + Stream(0, [this] { this->send_rc_raw(); }), + Stream(20000, [this] { this->send_low_priority(); })}; // the time of week stamp for the last sent GNSS message, to prevent re-sending uint32_t last_sent_gnss_tow_ = 0; uint32_t last_sent_gnss_full_tow_ = 0; public: - CommManager(ROSflight& rf, CommLinkInterface& comm_link); + CommManager(ROSflight & rf, CommLinkInterface & comm_link); void init(); void param_change_callback(uint16_t param_id) override; @@ -186,12 +195,12 @@ class CommManager : public CommLinkInterface::ListenerInterface, public ParamLis void send_param_value(uint16_t param_id); void set_streaming_rate(uint8_t stream_id, int16_t param_id); void update_status(); - void log(CommLinkInterface::LogSeverity severity, const char* fmt, ...); + void log(CommLinkInterface::LogSeverity severity, const char * fmt, ...); void send_parameter_list(); - void send_named_value_float(const char* const name, float value); + void send_named_value_float(const char * const name, float value); - void send_backup_data(const StateManager::BackupData& backup_data); + void send_backup_data(const StateManager::BackupData & backup_data); }; } // namespace rosflight_firmware diff --git a/include/command_manager.h b/include/command_manager.h index 0ba8d8de..4f39564e 100644 --- a/include/command_manager.h +++ b/include/command_manager.h @@ -72,9 +72,9 @@ class CommandManager : public ParamListenerInterface private: typedef struct { - control_channel_t *rc; - control_channel_t *onboard; - control_channel_t *combined; + control_channel_t * rc; + control_channel_t * onboard; + control_channel_t * combined; } mux_t; mux_t muxes[4] = {{&rc_command_.x, &offboard_command_.x, &combined_command_.x}, @@ -131,14 +131,16 @@ class CommandManager : public ParamListenerInterface uint32_t last_override_time; } rc_stick_override_t; - rc_stick_override_t rc_stick_override_[3] = {{RC::STICK_X, 0}, {RC::STICK_Y, 0}, {RC::STICK_Z, 0}}; + rc_stick_override_t rc_stick_override_[3] = {{RC::STICK_X, 0}, + {RC::STICK_Y, 0}, + {RC::STICK_Z, 0}}; - ROSflight &RF_; + ROSflight & RF_; bool new_command_; bool rc_override_; - control_t &failsafe_command_; + control_t & failsafe_command_; void param_change_callback(uint16_t param_id) override; void init_failsafe(); @@ -151,7 +153,7 @@ class CommandManager : public ParamListenerInterface bool stick_deviated(MuxChannel channel); public: - CommandManager(ROSflight &_rf); + CommandManager(ROSflight & _rf); void init(); bool run(); bool rc_override_active(); @@ -159,8 +161,8 @@ class CommandManager : public ParamListenerInterface void set_new_offboard_command(control_t new_offboard_command); void set_new_rc_command(control_t new_rc_command); void override_combined_command_with_rc(); - inline const control_t &combined_control() const { return combined_command_; } - inline const control_t &rc_control() const { return rc_command_; } + inline const control_t & combined_control() const { return combined_command_; } + inline const control_t & rc_control() const { return rc_command_; } }; } // namespace rosflight_firmware diff --git a/include/controller.h b/include/controller.h index 3160ee74..b8a9e9d1 100644 --- a/include/controller.h +++ b/include/controller.h @@ -57,9 +57,9 @@ class Controller : public ParamListenerInterface float z; }; - Controller(ROSflight &rf); + Controller(ROSflight & rf); - inline const Output &output() const { return output_; } + inline const Output & output() const { return output_; } void init(); void run(); @@ -90,12 +90,10 @@ class Controller : public ParamListenerInterface float tau_; }; - ROSflight &RF_; + ROSflight & RF_; - turbomath::Vector run_pid_loops(uint32_t dt, - const Estimator::State &state, - const control_t &command, - bool update_integrators); + turbomath::Vector run_pid_loops(uint32_t dt, const Estimator::State & state, + const control_t & command, bool update_integrators); Output output_; diff --git a/include/estimator.h b/include/estimator.h index b60d457a..ad402a19 100644 --- a/include/estimator.h +++ b/include/estimator.h @@ -57,27 +57,27 @@ class Estimator : public ParamListenerInterface uint64_t timestamp_us; }; - Estimator(ROSflight& _rf); + Estimator(ROSflight & _rf); - inline const State& state() const { return state_; } + inline const State & state() const { return state_; } - inline const turbomath::Vector& bias() { return bias_; } + inline const turbomath::Vector & bias() { return bias_; } - inline const turbomath::Vector& accLPF() { return accel_LPF_; } + inline const turbomath::Vector & accLPF() { return accel_LPF_; } - inline const turbomath::Vector& gyroLPF() { return gyro_LPF_; } + inline const turbomath::Vector & gyroLPF() { return gyro_LPF_; } void init(); void param_change_callback(uint16_t param_id) override; void run(); void reset_state(); void reset_adaptive_bias(); - void set_external_attitude_update(const turbomath::Quaternion& q); + void set_external_attitude_update(const turbomath::Quaternion & q); private: const turbomath::Vector g_ = {0.0f, 0.0f, -1.0f}; - ROSflight& RF_; + ROSflight & RF_; State state_; uint64_t last_time_; @@ -104,11 +104,10 @@ class Estimator : public ParamListenerInterface turbomath::Vector accel_correction() const; turbomath::Vector extatt_correction() const; turbomath::Vector smoothed_gyro_measurement(); - void integrate_angular_rate(turbomath::Quaternion& quat, const turbomath::Vector& omega, const float dt) const; - void quaternion_to_dcm(const turbomath::Quaternion& q, - turbomath::Vector& X, - turbomath::Vector& Y, - turbomath::Vector& Z) const; + void integrate_angular_rate(turbomath::Quaternion & quat, const turbomath::Vector & omega, + const float dt) const; + void quaternion_to_dcm(const turbomath::Quaternion & q, turbomath::Vector & X, + turbomath::Vector & Y, turbomath::Vector & Z) const; }; } // namespace rosflight_firmware diff --git a/include/interface/comm_link.h b/include/interface/comm_link.h index a9ad4abc..c5a156e0 100644 --- a/include/interface/comm_link.h +++ b/include/interface/comm_link.h @@ -113,16 +113,17 @@ class CommLinkInterface { public: virtual void param_request_list_callback(uint8_t target_system) = 0; - virtual void param_request_read_callback(uint8_t target_system, - const char *const param_name, + virtual void param_request_read_callback(uint8_t target_system, const char * const param_name, int16_t param_index) = 0; - virtual void param_set_int_callback(uint8_t target_system, const char *const param_name, int32_t param_value) = 0; - virtual void param_set_float_callback(uint8_t target_system, const char *const param_name, float param_value) = 0; + virtual void param_set_int_callback(uint8_t target_system, const char * const param_name, + int32_t param_value) = 0; + virtual void param_set_float_callback(uint8_t target_system, const char * const param_name, + float param_value) = 0; virtual void command_callback(Command command) = 0; virtual void timesync_callback(int64_t tc1, int64_t ts1) = 0; - virtual void offboard_control_callback(const OffboardControl &control) = 0; - virtual void aux_command_callback(const AuxCommand &command) = 0; - virtual void external_attitude_callback(const turbomath::Quaternion &q) = 0; + virtual void offboard_control_callback(const OffboardControl & control) = 0; + virtual void aux_command_callback(const AuxCommand & command) = 0; + virtual void external_attitude_callback(const turbomath::Quaternion & q) = 0; virtual void heartbeat_callback() = 0; }; @@ -131,64 +132,45 @@ class CommLinkInterface // send functions - virtual void send_attitude_quaternion(uint8_t system_id, - uint64_t timestamp_us, - const turbomath::Quaternion &attitude, - const turbomath::Vector &angular_velocity) = 0; + virtual void send_attitude_quaternion(uint8_t system_id, uint64_t timestamp_us, + const turbomath::Quaternion & attitude, + const turbomath::Vector & angular_velocity) = 0; virtual void send_baro(uint8_t system_id, float altitude, float pressure, float temperature) = 0; virtual void send_command_ack(uint8_t system_id, Command command, bool success) = 0; - virtual void send_diff_pressure(uint8_t system_id, float velocity, float pressure, float temperature) = 0; + virtual void send_diff_pressure(uint8_t system_id, float velocity, float pressure, + float temperature) = 0; virtual void send_heartbeat(uint8_t system_id, bool fixed_wing) = 0; - virtual void send_imu(uint8_t system_id, - uint64_t timestamp_us, - const turbomath::Vector &accel, - const turbomath::Vector &gyro, - float temperature) = 0; - virtual void send_log_message(uint8_t system_id, LogSeverity severity, const char *text) = 0; - virtual void send_mag(uint8_t system_id, const turbomath::Vector &mag) = 0; - virtual void send_named_value_int(uint8_t system_id, - uint32_t timestamp_ms, - const char *const name, - int32_t value) = 0; - virtual void send_named_value_float(uint8_t system_id, - uint32_t timestamp_ms, - const char *const name, - float value) = 0; - virtual void send_output_raw(uint8_t system_id, uint32_t timestamp_ms, const float raw_outputs[14]) = 0; - virtual void send_param_value_int(uint8_t system_id, - uint16_t index, - const char *const name, - int32_t value, - uint16_t param_count) = 0; - virtual void send_param_value_float(uint8_t system_id, - uint16_t index, - const char *const name, - float value, - uint16_t param_count) = 0; - virtual void send_rc_raw(uint8_t system_id, uint32_t timestamp_ms, const uint16_t channels[8]) = 0; + virtual void send_imu(uint8_t system_id, uint64_t timestamp_us, const turbomath::Vector & accel, + const turbomath::Vector & gyro, float temperature) = 0; + virtual void send_log_message(uint8_t system_id, LogSeverity severity, const char * text) = 0; + virtual void send_mag(uint8_t system_id, const turbomath::Vector & mag) = 0; + virtual void send_named_value_int(uint8_t system_id, uint32_t timestamp_ms, + const char * const name, int32_t value) = 0; + virtual void send_named_value_float(uint8_t system_id, uint32_t timestamp_ms, + const char * const name, float value) = 0; + virtual void send_output_raw(uint8_t system_id, uint32_t timestamp_ms, + const float raw_outputs[14]) = 0; + virtual void send_param_value_int(uint8_t system_id, uint16_t index, const char * const name, + int32_t value, uint16_t param_count) = 0; + virtual void send_param_value_float(uint8_t system_id, uint16_t index, const char * const name, + float value, uint16_t param_count) = 0; + virtual void send_rc_raw(uint8_t system_id, uint32_t timestamp_ms, + const uint16_t channels[8]) = 0; virtual void send_sonar(uint8_t system_id, - /* TODO enum type*/ uint8_t type, - float range, - float max_range, + /* TODO enum type*/ uint8_t type, float range, float max_range, float min_range) = 0; - virtual void send_status(uint8_t system_id, - bool armed, - bool failsafe, - bool rc_override, - bool offboard, - uint8_t error_code, - uint8_t control_mode, - int16_t num_errors, - int16_t loop_time_us) = 0; + virtual void send_status(uint8_t system_id, bool armed, bool failsafe, bool rc_override, + bool offboard, uint8_t error_code, uint8_t control_mode, + int16_t num_errors, int16_t loop_time_us) = 0; virtual void send_timesync(uint8_t system_id, int64_t tc1, int64_t ts1) = 0; - virtual void send_version(uint8_t system_id, const char *const version) = 0; - virtual void send_gnss(uint8_t system_id, const GNSSData &data) = 0; - virtual void send_gnss_full(uint8_t system_id, const GNSSFull &data) = 0; - virtual void send_error_data(uint8_t system_id, const StateManager::BackupData &error_data) = 0; + virtual void send_version(uint8_t system_id, const char * const version) = 0; + virtual void send_gnss(uint8_t system_id, const GNSSData & data) = 0; + virtual void send_gnss_full(uint8_t system_id, const GNSSFull & data) = 0; + virtual void send_error_data(uint8_t system_id, const StateManager::BackupData & error_data) = 0; virtual void send_battery_status(uint8_t system_id, float voltage, float current) = 0; // register listener - virtual void set_listener(ListenerInterface *listener) = 0; + virtual void set_listener(ListenerInterface * listener) = 0; }; } // namespace rosflight_firmware diff --git a/include/mixer.h b/include/mixer.h index a17cf78c..df6397e1 100644 --- a/include/mixer.h +++ b/include/mixer.h @@ -95,7 +95,7 @@ class Mixer : public ParamListenerInterface } aux_command_t; private: - ROSflight& RF_; + ROSflight & RF_; float raw_outputs_[NUM_TOTAL_OUTPUTS]; float outputs_[NUM_TOTAL_OUTPUTS]; @@ -112,13 +112,14 @@ class Mixer : public ParamListenerInterface {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix 490}; - const mixer_t quadcopter_plus_mixing = {{M, M, M, M, NONE, NONE, NONE, NONE}, // output_type + const mixer_t quadcopter_plus_mixing = { + {M, M, M, M, NONE, NONE, NONE, NONE}, // output_type - {1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix - {0.0f, -1.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix - {1.0f, 0.0f, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix - {1.0f, -1.0f, 1.0f, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Z Mix - 490}; + {1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix + {0.0f, -1.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix + {1.0f, 0.0f, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix + {1.0f, -1.0f, 1.0f, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Z Mix + 490}; const mixer_t quadcopter_x_mixing = {{M, M, M, M, NONE, NONE, NONE, NONE}, // output_type @@ -128,45 +129,50 @@ class Mixer : public ParamListenerInterface {1.0f, -1.0f, 1.0f, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Z Mix 490}; - const mixer_t hex_plus_mixing = {{M, M, M, M, M, M, M, M}, // output_type + const mixer_t hex_plus_mixing = { + {M, M, M, M, M, M, M, M}, // output_type - {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f}, // F Mix - {0.0f, -0.866025f, -0.866025f, 0.0f, 0.866025f, 0.866025f, 0.0f, 0.0f}, // X Mix - {1.0f, 0.5f, -0.5f, -1.0f, -0.5f, 0.5f, 0.0f, 0.0f}, // Y Mix - {1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 0.0f, 0.0f}, // Z Mix - 490}; + {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f}, // F Mix + {0.0f, -0.866025f, -0.866025f, 0.0f, 0.866025f, 0.866025f, 0.0f, 0.0f}, // X Mix + {1.0f, 0.5f, -0.5f, -1.0f, -0.5f, 0.5f, 0.0f, 0.0f}, // Y Mix + {1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 0.0f, 0.0f}, // Z Mix + 490}; - const mixer_t hex_x_mixing = {{M, M, M, M, M, M, M, M}, // output_type + const mixer_t hex_x_mixing = { + {M, M, M, M, M, M, M, M}, // output_type - {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f}, // F Mix - {-0.5f, -1.0f, -0.5f, 0.5f, 1.0f, 0.5f, 0.0f, 0.0f}, // X Mix - {0.866025f, 0.0f, -0.866025f, -0.866025f, 0.0f, 0.866025f, 0.0f, 0.0f}, // Y Mix - {1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 0.0f, 0.0f}, // Z Mix - 490}; + {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f}, // F Mix + {-0.5f, -1.0f, -0.5f, 0.5f, 1.0f, 0.5f, 0.0f, 0.0f}, // X Mix + {0.866025f, 0.0f, -0.866025f, -0.866025f, 0.0f, 0.866025f, 0.0f, 0.0f}, // Y Mix + {1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 0.0f, 0.0f}, // Z Mix + 490}; - const mixer_t octocopter_plus_mixing = {{M, M, M, M, M, M, M, M}, // output_type + const mixer_t octocopter_plus_mixing = { + {M, M, M, M, M, M, M, M}, // output_type - {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f}, // F Mix - {0.0f, -0.707f, -1.0f, -0.707f, 0.0f, 0.707f, 1.0f, 0.707f}, // X Mix - {1.0f, 0.707f, 0.0f, -0.707f, -1.0f, -0.707f, 0.0f, 0.707f}, // Y Mix - {1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f}, // Z Mix - 490}; + {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f}, // F Mix + {0.0f, -0.707f, -1.0f, -0.707f, 0.0f, 0.707f, 1.0f, 0.707f}, // X Mix + {1.0f, 0.707f, 0.0f, -0.707f, -1.0f, -0.707f, 0.0f, 0.707f}, // Y Mix + {1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f}, // Z Mix + 490}; - const mixer_t octocopter_x_mixing = {{M, M, M, M, M, M, M, M}, // output_type + const mixer_t octocopter_x_mixing = { + {M, M, M, M, M, M, M, M}, // output_type - {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f}, // F Mix - {-0.414f, -1.0f, -1.0f, -0.414f, 0.414f, 1.0f, 1.0f, 0.414}, // X Mix - {1.0f, 0.414f, -0.414f, -1.0f, -1.0f, -0.414f, 0.414f, 1.0}, // Y Mix - {1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f}, // Z Mix - 490}; + {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f}, // F Mix + {-0.414f, -1.0f, -1.0f, -0.414f, 0.414f, 1.0f, 1.0f, 0.414}, // X Mix + {1.0f, 0.414f, -0.414f, -1.0f, -1.0f, -0.414f, 0.414f, 1.0}, // Y Mix + {1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f}, // Z Mix + 490}; - const mixer_t Y6_mixing = {{M, M, M, M, M, M, NONE, NONE}, // output_type + const mixer_t Y6_mixing = { + {M, M, M, M, M, M, NONE, NONE}, // output_type - {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f}, // F Mix - {-1.0f, -1.0f, 0.0f, 0.0f, 1.0f, 1.0f, 0.0f, 0.0f}, // X Mix - {0.667f, 0.667f, -1.333f, -1.333f, 0.667f, 0.667f, 0.0f, 0.0f}, // Y Mix - {1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 0.0f, 0.0f}, // Z Mix - 490}; + {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f}, // F Mix + {-1.0f, -1.0f, 0.0f, 0.0f, 1.0f, 1.0f, 0.0f, 0.0f}, // X Mix + {0.667f, 0.667f, -1.333f, -1.333f, 0.667f, 0.667f, 0.0f, 0.0f}, // Y Mix + {1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 0.0f, 0.0f}, // Z Mix + 490}; const mixer_t X8_mixing = {{M, M, M, M, M, M, M, M}, // output_type @@ -176,13 +182,14 @@ class Mixer : public ParamListenerInterface {1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f}, // Z Mix 490}; - const mixer_t tricopter_mixing = {{M, M, M, S, NONE, NONE, NONE, NONE}, // output_type + const mixer_t tricopter_mixing = { + {M, M, M, S, NONE, NONE, NONE, NONE}, // output_type - {1.0f, 0.0f, 1.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix - {-1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix - {0.667f, 0.0f, 0.667f, -1.333f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix - {0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Z Mix - 490}; + {1.0f, 0.0f, 1.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix + {-1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix + {0.667f, 0.0f, 0.667f, -1.333f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix + {0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Z Mix + 490}; const mixer_t fixedwing_mixing = {{S, S, M, S, S, M, NONE, NONE}, @@ -200,7 +207,7 @@ class Mixer : public ParamListenerInterface {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Z Mix 50}; - const mixer_t* mixer_to_use_; + const mixer_t * mixer_to_use_; // clang-format off const mixer_t* array_of_mixers_[NUM_MIXERS] = {&esc_calibration_mixing, @@ -218,14 +225,14 @@ class Mixer : public ParamListenerInterface // clang-format on public: - Mixer(ROSflight& _rf); + Mixer(ROSflight & _rf); void init(); void init_PWM(); void init_mixing(); void mix_output(); void param_change_callback(uint16_t param_id) override; void set_new_aux_command(aux_command_t new_aux_command); - inline const float* get_outputs() const { return raw_outputs_; } + inline const float * get_outputs() const { return raw_outputs_; } }; } // namespace rosflight_firmware diff --git a/include/param.h b/include/param.h index 9233d363..6d1bb346 100644 --- a/include/param.h +++ b/include/param.h @@ -254,17 +254,17 @@ class Params } params_t; params_t params; - ROSflight &RF_; + ROSflight & RF_; void init_param_int(uint16_t id, const char name[PARAMS_NAME_LENGTH], int32_t value); void init_param_float(uint16_t id, const char name[PARAMS_NAME_LENGTH], float value); uint8_t compute_checksum(void); - ParamListenerInterface *const *listeners_; + ParamListenerInterface * const * listeners_; size_t num_listeners_; public: - Params(ROSflight &_rf); + Params(ROSflight & _rf); // function declarations @@ -284,7 +284,7 @@ class Params * interface * @param num_listeners The length of the array passed as the listeners parameter */ - void set_listeners(ParamListenerInterface *const listeners[], size_t num_listeners); + void set_listeners(ParamListenerInterface * const listeners[], size_t num_listeners); /** * @brief Read parameter values from non-volatile memory @@ -330,7 +330,7 @@ class Params * @param id The ID of the parameter * @return The name of the parameter */ - inline const char *get_param_name(uint16_t id) const { return params.names[id]; } + inline const char * get_param_name(uint16_t id) const { return params.names[id]; } /** * @brief Get the type of a parameter diff --git a/include/rc.h b/include/rc.h index 1cb86ed5..1ddee92f 100644 --- a/include/rc.h +++ b/include/rc.h @@ -62,7 +62,7 @@ class RC : public ParamListenerInterface SWITCHES_COUNT }; - RC(ROSflight &_rf); + RC(ROSflight & _rf); void init(); float stick(Stick channel); @@ -73,7 +73,7 @@ class RC : public ParamListenerInterface void param_change_callback(uint16_t param_id) override; private: - ROSflight &RF_; + ROSflight & RF_; typedef struct { diff --git a/include/rosflight.h b/include/rosflight.h index 9b2927bc..778c651a 100644 --- a/include/rosflight.h +++ b/include/rosflight.h @@ -53,9 +53,9 @@ namespace rosflight_firmware class ROSflight { public: - ROSflight(Board& board, CommLinkInterface& comm_link); + ROSflight(Board & board, CommLinkInterface & comm_link); - Board& board_; + Board & board_; CommManager comm_manager_; Params params_; @@ -84,8 +84,8 @@ class ROSflight private: static constexpr size_t num_param_listeners_ = 7; - ParamListenerInterface* const param_listeners_[num_param_listeners_] = { - &comm_manager_, &command_manager_, &controller_, &estimator_, &mixer_, &rc_, &sensors_}; + ParamListenerInterface * const param_listeners_[num_param_listeners_] = { + &comm_manager_, &command_manager_, &controller_, &estimator_, &mixer_, &rc_, &sensors_}; }; } // namespace rosflight_firmware diff --git a/include/sensors.h b/include/sensors.h index bdd0f6f8..ec526d2f 100644 --- a/include/sensors.h +++ b/include/sensors.h @@ -162,10 +162,10 @@ class Sensors : public ParamListenerInterface float battery_current = 0; }; - Sensors(ROSflight &rosflight); + Sensors(ROSflight & rosflight); - inline const Data &data() const { return data_; } - void get_filtered_IMU(turbomath::Vector &accel, turbomath::Vector &gyro, uint64_t &stamp_us); + inline const Data & data() const { return data_; } + void get_filtered_IMU(turbomath::Vector & accel, turbomath::Vector & gyro, uint64_t & stamp_us); // function declarations void init(); @@ -181,8 +181,7 @@ class Sensors : public ParamListenerInterface inline bool should_send_imu_data(void) { - if (imu_data_sent_) - return false; + if (imu_data_sent_) return false; else imu_data_sent_ = true; return true; @@ -212,7 +211,7 @@ class Sensors : public ParamListenerInterface public: OutlierFilter() {} void init(float max_change_rate, float update_rate, float center); - bool update(float new_val, float *val); + bool update(float new_val, float * val); }; enum : uint8_t @@ -226,7 +225,7 @@ class Sensors : public ParamListenerInterface NUM_LOW_PRIORITY_SENSORS }; - ROSflight &rf_; + ROSflight & rf_; Data data_; diff --git a/include/state_manager.h b/include/state_manager.h index 73d85ec3..9283aa21 100644 --- a/include/state_manager.h +++ b/include/state_manager.h @@ -86,11 +86,12 @@ class StateManager struct __attribute__((packed)) BackupData { static constexpr uint32_t ARM_MAGIC = - 0xbad2fa11; //!< magic number to ensure we only arm on startup if we really intended to + 0xbad2fa11; //!< magic number to ensure we only arm on startup if we really intended to uint16_t reset_count = 0; //!< number of hard faults since normal system startup uint16_t error_code = 0; //!< state manager error codes - uint32_t arm_flag = 0; //!< set to ARM_MAGIC if the system was armed when the hard fault occured, 0 otherwise + uint32_t arm_flag = + 0; //!< set to ARM_MAGIC if the system was armed when the hard fault occured, 0 otherwise /** * @brief Low-level debugging information for case of hard fault @@ -119,7 +120,8 @@ class StateManager */ void finalize() { - checksum = checksum_fletcher16(reinterpret_cast(this), sizeof(BackupData) - sizeof(checksum)); + checksum = checksum_fletcher16(reinterpret_cast(this), + sizeof(BackupData) - sizeof(checksum)); } /** @@ -130,15 +132,17 @@ class StateManager */ bool valid_checksum() { - return checksum == checksum_fletcher16(reinterpret_cast(this), sizeof(BackupData) - sizeof(checksum)); + return checksum + == checksum_fletcher16(reinterpret_cast(this), + sizeof(BackupData) - sizeof(checksum)); } }; - StateManager(ROSflight& parent); + StateManager(ROSflight & parent); void init(); void run(); - inline const State& state() const { return state_; } + inline const State & state() const { return state_; } void set_event(Event event); void set_error(uint16_t error); @@ -155,12 +159,12 @@ class StateManager * * @param debug Low-level debugging data populated by the hardfault handler */ - void write_backup_data(const BackupData::DebugInfo& debug); + void write_backup_data(const BackupData::DebugInfo & debug); void check_backup_memory(); private: - ROSflight& RF_; + ROSflight & RF_; State state_; uint32_t next_led_blink_ms_ = 0; diff --git a/include/util.h b/include/util.h index 1868db11..6cb4ac05 100644 --- a/include/util.h +++ b/include/util.h @@ -49,20 +49,20 @@ namespace rosflight_firmware * non-contiguous data * @return uint16_t Fletcher 16-bit checksum */ -inline uint16_t checksum_fletcher16(const uint8_t *src, size_t len, bool finalize = true, uint16_t start = 0) +inline uint16_t checksum_fletcher16(const uint8_t * src, size_t len, bool finalize = true, + uint16_t start = 0) { - static constexpr size_t max_block_length = 5800; // guarantee that no overflow will occur (reduce from standard value - // to account for values in 'start') + static constexpr size_t max_block_length = + 5800; // guarantee that no overflow will occur (reduce from standard value + // to account for values in 'start') uint32_t c1 = (start & 0xFF00) >> 8; uint32_t c2 = start & 0x00FF; size_t block_length; - for (; len > 0; len -= block_length) - { + for (; len > 0; len -= block_length) { block_length = len < max_block_length ? len : max_block_length; - for (size_t i = 0; i < block_length; i++) - { + for (size_t i = 0; i < block_length; i++) { c1 += *(src++); c2 += c1; } @@ -73,8 +73,7 @@ inline uint16_t checksum_fletcher16(const uint8_t *src, size_t len, bool finaliz uint16_t checksum = c1 << 8 | c2; - if (finalize && checksum == 0) - checksum = 0xFFFF; + if (finalize && checksum == 0) checksum = 0xFFFF; return checksum; } diff --git a/lib/turbomath/turbomath.cpp b/lib/turbomath/turbomath.cpp index b9ca4ff8..68bbbba9 100644 --- a/lib/turbomath/turbomath.cpp +++ b/lib/turbomath/turbomath.cpp @@ -35,21 +35,23 @@ namespace turbomath { -Vector::Vector() : x(0.0f), y(0.0f), z(0.0f) {} +Vector::Vector() + : x(0.0f) + , y(0.0f) + , z(0.0f) +{} -Vector::Vector(float x_, float y_, float z_) : x(x_), y(y_), z(z_) {} +Vector::Vector(float x_, float y_, float z_) + : x(x_) + , y(y_) + , z(z_) +{} -float Vector::norm() const -{ - return 1.0f / inv_sqrt(x * x + y * y + z * z); -} +float Vector::norm() const { return 1.0f / inv_sqrt(x * x + y * y + z * z); } -float Vector::sqrd_norm() const -{ - return x * x + y * y + z * z; -} +float Vector::sqrd_norm() const { return x * x + y * y + z * z; } -Vector& Vector::normalize() +Vector & Vector::normalize() { float recip_norm = inv_sqrt(x * x + y * y + z * z); x *= recip_norm; @@ -65,17 +67,11 @@ Vector Vector::normalized() const return out; } -Vector Vector::operator+(const Vector& v) const -{ - return Vector(x + v.x, y + v.y, z + v.z); -} +Vector Vector::operator+(const Vector & v) const { return Vector(x + v.x, y + v.y, z + v.z); } -Vector Vector::operator-(const Vector& v) const -{ - return Vector(x - v.x, y - v.y, z - v.z); -} +Vector Vector::operator-(const Vector & v) const { return Vector(x - v.x, y - v.y, z - v.z); } -Vector& Vector::operator+=(const Vector& v) +Vector & Vector::operator+=(const Vector & v) { x += v.x; y += v.y; @@ -83,7 +79,7 @@ Vector& Vector::operator+=(const Vector& v) return *this; } -Vector& Vector::operator-=(const Vector& v) +Vector & Vector::operator-=(const Vector & v) { x -= v.x; y -= v.y; @@ -91,17 +87,11 @@ Vector& Vector::operator-=(const Vector& v) return *this; } -Vector Vector::operator*(float s) const -{ - return Vector(x * s, y * s, z * s); -} +Vector Vector::operator*(float s) const { return Vector(x * s, y * s, z * s); } -Vector Vector::operator/(float s) const -{ - return Vector(x / s, y / s, z / s); -} +Vector Vector::operator/(float s) const { return Vector(x / s, y / s, z / s); } -Vector& Vector::operator*=(float s) +Vector & Vector::operator*=(float s) { x *= s; y *= s; @@ -109,7 +99,7 @@ Vector& Vector::operator*=(float s) return *this; } -Vector& Vector::operator/=(float s) +Vector & Vector::operator/=(float s) { x /= s; y /= s; @@ -117,31 +107,32 @@ Vector& Vector::operator/=(float s) return *this; } -float Vector::dot(const Vector& v) const -{ - return x * v.x + y * v.y + z * v.z; -} +float Vector::dot(const Vector & v) const { return x * v.x + y * v.y + z * v.z; } -Vector Vector::cross(const Vector& v) const +Vector Vector::cross(const Vector & v) const { return Vector(y * v.z - z * v.y, z * v.x - x * v.z, x * v.y - y * v.x); } -Quaternion::Quaternion() : w(1.0f), x(0.0f), y(0.0f), z(0.0f) {} +Quaternion::Quaternion() + : w(1.0f) + , x(0.0f) + , y(0.0f) + , z(0.0f) +{} -Quaternion::Quaternion(float w_, float x_, float y_, float z_) : w(w_), x(x_), y(y_), z(z_) {} +Quaternion::Quaternion(float w_, float x_, float y_, float z_) + : w(w_) + , x(x_) + , y(y_) + , z(z_) +{} -Quaternion::Quaternion(const Vector& u, const Vector& v) -{ - from_two_unit_vectors(u, v); -} +Quaternion::Quaternion(const Vector & u, const Vector & v) { from_two_unit_vectors(u, v); } -Quaternion::Quaternion(float roll, float pitch, float yaw) -{ - from_RPY(roll, pitch, yaw); -} +Quaternion::Quaternion(float roll, float pitch, float yaw) { from_RPY(roll, pitch, yaw); } -Quaternion& Quaternion::normalize() +Quaternion & Quaternion::normalize() { float recip_norm = inv_sqrt(w * w + x * x + y * y + z * z); w *= recip_norm; @@ -150,8 +141,7 @@ Quaternion& Quaternion::normalize() z *= recip_norm; // Make sure the quaternion is canonical (w is always positive) - if (w < 0.0f) - { + if (w < 0.0f) { w *= -1.0f; x *= -1.0f; y *= -1.0f; @@ -161,13 +151,13 @@ Quaternion& Quaternion::normalize() return *this; } -Quaternion Quaternion::operator*(const Quaternion& q) const +Quaternion Quaternion::operator*(const Quaternion & q) const { return Quaternion(w * q.w - x * q.x - y * q.y - z * q.z, w * q.x + x * q.w - y * q.z + z * q.y, w * q.y + x * q.z + y * q.w - z * q.x, w * q.z - x * q.y + y * q.x + z * q.w); } -Quaternion& Quaternion::operator*=(const Quaternion& q) +Quaternion & Quaternion::operator*=(const Quaternion & q) { w = w * q.w - x * q.x - y * q.y - z * q.z; x = w * q.x + x * q.w - y * q.z + z * q.y; @@ -176,25 +166,21 @@ Quaternion& Quaternion::operator*=(const Quaternion& q) return *this; } -Vector Quaternion::rotate(const Vector& v) const +Vector Quaternion::rotate(const Vector & v) const { - return Vector( - (1.0f - 2.0f * y * y - 2.0f * z * z) * v.x + (2.0f * (x * y + w * z)) * v.y + 2.0f * (x * z - w * y) * v.z, - (2.0f * (x * y - w * z)) * v.x + (1.0f - 2.0f * x * x - 2.0f * z * z) * v.y + 2.0f * (y * z + w * x) * v.z, - (2.0f * (x * z + w * y)) * v.x + 2.0f * (y * z - w * x) * v.y + (1.0f - 2.0f * x * x - 2.0f * y * y) * v.z); + return Vector((1.0f - 2.0f * y * y - 2.0f * z * z) * v.x + (2.0f * (x * y + w * z)) * v.y + + 2.0f * (x * z - w * y) * v.z, + (2.0f * (x * y - w * z)) * v.x + (1.0f - 2.0f * x * x - 2.0f * z * z) * v.y + + 2.0f * (y * z + w * x) * v.z, + (2.0f * (x * z + w * y)) * v.x + 2.0f * (y * z - w * x) * v.y + + (1.0f - 2.0f * x * x - 2.0f * y * y) * v.z); } -Vector Quaternion::operator*(const Vector& v) const -{ - return rotate(v); -} +Vector Quaternion::operator*(const Vector & v) const { return rotate(v); } -Quaternion Quaternion::inverse() const -{ - return Quaternion(w, -x, -y, -z); -} +Quaternion Quaternion::inverse() const { return Quaternion(w, -x, -y, -z); } -Quaternion& Quaternion::invert() +Quaternion & Quaternion::invert() { x *= -1.0f; y *= -1.0f; @@ -202,21 +188,18 @@ Quaternion& Quaternion::invert() return *this; } -Quaternion& Quaternion::from_two_unit_vectors(const Vector& u, const Vector& v) +Quaternion & Quaternion::from_two_unit_vectors(const Vector & u, const Vector & v) { // Adapted From the Ogre3d source code // https://bitbucket.org/sinbad/ogre/src/9db75e3ba05c/OgreMain/include/OgreVector3.h?fileviewer=file-view-default#cl-651 float d = u.dot(v); - if (d >= 1.0f) - { + if (d >= 1.0f) { w = 1.0f; x = 0.0f; y = 0.0f; z = 0.0f; return *this; - } - else - { + } else { float invs = inv_sqrt(2.0f * (1.0f + d)); Vector xyz = u.cross(v) * invs; w = 0.5f / invs; @@ -228,7 +211,7 @@ Quaternion& Quaternion::from_two_unit_vectors(const Vector& u, const Vector& v) return *this; } -Quaternion& Quaternion::from_RPY(float roll, float pitch, float yaw) +Quaternion & Quaternion::from_RPY(float roll, float pitch, float yaw) { // p 259 of "Small unmanned aircraft: Theory and Practice" by Randy Beard and Tim McLain float cp = turbomath::cos(roll / 2.0); @@ -247,11 +230,10 @@ Quaternion& Quaternion::from_RPY(float roll, float pitch, float yaw) return *this; } -Vector Quaternion::boxminus(const Quaternion& q) const +Vector Quaternion::boxminus(const Quaternion & q) const { Quaternion dq = q.inverse() * (*this); - if (dq.w < 0.0) - { + if (dq.w < 0.0) { dq.w *= -1.0; dq.x *= -1.0; dq.y *= -1.0; @@ -260,7 +242,7 @@ Vector Quaternion::boxminus(const Quaternion& q) const return log(dq); } -void Quaternion::get_RPY(float* roll, float* pitch, float* yaw) const +void Quaternion::get_RPY(float * roll, float * pitch, float * yaw) const { *roll = turbomath::atan2(2.0f * (w * x + y * z), 1.0f - 2.0f * (x * x + y * y)); *pitch = turbomath::asin(2.0f * (w * y - z * x)); @@ -276,14 +258,15 @@ static const float atan_min_x = 0.000000; static const float atan_scale_factor = 41720.240162; static const int16_t atan_num_entries = 125; static const int16_t atan_lookup_table[125] = { - 0, 334, 667, 1001, 1335, 1668, 2001, 2334, 2666, 2999, 3331, 3662, 3993, 4323, 4653, 4983, - 5311, 5639, 5967, 6293, 6619, 6944, 7268, 7592, 7914, 8235, 8556, 8875, 9194, 9511, 9827, 10142, - 10456, 10768, 11080, 11390, 11699, 12006, 12313, 12617, 12921, 13223, 13524, 13823, 14120, 14417, 14711, 15005, - 15296, 15586, 15875, 16162, 16447, 16731, 17013, 17293, 17572, 17849, 18125, 18399, 18671, 18941, 19210, 19477, - 19742, 20006, 20268, 20528, 20786, 21043, 21298, 21551, 21802, 22052, 22300, 22546, 22791, 23034, 23275, 23514, - 23752, 23988, 24222, 24454, 24685, 24914, 25142, 25367, 25591, 25814, 26034, 26253, 26471, 26686, 26900, 27113, - 27324, 27533, 27740, 27946, 28150, 28353, 28554, 28754, 28952, 29148, 29343, 29537, 29728, 29919, 30108, 30295, - 30481, 30665, 30848, 31030, 31210, 31388, 31566, 31741, 31916, 32089, 32260, 32431, 32599, + 0, 334, 667, 1001, 1335, 1668, 2001, 2334, 2666, 2999, 3331, 3662, 3993, 4323, + 4653, 4983, 5311, 5639, 5967, 6293, 6619, 6944, 7268, 7592, 7914, 8235, 8556, 8875, + 9194, 9511, 9827, 10142, 10456, 10768, 11080, 11390, 11699, 12006, 12313, 12617, 12921, 13223, + 13524, 13823, 14120, 14417, 14711, 15005, 15296, 15586, 15875, 16162, 16447, 16731, 17013, 17293, + 17572, 17849, 18125, 18399, 18671, 18941, 19210, 19477, 19742, 20006, 20268, 20528, 20786, 21043, + 21298, 21551, 21802, 22052, 22300, 22546, 22791, 23034, 23275, 23514, 23752, 23988, 24222, 24454, + 24685, 24914, 25142, 25367, 25591, 25814, 26034, 26253, 26471, 26686, 26900, 27113, 27324, 27533, + 27740, 27946, 28150, 28353, 28554, 28754, 28952, 29148, 29343, 29537, 29728, 29919, 30108, 30295, + 30481, 30665, 30848, 31030, 31210, 31388, 31566, 31741, 31916, 32089, 32260, 32431, 32599, }; static const float asin_max_x = 1.000000; @@ -291,19 +274,21 @@ static const float asin_min_x = 0.000000; static const float asin_scale_factor = 20860.120081; static const int16_t asin_num_entries = 200; static const int16_t asin_lookup_table[200] = { - 0, 104, 209, 313, 417, 522, 626, 730, 835, 939, 1043, 1148, 1252, 1357, 1461, 1566, - 1671, 1775, 1880, 1985, 2090, 2194, 2299, 2404, 2509, 2614, 2720, 2825, 2930, 3035, 3141, 3246, - 3352, 3458, 3564, 3669, 3775, 3881, 3988, 4094, 4200, 4307, 4413, 4520, 4627, 4734, 4841, 4948, - 5056, 5163, 5271, 5379, 5487, 5595, 5703, 5811, 5920, 6029, 6138, 6247, 6356, 6465, 6575, 6685, - 6795, 6905, 7015, 7126, 7237, 7348, 7459, 7570, 7682, 7794, 7906, 8019, 8131, 8244, 8357, 8471, - 8584, 8698, 8812, 8927, 9042, 9157, 9272, 9388, 9504, 9620, 9737, 9854, 9971, 10089, 10207, 10325, - 10444, 10563, 10682, 10802, 10922, 11043, 11164, 11285, 11407, 11530, 11652, 11776, 11899, 12024, 12148, 12273, - 12399, 12525, 12652, 12779, 12907, 13035, 13164, 13293, 13424, 13554, 13686, 13817, 13950, 14083, 14217, 14352, - 14487, 14623, 14760, 14898, 15036, 15176, 15316, 15457, 15598, 15741, 15885, 16029, 16175, 16321, 16469, 16618, - 16767, 16918, 17070, 17224, 17378, 17534, 17691, 17849, 18009, 18170, 18333, 18497, 18663, 18830, 19000, 19171, - 19343, 19518, 19695, 19874, 20055, 20239, 20424, 20613, 20803, 20997, 21194, 21393, 21596, 21802, 22012, 22225, - 22443, 22664, 22891, 23122, 23359, 23601, 23849, 24104, 24366, 24637, 24916, 25204, 25504, 25816, 26143, 26485, - 26847, 27232, 27644, 28093, 28588, 29149, 29814, 30680, + 0, 104, 209, 313, 417, 522, 626, 730, 835, 939, 1043, 1148, 1252, 1357, + 1461, 1566, 1671, 1775, 1880, 1985, 2090, 2194, 2299, 2404, 2509, 2614, 2720, 2825, + 2930, 3035, 3141, 3246, 3352, 3458, 3564, 3669, 3775, 3881, 3988, 4094, 4200, 4307, + 4413, 4520, 4627, 4734, 4841, 4948, 5056, 5163, 5271, 5379, 5487, 5595, 5703, 5811, + 5920, 6029, 6138, 6247, 6356, 6465, 6575, 6685, 6795, 6905, 7015, 7126, 7237, 7348, + 7459, 7570, 7682, 7794, 7906, 8019, 8131, 8244, 8357, 8471, 8584, 8698, 8812, 8927, + 9042, 9157, 9272, 9388, 9504, 9620, 9737, 9854, 9971, 10089, 10207, 10325, 10444, 10563, + 10682, 10802, 10922, 11043, 11164, 11285, 11407, 11530, 11652, 11776, 11899, 12024, 12148, 12273, + 12399, 12525, 12652, 12779, 12907, 13035, 13164, 13293, 13424, 13554, 13686, 13817, 13950, 14083, + 14217, 14352, 14487, 14623, 14760, 14898, 15036, 15176, 15316, 15457, 15598, 15741, 15885, 16029, + 16175, 16321, 16469, 16618, 16767, 16918, 17070, 17224, 17378, 17534, 17691, 17849, 18009, 18170, + 18333, 18497, 18663, 18830, 19000, 19171, 19343, 19518, 19695, 19874, 20055, 20239, 20424, 20613, + 20803, 20997, 21194, 21393, 21596, 21802, 22012, 22225, 22443, 22664, 22891, 23122, 23359, 23601, + 23849, 24104, 24366, 24637, 24916, 25204, 25504, 25816, 26143, 26485, 26847, 27232, 27644, 28093, + 28588, 29149, 29814, 30680, }; static const float max_pressure = 106598.405011; @@ -311,19 +296,21 @@ static const float min_pressure = 69681.635473; static const float pressure_scale_factor = 10.754785; static const int16_t pressure_num_entries = 200; static const int16_t pressure_lookup_table[200] = { - 32767, 32544, 32321, 32098, 31876, 31655, 31434, 31213, 30993, 30773, 30554, 30335, 30117, 29899, 29682, 29465, - 29248, 29032, 28816, 28601, 28386, 28172, 27958, 27745, 27532, 27319, 27107, 26895, 26684, 26473, 26263, 26053, - 25843, 25634, 25425, 25217, 25009, 24801, 24594, 24387, 24181, 23975, 23769, 23564, 23359, 23155, 22951, 22748, - 22544, 22341, 22139, 21937, 21735, 21534, 21333, 21133, 20932, 20733, 20533, 20334, 20135, 19937, 19739, 19542, - 19344, 19148, 18951, 18755, 18559, 18364, 18169, 17974, 17780, 17586, 17392, 17199, 17006, 16813, 16621, 16429, - 16237, 16046, 15855, 15664, 15474, 15284, 15095, 14905, 14716, 14528, 14339, 14151, 13964, 13777, 13590, 13403, - 13217, 13031, 12845, 12659, 12474, 12290, 12105, 11921, 11737, 11554, 11370, 11188, 11005, 10823, 10641, 10459, - 10278, 10096, 9916, 9735, 9555, 9375, 9195, 9016, 8837, 8658, 8480, 8302, 8124, 7946, 7769, 7592, - 7415, 7239, 7063, 6887, 6711, 6536, 6361, 6186, 6012, 5837, 5664, 5490, 5316, 5143, 4970, 4798, - 4626, 4454, 4282, 4110, 3939, 3768, 3597, 3427, 3257, 3087, 2917, 2748, 2578, 2409, 2241, 2072, - 1904, 1736, 1569, 1401, 1234, 1067, 901, 734, 568, 402, 237, 71, -94, -259, -424, -588, - -752, -916, -1080, -1243, -1407, -1570, -1732, -1895, -2057, -2219, -2381, -2543, -2704, -2865, -3026, -3187, - -3347, -3507, -3667, -3827, -3987, -4146, -4305, -4464, + 32767, 32544, 32321, 32098, 31876, 31655, 31434, 31213, 30993, 30773, 30554, 30335, 30117, 29899, + 29682, 29465, 29248, 29032, 28816, 28601, 28386, 28172, 27958, 27745, 27532, 27319, 27107, 26895, + 26684, 26473, 26263, 26053, 25843, 25634, 25425, 25217, 25009, 24801, 24594, 24387, 24181, 23975, + 23769, 23564, 23359, 23155, 22951, 22748, 22544, 22341, 22139, 21937, 21735, 21534, 21333, 21133, + 20932, 20733, 20533, 20334, 20135, 19937, 19739, 19542, 19344, 19148, 18951, 18755, 18559, 18364, + 18169, 17974, 17780, 17586, 17392, 17199, 17006, 16813, 16621, 16429, 16237, 16046, 15855, 15664, + 15474, 15284, 15095, 14905, 14716, 14528, 14339, 14151, 13964, 13777, 13590, 13403, 13217, 13031, + 12845, 12659, 12474, 12290, 12105, 11921, 11737, 11554, 11370, 11188, 11005, 10823, 10641, 10459, + 10278, 10096, 9916, 9735, 9555, 9375, 9195, 9016, 8837, 8658, 8480, 8302, 8124, 7946, + 7769, 7592, 7415, 7239, 7063, 6887, 6711, 6536, 6361, 6186, 6012, 5837, 5664, 5490, + 5316, 5143, 4970, 4798, 4626, 4454, 4282, 4110, 3939, 3768, 3597, 3427, 3257, 3087, + 2917, 2748, 2578, 2409, 2241, 2072, 1904, 1736, 1569, 1401, 1234, 1067, 901, 734, + 568, 402, 237, 71, -94, -259, -424, -588, -752, -916, -1080, -1243, -1407, -1570, + -1732, -1895, -2057, -2219, -2381, -2543, -2704, -2865, -3026, -3187, -3347, -3507, -3667, -3827, + -3987, -4146, -4305, -4464, }; static const float sin_max_x = 3.141593; @@ -331,24 +318,19 @@ static const float sin_min_x = 0.000000; static const float sin_scale_factor = 32767.000000; static const int16_t sin_num_entries = 125; static const int16_t sin_lookup_table[125] = { - 0, 823, 1646, 2468, 3289, 4107, 4922, 5735, 6544, 7349, 8149, 8944, 9733, 10516, 11293, 12062, - 12824, 13578, 14323, 15059, 15786, 16502, 17208, 17904, 18588, 19260, 19920, 20568, 21202, 21823, 22431, 23024, - 23602, 24166, 24715, 25247, 25764, 26265, 26749, 27216, 27666, 28099, 28513, 28910, 29289, 29648, 29990, 30312, - 30615, 30899, 31163, 31408, 31633, 31837, 32022, 32187, 32331, 32454, 32558, 32640, 32702, 32744, 32764, 32764, - 32744, 32702, 32640, 32558, 32454, 32331, 32187, 32022, 31837, 31633, 31408, 31163, 30899, 30615, 30312, 29990, - 29648, 29289, 28910, 28513, 28099, 27666, 27216, 26749, 26265, 25764, 25247, 24715, 24166, 23602, 23024, 22431, - 21823, 21202, 20568, 19920, 19260, 18588, 17904, 17208, 16502, 15786, 15059, 14323, 13578, 12824, 12062, 11293, - 10516, 9733, 8944, 8149, 7349, 6544, 5735, 4922, 4107, 3289, 2468, 1646, 823}; - -float fsign(float y) -{ - return (0.0f < y) - (y < 0.0f); -} + 0, 823, 1646, 2468, 3289, 4107, 4922, 5735, 6544, 7349, 8149, 8944, 9733, 10516, + 11293, 12062, 12824, 13578, 14323, 15059, 15786, 16502, 17208, 17904, 18588, 19260, 19920, 20568, + 21202, 21823, 22431, 23024, 23602, 24166, 24715, 25247, 25764, 26265, 26749, 27216, 27666, 28099, + 28513, 28910, 29289, 29648, 29990, 30312, 30615, 30899, 31163, 31408, 31633, 31837, 32022, 32187, + 32331, 32454, 32558, 32640, 32702, 32744, 32764, 32764, 32744, 32702, 32640, 32558, 32454, 32331, + 32187, 32022, 31837, 31633, 31408, 31163, 30899, 30615, 30312, 29990, 29648, 29289, 28910, 28513, + 28099, 27666, 27216, 26749, 26265, 25764, 25247, 24715, 24166, 23602, 23024, 22431, 21823, 21202, + 20568, 19920, 19260, 18588, 17904, 17208, 16502, 15786, 15059, 14323, 13578, 12824, 12062, 11293, + 10516, 9733, 8944, 8149, 7349, 6544, 5735, 4922, 4107, 3289, 2468, 1646, 823}; -float cos(float x) -{ - return sin(M_PI / 2.0 - x); -} +float fsign(float y) { return (0.0f < y) - (y < 0.0f); } + +float cos(float x) { return sin(M_PI / 2.0 - x); } float sin(float x) { @@ -358,120 +340,95 @@ float sin(float x) while (x <= -M_PI) x += 2.0 * M_PI; // sin is symmetric - if (x < 0) - return -1.0 * sin(-x); + if (x < 0) return -1.0 * sin(-x); // wrap onto (0, PI) - if (x > M_PI) - return -1.0 * sin(x - M_PI); + if (x > M_PI) return -1.0 * sin(x - M_PI); // Now, all we have left is the range 0 to PI, use the lookup table float t = (x - sin_min_x) / (sin_max_x - sin_min_x) * static_cast(sin_num_entries); int16_t index = static_cast(t); float delta_x = t - index; - if (index >= sin_num_entries) - return sin_lookup_table[sin_num_entries - 1] / sin_scale_factor; + if (index >= sin_num_entries) return sin_lookup_table[sin_num_entries - 1] / sin_scale_factor; else if (index < sin_num_entries - 1) return sin_lookup_table[index] / sin_scale_factor - + delta_x * (sin_lookup_table[index + 1] - sin_lookup_table[index]) / sin_scale_factor; + + delta_x * (sin_lookup_table[index + 1] - sin_lookup_table[index]) / sin_scale_factor; else return sin_lookup_table[index] / sin_scale_factor - + delta_x * (sin_lookup_table[index] - sin_lookup_table[index - 1]) / sin_scale_factor; + + delta_x * (sin_lookup_table[index] - sin_lookup_table[index - 1]) / sin_scale_factor; } float atan(float x) { // atan is symmetric - if (x < 0) - { - return -1.0 * atan(-1.0 * x); - } + if (x < 0) { return -1.0 * atan(-1.0 * x); } // This uses a sweet identity to wrap the domain of atan onto (0,1) - if (x > 1.0) - { - return M_PI / 2.0 - atan(1.0 / x); - } + if (x > 1.0) { return M_PI / 2.0 - atan(1.0 / x); } float t = (x - atan_min_x) / (atan_max_x - atan_min_x) * static_cast(atan_num_entries); int16_t index = static_cast(t); float delta_x = t - index; - if (index >= atan_num_entries) - return atan_lookup_table[atan_num_entries - 1] / atan_scale_factor; + if (index >= atan_num_entries) return atan_lookup_table[atan_num_entries - 1] / atan_scale_factor; else if (index < atan_num_entries - 1) return atan_lookup_table[index] / atan_scale_factor - + delta_x * (atan_lookup_table[index + 1] - atan_lookup_table[index]) / atan_scale_factor; + + delta_x * (atan_lookup_table[index + 1] - atan_lookup_table[index]) / atan_scale_factor; else return atan_lookup_table[index] / atan_scale_factor - + delta_x * (atan_lookup_table[index] - atan_lookup_table[index - 1]) / atan_scale_factor; + + delta_x * (atan_lookup_table[index] - atan_lookup_table[index - 1]) / atan_scale_factor; } float atan2(float y, float x) { // algorithm from wikipedia: https://en.wikipedia.org/wiki/Atan2 - if (x == 0.0) - { - if (y < 0.0) - { + if (x == 0.0) { + if (y < 0.0) { return -M_PI / 2.0; - } - else if (y > 0.0) - { + } else if (y > 0.0) { return M_PI / 2.0; - } - else - { + } else { return 0.0; } } float arctan = atan(y / x); - if (x < 0.0) - { - if (y < 0) - { + if (x < 0.0) { + if (y < 0) { return arctan - M_PI; - } - else - { + } else { return arctan + M_PI; } } - else - { + else { return arctan; } } float asin(float x) { - if (x < 0.0) - { - return -1.0 * asin(-1.0 * x); - } + if (x < 0.0) { return -1.0 * asin(-1.0 * x); } float t = (x - asin_min_x) / (asin_max_x - asin_min_x) * static_cast(asin_num_entries); int16_t index = static_cast(t); float delta_x = t - index; - if (index >= asin_num_entries) - return asin_lookup_table[asin_num_entries - 1] / asin_scale_factor; + if (index >= asin_num_entries) return asin_lookup_table[asin_num_entries - 1] / asin_scale_factor; else if (index < asin_num_entries - 1) return asin_lookup_table[index] / asin_scale_factor - + delta_x * (asin_lookup_table[index + 1] - asin_lookup_table[index]) / asin_scale_factor; + + delta_x * (asin_lookup_table[index + 1] - asin_lookup_table[index]) / asin_scale_factor; else return asin_lookup_table[index] / asin_scale_factor - + delta_x * (asin_lookup_table[index] - asin_lookup_table[index - 1]) / asin_scale_factor; + + delta_x * (asin_lookup_table[index] - asin_lookup_table[index - 1]) / asin_scale_factor; } float alt(float press) { - if (press < max_pressure && press > min_pressure) - { - float t = (press - min_pressure) / (max_pressure - min_pressure) * static_cast(pressure_num_entries); + if (press < max_pressure && press > min_pressure) { + float t = (press - min_pressure) / (max_pressure - min_pressure) + * static_cast(pressure_num_entries); int16_t index = static_cast(t); float delta_x = t - index; @@ -479,19 +436,19 @@ float alt(float press) return asin_lookup_table[pressure_num_entries - 1] / pressure_scale_factor; else if (index < pressure_num_entries - 1) return pressure_lookup_table[index] / pressure_scale_factor - + delta_x * (pressure_lookup_table[index + 1] - pressure_lookup_table[index]) / pressure_scale_factor; + + delta_x * (pressure_lookup_table[index + 1] - pressure_lookup_table[index]) + / pressure_scale_factor; else return pressure_lookup_table[index] / pressure_scale_factor - + delta_x * (pressure_lookup_table[index] - pressure_lookup_table[index - 1]) / pressure_scale_factor; - } - else + + delta_x * (pressure_lookup_table[index] - pressure_lookup_table[index - 1]) + / pressure_scale_factor; + } else return 0.0; } float fabs(float x) { - if (x < 0) - return -x; + if (x < 0) return -x; else return x; } @@ -508,7 +465,8 @@ float inv_sqrt(float x) i.ivalue = 0x5f3759df - (i.ivalue >> 1); y.fvalue = i.fvalue; y.fvalue = y.fvalue * (threehalfs - (x2 * y.fvalue * y.fvalue)); // 1st iteration - y.fvalue = y.fvalue * (threehalfs - (x2 * y.fvalue * y.fvalue)); // 2nd iteration, this can be removed + y.fvalue = + y.fvalue * (threehalfs - (x2 * y.fvalue * y.fvalue)); // 2nd iteration, this can be removed return fabs(y.fvalue); } diff --git a/lib/turbomath/turbomath.h b/lib/turbomath/turbomath.h index d22326aa..8838ceaf 100644 --- a/lib/turbomath/turbomath.h +++ b/lib/turbomath/turbomath.h @@ -71,30 +71,24 @@ class Vector float norm() const; float sqrd_norm() const; - Vector& normalize(); + Vector & normalize(); Vector normalized() const; - float dot(const Vector& v) const; - Vector cross(const Vector& v) const; + float dot(const Vector & v) const; + Vector cross(const Vector & v) const; Vector operator*(float s) const; Vector operator/(float s) const; - Vector& operator*=(float s); - Vector& operator/=(float s); - Vector operator+(const Vector& v) const; - Vector operator-(const Vector& v) const; - Vector& operator+=(const Vector& v); - Vector& operator-=(const Vector& v); + Vector & operator*=(float s); + Vector & operator/=(float s); + Vector operator+(const Vector & v) const; + Vector operator-(const Vector & v) const; + Vector & operator+=(const Vector & v); + Vector & operator-=(const Vector & v); }; -inline Vector operator*(float s, const Vector& v) -{ - return v * s; -} -inline Vector operator/(float s, const Vector& v) -{ - return v / s; -} +inline Vector operator*(float s, const Vector & v) { return v * s; } +inline Vector operator/(float s, const Vector & v) { return v / s; } class Quaternion { @@ -106,39 +100,36 @@ class Quaternion Quaternion(); Quaternion(float w_, float x_, float y_, float z_); - Quaternion(const Vector& u, const Vector& v); + Quaternion(const Vector & u, const Vector & v); Quaternion(float roll, float pitch, float yaw); - Vector rotate(const Vector& v) const; - Quaternion& normalize(); + Vector rotate(const Vector & v) const; + Quaternion & normalize(); Quaternion inverse() const; - Quaternion& invert(); - Quaternion& from_two_unit_vectors(const Vector& u, const Vector& v); - Quaternion& from_RPY(float roll, float pitch, float yaw); - void get_RPY(float* roll, float* pitch, float* yaw) const; - - Vector operator*(const Vector& v) const; - Quaternion operator*(const Quaternion& q) const; - Quaternion& operator*=(const Quaternion& q); - Vector boxminus(const Quaternion& q) const; - static Vector log(const Quaternion& q) + Quaternion & invert(); + Quaternion & from_two_unit_vectors(const Vector & u, const Vector & v); + Quaternion & from_RPY(float roll, float pitch, float yaw); + void get_RPY(float * roll, float * pitch, float * yaw) const; + + Vector operator*(const Vector & v) const; + Quaternion operator*(const Quaternion & q) const; + Quaternion & operator*=(const Quaternion & q); + Vector boxminus(const Quaternion & q) const; + static Vector log(const Quaternion & q) { Vector v{q.x, q.y, q.z}; float norm_v = v.norm(); Vector out; - if (norm_v < 1e-8) - { + if (norm_v < 1e-8) { out.x = out.y = out.z = 0.0; - } - else - { + } else { out = 2.0 * atan2(norm_v, q.w) * v / norm_v; } return out; } - Vector operator-(const Quaternion& q) const { return boxminus(q); } + Vector operator-(const Quaternion & q) const { return boxminus(q); } }; } // namespace turbomath diff --git a/src/comm_manager.cpp b/src/comm_manager.cpp index fa365144..8ea4b774 100644 --- a/src/comm_manager.cpp +++ b/src/comm_manager.cpp @@ -40,14 +40,11 @@ namespace rosflight_firmware { -CommManager::LogMessageBuffer::LogMessageBuffer() -{ - memset(buffer_, 0, sizeof(buffer_)); -} +CommManager::LogMessageBuffer::LogMessageBuffer() { memset(buffer_, 0, sizeof(buffer_)); } void CommManager::LogMessageBuffer::add_message(CommLinkInterface::LogSeverity severity, char msg[]) { - LogMessage& newest_msg = buffer_[newest_]; + LogMessage & newest_msg = buffer_[newest_]; strcpy(newest_msg.msg, msg); newest_msg.severity = severity; @@ -55,8 +52,7 @@ void CommManager::LogMessageBuffer::add_message(CommLinkInterface::LogSeverity s // quietly over-write old messages (what else can we do?) length_ += 1; - if (length_ > LOG_BUF_SIZE) - { + if (length_ > LOG_BUF_SIZE) { length_ = LOG_BUF_SIZE; oldest_ = (oldest_ + 1) % LOG_BUF_SIZE; } @@ -64,14 +60,16 @@ void CommManager::LogMessageBuffer::add_message(CommLinkInterface::LogSeverity s void CommManager::LogMessageBuffer::pop() { - if (length_ > 0) - { + if (length_ > 0) { length_--; oldest_ = (oldest_ + 1) % LOG_BUF_SIZE; } } -CommManager::CommManager(ROSflight& rf, CommLinkInterface& comm_link) : RF_(rf), comm_link_(comm_link) {} +CommManager::CommManager(ROSflight & rf, CommLinkInterface & comm_link) + : RF_(rf) + , comm_link_(comm_link) +{} // function definitions void CommManager::init() @@ -102,53 +100,52 @@ void CommManager::init() void CommManager::param_change_callback(uint16_t param_id) { - switch (param_id) - { - case PARAM_SYSTEM_ID: - update_system_id(param_id); - break; - case PARAM_STREAM_HEARTBEAT_RATE: - set_streaming_rate(STREAM_ID_HEARTBEAT, param_id); - break; - case PARAM_STREAM_STATUS_RATE: - set_streaming_rate(STREAM_ID_STATUS, param_id); - break; - case PARAM_STREAM_IMU_RATE: - set_streaming_rate(STREAM_ID_IMU, param_id); - break; - case PARAM_STREAM_ATTITUDE_RATE: - set_streaming_rate(STREAM_ID_ATTITUDE, param_id); - break; - case PARAM_STREAM_AIRSPEED_RATE: - set_streaming_rate(STREAM_ID_DIFF_PRESSURE, param_id); - break; - case PARAM_STREAM_BARO_RATE: - set_streaming_rate(STREAM_ID_BARO, param_id); - break; - case PARAM_STREAM_SONAR_RATE: - set_streaming_rate(STREAM_ID_SONAR, param_id); - break; - case PARAM_STREAM_GNSS_RATE: - set_streaming_rate(STREAM_ID_GNSS, param_id); - break; - case PARAM_STREAM_GNSS_FULL_RATE: - set_streaming_rate(STREAM_ID_GNSS_FULL, param_id); - break; - case PARAM_STREAM_MAG_RATE: - set_streaming_rate(STREAM_ID_MAG, param_id); - break; - case PARAM_STREAM_OUTPUT_RAW_RATE: - set_streaming_rate(STREAM_ID_SERVO_OUTPUT_RAW, param_id); - break; - case PARAM_STREAM_RC_RAW_RATE: - set_streaming_rate(STREAM_ID_RC_RAW, param_id); - break; - case PARAM_STREAM_BATTERY_STATUS_RATE: - set_streaming_rate(STREAM_ID_BATTERY_STATUS, param_id); - break; - default: - // do nothing - break; + switch (param_id) { + case PARAM_SYSTEM_ID: + update_system_id(param_id); + break; + case PARAM_STREAM_HEARTBEAT_RATE: + set_streaming_rate(STREAM_ID_HEARTBEAT, param_id); + break; + case PARAM_STREAM_STATUS_RATE: + set_streaming_rate(STREAM_ID_STATUS, param_id); + break; + case PARAM_STREAM_IMU_RATE: + set_streaming_rate(STREAM_ID_IMU, param_id); + break; + case PARAM_STREAM_ATTITUDE_RATE: + set_streaming_rate(STREAM_ID_ATTITUDE, param_id); + break; + case PARAM_STREAM_AIRSPEED_RATE: + set_streaming_rate(STREAM_ID_DIFF_PRESSURE, param_id); + break; + case PARAM_STREAM_BARO_RATE: + set_streaming_rate(STREAM_ID_BARO, param_id); + break; + case PARAM_STREAM_SONAR_RATE: + set_streaming_rate(STREAM_ID_SONAR, param_id); + break; + case PARAM_STREAM_GNSS_RATE: + set_streaming_rate(STREAM_ID_GNSS, param_id); + break; + case PARAM_STREAM_GNSS_FULL_RATE: + set_streaming_rate(STREAM_ID_GNSS_FULL, param_id); + break; + case PARAM_STREAM_MAG_RATE: + set_streaming_rate(STREAM_ID_MAG, param_id); + break; + case PARAM_STREAM_OUTPUT_RAW_RATE: + set_streaming_rate(STREAM_ID_SERVO_OUTPUT_RAW, param_id); + break; + case PARAM_STREAM_RC_RAW_RATE: + set_streaming_rate(STREAM_ID_RC_RAW, param_id); + break; + case PARAM_STREAM_BATTERY_STATUS_RATE: + set_streaming_rate(STREAM_ID_BATTERY_STATUS, param_id); + break; + default: + // do nothing + break; } } @@ -157,74 +154,65 @@ void CommManager::update_system_id(uint16_t param_id) sysid_ = static_cast(RF_.params_.get_param_int(param_id)); } -void CommManager::update_status() -{ - send_status(); -} +void CommManager::update_status() { send_status(); } void CommManager::send_param_value(uint16_t param_id) { - if (param_id < PARAMS_COUNT) - { - switch (RF_.params_.get_param_type(param_id)) - { - case PARAM_TYPE_INT32: - comm_link_.send_param_value_int(sysid_, param_id, RF_.params_.get_param_name(param_id), - RF_.params_.get_param_int(param_id), static_cast(PARAMS_COUNT)); - break; - case PARAM_TYPE_FLOAT: - comm_link_.send_param_value_float(sysid_, param_id, RF_.params_.get_param_name(param_id), - RF_.params_.get_param_float(param_id), static_cast(PARAMS_COUNT)); - break; - default: - break; + if (param_id < PARAMS_COUNT) { + switch (RF_.params_.get_param_type(param_id)) { + case PARAM_TYPE_INT32: + comm_link_.send_param_value_int(sysid_, param_id, RF_.params_.get_param_name(param_id), + RF_.params_.get_param_int(param_id), + static_cast(PARAMS_COUNT)); + break; + case PARAM_TYPE_FLOAT: + comm_link_.send_param_value_float(sysid_, param_id, RF_.params_.get_param_name(param_id), + RF_.params_.get_param_float(param_id), + static_cast(PARAMS_COUNT)); + break; + default: + break; } } } void CommManager::param_request_list_callback(uint8_t target_system) { - if (target_system == sysid_) - send_params_index_ = 0; + if (target_system == sysid_) send_params_index_ = 0; } -void CommManager::send_parameter_list() -{ - send_params_index_ = 0; -} +void CommManager::send_parameter_list() { send_params_index_ = 0; } -void CommManager::param_request_read_callback(uint8_t target_system, const char* const param_name, int16_t param_index) +void CommManager::param_request_read_callback(uint8_t target_system, const char * const param_name, + int16_t param_index) { - if (target_system == sysid_) - { - uint16_t id = (param_index < 0) ? RF_.params_.lookup_param_id(param_name) : static_cast(param_index); + if (target_system == sysid_) { + uint16_t id = (param_index < 0) ? RF_.params_.lookup_param_id(param_name) + : static_cast(param_index); - if (id < PARAMS_COUNT) - send_param_value(id); + if (id < PARAMS_COUNT) send_param_value(id); } } -void CommManager::param_set_int_callback(uint8_t target_system, const char* const param_name, int32_t param_value) +void CommManager::param_set_int_callback(uint8_t target_system, const char * const param_name, + int32_t param_value) { - if (target_system == sysid_) - { + if (target_system == sysid_) { uint16_t id = RF_.params_.lookup_param_id(param_name); - if (id < PARAMS_COUNT && RF_.params_.get_param_type(id) == PARAM_TYPE_INT32) - { + if (id < PARAMS_COUNT && RF_.params_.get_param_type(id) == PARAM_TYPE_INT32) { RF_.params_.set_param_int(id, param_value); } } } -void CommManager::param_set_float_callback(uint8_t target_system, const char* const param_name, float param_value) +void CommManager::param_set_float_callback(uint8_t target_system, const char * const param_name, + float param_value) { - if (target_system == sysid_) - { + if (target_system == sysid_) { uint16_t id = RF_.params_.lookup_param_id(param_name); - if (id < PARAMS_COUNT && RF_.params_.get_param_type(id) == PARAM_TYPE_FLOAT) - { + if (id < PARAMS_COUNT && RF_.params_.get_param_type(id) == PARAM_TYPE_FLOAT) { RF_.params_.set_param_float(id, param_value); } } @@ -237,55 +225,50 @@ void CommManager::command_callback(CommLinkInterface::Command command) bool reboot_to_bootloader_flag = false; // None of these actions can be performed if we are armed - if (RF_.state_manager_.state().armed) - { + if (RF_.state_manager_.state().armed) { result = false; - } - else - { + } else { result = true; - switch (command) - { - case CommLinkInterface::Command::COMMAND_READ_PARAMS: - result = RF_.params_.read(); - break; - case CommLinkInterface::Command::COMMAND_WRITE_PARAMS: - result = RF_.params_.write(); - break; - case CommLinkInterface::Command::COMMAND_SET_PARAM_DEFAULTS: - RF_.params_.set_defaults(); - break; - case CommLinkInterface::Command::COMMAND_ACCEL_CALIBRATION: - result = RF_.sensors_.start_imu_calibration(); - break; - case CommLinkInterface::Command::COMMAND_GYRO_CALIBRATION: - result = RF_.sensors_.start_gyro_calibration(); - break; - case CommLinkInterface::Command::COMMAND_BARO_CALIBRATION: - result = RF_.sensors_.start_baro_calibration(); - break; - case CommLinkInterface::Command::COMMAND_AIRSPEED_CALIBRATION: - result = RF_.sensors_.start_diff_pressure_calibration(); - break; - case CommLinkInterface::Command::COMMAND_RC_CALIBRATION: - RF_.controller_.calculate_equilbrium_torque_from_rc(); - break; - case CommLinkInterface::Command::COMMAND_REBOOT: - reboot_flag = true; - break; - case CommLinkInterface::Command::COMMAND_REBOOT_TO_BOOTLOADER: - reboot_to_bootloader_flag = true; - break; - case CommLinkInterface::Command::COMMAND_SEND_VERSION: - comm_link_.send_version(sysid_, GIT_VERSION_STRING); - break; + switch (command) { + case CommLinkInterface::Command::COMMAND_READ_PARAMS: + result = RF_.params_.read(); + break; + case CommLinkInterface::Command::COMMAND_WRITE_PARAMS: + result = RF_.params_.write(); + break; + case CommLinkInterface::Command::COMMAND_SET_PARAM_DEFAULTS: + RF_.params_.set_defaults(); + break; + case CommLinkInterface::Command::COMMAND_ACCEL_CALIBRATION: + result = RF_.sensors_.start_imu_calibration(); + break; + case CommLinkInterface::Command::COMMAND_GYRO_CALIBRATION: + result = RF_.sensors_.start_gyro_calibration(); + break; + case CommLinkInterface::Command::COMMAND_BARO_CALIBRATION: + result = RF_.sensors_.start_baro_calibration(); + break; + case CommLinkInterface::Command::COMMAND_AIRSPEED_CALIBRATION: + result = RF_.sensors_.start_diff_pressure_calibration(); + break; + case CommLinkInterface::Command::COMMAND_RC_CALIBRATION: + RF_.controller_.calculate_equilbrium_torque_from_rc(); + break; + case CommLinkInterface::Command::COMMAND_REBOOT: + reboot_flag = true; + break; + case CommLinkInterface::Command::COMMAND_REBOOT_TO_BOOTLOADER: + reboot_to_bootloader_flag = true; + break; + case CommLinkInterface::Command::COMMAND_SEND_VERSION: + comm_link_.send_version(sysid_, GIT_VERSION_STRING); + break; } } comm_link_.send_command_ack(sysid_, command, result); - if (reboot_flag || reboot_to_bootloader_flag) - { + if (reboot_flag || reboot_to_bootloader_flag) { RF_.board_.clock_delay(20); RF_.board_.board_reset(reboot_to_bootloader_flag); } @@ -300,7 +283,7 @@ void CommManager::timesync_callback(int64_t tc1, int64_t ts1) comm_link_.send_timesync(sysid_, static_cast(now_us) * 1000, ts1); } -void CommManager::offboard_control_callback(const CommLinkInterface::OffboardControl& control) +void CommManager::offboard_control_callback(const CommLinkInterface::OffboardControl & control) { // put values into a new command struct control_t new_offboard_command; @@ -316,26 +299,25 @@ void CommManager::offboard_control_callback(const CommLinkInterface::OffboardCon new_offboard_command.F.active = control.F.valid; // translate modes into standard message - switch (control.mode) - { - case CommLinkInterface::OffboardControl::Mode::PASS_THROUGH: - new_offboard_command.x.type = PASSTHROUGH; - new_offboard_command.y.type = PASSTHROUGH; - new_offboard_command.z.type = PASSTHROUGH; - new_offboard_command.F.type = THROTTLE; - break; - case CommLinkInterface::OffboardControl::Mode::ROLLRATE_PITCHRATE_YAWRATE_THROTTLE: - new_offboard_command.x.type = RATE; - new_offboard_command.y.type = RATE; - new_offboard_command.z.type = RATE; - new_offboard_command.F.type = THROTTLE; - break; - case CommLinkInterface::OffboardControl::Mode::ROLL_PITCH_YAWRATE_THROTTLE: - new_offboard_command.x.type = ANGLE; - new_offboard_command.y.type = ANGLE; - new_offboard_command.z.type = RATE; - new_offboard_command.F.type = THROTTLE; - break; + switch (control.mode) { + case CommLinkInterface::OffboardControl::Mode::PASS_THROUGH: + new_offboard_command.x.type = PASSTHROUGH; + new_offboard_command.y.type = PASSTHROUGH; + new_offboard_command.z.type = PASSTHROUGH; + new_offboard_command.F.type = THROTTLE; + break; + case CommLinkInterface::OffboardControl::Mode::ROLLRATE_PITCHRATE_YAWRATE_THROTTLE: + new_offboard_command.x.type = RATE; + new_offboard_command.y.type = RATE; + new_offboard_command.z.type = RATE; + new_offboard_command.F.type = THROTTLE; + break; + case CommLinkInterface::OffboardControl::Mode::ROLL_PITCH_YAWRATE_THROTTLE: + new_offboard_command.x.type = ANGLE; + new_offboard_command.y.type = ANGLE; + new_offboard_command.z.type = RATE; + new_offboard_command.F.type = THROTTLE; + break; } // Tell the command_manager that we have a new command we need to mux @@ -343,29 +325,27 @@ void CommManager::offboard_control_callback(const CommLinkInterface::OffboardCon RF_.command_manager_.set_new_offboard_command(new_offboard_command); } -void CommManager::aux_command_callback(const CommLinkInterface::AuxCommand& command) +void CommManager::aux_command_callback(const CommLinkInterface::AuxCommand & command) { Mixer::aux_command_t new_aux_command; - for (int i = 0; i < 14; i++) - { - switch (command.cmd_array[i].type) - { - case CommLinkInterface::AuxCommand::Type::DISABLED: - // Channel is either not used or is controlled by the mixer - new_aux_command.channel[i].type = Mixer::NONE; - new_aux_command.channel[i].value = 0; - break; - case CommLinkInterface::AuxCommand::Type::SERVO: - // PWM value should be mapped to servo position - new_aux_command.channel[i].type = Mixer::S; - new_aux_command.channel[i].value = command.cmd_array[i].value; - break; - case CommLinkInterface::AuxCommand::Type::MOTOR: - // PWM value should be mapped to motor speed - new_aux_command.channel[i].type = Mixer::M; - new_aux_command.channel[i].value = command.cmd_array[i].value; - break; + for (int i = 0; i < 14; i++) { + switch (command.cmd_array[i].type) { + case CommLinkInterface::AuxCommand::Type::DISABLED: + // Channel is either not used or is controlled by the mixer + new_aux_command.channel[i].type = Mixer::NONE; + new_aux_command.channel[i].value = 0; + break; + case CommLinkInterface::AuxCommand::Type::SERVO: + // PWM value should be mapped to servo position + new_aux_command.channel[i].type = Mixer::S; + new_aux_command.channel[i].value = command.cmd_array[i].value; + break; + case CommLinkInterface::AuxCommand::Type::MOTOR: + // PWM value should be mapped to motor speed + new_aux_command.channel[i].type = Mixer::M; + new_aux_command.channel[i].value = command.cmd_array[i].value; + break; } } @@ -373,7 +353,7 @@ void CommManager::aux_command_callback(const CommLinkInterface::AuxCommand& comm RF_.mixer_.set_new_aux_command(new_aux_command); } -void CommManager::external_attitude_callback(const turbomath::Quaternion& q) +void CommManager::external_attitude_callback(const turbomath::Quaternion & q) { RF_.estimator_.set_external_attitude_update(q); } @@ -385,8 +365,7 @@ void CommManager::heartbeat_callback(void) connected_ = true; // send backup data if we have it buffered - if (have_backup_data_) - { + if (have_backup_data_) { comm_link_.send_error_data(sysid_, backup_data_buffer_); have_backup_data_ = false; } @@ -397,12 +376,9 @@ void CommManager::heartbeat_callback(void) } // function definitions -void CommManager::receive(void) -{ - comm_link_.receive(); -} +void CommManager::receive(void) { comm_link_.receive(); } -void CommManager::log(CommLinkInterface::LogSeverity severity, const char* fmt, ...) +void CommManager::log(CommLinkInterface::LogSeverity severity, const char * fmt, ...) { // Convert the format string to a raw char array va_list args; @@ -411,12 +387,9 @@ void CommManager::log(CommLinkInterface::LogSeverity severity, const char* fmt, vsnprintf(text, LOG_MSG_SIZE, fmt, args); va_end(args); - if (initialized_ && connected_) - { + if (initialized_ && connected_) { comm_link_.send_log_message(sysid_, severity, text); - } - else - { + } else { log_buffer_.add_message(severity, text); } } @@ -428,26 +401,26 @@ void CommManager::send_heartbeat(void) void CommManager::send_status(void) { - if (!initialized_) - return; + if (!initialized_) return; uint8_t control_mode = 0; - if (RF_.params_.get_param_int(PARAM_FIXED_WING)) - control_mode = MODE_PASS_THROUGH; + if (RF_.params_.get_param_int(PARAM_FIXED_WING)) control_mode = MODE_PASS_THROUGH; else if (RF_.command_manager_.combined_control().x.type == ANGLE) control_mode = MODE_ROLL_PITCH_YAWRATE_THROTTLE; else control_mode = MODE_ROLLRATE_PITCHRATE_YAWRATE_THROTTLE; - comm_link_.send_status(sysid_, RF_.state_manager_.state().armed, RF_.state_manager_.state().failsafe, - RF_.command_manager_.rc_override_active(), RF_.command_manager_.offboard_control_active(), - RF_.state_manager_.state().error_codes, control_mode, RF_.board_.num_sensor_errors(), - RF_.get_loop_time_us()); + comm_link_.send_status( + sysid_, RF_.state_manager_.state().armed, RF_.state_manager_.state().failsafe, + RF_.command_manager_.rc_override_active(), RF_.command_manager_.offboard_control_active(), + RF_.state_manager_.state().error_codes, control_mode, RF_.board_.num_sensor_errors(), + RF_.get_loop_time_us()); } void CommManager::send_attitude(void) { - comm_link_.send_attitude_quaternion(sysid_, RF_.estimator_.state().timestamp_us, RF_.estimator_.state().attitude, + comm_link_.send_attitude_quaternion(sysid_, RF_.estimator_.state().timestamp_us, + RF_.estimator_.state().attitude, RF_.estimator_.state().angular_velocity); } @@ -480,26 +453,24 @@ void CommManager::send_rc_raw(void) void CommManager::send_diff_pressure(void) { - if (RF_.sensors_.data().diff_pressure_valid) - { - comm_link_.send_diff_pressure(sysid_, RF_.sensors_.data().diff_pressure_velocity, RF_.sensors_.data().diff_pressure, + if (RF_.sensors_.data().diff_pressure_valid) { + comm_link_.send_diff_pressure(sysid_, RF_.sensors_.data().diff_pressure_velocity, + RF_.sensors_.data().diff_pressure, RF_.sensors_.data().diff_pressure_temp); } } void CommManager::send_baro(void) { - if (RF_.sensors_.data().baro_valid) - { - comm_link_.send_baro(sysid_, RF_.sensors_.data().baro_altitude, RF_.sensors_.data().baro_pressure, - RF_.sensors_.data().baro_temperature); + if (RF_.sensors_.data().baro_valid) { + comm_link_.send_baro(sysid_, RF_.sensors_.data().baro_altitude, + RF_.sensors_.data().baro_pressure, RF_.sensors_.data().baro_temperature); } } void CommManager::send_sonar(void) { - if (RF_.sensors_.data().sonar_range_valid) - { + if (RF_.sensors_.data().sonar_range_valid) { comm_link_.send_sonar(sysid_, 0, // TODO set sensor type (sonar/lidar), use enum RF_.sensors_.data().sonar_range, 8.0, 0.25); @@ -508,23 +479,20 @@ void CommManager::send_sonar(void) void CommManager::send_mag(void) { - if (RF_.sensors_.data().mag_present) - comm_link_.send_mag(sysid_, RF_.sensors_.data().mag); + if (RF_.sensors_.data().mag_present) comm_link_.send_mag(sysid_, RF_.sensors_.data().mag); } void CommManager::send_battery_status(void) { if (RF_.sensors_.data().battery_monitor_present) - comm_link_.send_battery_status(sysid_, RF_.sensors_.data().battery_voltage, RF_.sensors_.data().battery_current); + comm_link_.send_battery_status(sysid_, RF_.sensors_.data().battery_voltage, + RF_.sensors_.data().battery_current); } -void CommManager::send_backup_data(const StateManager::BackupData& backup_data) +void CommManager::send_backup_data(const StateManager::BackupData & backup_data) { - if (connected_) - { + if (connected_) { comm_link_.send_error_data(sysid_, backup_data); - } - else - { + } else { backup_data_buffer_ = backup_data; have_backup_data_ = true; } @@ -532,12 +500,10 @@ void CommManager::send_backup_data(const StateManager::BackupData& backup_data) void CommManager::send_gnss(void) { - const GNSSData& gnss_data = RF_.sensors_.data().gnss_data; + const GNSSData & gnss_data = RF_.sensors_.data().gnss_data; - if (RF_.sensors_.data().gnss_present) - { - if (gnss_data.time_of_week != last_sent_gnss_tow_) - { + if (RF_.sensors_.data().gnss_present) { + if (gnss_data.time_of_week != last_sent_gnss_tow_) { comm_link_.send_gnss(sysid_, gnss_data); last_sent_gnss_tow_ = gnss_data.time_of_week; } @@ -546,12 +512,10 @@ void CommManager::send_gnss(void) void CommManager::send_gnss_full() { - const GNSSFull& gnss_full = RF_.sensors_.data().gnss_full; + const GNSSFull & gnss_full = RF_.sensors_.data().gnss_full; - if (RF_.sensors_.data().gnss_present) - { - if (gnss_full.time_of_week != last_sent_gnss_full_tow_) - { + if (RF_.sensors_.data().gnss_present) { + if (gnss_full.time_of_week != last_sent_gnss_full_tow_) { comm_link_.send_gnss_full(sysid_, RF_.sensors_.data().gnss_full); last_sent_gnss_full_tow_ = gnss_full.time_of_week; } @@ -563,9 +527,8 @@ void CommManager::send_low_priority(void) send_next_param(); // send buffered log messages - if (connected_ && !log_buffer_.empty()) - { - const LogMessageBuffer::LogMessage& msg = log_buffer_.oldest(); + if (connected_ && !log_buffer_.empty()) { + const LogMessageBuffer::LogMessage & msg = log_buffer_.oldest(); comm_link_.send_log_message(sysid_, msg.severity, msg.msg); log_buffer_.pop(); } @@ -575,10 +538,7 @@ void CommManager::send_low_priority(void) void CommManager::stream() { uint64_t time_us = RF_.board_.clock_micros(); - for (int i = 0; i < STREAM_COUNT; i++) - { - streams_[i].stream(time_us); - } + for (int i = 0; i < STREAM_COUNT; i++) { streams_[i].stream(time_us); } RF_.board_.serial_flush(); } @@ -587,39 +547,35 @@ void CommManager::set_streaming_rate(uint8_t stream_id, int16_t param_id) streams_[stream_id].set_rate(RF_.params_.get_param_int(param_id)); } -void CommManager::send_named_value_int(const char* const name, int32_t value) +void CommManager::send_named_value_int(const char * const name, int32_t value) { comm_link_.send_named_value_int(sysid_, RF_.board_.clock_millis(), name, value); } -void CommManager::send_named_value_float(const char* const name, float value) +void CommManager::send_named_value_float(const char * const name, float value) { comm_link_.send_named_value_float(sysid_, RF_.board_.clock_millis(), name, value); } void CommManager::send_next_param(void) { - if (send_params_index_ < PARAMS_COUNT) - { + if (send_params_index_ < PARAMS_COUNT) { send_param_value(static_cast(send_params_index_)); send_params_index_++; } } -CommManager::Stream::Stream(uint32_t period_us, std::function send_function) : - period_us_(period_us), - next_time_us_(0), - send_function_(send_function) -{ -} +CommManager::Stream::Stream(uint32_t period_us, std::function send_function) + : period_us_(period_us) + , next_time_us_(0) + , send_function_(send_function) +{} void CommManager::Stream::stream(uint64_t now_us) { - if (period_us_ > 0 && now_us >= next_time_us_) - { + if (period_us_ > 0 && now_us >= next_time_us_) { // if you fall behind, skip messages - do - { + do { next_time_us_ += period_us_; } while (next_time_us_ < now_us); diff --git a/src/command_manager.cpp b/src/command_manager.cpp index c25c2095..abab3aff 100644 --- a/src/command_manager.cpp +++ b/src/command_manager.cpp @@ -54,29 +54,28 @@ rc_stick_override_t rc_stick_override[] = {{RC::STICK_X, 0}, {RC::STICK_Y, 0}, { typedef struct { - control_channel_t *rc; - control_channel_t *onboard; - control_channel_t *combined; + control_channel_t * rc; + control_channel_t * onboard; + control_channel_t * combined; } mux_t; -CommandManager::CommandManager(ROSflight &_rf) : RF_(_rf), failsafe_command_(multirotor_failsafe_command_) {} +CommandManager::CommandManager(ROSflight & _rf) + : RF_(_rf) + , failsafe_command_(multirotor_failsafe_command_) +{} -void CommandManager::init() -{ - init_failsafe(); -} +void CommandManager::init() { init_failsafe(); } void CommandManager::param_change_callback(uint16_t param_id) { - switch (param_id) - { - case PARAM_FIXED_WING: - case PARAM_FAILSAFE_THROTTLE: - init_failsafe(); - break; - default: - // do nothing - break; + switch (param_id) { + case PARAM_FIXED_WING: + case PARAM_FAILSAFE_THROTTLE: + init_failsafe(); + break; + default: + // do nothing + break; } } @@ -85,20 +84,18 @@ void CommandManager::init_failsafe() float failsafe_thr_param = RF_.params_.get_param_float(PARAM_FAILSAFE_THROTTLE); bool fixedwing = RF_.params_.get_param_int(PARAM_FIXED_WING); if (!fixedwing - && (failsafe_thr_param < 0. || failsafe_thr_param > 1.0)) // fixed wings always have 0 failsafe throttle + && (failsafe_thr_param < 0. + || failsafe_thr_param > 1.0)) // fixed wings always have 0 failsafe throttle { RF_.state_manager_.set_error(StateManager::ERROR_INVALID_FAILSAFE); failsafe_thr_param = 0.; - } - else - { + } else { RF_.state_manager_.clear_error(StateManager::ERROR_INVALID_FAILSAFE); } multirotor_failsafe_command_.F.value = failsafe_thr_param; - if (fixedwing) - failsafe_command_ = fixedwing_failsafe_command_; + if (fixedwing) failsafe_command_ = fixedwing_failsafe_command_; else failsafe_command_ = multirotor_failsafe_command_; } @@ -112,41 +109,35 @@ void CommandManager::interpret_rc(void) rc_command_.F.value = RF_.rc_.stick(RC::STICK_F); // determine control mode for each channel and scale command values accordingly - if (RF_.params_.get_param_int(PARAM_FIXED_WING)) - { + if (RF_.params_.get_param_int(PARAM_FIXED_WING)) { rc_command_.x.type = PASSTHROUGH; rc_command_.y.type = PASSTHROUGH; rc_command_.z.type = PASSTHROUGH; rc_command_.F.type = THROTTLE; - } - else - { + } else { // roll and pitch control_type_t roll_pitch_type; - if (RF_.rc_.switch_mapped(RC::SWITCH_ATT_TYPE)) - { + if (RF_.rc_.switch_mapped(RC::SWITCH_ATT_TYPE)) { roll_pitch_type = RF_.rc_.switch_on(RC::SWITCH_ATT_TYPE) ? ANGLE : RATE; - } - else - { - roll_pitch_type = (RF_.params_.get_param_int(PARAM_RC_ATTITUDE_MODE) == ATT_MODE_RATE) ? RATE : ANGLE; + } else { + roll_pitch_type = + (RF_.params_.get_param_int(PARAM_RC_ATTITUDE_MODE) == ATT_MODE_RATE) ? RATE : ANGLE; } rc_command_.x.type = roll_pitch_type; rc_command_.y.type = roll_pitch_type; // Scale command to appropriate units - switch (roll_pitch_type) - { - case RATE: - rc_command_.x.value *= RF_.params_.get_param_float(PARAM_RC_MAX_ROLLRATE); - rc_command_.y.value *= RF_.params_.get_param_float(PARAM_RC_MAX_PITCHRATE); - break; - case ANGLE: - rc_command_.x.value *= RF_.params_.get_param_float(PARAM_RC_MAX_ROLL); - rc_command_.y.value *= RF_.params_.get_param_float(PARAM_RC_MAX_PITCH); - default: - break; + switch (roll_pitch_type) { + case RATE: + rc_command_.x.value *= RF_.params_.get_param_float(PARAM_RC_MAX_ROLLRATE); + rc_command_.y.value *= RF_.params_.get_param_float(PARAM_RC_MAX_PITCHRATE); + break; + case ANGLE: + rc_command_.x.value *= RF_.params_.get_param_float(PARAM_RC_MAX_ROLL); + rc_command_.y.value *= RF_.params_.get_param_float(PARAM_RC_MAX_PITCH); + default: + break; } // yaw @@ -163,15 +154,12 @@ bool CommandManager::stick_deviated(MuxChannel channel) uint32_t now = RF_.board_.clock_millis(); // if we are still in the lag time, return true - if (now < rc_stick_override_[channel].last_override_time + RF_.params_.get_param_int(PARAM_OVERRIDE_LAG_TIME)) - { + if (now < rc_stick_override_[channel].last_override_time + + RF_.params_.get_param_int(PARAM_OVERRIDE_LAG_TIME)) { return true; - } - else - { + } else { if (fabsf(RF_.rc_.stick(rc_stick_override_[channel].rc_channel)) - > RF_.params_.get_param_float(PARAM_RC_OVERRIDE_DEVIATION)) - { + > RF_.params_.get_param_float(PARAM_RC_OVERRIDE_DEVIATION)) { rc_stick_override_[channel].last_override_time = now; return true; } @@ -185,18 +173,13 @@ bool CommandManager::do_roll_pitch_yaw_muxing(MuxChannel channel) // Check if the override switch exists and is triggered, or if the sticks have deviated enough to // trigger an override if ((RF_.rc_.switch_mapped(RC::SWITCH_ATT_OVERRIDE) && RF_.rc_.switch_on(RC::SWITCH_ATT_OVERRIDE)) - || stick_deviated(channel)) - { + || stick_deviated(channel)) { override_this_channel = true; - } - else // Otherwise only have RC override if the offboard channel is inactive + } else // Otherwise only have RC override if the offboard channel is inactive { - if (muxes[channel].onboard->active) - { + if (muxes[channel].onboard->active) { override_this_channel = false; - } - else - { + } else { override_this_channel = true; } } @@ -209,26 +192,19 @@ bool CommandManager::do_throttle_muxing(void) { bool override_this_channel = false; // Check if the override switch exists and is triggered - if (RF_.rc_.switch_mapped(RC::SWITCH_THROTTLE_OVERRIDE) && RF_.rc_.switch_on(RC::SWITCH_THROTTLE_OVERRIDE)) - { + if (RF_.rc_.switch_mapped(RC::SWITCH_THROTTLE_OVERRIDE) + && RF_.rc_.switch_on(RC::SWITCH_THROTTLE_OVERRIDE)) { override_this_channel = true; - } - else // Otherwise check if the offboard throttle channel is active, if it isn't, have RC override + } else // Otherwise check if the offboard throttle channel is active, if it isn't, have RC override { - if (muxes[MUX_F].onboard->active) - { + if (muxes[MUX_F].onboard->active) { // Check if the parameter flag is set to have us always take the smaller throttle - if (RF_.params_.get_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE)) - { + if (RF_.params_.get_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE)) { override_this_channel = (muxes[MUX_F].rc->value < muxes[MUX_F].onboard->value); - } - else - { + } else { override_this_channel = false; } - } - else - { + } else { override_this_channel = true; } } @@ -238,17 +214,12 @@ bool CommandManager::do_throttle_muxing(void) return override_this_channel; } -bool CommandManager::rc_override_active() -{ - return rc_override_; -} +bool CommandManager::rc_override_active() { return rc_override_; } bool CommandManager::offboard_control_active() { - for (int i = 0; i < 4; i++) - { - if (muxes[i].onboard->active) - return true; + for (int i = 0; i < 4; i++) { + if (muxes[i].onboard->active) return true; } return false; } @@ -276,18 +247,15 @@ bool CommandManager::run() bool last_rc_override = rc_override_; // Check for and apply failsafe command - if (RF_.state_manager_.state().failsafe) - { + if (RF_.state_manager_.state().failsafe) { combined_command_ = failsafe_command_; - } - else if (RF_.rc_.new_command()) - { + } else if (RF_.rc_.new_command()) { // Read RC interpret_rc(); // Check for offboard control timeout (100 ms) - if (RF_.board_.clock_millis() > offboard_command_.stamp_ms + RF_.params_.get_param_int(PARAM_OFFBOARD_TIMEOUT)) - { + if (RF_.board_.clock_millis() + > offboard_command_.stamp_ms + RF_.params_.get_param_int(PARAM_OFFBOARD_TIMEOUT)) { // If it has been longer than 100 ms, then disable the offboard control offboard_command_.F.active = false; offboard_command_.x.active = false; @@ -302,21 +270,15 @@ bool CommandManager::run() rc_override_ |= do_throttle_muxing(); // Light to indicate override - if (rc_override_) - { + if (rc_override_) { RF_.board_.led0_on(); - } - else - { + } else { RF_.board_.led0_off(); } } // There was a change in rc_override state - if (last_rc_override != rc_override_) - { - RF_.comm_manager_.update_status(); - } + if (last_rc_override != rc_override_) { RF_.comm_manager_.update_status(); } return true; } diff --git a/src/controller.cpp b/src/controller.cpp index 0e4b80af..e04036fb 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -41,7 +41,9 @@ namespace rosflight_firmware { -Controller::Controller(ROSflight &rf) : RF_(rf) {} +Controller::Controller(ROSflight & rf) + : RF_(rf) +{} void Controller::init() { @@ -51,7 +53,8 @@ void Controller::init() float min = -max; float tau = RF_.params_.get_param_float(PARAM_PID_TAU); - roll_.init(RF_.params_.get_param_float(PARAM_PID_ROLL_ANGLE_P), RF_.params_.get_param_float(PARAM_PID_ROLL_ANGLE_I), + roll_.init(RF_.params_.get_param_float(PARAM_PID_ROLL_ANGLE_P), + RF_.params_.get_param_float(PARAM_PID_ROLL_ANGLE_I), RF_.params_.get_param_float(PARAM_PID_ROLL_ANGLE_D), max, min, tau); roll_rate_.init(RF_.params_.get_param_float(PARAM_PID_ROLL_RATE_P), RF_.params_.get_param_float(PARAM_PID_ROLL_RATE_I), @@ -62,22 +65,21 @@ void Controller::init() pitch_rate_.init(RF_.params_.get_param_float(PARAM_PID_PITCH_RATE_P), RF_.params_.get_param_float(PARAM_PID_PITCH_RATE_I), RF_.params_.get_param_float(PARAM_PID_PITCH_RATE_D), max, min, tau); - yaw_rate_.init(RF_.params_.get_param_float(PARAM_PID_YAW_RATE_P), RF_.params_.get_param_float(PARAM_PID_YAW_RATE_I), + yaw_rate_.init(RF_.params_.get_param_float(PARAM_PID_YAW_RATE_P), + RF_.params_.get_param_float(PARAM_PID_YAW_RATE_I), RF_.params_.get_param_float(PARAM_PID_YAW_RATE_D), max, min, tau); } void Controller::run() { // Time calculation - if (prev_time_us_ == 0) - { + if (prev_time_us_ == 0) { prev_time_us_ = RF_.estimator_.state().timestamp_us; return; } int32_t dt_us = (RF_.estimator_.state().timestamp_us - prev_time_us_); - if (dt_us < 0) - { + if (dt_us < 0) { RF_.state_manager_.set_error(StateManager::ERROR_TIME_GOING_BACKWARDS); return; } @@ -85,12 +87,12 @@ void Controller::run() // Check if integrators should be updated //! @todo better way to figure out if throttle is high - bool update_integrators = - (RF_.state_manager_.state().armed) && (RF_.command_manager_.combined_control().F.value > 0.1f) && dt_us < 10000; + bool update_integrators = (RF_.state_manager_.state().armed) + && (RF_.command_manager_.combined_control().F.value > 0.1f) && dt_us < 10000; // Run the PID loops - turbomath::Vector pid_output = - run_pid_loops(dt_us, RF_.estimator_.state(), RF_.command_manager_.combined_control(), update_integrators); + turbomath::Vector pid_output = run_pid_loops( + dt_us, RF_.estimator_.state(), RF_.command_manager_.combined_control(), update_integrators); // Add feedforward torques output_.x = pid_output.x + RF_.params_.get_param_float(PARAM_X_EQ_TORQUE); @@ -102,10 +104,10 @@ void Controller::run() void Controller::calculate_equilbrium_torque_from_rc() { // Make sure we are disarmed - if (!(RF_.state_manager_.state().armed)) - { + if (!(RF_.state_manager_.state().armed)) { // Tell the user that we are doing a equilibrium torque calibration - RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_WARNING, "Capturing equilbrium offsets from RC"); + RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_WARNING, + "Capturing equilbrium offsets from RC"); // Prepare for calibration // artificially tell the flight controller it is leveled @@ -127,18 +129,22 @@ void Controller::calculate_equilbrium_torque_from_rc() // dt is zero, so what this really does is applies the P gain with the settings // your RC transmitter, which if it flies level is a really good guess for // the static offset torques - turbomath::Vector pid_output = run_pid_loops(0, fake_state, RF_.command_manager_.rc_control(), false); + turbomath::Vector pid_output = + run_pid_loops(0, fake_state, RF_.command_manager_.rc_control(), false); // the output from the controller is going to be the static offsets - RF_.params_.set_param_float(PARAM_X_EQ_TORQUE, pid_output.x + RF_.params_.get_param_float(PARAM_X_EQ_TORQUE)); - RF_.params_.set_param_float(PARAM_Y_EQ_TORQUE, pid_output.y + RF_.params_.get_param_float(PARAM_Y_EQ_TORQUE)); - RF_.params_.set_param_float(PARAM_Z_EQ_TORQUE, pid_output.z + RF_.params_.get_param_float(PARAM_Z_EQ_TORQUE)); + RF_.params_.set_param_float(PARAM_X_EQ_TORQUE, + pid_output.x + RF_.params_.get_param_float(PARAM_X_EQ_TORQUE)); + RF_.params_.set_param_float(PARAM_Y_EQ_TORQUE, + pid_output.y + RF_.params_.get_param_float(PARAM_Y_EQ_TORQUE)); + RF_.params_.set_param_float(PARAM_Z_EQ_TORQUE, + pid_output.z + RF_.params_.get_param_float(PARAM_Z_EQ_TORQUE)); - RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_WARNING, "Equilibrium torques found and applied."); - RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_WARNING, "Please zero out trims on your transmitter"); - } - else - { + RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_WARNING, + "Equilibrium torques found and applied."); + RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_WARNING, + "Please zero out trims on your transmitter"); + } else { RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_WARNING, "Cannot perform equilibrium offset calibration while armed"); } @@ -146,37 +152,34 @@ void Controller::calculate_equilbrium_torque_from_rc() void Controller::param_change_callback(uint16_t param_id) { - switch (param_id) - { - case PARAM_PID_ROLL_ANGLE_P: - case PARAM_PID_ROLL_ANGLE_I: - case PARAM_PID_ROLL_ANGLE_D: - case PARAM_PID_ROLL_RATE_P: - case PARAM_PID_ROLL_RATE_I: - case PARAM_PID_ROLL_RATE_D: - case PARAM_PID_PITCH_ANGLE_P: - case PARAM_PID_PITCH_ANGLE_I: - case PARAM_PID_PITCH_ANGLE_D: - case PARAM_PID_PITCH_RATE_P: - case PARAM_PID_PITCH_RATE_I: - case PARAM_PID_PITCH_RATE_D: - case PARAM_PID_YAW_RATE_P: - case PARAM_PID_YAW_RATE_I: - case PARAM_PID_YAW_RATE_D: - case PARAM_MAX_COMMAND: - case PARAM_PID_TAU: - init(); - break; - default: - // do nothing - break; + switch (param_id) { + case PARAM_PID_ROLL_ANGLE_P: + case PARAM_PID_ROLL_ANGLE_I: + case PARAM_PID_ROLL_ANGLE_D: + case PARAM_PID_ROLL_RATE_P: + case PARAM_PID_ROLL_RATE_I: + case PARAM_PID_ROLL_RATE_D: + case PARAM_PID_PITCH_ANGLE_P: + case PARAM_PID_PITCH_ANGLE_I: + case PARAM_PID_PITCH_ANGLE_D: + case PARAM_PID_PITCH_RATE_P: + case PARAM_PID_PITCH_RATE_I: + case PARAM_PID_PITCH_RATE_D: + case PARAM_PID_YAW_RATE_P: + case PARAM_PID_YAW_RATE_I: + case PARAM_PID_YAW_RATE_D: + case PARAM_MAX_COMMAND: + case PARAM_PID_TAU: + init(); + break; + default: + // do nothing + break; } } -turbomath::Vector Controller::run_pid_loops(uint32_t dt_us, - const Estimator::State &state, - const control_t &command, - bool update_integrators) +turbomath::Vector Controller::run_pid_loops(uint32_t dt_us, const Estimator::State & state, + const control_t & command, bool update_integrators) { // Based on the control types coming from the command manager, run the appropriate PID loops turbomath::Vector out; @@ -187,7 +190,8 @@ turbomath::Vector Controller::run_pid_loops(uint32_t dt_us, if (command.x.type == RATE) out.x = roll_rate_.run(dt, state.angular_velocity.x, command.x.value, update_integrators); else if (command.x.type == ANGLE) - out.x = roll_.run(dt, state.roll, command.x.value, update_integrators, state.angular_velocity.x); + out.x = + roll_.run(dt, state.roll, command.x.value, update_integrators, state.angular_velocity.x); else out.x = command.x.value; @@ -195,7 +199,8 @@ turbomath::Vector Controller::run_pid_loops(uint32_t dt_us, if (command.y.type == RATE) out.y = pitch_rate_.run(dt, state.angular_velocity.y, command.y.value, update_integrators); else if (command.y.type == ANGLE) - out.y = pitch_.run(dt, state.pitch, command.y.value, update_integrators, state.angular_velocity.y); + out.y = + pitch_.run(dt, state.pitch, command.y.value, update_integrators, state.angular_velocity.y); else out.y = command.y.value; @@ -208,18 +213,17 @@ turbomath::Vector Controller::run_pid_loops(uint32_t dt_us, return out; } -Controller::PID::PID() : - kp_(0.0f), - ki_(0.0f), - kd_(0.0f), - max_(1.0f), - min_(-1.0f), - integrator_(0.0f), - differentiator_(0.0f), - prev_x_(0.0f), - tau_(0.05) -{ -} +Controller::PID::PID() + : kp_(0.0f) + , ki_(0.0f) + , kd_(0.0f) + , max_(1.0f) + , min_(-1.0f) + , integrator_(0.0f) + , differentiator_(0.0f) + , prev_x_(0.0f) + , tau_(0.05) +{} void Controller::PID::init(float kp, float ki, float kd, float max, float min, float tau) { @@ -234,17 +238,14 @@ void Controller::PID::init(float kp, float ki, float kd, float max, float min, f float Controller::PID::run(float dt, float x, float x_c, bool update_integrator) { float xdot; - if (dt > 0.0001f) - { + if (dt > 0.0001f) { // calculate D term (use dirty derivative if we don't have access to a measurement of the // derivative) The dirty derivative is a sort of low-pass filtered version of the derivative. //// (Include reference to Dr. Beard's notes here) - differentiator_ = - (2.0f * tau_ - dt) / (2.0f * tau_ + dt) * differentiator_ + 2.0f / (2.0f * tau_ + dt) * (x - prev_x_); + differentiator_ = (2.0f * tau_ - dt) / (2.0f * tau_ + dt) * differentiator_ + + 2.0f / (2.0f * tau_ + dt) * (x - prev_x_); xdot = differentiator_; - } - else - { + } else { xdot = 0.0f; } prev_x_ = x; @@ -263,14 +264,10 @@ float Controller::PID::run(float dt, float x, float x_c, bool update_integrator, float d_term = 0.0f; // If there is a derivative term - if (kd_ > 0.0f) - { - d_term = kd_ * xdot; - } + if (kd_ > 0.0f) { d_term = kd_ * xdot; } // If there is an integrator term and we are updating integrators - if ((ki_ > 0.0f) && update_integrator) - { + if ((ki_ > 0.0f) && update_integrator) { // integrate integrator_ += error * dt; // calculate I term diff --git a/src/estimator.cpp b/src/estimator.cpp index 860d9fdd..4db72f0a 100644 --- a/src/estimator.cpp +++ b/src/estimator.cpp @@ -35,7 +35,9 @@ namespace rosflight_firmware { -Estimator::Estimator(ROSflight& _rf) : RF_(_rf) {} +Estimator::Estimator(ROSflight & _rf) + : RF_(_rf) +{} void Estimator::reset_state() { @@ -95,28 +97,25 @@ void Estimator::init() reset_state(); } -void Estimator::param_change_callback(uint16_t param_id) -{ - (void)param_id; -} +void Estimator::param_change_callback(uint16_t param_id) { (void) param_id; } void Estimator::run_LPF() { float alpha_acc = RF_.params_.get_param_float(PARAM_ACC_ALPHA); - const turbomath::Vector& raw_accel = RF_.sensors_.data().accel; + const turbomath::Vector & raw_accel = RF_.sensors_.data().accel; accel_LPF_.x = (1.0f - alpha_acc) * raw_accel.x + alpha_acc * accel_LPF_.x; accel_LPF_.y = (1.0f - alpha_acc) * raw_accel.y + alpha_acc * accel_LPF_.y; accel_LPF_.z = (1.0f - alpha_acc) * raw_accel.z + alpha_acc * accel_LPF_.z; float alpha_gyro_xy = RF_.params_.get_param_float(PARAM_GYRO_XY_ALPHA); float alpha_gyro_z = RF_.params_.get_param_float(PARAM_GYRO_Z_ALPHA); - const turbomath::Vector& raw_gyro = RF_.sensors_.data().gyro; + const turbomath::Vector & raw_gyro = RF_.sensors_.data().gyro; gyro_LPF_.x = (1.0f - alpha_gyro_xy) * raw_gyro.x + alpha_gyro_xy * gyro_LPF_.x; gyro_LPF_.y = (1.0f - alpha_gyro_xy) * raw_gyro.y + alpha_gyro_xy * gyro_LPF_.y; gyro_LPF_.z = (1.0f - alpha_gyro_z) * raw_gyro.z + alpha_gyro_z * gyro_LPF_.z; } -void Estimator::set_external_attitude_update(const turbomath::Quaternion& q) +void Estimator::set_external_attitude_update(const turbomath::Quaternion & q) { extatt_update_next_run_ = true; q_extatt_ = q; @@ -129,15 +128,12 @@ void Estimator::run() // const uint64_t now_us = RF_.sensors_.data().imu_time; - if (last_time_ == 0) - { + if (last_time_ == 0) { last_time_ = now_us; last_acc_update_us_ = now_us; last_extatt_update_us_ = now_us; return; - } - else if (now_us < last_time_) - { + } else if (now_us < last_time_) { // this shouldn't happen RF_.state_manager_.set_error(StateManager::ERROR_TIME_GOING_BACKWARDS); last_time_ = now_us; @@ -162,8 +158,7 @@ void Estimator::run() turbomath::Vector w_err; - if (can_use_accel()) - { + if (can_use_accel()) { // Get error estimated by accelerometer measurement w_err = accel_correction(); kp = RF_.params_.get_param_float(PARAM_FILTER_KP_ACC); @@ -171,8 +166,7 @@ void Estimator::run() last_acc_update_us_ = now_us; } - if (can_use_extatt()) - { + if (can_use_extatt()) { // Get error estimated by external attitude measurement. Overwrite any // correction based on the accelerometer (assumption: extatt is better). w_err = extatt_correction(); @@ -191,8 +185,7 @@ void Estimator::run() } // Crank up the gains for the first few seconds for quick convergence - if (now_us < static_cast(RF_.params_.get_param_int(PARAM_INIT_TIME)) * 1000) - { + if (now_us < static_cast(RF_.params_.get_param_int(PARAM_INIT_TIME)) * 1000) { kp = RF_.params_.get_param_float(PARAM_FILTER_KP_ACC) * 10.0f; ki = RF_.params_.get_param_float(PARAM_FILTER_KI) * 10.0f; } @@ -229,12 +222,9 @@ void Estimator::run() // If it has been more than 0.5 seconds since the accel update ran and we // are supposed to be getting them then trigger an unhealthy estimator error. if (RF_.params_.get_param_int(PARAM_FILTER_USE_ACC) && now_us > 500000 + last_acc_update_us_ - && !RF_.params_.get_param_int(PARAM_FIXED_WING)) - { + && !RF_.params_.get_param_int(PARAM_FIXED_WING)) { RF_.state_manager_.set_error(StateManager::ERROR_UNHEALTHY_ESTIMATOR); - } - else - { + } else { RF_.state_manager_.clear_error(StateManager::ERROR_UNHEALTHY_ESTIMATOR); } } @@ -242,8 +232,7 @@ void Estimator::run() bool Estimator::can_use_accel() const { // if we are not using accel, just bail - if (!RF_.params_.get_param_int(PARAM_FILTER_USE_ACC)) - return false; + if (!RF_.params_.get_param_int(PARAM_FILTER_USE_ACC)) return false; // current magnitude of LPF'd accelerometer const float a_sqrd_norm = accel_LPF_.sqrd_norm(); @@ -262,10 +251,7 @@ bool Estimator::can_use_accel() const return (lowerbound < a_sqrd_norm && a_sqrd_norm < upperbound); } -bool Estimator::can_use_extatt() const -{ - return extatt_update_next_run_; -} +bool Estimator::can_use_extatt() const { return extatt_update_next_run_; } turbomath::Vector Estimator::accel_correction() const { @@ -308,7 +294,8 @@ turbomath::Vector Estimator::extatt_correction() const // level but extatt had a different yaw angle than the internal estimate, xext_BW.cross(xhat_BW) // would be a measure of how the filter needs to update in order to the internal estimate's yaw // to closer to the extatt measurement. This is done for each axis. - turbomath::Vector w_ext = xext_BW.cross(xhat_BW) + yext_BW.cross(yhat_BW) + zext_BW.cross(zhat_BW); + turbomath::Vector w_ext = + xext_BW.cross(xhat_BW) + yext_BW.cross(yhat_BW) + zext_BW.cross(zhat_BW); return w_ext; } @@ -316,37 +303,31 @@ turbomath::Vector Estimator::extatt_correction() const turbomath::Vector Estimator::smoothed_gyro_measurement() { turbomath::Vector wbar; - if (RF_.params_.get_param_int(PARAM_FILTER_USE_QUAD_INT)) - { + if (RF_.params_.get_param_int(PARAM_FILTER_USE_QUAD_INT)) { // Quadratic Interpolation (Eq. 14 Casey Paper) // this step adds 12 us on the STM32F10x chips wbar = (w2_ / -12.0f) + w1_ * (8.0f / 12.0f) + gyro_LPF_ * (5.0f / 12.0f); w2_ = w1_; w1_ = gyro_LPF_; - } - else - { + } else { wbar = gyro_LPF_; } return wbar; } -void Estimator::integrate_angular_rate(turbomath::Quaternion& quat, - const turbomath::Vector& omega, - const float dt) const +void Estimator::integrate_angular_rate(turbomath::Quaternion & quat, + const turbomath::Vector & omega, const float dt) const { // only propagate if we've moved // TODO[PCL]: Will this ever be true? We should add a margin to this const float sqrd_norm_w = omega.sqrd_norm(); - if (sqrd_norm_w == 0.0f) - return; + if (sqrd_norm_w == 0.0f) return; // for convenience const float &p = omega.x, &q = omega.y, &r = omega.z; - if (RF_.params_.get_param_int(PARAM_FILTER_USE_MAT_EXP)) - { + if (RF_.params_.get_param_int(PARAM_FILTER_USE_MAT_EXP)) { // Matrix Exponential Approximation (From Attitude Representation and Kinematic // Propagation for Low-Cost UAVs by Robert T. Casey) // (Eq. 12 Casey Paper) @@ -359,14 +340,12 @@ void Estimator::integrate_angular_rate(turbomath::Quaternion& quat, quat.y = t1 * quat.y + t2 * (q * quat.w - r * quat.x + p * quat.z); quat.z = t1 * quat.z + t2 * (r * quat.w + q * quat.x - p * quat.y); quat.normalize(); - } - else - { + } else { // Euler Integration // (Eq. 47a Mahony Paper) turbomath::Quaternion qdot( - 0.5f * (-p * quat.x - q * quat.y - r * quat.z), 0.5f * (p * quat.w + r * quat.y - q * quat.z), - 0.5f * (q * quat.w - r * quat.x + p * quat.z), 0.5f * (r * quat.w + q * quat.x - p * quat.y)); + 0.5f * (-p * quat.x - q * quat.y - r * quat.z), 0.5f * (p * quat.w + r * quat.y - q * quat.z), + 0.5f * (q * quat.w - r * quat.x + p * quat.z), 0.5f * (r * quat.w + q * quat.x - p * quat.y)); quat.w += qdot.w * dt; quat.x += qdot.x * dt; quat.y += qdot.y * dt; @@ -375,10 +354,8 @@ void Estimator::integrate_angular_rate(turbomath::Quaternion& quat, } } -void Estimator::quaternion_to_dcm(const turbomath::Quaternion& q, - turbomath::Vector& X, - turbomath::Vector& Y, - turbomath::Vector& Z) const +void Estimator::quaternion_to_dcm(const turbomath::Quaternion & q, turbomath::Vector & X, + turbomath::Vector & Y, turbomath::Vector & Z) const { // R(q) = [X.x X.y X.z] // [Y.x Y.y Y.z] diff --git a/src/mixer.cpp b/src/mixer.cpp index 8dd5756f..2df2c9cc 100644 --- a/src/mixer.cpp +++ b/src/mixer.cpp @@ -37,30 +37,27 @@ namespace rosflight_firmware { -Mixer::Mixer(ROSflight &_rf) : RF_(_rf) +Mixer::Mixer(ROSflight & _rf) + : RF_(_rf) { mixer_to_use_ = nullptr; } -void Mixer::init() -{ - init_mixing(); -} +void Mixer::init() { init_mixing(); } void Mixer::param_change_callback(uint16_t param_id) { - switch (param_id) - { - case PARAM_MIXER: - init_mixing(); - break; - case PARAM_MOTOR_PWM_SEND_RATE: - case PARAM_RC_TYPE: - init_PWM(); - break; - default: - // do nothing - break; + switch (param_id) { + case PARAM_MIXER: + init_mixing(); + break; + case PARAM_MOTOR_PWM_SEND_RATE: + case PARAM_RC_TYPE: + init_PWM(); + break; + default: + // do nothing + break; } } @@ -71,23 +68,19 @@ void Mixer::init_mixing() uint8_t mixer_choice = RF_.params_.get_param_int(PARAM_MIXER); - if (mixer_choice >= NUM_MIXERS) - { + if (mixer_choice >= NUM_MIXERS) { RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, "Invalid Mixer Choice"); // set the invalid mixer flag RF_.state_manager_.set_error(StateManager::ERROR_INVALID_MIXER); mixer_to_use_ = nullptr; - } - else - { + } else { mixer_to_use_ = array_of_mixers_[mixer_choice]; } init_PWM(); - for (int8_t i = 0; i < NUM_TOTAL_OUTPUTS; i++) - { + for (int8_t i = 0; i < NUM_TOTAL_OUTPUTS; i++) { raw_outputs_[i] = 0.0f; outputs_[i] = 0.0f; } @@ -96,38 +89,28 @@ void Mixer::init_mixing() void Mixer::init_PWM() { uint32_t refresh_rate = RF_.params_.get_param_int(PARAM_MOTOR_PWM_SEND_RATE); - if (refresh_rate == 0 && mixer_to_use_ != nullptr) - { + if (refresh_rate == 0 && mixer_to_use_ != nullptr) { refresh_rate = mixer_to_use_->default_pwm_rate; } int16_t off_pwm = 1000; - if (mixer_to_use_ == nullptr || refresh_rate == 0) - RF_.board_.pwm_init(50, 0); + if (mixer_to_use_ == nullptr || refresh_rate == 0) RF_.board_.pwm_init(50, 0); else RF_.board_.pwm_init(refresh_rate, off_pwm); } void Mixer::write_motor(uint8_t index, float value) { - if (RF_.state_manager_.state().armed) - { - if (value > 1.0) - { + if (RF_.state_manager_.state().armed) { + if (value > 1.0) { value = 1.0; - } - else if (value < RF_.params_.get_param_float(PARAM_MOTOR_IDLE_THROTTLE) - && RF_.params_.get_param_int(PARAM_SPIN_MOTORS_WHEN_ARMED)) - { + } else if (value < RF_.params_.get_param_float(PARAM_MOTOR_IDLE_THROTTLE) + && RF_.params_.get_param_int(PARAM_SPIN_MOTORS_WHEN_ARMED)) { value = RF_.params_.get_param_float(PARAM_MOTOR_IDLE_THROTTLE); - } - else if (value < 0.0) - { + } else if (value < 0.0) { value = 0.0; } - } - else - { + } else { value = 0.0; } raw_outputs_[index] = value; @@ -136,12 +119,9 @@ void Mixer::write_motor(uint8_t index, float value) void Mixer::write_servo(uint8_t index, float value) { - if (value > 1.0) - { + if (value > 1.0) { value = 1.0; - } - else if (value < -1.0) - { + } else if (value < -1.0) { value = -1.0; } raw_outputs_[index] = value; @@ -150,8 +130,7 @@ void Mixer::write_servo(uint8_t index, float value) void Mixer::set_new_aux_command(aux_command_t new_aux_command) { - for (uint8_t i = 0; i < NUM_TOTAL_OUTPUTS; i++) - { + for (uint8_t i = 0; i < NUM_TOTAL_OUTPUTS; i++) { aux_command_.channel[i].type = new_aux_command.channel[i].type; aux_command_.channel[i].value = new_aux_command.channel[i].value; } @@ -163,48 +142,35 @@ void Mixer::mix_output() float max_output = 1.0f; // Reverse fixed-wing channels just before mixing if we need to - if (RF_.params_.get_param_int(PARAM_FIXED_WING)) - { + if (RF_.params_.get_param_int(PARAM_FIXED_WING)) { commands.x *= RF_.params_.get_param_int(PARAM_AILERON_REVERSE) ? -1 : 1; commands.y *= RF_.params_.get_param_int(PARAM_ELEVATOR_REVERSE) ? -1 : 1; commands.z *= RF_.params_.get_param_int(PARAM_RUDDER_REVERSE) ? -1 : 1; - } - else if (commands.F < RF_.params_.get_param_float(PARAM_MOTOR_IDLE_THROTTLE)) - { + } else if (commands.F < RF_.params_.get_param_float(PARAM_MOTOR_IDLE_THROTTLE)) { // For multirotors, disregard yaw commands if throttle is low to prevent motor spin-up while // arming/disarming commands.z = 0.0; } - if (mixer_to_use_ == nullptr) - return; + if (mixer_to_use_ == nullptr) return; - for (uint8_t i = 0; i < NUM_MIXER_OUTPUTS; i++) - { - if (mixer_to_use_->output_type[i] != NONE) - { + for (uint8_t i = 0; i < NUM_MIXER_OUTPUTS; i++) { + if (mixer_to_use_->output_type[i] != NONE) { // Matrix multiply to mix outputs outputs_[i] = (commands.F * mixer_to_use_->F[i] + commands.x * mixer_to_use_->x[i] + commands.y * mixer_to_use_->y[i] + commands.z * mixer_to_use_->z[i]); // Save off the largest control output if it is greater than 1.0 for future scaling - if (outputs_[i] > max_output) - { - max_output = outputs_[i]; - } + if (outputs_[i] > max_output) { max_output = outputs_[i]; } } } // saturate outputs to maintain controllability even during aggressive maneuvers float scale_factor = 1.0; - if (max_output > 1.0) - { - scale_factor = 1.0 / max_output; - } + if (max_output > 1.0) { scale_factor = 1.0 / max_output; } // Perform Motor Output Scaling - for (uint8_t i = 0; i < NUM_MIXER_OUTPUTS; i++) - { + for (uint8_t i = 0; i < NUM_MIXER_OUTPUTS; i++) { // scale all motor outputs by scale factor (this is usually 1.0, unless we saturated) outputs_[i] *= scale_factor; } @@ -213,35 +179,26 @@ void Mixer::mix_output() // For the first NUM_MIXER_OUTPUTS channels, only write aux_command to channels the mixer is not // using - for (uint8_t i = 0; i < NUM_MIXER_OUTPUTS; i++) - { - if (mixer_to_use_->output_type[i] == NONE) - { + for (uint8_t i = 0; i < NUM_MIXER_OUTPUTS; i++) { + if (mixer_to_use_->output_type[i] == NONE) { outputs_[i] = aux_command_.channel[i].value; combined_output_type_[i] = aux_command_.channel[i].type; - } - else - { + } else { combined_output_type_[i] = mixer_to_use_->output_type[i]; } } // The other channels are never used by the mixer - for (uint8_t i = NUM_MIXER_OUTPUTS; i < NUM_TOTAL_OUTPUTS; i++) - { + for (uint8_t i = NUM_MIXER_OUTPUTS; i < NUM_TOTAL_OUTPUTS; i++) { outputs_[i] = aux_command_.channel[i].value; combined_output_type_[i] = aux_command_.channel[i].type; } // Write to outputs - for (uint8_t i = 0; i < NUM_TOTAL_OUTPUTS; i++) - { - if (combined_output_type_[i] == S) - { + for (uint8_t i = 0; i < NUM_TOTAL_OUTPUTS; i++) { + if (combined_output_type_[i] == S) { write_servo(i, outputs_[i]); - } - else if (combined_output_type_[i] == M) - { + } else if (combined_output_type_[i] == M) { write_motor(i, outputs_[i]); } } diff --git a/src/param.cpp b/src/param.cpp index 36a5f15b..8b9d7617 100644 --- a/src/param.cpp +++ b/src/param.cpp @@ -56,7 +56,11 @@ namespace rosflight_firmware { -Params::Params(ROSflight &_rf) : RF_(_rf), listeners_(nullptr), num_listeners_(0) {} +Params::Params(ROSflight & _rf) + : RF_(_rf) + , listeners_(nullptr) + , num_listeners_(0) +{} // local function definitions void Params::init_param_int(uint16_t id, const char name[PARAMS_NAME_LENGTH], int32_t value) @@ -80,7 +84,7 @@ void Params::init_param_float(uint16_t id, const char name[PARAMS_NAME_LENGTH], uint8_t Params::compute_checksum(void) { uint8_t chk = 0; - const char *p; + const char * p; for (p = reinterpret_cast(¶ms.values); p < reinterpret_cast(¶ms.values) + 4 * PARAMS_COUNT; p++) @@ -99,8 +103,7 @@ uint8_t Params::compute_checksum(void) void Params::init() { RF_.board_.memory_init(); - if (!read()) - { + if (!read()) { RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_WARNING, "Unable to load parameters; using default values"); set_defaults(); @@ -289,7 +292,7 @@ void Params::set_defaults(void) } // clang-format on -void Params::set_listeners(ParamListenerInterface *const listeners[], size_t num_listeners) +void Params::set_listeners(ParamListenerInterface * const listeners[], size_t num_listeners) { listeners_ = listeners; num_listeners_ = num_listeners; @@ -297,17 +300,14 @@ void Params::set_listeners(ParamListenerInterface *const listeners[], size_t num bool Params::read(void) { - if (!RF_.board_.memory_read(¶ms, sizeof(params_t))) - return false; + if (!RF_.board_.memory_read(¶ms, sizeof(params_t))) return false; - if (params.version != GIT_VERSION_HASH) - return false; + if (params.version != GIT_VERSION_HASH) return false; if (params.size != sizeof(params_t) || params.magic_be != 0xBE || params.magic_ef != 0xEF) return false; - if (compute_checksum() != params.chk) - return false; + if (compute_checksum() != params.chk) return false; return true; } @@ -320,44 +320,34 @@ bool Params::write(void) params.magic_ef = 0xEF; params.chk = compute_checksum(); - if (!RF_.board_.memory_write(¶ms, sizeof(params_t))) - return false; + if (!RF_.board_.memory_write(¶ms, sizeof(params_t))) return false; return true; } void Params::change_callback(uint16_t id) { // call the callback function for all listeners - if (listeners_ != nullptr) - { - for (size_t i = 0; i < num_listeners_; i++) - { - listeners_[i]->param_change_callback(id); - } + if (listeners_ != nullptr) { + for (size_t i = 0; i < num_listeners_; i++) { listeners_[i]->param_change_callback(id); } } } uint16_t Params::lookup_param_id(const char name[PARAMS_NAME_LENGTH]) { - for (uint16_t id = 0; id < PARAMS_COUNT; id++) - { + for (uint16_t id = 0; id < PARAMS_COUNT; id++) { bool match = true; - for (uint8_t i = 0; i < PARAMS_NAME_LENGTH; i++) - { + for (uint8_t i = 0; i < PARAMS_NAME_LENGTH; i++) { // compare each character - if (name[i] != params.names[id][i]) - { + if (name[i] != params.names[id][i]) { match = false; break; } // stop comparing if end of string is reached - if (params.names[id][i] == '\0') - break; + if (params.names[id][i] == '\0') break; } - if (match) - return id; + if (match) return id; } return PARAMS_COUNT; @@ -365,8 +355,7 @@ uint16_t Params::lookup_param_id(const char name[PARAMS_NAME_LENGTH]) bool Params::set_param_int(uint16_t id, int32_t value) { - if (id < PARAMS_COUNT && value != params.values[id].ivalue) - { + if (id < PARAMS_COUNT && value != params.values[id].ivalue) { params.values[id].ivalue = value; change_callback(id); RF_.comm_manager_.send_param_value(id); @@ -377,8 +366,7 @@ bool Params::set_param_int(uint16_t id, int32_t value) bool Params::set_param_float(uint16_t id, float value) { - if (id < PARAMS_COUNT && value != params.values[id].fvalue) - { + if (id < PARAMS_COUNT && value != params.values[id].fvalue) { params.values[id].fvalue = value; change_callback(id); RF_.comm_manager_.send_parameter_list(); diff --git a/src/rc.cpp b/src/rc.cpp index df58f4e1..1734fa01 100644 --- a/src/rc.cpp +++ b/src/rc.cpp @@ -37,7 +37,9 @@ namespace rosflight_firmware { -RC::RC(ROSflight &_rf) : RF_(_rf) {} +RC::RC(ROSflight & _rf) + : RF_(_rf) +{} void RC::init() { @@ -54,47 +56,37 @@ void RC::init_rc() void RC::param_change_callback(uint16_t param_id) { - switch (param_id) - { - case PARAM_RC_TYPE: - RF_.board_.rc_init(static_cast(RF_.params_.get_param_int(PARAM_RC_TYPE))); - break; - case PARAM_RC_X_CHANNEL: - case PARAM_RC_Y_CHANNEL: - case PARAM_RC_Z_CHANNEL: - case PARAM_RC_F_CHANNEL: - init_sticks(); - break; - case PARAM_RC_ATTITUDE_OVERRIDE_CHANNEL: - case PARAM_RC_THROTTLE_OVERRIDE_CHANNEL: - case PARAM_RC_ATT_CONTROL_TYPE_CHANNEL: - case PARAM_RC_ARM_CHANNEL: - case PARAM_RC_SWITCH_5_DIRECTION: - case PARAM_RC_SWITCH_6_DIRECTION: - case PARAM_RC_SWITCH_7_DIRECTION: - case PARAM_RC_SWITCH_8_DIRECTION: - init_switches(); - break; - default: - // do nothing - break; + switch (param_id) { + case PARAM_RC_TYPE: + RF_.board_.rc_init(static_cast(RF_.params_.get_param_int(PARAM_RC_TYPE))); + break; + case PARAM_RC_X_CHANNEL: + case PARAM_RC_Y_CHANNEL: + case PARAM_RC_Z_CHANNEL: + case PARAM_RC_F_CHANNEL: + init_sticks(); + break; + case PARAM_RC_ATTITUDE_OVERRIDE_CHANNEL: + case PARAM_RC_THROTTLE_OVERRIDE_CHANNEL: + case PARAM_RC_ATT_CONTROL_TYPE_CHANNEL: + case PARAM_RC_ARM_CHANNEL: + case PARAM_RC_SWITCH_5_DIRECTION: + case PARAM_RC_SWITCH_6_DIRECTION: + case PARAM_RC_SWITCH_7_DIRECTION: + case PARAM_RC_SWITCH_8_DIRECTION: + init_switches(); + break; + default: + // do nothing + break; } } -float RC::stick(Stick channel) -{ - return stick_values[channel]; -} +float RC::stick(Stick channel) { return stick_values[channel]; } -bool RC::switch_on(Switch channel) -{ - return switch_values[channel]; -} +bool RC::switch_on(Switch channel) { return switch_values[channel]; } -bool RC::switch_mapped(Switch channel) -{ - return switches[channel].mapped; -} +bool RC::switch_mapped(Switch channel) { return switches[channel].mapped; } void RC::init_sticks(void) { @@ -113,60 +105,59 @@ void RC::init_sticks(void) void RC::init_switches() { - for (uint8_t chan = 0; chan < static_cast(SWITCHES_COUNT); chan++) - { + for (uint8_t chan = 0; chan < static_cast(SWITCHES_COUNT); chan++) { char channel_name[18]; - switch (chan) - { - case SWITCH_ARM: - strcpy(channel_name, "ARM"); - switches[chan].channel = RF_.params_.get_param_int(PARAM_RC_ARM_CHANNEL); - break; - case SWITCH_ATT_OVERRIDE: - strcpy(channel_name, "ATTITUDE OVERRIDE"); - switches[chan].channel = RF_.params_.get_param_int(PARAM_RC_ATTITUDE_OVERRIDE_CHANNEL); - break; - case SWITCH_THROTTLE_OVERRIDE: - strcpy(channel_name, "THROTTLE OVERRIDE"); - switches[chan].channel = RF_.params_.get_param_int(PARAM_RC_THROTTLE_OVERRIDE_CHANNEL); - break; - case SWITCH_ATT_TYPE: - strcpy(channel_name, "ATTITUDE TYPE"); - switches[chan].channel = RF_.params_.get_param_int(PARAM_RC_ATT_CONTROL_TYPE_CHANNEL); - break; - default: - strcpy(channel_name, "INVALID"); - switches[chan].channel = 255; - break; + switch (chan) { + case SWITCH_ARM: + strcpy(channel_name, "ARM"); + switches[chan].channel = RF_.params_.get_param_int(PARAM_RC_ARM_CHANNEL); + break; + case SWITCH_ATT_OVERRIDE: + strcpy(channel_name, "ATTITUDE OVERRIDE"); + switches[chan].channel = RF_.params_.get_param_int(PARAM_RC_ATTITUDE_OVERRIDE_CHANNEL); + break; + case SWITCH_THROTTLE_OVERRIDE: + strcpy(channel_name, "THROTTLE OVERRIDE"); + switches[chan].channel = RF_.params_.get_param_int(PARAM_RC_THROTTLE_OVERRIDE_CHANNEL); + break; + case SWITCH_ATT_TYPE: + strcpy(channel_name, "ATTITUDE TYPE"); + switches[chan].channel = RF_.params_.get_param_int(PARAM_RC_ATT_CONTROL_TYPE_CHANNEL); + break; + default: + strcpy(channel_name, "INVALID"); + switches[chan].channel = 255; + break; } - switches[chan].mapped = - switches[chan].channel > 3 && switches[chan].channel < RF_.params_.get_param_int(PARAM_RC_NUM_CHANNELS); - - switch (switches[chan].channel) - { - case 4: - switches[chan].direction = RF_.params_.get_param_int(PARAM_RC_SWITCH_5_DIRECTION); - break; - case 5: - switches[chan].direction = RF_.params_.get_param_int(PARAM_RC_SWITCH_6_DIRECTION); - break; - case 6: - switches[chan].direction = RF_.params_.get_param_int(PARAM_RC_SWITCH_7_DIRECTION); - break; - case 7: - switches[chan].direction = RF_.params_.get_param_int(PARAM_RC_SWITCH_8_DIRECTION); - break; - default: - switches[chan].direction = 1; - break; + switches[chan].mapped = switches[chan].channel > 3 + && switches[chan].channel < RF_.params_.get_param_int(PARAM_RC_NUM_CHANNELS); + + switch (switches[chan].channel) { + case 4: + switches[chan].direction = RF_.params_.get_param_int(PARAM_RC_SWITCH_5_DIRECTION); + break; + case 5: + switches[chan].direction = RF_.params_.get_param_int(PARAM_RC_SWITCH_6_DIRECTION); + break; + case 6: + switches[chan].direction = RF_.params_.get_param_int(PARAM_RC_SWITCH_7_DIRECTION); + break; + case 7: + switches[chan].direction = RF_.params_.get_param_int(PARAM_RC_SWITCH_8_DIRECTION); + break; + default: + switches[chan].direction = 1; + break; } if (switches[chan].mapped) - RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_INFO, "%s switch mapped to RC channel %d", channel_name, + RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_INFO, + "%s switch mapped to RC channel %d", channel_name, switches[chan].channel); else - RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_INFO, "%s switch not mapped", channel_name); + RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_INFO, "%s switch not mapped", + channel_name); } } @@ -175,20 +166,13 @@ bool RC::check_rc_lost() bool failsafe = false; // If the board reports that we have lost RC, tell the state manager - if (RF_.board_.rc_lost()) - { + if (RF_.board_.rc_lost()) { failsafe = true; - } - else - { + } else { // go into failsafe if we get an invalid RC command for any channel - for (int8_t i = 0; i < RF_.params_.get_param_int(PARAM_RC_NUM_CHANNELS); i++) - { + for (int8_t i = 0; i < RF_.params_.get_param_int(PARAM_RC_NUM_CHANNELS); i++) { float pwm = RF_.board_.rc_read(i); - if (pwm < -0.25 || pwm > 1.25) - { - failsafe = true; - } + if (pwm < -0.25 || pwm > 1.25) { failsafe = true; } } } @@ -208,54 +192,40 @@ void RC::look_for_arm_disarm_signal() uint32_t dt = now_ms - prev_time_ms; prev_time_ms = now_ms; // check for arming switch - if (!switch_mapped(SWITCH_ARM)) - { + if (!switch_mapped(SWITCH_ARM)) { if (!RF_.state_manager_.state().armed) // we are DISARMED { // if left stick is down and to the right if ((RF_.rc_.stick(STICK_F) < RF_.params_.get_param_float(PARAM_ARM_THRESHOLD)) - && (RF_.rc_.stick(STICK_Z) > (1.0f - RF_.params_.get_param_float(PARAM_ARM_THRESHOLD)))) - { + && (RF_.rc_.stick(STICK_Z) > (1.0f - RF_.params_.get_param_float(PARAM_ARM_THRESHOLD)))) { time_sticks_have_been_in_arming_position_ms += dt; - } - else - { + } else { time_sticks_have_been_in_arming_position_ms = 0; } - if (time_sticks_have_been_in_arming_position_ms > 1000) - { + if (time_sticks_have_been_in_arming_position_ms > 1000) { RF_.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM); } - } - else // we are ARMED + } else // we are ARMED { // if left stick is down and to the left if (RF_.rc_.stick(STICK_F) < RF_.params_.get_param_float(PARAM_ARM_THRESHOLD) - && RF_.rc_.stick(STICK_Z) < -(1.0f - RF_.params_.get_param_float(PARAM_ARM_THRESHOLD))) - { + && RF_.rc_.stick(STICK_Z) < -(1.0f - RF_.params_.get_param_float(PARAM_ARM_THRESHOLD))) { time_sticks_have_been_in_arming_position_ms += dt; - } - else - { + } else { time_sticks_have_been_in_arming_position_ms = 0; } - if (time_sticks_have_been_in_arming_position_ms > 1000) - { + if (time_sticks_have_been_in_arming_position_ms > 1000) { RF_.state_manager_.set_event(StateManager::EVENT_REQUEST_DISARM); time_sticks_have_been_in_arming_position_ms = 0; } } - } - else // ARMING WITH SWITCH + } else // ARMING WITH SWITCH { - if (RF_.rc_.switch_on(SWITCH_ARM)) - { + if (RF_.rc_.switch_on(SWITCH_ARM)) { if (!RF_.state_manager_.state().armed) RF_.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM); ; - } - else - { + } else { RF_.state_manager_.set_event(StateManager::EVENT_REQUEST_DISARM); } } @@ -266,46 +236,32 @@ bool RC::run() uint32_t now = RF_.board_.clock_millis(); // if it has been more than 20ms then look for new RC values and parse them - if (now - last_rc_receive_time < 20) - { - return false; - } + if (now - last_rc_receive_time < 20) { return false; } last_rc_receive_time = now; // Check for rc lost - if (check_rc_lost()) - return false; + if (check_rc_lost()) return false; // read and normalize stick values - for (uint8_t channel = 0; channel < static_cast(STICKS_COUNT); channel++) - { + for (uint8_t channel = 0; channel < static_cast(STICKS_COUNT); channel++) { float pwm = RF_.board_.rc_read(sticks[channel].channel); if (sticks[channel].one_sided) // generally only F is one_sided { stick_values[channel] = pwm; - } - else - { + } else { stick_values[channel] = 2.0 * (pwm - 0.5); } } // read and interpret switch values - for (uint8_t channel = 0; channel < static_cast(SWITCHES_COUNT); channel++) - { - if (switches[channel].mapped) - { - if (switches[channel].direction < 0) - { + for (uint8_t channel = 0; channel < static_cast(SWITCHES_COUNT); channel++) { + if (switches[channel].mapped) { + if (switches[channel].direction < 0) { switch_values[channel] = RF_.board_.rc_read(switches[channel].channel) < 0.2; - } - else - { + } else { switch_values[channel] = RF_.board_.rc_read(switches[channel].channel) >= 0.8; } - } - else - { + } else { switch_values[channel] = false; } } @@ -320,12 +276,10 @@ bool RC::run() bool RC::new_command() { - if (new_command_) - { + if (new_command_) { new_command_ = false; return true; - } - else + } else return false; ; } diff --git a/src/rosflight.cpp b/src/rosflight.cpp index 09647da8..b6022040 100644 --- a/src/rosflight.cpp +++ b/src/rosflight.cpp @@ -35,17 +35,17 @@ namespace rosflight_firmware { -ROSflight::ROSflight(Board& board, CommLinkInterface& comm_link) : - board_(board), - comm_manager_(*this, comm_link), - params_(*this), - command_manager_(*this), - controller_(*this), - estimator_(*this), - mixer_(*this), - rc_(*this), - sensors_(*this), - state_manager_(*this) +ROSflight::ROSflight(Board & board, CommLinkInterface & comm_link) + : board_(board) + , comm_manager_(*this, comm_link) + , params_(*this) + , command_manager_(*this) + , controller_(*this) + , estimator_(*this) + , mixer_(*this) + , rc_(*this) + , sensors_(*this) + , state_manager_(*this) { comm_link.set_listener(&comm_manager_); params_.set_listeners(param_listeners_, num_param_listeners_); @@ -103,8 +103,7 @@ void ROSflight::run() /*** Control Loop ***/ /*********************/ uint64_t start = board_.clock_micros(); - if (sensors_.run()) - { + if (sensors_.run()) { // If I have new IMU data, then perform control estimator_.run(); controller_.run(); @@ -131,9 +130,6 @@ void ROSflight::run() command_manager_.run(); } -uint32_t ROSflight::get_loop_time_us() -{ - return loop_time_us; -} +uint32_t ROSflight::get_loop_time_us() { return loop_time_us; } } // namespace rosflight_firmware diff --git a/src/sensors.cpp b/src/sensors.cpp index 9b733c0c..5f90cddd 100644 --- a/src/sensors.cpp +++ b/src/sensors.cpp @@ -57,7 +57,9 @@ const int Sensors::SENSOR_CAL_CYCLES = 127; const float Sensors::BARO_MAX_CALIBRATION_VARIANCE = 25.0; // standard dev about 0.2 m const float Sensors::DIFF_PRESSURE_MAX_CALIBRATION_VARIANCE = 100.0; // standard dev about 3 m/s -Sensors::Sensors(ROSflight &rosflight) : rf_(rosflight) {} +Sensors::Sensors(ROSflight & rosflight) + : rf_(rosflight) +{} void Sensors::init() { @@ -91,36 +93,37 @@ void Sensors::init_imu() data_.fcu_orientation = turbomath::Quaternion(roll, pitch, yaw); // See if the IMU is uncalibrated, and throw an error if it is - if (rf_.params_.get_param_float(PARAM_ACC_X_BIAS) == 0.0 && rf_.params_.get_param_float(PARAM_ACC_Y_BIAS) == 0.0 - && rf_.params_.get_param_float(PARAM_ACC_Z_BIAS) == 0.0 && rf_.params_.get_param_float(PARAM_GYRO_X_BIAS) == 0.0 - && rf_.params_.get_param_float(PARAM_GYRO_Y_BIAS) == 0.0 && rf_.params_.get_param_float(PARAM_GYRO_Z_BIAS) == 0.0) - { + if (rf_.params_.get_param_float(PARAM_ACC_X_BIAS) == 0.0 + && rf_.params_.get_param_float(PARAM_ACC_Y_BIAS) == 0.0 + && rf_.params_.get_param_float(PARAM_ACC_Z_BIAS) == 0.0 + && rf_.params_.get_param_float(PARAM_GYRO_X_BIAS) == 0.0 + && rf_.params_.get_param_float(PARAM_GYRO_Y_BIAS) == 0.0 + && rf_.params_.get_param_float(PARAM_GYRO_Z_BIAS) == 0.0) { rf_.state_manager_.set_error(StateManager::ERROR_UNCALIBRATED_IMU); } } void Sensors::param_change_callback(uint16_t param_id) { - switch (param_id) - { - case PARAM_FC_ROLL: - case PARAM_FC_PITCH: - case PARAM_FC_YAW: - init_imu(); - break; - case PARAM_BATTERY_VOLTAGE_MULTIPLIER: - case PARAM_BATTERY_CURRENT_MULTIPLIER: - update_battery_monitor_multipliers(); - break; - case PARAM_BATTERY_VOLTAGE_ALPHA: - battery_voltage_alpha_ = rf_.params_.get_param_float(PARAM_BATTERY_VOLTAGE_ALPHA); - break; - case PARAM_BATTERY_CURRENT_ALPHA: - battery_current_alpha_ = rf_.params_.get_param_float(PARAM_BATTERY_CURRENT_ALPHA); - break; - default: - // do nothing - break; + switch (param_id) { + case PARAM_FC_ROLL: + case PARAM_FC_PITCH: + case PARAM_FC_YAW: + init_imu(); + break; + case PARAM_BATTERY_VOLTAGE_MULTIPLIER: + case PARAM_BATTERY_CURRENT_MULTIPLIER: + update_battery_monitor_multipliers(); + break; + case PARAM_BATTERY_VOLTAGE_ALPHA: + battery_voltage_alpha_ = rf_.params_.get_param_float(PARAM_BATTERY_VOLTAGE_ALPHA); + break; + case PARAM_BATTERY_CURRENT_ALPHA: + battery_current_alpha_ = rf_.params_.get_param_float(PARAM_BATTERY_CURRENT_ALPHA); + break; + default: + // do nothing + break; } } @@ -131,8 +134,7 @@ bool Sensors::run(void) // Look for sensors that may not have been recognized at start because they weren't attached // to the 5V rail (only if disarmed) - if (!rf_.state_manager_.state().armed) - look_for_disabled_sensors(); + if (!rf_.state_manager_.state().armed) look_for_disabled_sensors(); // Update other sensors update_other_sensors(); @@ -141,94 +143,85 @@ bool Sensors::run(void) void Sensors::update_other_sensors() { - switch (next_sensor_to_update_) - { - case GNSS: - if (rf_.board_.gnss_present() && rf_.board_.gnss_has_new_data()) - { - data_.gnss_present = true; - data_.gnss_new_data = true; - rf_.board_.gnss_update(); - this->data_.gnss_data = rf_.board_.gnss_read(); - this->data_.gnss_full = rf_.board_.gnss_full_read(); - } - break; - - case BAROMETER: - if (rf_.board_.baro_present()) - { - data_.baro_present = true; - rf_.board_.baro_update(); - float raw_pressure; - float raw_temp; - rf_.board_.baro_read(&raw_pressure, &raw_temp); - data_.baro_valid = baro_outlier_filt_.update(raw_pressure, &data_.baro_pressure); - if (data_.baro_valid) - { - data_.baro_temperature = raw_temp; - correct_baro(); + switch (next_sensor_to_update_) { + case GNSS: + if (rf_.board_.gnss_present() && rf_.board_.gnss_has_new_data()) { + data_.gnss_present = true; + data_.gnss_new_data = true; + rf_.board_.gnss_update(); + this->data_.gnss_data = rf_.board_.gnss_read(); + this->data_.gnss_full = rf_.board_.gnss_full_read(); } - } - break; - - case MAGNETOMETER: - if (rf_.board_.mag_present()) - { - data_.mag_present = true; - float mag[3]; - rf_.board_.mag_update(); - rf_.board_.mag_read(mag); - data_.mag.x = mag[0]; - data_.mag.y = mag[1]; - data_.mag.z = mag[2]; - correct_mag(); - } - break; - - case DIFF_PRESSURE: - if (rf_.board_.diff_pressure_present() || data_.diff_pressure_present) - { - // if diff_pressure is currently present OR if it has historically been - // present (diff_pressure_present default is false) - rf_.board_.diff_pressure_update(); // update assists in recovering sensor if it temporarily - // disappears - - if (rf_.board_.diff_pressure_present()) - { - data_.diff_pressure_present = true; + break; + + case BAROMETER: + if (rf_.board_.baro_present()) { + data_.baro_present = true; + rf_.board_.baro_update(); float raw_pressure; float raw_temp; - rf_.board_.diff_pressure_read(&raw_pressure, &raw_temp); - data_.diff_pressure_valid = diff_outlier_filt_.update(raw_pressure, &data_.diff_pressure); - if (data_.diff_pressure_valid) - { - data_.diff_pressure_temp = raw_temp; - correct_diff_pressure(); + rf_.board_.baro_read(&raw_pressure, &raw_temp); + data_.baro_valid = baro_outlier_filt_.update(raw_pressure, &data_.baro_pressure); + if (data_.baro_valid) { + data_.baro_temperature = raw_temp; + correct_baro(); } } - } - break; + break; + + case MAGNETOMETER: + if (rf_.board_.mag_present()) { + data_.mag_present = true; + float mag[3]; + rf_.board_.mag_update(); + rf_.board_.mag_read(mag); + data_.mag.x = mag[0]; + data_.mag.y = mag[1]; + data_.mag.z = mag[2]; + correct_mag(); + } + break; + + case DIFF_PRESSURE: + if (rf_.board_.diff_pressure_present() || data_.diff_pressure_present) { + // if diff_pressure is currently present OR if it has historically been + // present (diff_pressure_present default is false) + rf_.board_.diff_pressure_update(); // update assists in recovering sensor if it temporarily + // disappears + + if (rf_.board_.diff_pressure_present()) { + data_.diff_pressure_present = true; + float raw_pressure; + float raw_temp; + rf_.board_.diff_pressure_read(&raw_pressure, &raw_temp); + data_.diff_pressure_valid = diff_outlier_filt_.update(raw_pressure, &data_.diff_pressure); + if (data_.diff_pressure_valid) { + data_.diff_pressure_temp = raw_temp; + correct_diff_pressure(); + } + } + } + break; - case SONAR: - rf_.board_.sonar_update(); - if (rf_.board_.sonar_present()) - { - data_.sonar_present = true; - float raw_distance; + case SONAR: rf_.board_.sonar_update(); - raw_distance = rf_.board_.sonar_read(); - data_.sonar_range_valid = sonar_outlier_filt_.update(raw_distance, &data_.sonar_range); - } - break; - case BATTERY_MONITOR: - if (rf_.board_.clock_millis() - last_battery_monitor_update_ms_ > BATTERY_MONITOR_UPDATE_PERIOD_MS) - { - last_battery_monitor_update_ms_ = rf_.board_.clock_millis(); - update_battery_monitor(); - } - break; - default: - break; + if (rf_.board_.sonar_present()) { + data_.sonar_present = true; + float raw_distance; + rf_.board_.sonar_update(); + raw_distance = rf_.board_.sonar_read(); + data_.sonar_range_valid = sonar_outlier_filt_.update(raw_distance, &data_.sonar_range); + } + break; + case BATTERY_MONITOR: + if (rf_.board_.clock_millis() - last_battery_monitor_update_ms_ + > BATTERY_MONITOR_UPDATE_PERIOD_MS) { + last_battery_monitor_update_ms_ = rf_.board_.clock_millis(); + update_battery_monitor(); + } + break; + default: + break; } next_sensor_to_update_ = (next_sensor_to_update_ + 1) % NUM_LOW_PRIORITY_SENSORS; @@ -240,8 +233,7 @@ void Sensors::look_for_disabled_sensors() // These sensors need power to respond, so they might not have been // detected on startup, but will be detected whenever power is applied // to the 5V rail. - if (rf_.board_.clock_millis() > last_time_look_for_disarmed_sensors_ + 1000) - { + if (rf_.board_.clock_millis() > last_time_look_for_disarmed_sensors_ + 1000) { last_time_look_for_disarmed_sensors_ = rf_.board_.clock_millis(); rf_.board_.baro_update(); rf_.board_.mag_update(); @@ -290,21 +282,16 @@ bool Sensors::start_diff_pressure_calibration() return true; } -bool Sensors::gyro_calibration_complete(void) -{ - return !calibrating_gyro_flag_; -} +bool Sensors::gyro_calibration_complete(void) { return !calibrating_gyro_flag_; } //================================================================== // local function definitions bool Sensors::update_imu(void) { - if (rf_.board_.new_imu_data()) - { + if (rf_.board_.new_imu_data()) { rf_.state_manager_.clear_error(StateManager::ERROR_IMU_NOT_RESPONDING); last_imu_update_ms_ = rf_.board_.clock_millis(); - if (!rf_.board_.imu_read(accel_, &data_.imu_temperature, gyro_, &data_.imu_time)) - return false; + if (!rf_.board_.imu_read(accel_, &data_.imu_temperature, gyro_, &data_.imu_time)) return false; // Move data into local copy data_.accel.x = accel_[0]; @@ -319,10 +306,8 @@ bool Sensors::update_imu(void) data_.gyro = data_.fcu_orientation * data_.gyro; - if (calibrating_acc_flag_) - calibrate_accel(); - if (calibrating_gyro_flag_) - calibrate_gyro(); + if (calibrating_acc_flag_) calibrate_accel(); + if (calibrating_gyro_flag_) calibrate_gyro(); // Apply bias correction correct_imu(); @@ -334,20 +319,16 @@ bool Sensors::update_imu(void) prev_imu_read_time_us_ = data_.imu_time; return true; - } - else - { + } else { // if we have lost 10 IMU messages then something is wrong // However, because we look for disabled sensors while disarmed, // we get IMU timeouts, which last for at least 10 ms. Therefore // we have an adjustable imu_timeout. int imu_timeout = rf_.state_manager_.state().armed ? 10 : 1000; - if (rf_.board_.clock_millis() > last_imu_update_ms_ + imu_timeout) - { + if (rf_.board_.clock_millis() > last_imu_update_ms_ + imu_timeout) { // Tell the board to fix it last_imu_update_ms_ = rf_.board_.clock_millis(); - if (!rf_.state_manager_.state().armed) - rf_.board_.imu_not_responding_error(); + if (!rf_.state_manager_.state().armed) rf_.board_.imu_not_responding_error(); // Indicate an IMU error rf_.state_manager_.set_error(StateManager::ERROR_IMU_NOT_RESPONDING); @@ -356,7 +337,8 @@ bool Sensors::update_imu(void) } } -void Sensors::get_filtered_IMU(turbomath::Vector &accel, turbomath::Vector &gyro, uint64_t &stamp_us) +void Sensors::get_filtered_IMU(turbomath::Vector & accel, turbomath::Vector & gyro, + uint64_t & stamp_us) { float delta_t = (data_.imu_time - int_start_us_) * 1e-6; accel = accel_int_ / delta_t; @@ -369,17 +351,15 @@ void Sensors::get_filtered_IMU(turbomath::Vector &accel, turbomath::Vector &gyro void Sensors::update_battery_monitor() { - if (rf_.board_.battery_voltage_present()) - { + if (rf_.board_.battery_voltage_present()) { data_.battery_monitor_present = true; data_.battery_voltage = data_.battery_voltage * battery_voltage_alpha_ - + rf_.board_.battery_voltage_read() * (1 - battery_voltage_alpha_); + + rf_.board_.battery_voltage_read() * (1 - battery_voltage_alpha_); } - if (rf_.board_.battery_current_present()) - { + if (rf_.board_.battery_current_present()) { data_.battery_monitor_present = true; data_.battery_current = data_.battery_current * battery_current_alpha_ - + rf_.board_.battery_current_read() * (1 - battery_current_alpha_); + + rf_.board_.battery_current_read() * (1 - battery_current_alpha_); } } @@ -390,13 +370,11 @@ void Sensors::calibrate_gyro() gyro_sum_ += data_.gyro; gyro_calibration_count_++; - if (gyro_calibration_count_ > 1000) - { + if (gyro_calibration_count_ > 1000) { // Gyros are simple. Just find the average during the calibration turbomath::Vector gyro_bias = gyro_sum_ / static_cast(gyro_calibration_count_); - if (gyro_bias.norm() < 1.0) - { + if (gyro_bias.norm() < 1.0) { rf_.params_.set_param_float(PARAM_GYRO_X_BIAS, gyro_bias.x); rf_.params_.set_param_float(PARAM_GYRO_Y_BIAS, gyro_bias.y); rf_.params_.set_param_float(PARAM_GYRO_Z_BIAS, gyro_bias.z); @@ -406,12 +384,11 @@ void Sensors::calibrate_gyro() // Tell the state manager that we just completed a gyro calibration rf_.state_manager_.set_event(StateManager::EVENT_CALIBRATION_COMPLETE); - } - else - { + } else { // Tell the state manager that we just failed a gyro calibration rf_.state_manager_.set_event(StateManager::EVENT_CALIBRATION_FAILED); - rf_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, "Too much movement for gyro cal"); + rf_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, + "Too much movement for gyro cal"); } // reset calibration in case we do it again @@ -441,8 +418,7 @@ void Sensors::calibrate_accel(void) min_ = vector_min(min_, data_.accel); accel_calibration_count_++; - if (accel_calibration_count_ > 1000) - { + if (accel_calibration_count_ > 1000) { // The temperature bias is calculated using a least-squares regression. // This is computationally intensive, so it is done by the companion // computer in fcu_io and shipped over to the flight controller. @@ -456,24 +432,21 @@ void Sensors::calibrate_accel(void) // the contribution of temperature to the measurements during the calibration, // Then we are dividing by the number of measurements. turbomath::Vector accel_bias = - (acc_sum_ - (accel_temp_bias * acc_temp_sum_)) / static_cast(accel_calibration_count_); + (acc_sum_ - (accel_temp_bias * acc_temp_sum_)) / static_cast(accel_calibration_count_); // Sanity Check - // If the accelerometer is upside down or being spun around during the calibration, // then don't do anything - if ((max_ - min_).norm() > 1.0) - { - rf_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, "Too much movement for IMU cal"); + if ((max_ - min_).norm() > 1.0) { + rf_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, + "Too much movement for IMU cal"); calibrating_acc_flag_ = false; - } - else - { + } else { // reset the estimated state rf_.estimator_.reset_state(); calibrating_acc_flag_ = false; - if (accel_bias.norm() < 3.0) - { + if (accel_bias.norm() < 3.0) { rf_.params_.set_param_float(PARAM_ACC_X_BIAS, accel_bias.x); rf_.params_.set_param_float(PARAM_ACC_Y_BIAS, accel_bias.y); rf_.params_.set_param_float(PARAM_ACC_Z_BIAS, accel_bias.z); @@ -481,12 +454,11 @@ void Sensors::calibrate_accel(void) // clear uncalibrated IMU flag rf_.state_manager_.clear_error(StateManager::ERROR_UNCALIBRATED_IMU); - } - else - { + } else { // This usually means the user has the FCU in the wrong orientation, or something is wrong // with the board IMU (like it's a cheap chinese clone) - rf_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, "large accel bias: norm = %d.%d", + rf_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, + "large accel bias: norm = %d.%d", static_cast(accel_bias.norm()), static_cast(accel_bias.norm() * 1000) % 1000); } @@ -509,32 +481,27 @@ void Sensors::calibrate_accel(void) void Sensors::calibrate_baro() { - if (rf_.board_.clock_millis() > last_baro_cal_iter_ms_ + 20) - { + if (rf_.board_.clock_millis() > last_baro_cal_iter_ms_ + 20) { baro_calibration_count_++; // calibrate pressure reading to where it should be - if (baro_calibration_count_ > SENSOR_CAL_DELAY_CYCLES + SENSOR_CAL_CYCLES) - { + if (baro_calibration_count_ > SENSOR_CAL_DELAY_CYCLES + SENSOR_CAL_CYCLES) { // if sample variance within acceptable range, flag calibration as done // else reset cal variables and start over - if (baro_calibration_var_ < BARO_MAX_CALIBRATION_VARIANCE) - { + if (baro_calibration_var_ < BARO_MAX_CALIBRATION_VARIANCE) { rf_.params_.set_param_float(PARAM_BARO_BIAS, baro_calibration_mean_); baro_calibrated_ = true; rf_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_INFO, "Baro Cal successful!"); - } - else - { - rf_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, "Too much movement for barometer cal"); + } else { + rf_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, + "Too much movement for barometer cal"); } baro_calibration_mean_ = 0.0f; baro_calibration_var_ = 0.0f; baro_calibration_count_ = 0; } - else if (baro_calibration_count_ > SENSOR_CAL_DELAY_CYCLES) - { + else if (baro_calibration_count_ > SENSOR_CAL_DELAY_CYCLES) { float measurement = data_.baro_pressure - ground_pressure_; float delta = measurement - baro_calibration_mean_; baro_calibration_mean_ += delta / (baro_calibration_count_ - SENSOR_CAL_DELAY_CYCLES); @@ -547,32 +514,27 @@ void Sensors::calibrate_baro() void Sensors::calibrate_diff_pressure() { - if (rf_.board_.clock_millis() > last_diff_pressure_cal_iter_ms_ + 20) - { + if (rf_.board_.clock_millis() > last_diff_pressure_cal_iter_ms_ + 20) { diff_pressure_calibration_count_++; - if (diff_pressure_calibration_count_ > SENSOR_CAL_DELAY_CYCLES + SENSOR_CAL_CYCLES) - { + if (diff_pressure_calibration_count_ > SENSOR_CAL_DELAY_CYCLES + SENSOR_CAL_CYCLES) { // if sample variance within acceptable range, flag calibration as done // else reset cal variables and start over - if (diff_pressure_calibration_var_ < DIFF_PRESSURE_MAX_CALIBRATION_VARIANCE) - { + if (diff_pressure_calibration_var_ < DIFF_PRESSURE_MAX_CALIBRATION_VARIANCE) { rf_.params_.set_param_float(PARAM_DIFF_PRESS_BIAS, diff_pressure_calibration_mean_); diff_pressure_calibrated_ = true; rf_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_INFO, "Airspeed Cal Successful!"); - } - else - { - rf_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, "Too much movement for diff pressure cal"); + } else { + rf_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, + "Too much movement for diff pressure cal"); } diff_pressure_calibration_mean_ = 0.0f; diff_pressure_calibration_var_ = 0.0f; diff_pressure_calibration_count_ = 0; - } - else if (diff_pressure_calibration_count_ > SENSOR_CAL_DELAY_CYCLES) - { + } else if (diff_pressure_calibration_count_ > SENSOR_CAL_DELAY_CYCLES) { float delta = data_.diff_pressure - diff_pressure_calibration_mean_; - diff_pressure_calibration_mean_ += delta / (diff_pressure_calibration_count_ - SENSOR_CAL_DELAY_CYCLES); + diff_pressure_calibration_mean_ += + delta / (diff_pressure_calibration_count_ - SENSOR_CAL_DELAY_CYCLES); float delta2 = data_.diff_pressure - diff_pressure_calibration_mean_; diff_pressure_calibration_var_ += delta * delta2 / (SENSOR_CAL_CYCLES - 1); } @@ -586,11 +548,11 @@ void Sensors::correct_imu(void) { // correct according to known biases and temperature compensation data_.accel.x -= rf_.params_.get_param_float(PARAM_ACC_X_TEMP_COMP) * data_.imu_temperature - + rf_.params_.get_param_float(PARAM_ACC_X_BIAS); + + rf_.params_.get_param_float(PARAM_ACC_X_BIAS); data_.accel.y -= rf_.params_.get_param_float(PARAM_ACC_Y_TEMP_COMP) * data_.imu_temperature - + rf_.params_.get_param_float(PARAM_ACC_Y_BIAS); + + rf_.params_.get_param_float(PARAM_ACC_Y_BIAS); data_.accel.z -= rf_.params_.get_param_float(PARAM_ACC_Z_TEMP_COMP) * data_.imu_temperature - + rf_.params_.get_param_float(PARAM_ACC_Z_BIAS); + + rf_.params_.get_param_float(PARAM_ACC_Z_BIAS); data_.gyro.x -= rf_.params_.get_param_float(PARAM_GYRO_X_BIAS); data_.gyro.y -= rf_.params_.get_param_float(PARAM_GYRO_Y_BIAS); @@ -606,35 +568,32 @@ void Sensors::correct_mag(void) // correct according to known soft iron bias - converts to nT data_.mag.x = rf_.params_.get_param_float(PARAM_MAG_A11_COMP) * mag_hard_x - + rf_.params_.get_param_float(PARAM_MAG_A12_COMP) * mag_hard_y - + rf_.params_.get_param_float(PARAM_MAG_A13_COMP) * mag_hard_z; + + rf_.params_.get_param_float(PARAM_MAG_A12_COMP) * mag_hard_y + + rf_.params_.get_param_float(PARAM_MAG_A13_COMP) * mag_hard_z; data_.mag.y = rf_.params_.get_param_float(PARAM_MAG_A21_COMP) * mag_hard_x - + rf_.params_.get_param_float(PARAM_MAG_A22_COMP) * mag_hard_y - + rf_.params_.get_param_float(PARAM_MAG_A23_COMP) * mag_hard_z; + + rf_.params_.get_param_float(PARAM_MAG_A22_COMP) * mag_hard_y + + rf_.params_.get_param_float(PARAM_MAG_A23_COMP) * mag_hard_z; data_.mag.z = rf_.params_.get_param_float(PARAM_MAG_A31_COMP) * mag_hard_x - + rf_.params_.get_param_float(PARAM_MAG_A32_COMP) * mag_hard_y - + rf_.params_.get_param_float(PARAM_MAG_A33_COMP) * mag_hard_z; + + rf_.params_.get_param_float(PARAM_MAG_A32_COMP) * mag_hard_y + + rf_.params_.get_param_float(PARAM_MAG_A33_COMP) * mag_hard_z; } void Sensors::correct_baro(void) { - if (!baro_calibrated_) - calibrate_baro(); + if (!baro_calibrated_) calibrate_baro(); data_.baro_pressure -= rf_.params_.get_param_float(PARAM_BARO_BIAS); - data_.baro_altitude = turbomath::alt(data_.baro_pressure) - rf_.params_.get_param_float(PARAM_GROUND_LEVEL); + data_.baro_altitude = + turbomath::alt(data_.baro_pressure) - rf_.params_.get_param_float(PARAM_GROUND_LEVEL); } void Sensors::correct_diff_pressure() { - if (!diff_pressure_calibrated_) - calibrate_diff_pressure(); + if (!diff_pressure_calibrated_) calibrate_diff_pressure(); data_.diff_pressure -= rf_.params_.get_param_float(PARAM_DIFF_PRESS_BIAS); float atm = 101325.0f; - if (data_.baro_present) - atm = data_.baro_pressure; - data_.diff_pressure_velocity = - turbomath::fsign(data_.diff_pressure) * 24.574f - / turbomath::inv_sqrt((turbomath::fabs(data_.diff_pressure) * data_.diff_pressure_temp / atm)); + if (data_.baro_present) atm = data_.baro_pressure; + data_.diff_pressure_velocity = turbomath::fsign(data_.diff_pressure) * 24.574f + / turbomath::inv_sqrt((turbomath::fabs(data_.diff_pressure) * data_.diff_pressure_temp / atm)); } void Sensors::OutlierFilter::init(float max_change_rate, float update_rate, float center) @@ -645,22 +604,16 @@ void Sensors::OutlierFilter::init(float max_change_rate, float update_rate, floa init_ = true; } -bool Sensors::OutlierFilter::update(float new_val, float *val) +bool Sensors::OutlierFilter::update(float new_val, float * val) { float diff = new_val - center_; - if (fabsf(diff) < window_size_ * max_change_) - { + if (fabsf(diff) < window_size_ * max_change_) { *val = new_val; center_ += turbomath::fsign(diff) * fminf(max_change_, fabsf(diff)); - if (window_size_ > 1) - { - window_size_--; - } + if (window_size_ > 1) { window_size_--; } return true; - } - else - { + } else { window_size_++; return false; } diff --git a/src/state_manager.cpp b/src/state_manager.cpp index b1219cec..3cfdbbaa 100644 --- a/src/state_manager.cpp +++ b/src/state_manager.cpp @@ -35,7 +35,9 @@ namespace rosflight_firmware { -StateManager::StateManager(ROSflight &parent) : RF_(parent), fsm_state_(FSM_STATE_INIT) +StateManager::StateManager(ROSflight & parent) + : RF_(parent) + , fsm_state_(FSM_STATE_INIT) { state_.armed = false; state_.error = false; @@ -75,8 +77,7 @@ void StateManager::set_error(uint16_t error) void StateManager::clear_error(uint16_t error) { // If this error code was set, - if (state_.error_codes & error) - { + if (state_.error_codes & error) { // Clear the error code state_.error_codes &= ~(error); @@ -92,179 +93,170 @@ void StateManager::set_event(StateManager::Event event) { FsmState start_state = fsm_state_; uint16_t start_errors = state_.error_codes; - switch (fsm_state_) - { - case FSM_STATE_INIT: - if (event == EVENT_INITIALIZED) - { - fsm_state_ = FSM_STATE_PREFLIGHT; - } - break; - - case FSM_STATE_PREFLIGHT: - switch (event) - { - case EVENT_RC_FOUND: - clear_error(ERROR_RC_LOST); - state_.failsafe = false; - break; - case EVENT_RC_LOST: - set_error(ERROR_RC_LOST); + switch (fsm_state_) { + case FSM_STATE_INIT: + if (event == EVENT_INITIALIZED) { fsm_state_ = FSM_STATE_PREFLIGHT; } break; - case EVENT_ERROR: - state_.error = true; - fsm_state_ = FSM_STATE_ERROR; - break; - case EVENT_REQUEST_ARM: - // require low RC throttle to arm - if (RF_.rc_.stick(RC::Stick::STICK_F) < RF_.params_.get_param_float(PARAM_ARM_THRESHOLD)) - { - // require either min throttle to be enabled or throttle override switch to be on - if (RF_.params_.get_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE) - || RF_.rc_.switch_on(RC::Switch::SWITCH_THROTTLE_OVERRIDE)) - { - if (RF_.params_.get_param_int(PARAM_CALIBRATE_GYRO_ON_ARM)) - { - fsm_state_ = FSM_STATE_CALIBRATING; - RF_.sensors_.start_gyro_calibration(); - } - else - { - state_.armed = true; - RF_.comm_manager_.update_status(); - fsm_state_ = FSM_STATE_ARMED; + + case FSM_STATE_PREFLIGHT: + switch (event) { + case EVENT_RC_FOUND: + clear_error(ERROR_RC_LOST); + state_.failsafe = false; + break; + case EVENT_RC_LOST: + set_error(ERROR_RC_LOST); + break; + case EVENT_ERROR: + state_.error = true; + fsm_state_ = FSM_STATE_ERROR; + break; + case EVENT_REQUEST_ARM: + // require low RC throttle to arm + if (RF_.rc_.stick(RC::Stick::STICK_F) + < RF_.params_.get_param_float(PARAM_ARM_THRESHOLD)) { + // require either min throttle to be enabled or throttle override switch to be on + if (RF_.params_.get_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE) + || RF_.rc_.switch_on(RC::Switch::SWITCH_THROTTLE_OVERRIDE)) { + if (RF_.params_.get_param_int(PARAM_CALIBRATE_GYRO_ON_ARM)) { + fsm_state_ = FSM_STATE_CALIBRATING; + RF_.sensors_.start_gyro_calibration(); + } else { + state_.armed = true; + RF_.comm_manager_.update_status(); + fsm_state_ = FSM_STATE_ARMED; + } + } else { + RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, + "RC throttle override must be active to arm"); + } + } else { + RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, + "Cannot arm with RC throttle high"); } - } - else - { - RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, - "RC throttle override must be active to arm"); - } + break; + default: + break; } - else - { - RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, "Cannot arm with RC throttle high"); - } - break; - default: break; - } - break; - case FSM_STATE_ERROR: - switch (event) - { - case EVENT_RC_LOST: - set_error(ERROR_RC_LOST); // sometimes redundant, but reports RC lost error if another error - // got reported first - break; - case EVENT_RC_FOUND: - clear_error(ERROR_RC_LOST); - state_.failsafe = false; - break; - case EVENT_NO_ERROR: - state_.error = false; - fsm_state_ = FSM_STATE_PREFLIGHT; - break; - case EVENT_REQUEST_ARM: - if (next_arming_error_msg_ms_ < RF_.board_.clock_millis()) - { - if (state_.error_codes & StateManager::ERROR_INVALID_MIXER) - RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, "Unable to arm: Invalid mixer"); - if (state_.error_codes & StateManager::ERROR_IMU_NOT_RESPONDING) - RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, "Unable to arm: IMU not responding"); - if (state_.error_codes & StateManager::ERROR_RC_LOST) - RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, "Unable to arm: RC signal lost"); - if (state_.error_codes & StateManager::ERROR_UNHEALTHY_ESTIMATOR) - RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, "Unable to arm: Unhealthy estimator"); - if (state_.error_codes & StateManager::ERROR_TIME_GOING_BACKWARDS) - RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, "Unable to arm: Time going backwards"); - if (state_.error_codes & StateManager::ERROR_UNCALIBRATED_IMU) - RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, "Unable to arm: IMU not calibrated"); - if (state_.error_codes & StateManager::ERROR_INVALID_FAILSAFE) - RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, "Unable to arm: Invalid failsafe setting"); + case FSM_STATE_ERROR: + switch (event) { + case EVENT_RC_LOST: + set_error( + ERROR_RC_LOST); // sometimes redundant, but reports RC lost error if another error + // got reported first + break; + case EVENT_RC_FOUND: + clear_error(ERROR_RC_LOST); + state_.failsafe = false; + break; + case EVENT_NO_ERROR: + state_.error = false; + fsm_state_ = FSM_STATE_PREFLIGHT; + break; + case EVENT_REQUEST_ARM: + if (next_arming_error_msg_ms_ < RF_.board_.clock_millis()) { + if (state_.error_codes & StateManager::ERROR_INVALID_MIXER) + RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, + "Unable to arm: Invalid mixer"); + if (state_.error_codes & StateManager::ERROR_IMU_NOT_RESPONDING) + RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, + "Unable to arm: IMU not responding"); + if (state_.error_codes & StateManager::ERROR_RC_LOST) + RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, + "Unable to arm: RC signal lost"); + if (state_.error_codes & StateManager::ERROR_UNHEALTHY_ESTIMATOR) + RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, + "Unable to arm: Unhealthy estimator"); + if (state_.error_codes & StateManager::ERROR_TIME_GOING_BACKWARDS) + RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, + "Unable to arm: Time going backwards"); + if (state_.error_codes & StateManager::ERROR_UNCALIBRATED_IMU) + RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, + "Unable to arm: IMU not calibrated"); + if (state_.error_codes & StateManager::ERROR_INVALID_FAILSAFE) + RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, + "Unable to arm: Invalid failsafe setting"); - next_arming_error_msg_ms_ = RF_.board_.clock_millis() + 1000; // throttle messages to 1 Hz + next_arming_error_msg_ms_ = + RF_.board_.clock_millis() + 1000; // throttle messages to 1 Hz + } + break; + default: + break; } break; - default: - break; - } - break; - case FSM_STATE_CALIBRATING: - switch (event) - { - case EVENT_CALIBRATION_COMPLETE: - state_.armed = true; - fsm_state_ = FSM_STATE_ARMED; - break; - case EVENT_CALIBRATION_FAILED: - fsm_state_ = FSM_STATE_PREFLIGHT; - break; - case EVENT_RC_LOST: - set_error(ERROR_RC_LOST); - break; - case EVENT_ERROR: - fsm_state_ = FSM_STATE_ERROR; - state_.error = true; - break; - case EVENT_NO_ERROR: - state_.error = false; - break; - default: + case FSM_STATE_CALIBRATING: + switch (event) { + case EVENT_CALIBRATION_COMPLETE: + state_.armed = true; + fsm_state_ = FSM_STATE_ARMED; + break; + case EVENT_CALIBRATION_FAILED: + fsm_state_ = FSM_STATE_PREFLIGHT; + break; + case EVENT_RC_LOST: + set_error(ERROR_RC_LOST); + break; + case EVENT_ERROR: + fsm_state_ = FSM_STATE_ERROR; + state_.error = true; + break; + case EVENT_NO_ERROR: + state_.error = false; + break; + default: + break; + } break; - } - break; - case FSM_STATE_ARMED: - switch (event) - { - case EVENT_RC_LOST: - state_.failsafe = true; - fsm_state_ = FSM_STATE_FAILSAFE; - set_error(ERROR_RC_LOST); - RF_.comm_manager_.update_status(); - break; - case EVENT_REQUEST_DISARM: - state_.armed = false; - if (state_.error) - fsm_state_ = FSM_STATE_ERROR; - else - fsm_state_ = FSM_STATE_PREFLIGHT; - break; - case EVENT_ERROR: - state_.error = true; - break; - case EVENT_NO_ERROR: - state_.error = false; - break; - default: + case FSM_STATE_ARMED: + switch (event) { + case EVENT_RC_LOST: + state_.failsafe = true; + fsm_state_ = FSM_STATE_FAILSAFE; + set_error(ERROR_RC_LOST); + RF_.comm_manager_.update_status(); + break; + case EVENT_REQUEST_DISARM: + state_.armed = false; + if (state_.error) fsm_state_ = FSM_STATE_ERROR; + else + fsm_state_ = FSM_STATE_PREFLIGHT; + break; + case EVENT_ERROR: + state_.error = true; + break; + case EVENT_NO_ERROR: + state_.error = false; + break; + default: + break; + } break; - } - break; - case FSM_STATE_FAILSAFE: - switch (event) - { - case EVENT_ERROR: - state_.error = true; - break; - case EVENT_REQUEST_DISARM: - state_.armed = false; - fsm_state_ = FSM_STATE_ERROR; - break; - case EVENT_RC_FOUND: - state_.failsafe = false; - fsm_state_ = FSM_STATE_ARMED; - clear_error(ERROR_RC_LOST); + case FSM_STATE_FAILSAFE: + switch (event) { + case EVENT_ERROR: + state_.error = true; + break; + case EVENT_REQUEST_DISARM: + state_.armed = false; + fsm_state_ = FSM_STATE_ERROR; + break; + case EVENT_RC_FOUND: + state_.failsafe = false; + fsm_state_ = FSM_STATE_ARMED; + clear_error(ERROR_RC_LOST); + break; + default: + break; + } break; default: break; - } - break; - default: - break; } // If there has been a change, then report it to the user @@ -272,7 +264,7 @@ void StateManager::set_event(StateManager::Event event) RF_.comm_manager_.update_status(); } -void StateManager::write_backup_data(const BackupData::DebugInfo &debug) +void StateManager::write_backup_data(const BackupData::DebugInfo & debug) { BackupData data; data.reset_count = hardfault_count_ + 1; @@ -291,30 +283,27 @@ void StateManager::check_backup_memory() // check for hardfault recovery data in backup memory BackupData data; - if (RF_.board_.backup_memory_read(reinterpret_cast(&data), sizeof(data))) - { - if (data.valid_checksum()) - { + if (RF_.board_.backup_memory_read(reinterpret_cast(&data), sizeof(data))) { + if (data.valid_checksum()) { hardfault_count_ = data.reset_count; - if (data.arm_flag == BackupData::ARM_MAGIC) - { + if (data.arm_flag == BackupData::ARM_MAGIC) { // do emergency rearm if in a good state - if (fsm_state_ == FSM_STATE_PREFLIGHT) - { + if (fsm_state_ == FSM_STATE_PREFLIGHT) { state_.armed = true; fsm_state_ = FSM_STATE_ARMED; - RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_CRITICAL, "Rearming after hardfault!!!"); - } - else - { - RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_CRITICAL, "Failed to rearm after hardfault!!!"); + RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_CRITICAL, + "Rearming after hardfault!!!"); + } else { + RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_CRITICAL, + "Failed to rearm after hardfault!!!"); } } // queue sending backup data over comm link RF_.comm_manager_.send_backup_data(data); - RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_CRITICAL, "Recovered from hardfault!!!"); + RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_CRITICAL, + "Recovered from hardfault!!!"); } RF_.board_.backup_memory_clear(sizeof(data)); @@ -323,8 +312,7 @@ void StateManager::check_backup_memory() void StateManager::process_errors() { - if (state_.error_codes) - set_event(EVENT_ERROR); + if (state_.error_codes) set_event(EVENT_ERROR); else set_event(EVENT_NO_ERROR); } @@ -332,19 +320,15 @@ void StateManager::process_errors() void StateManager::update_leds() { // blink fast if in failsafe - if (state_.failsafe) - { - if (next_led_blink_ms_ < RF_.board_.clock_millis()) - { + if (state_.failsafe) { + if (next_led_blink_ms_ < RF_.board_.clock_millis()) { RF_.board_.led1_toggle(); next_led_blink_ms_ = RF_.board_.clock_millis() + 100; } } // blink slowly if in error - else if (state_.error) - { - if (next_led_blink_ms_ < RF_.board_.clock_millis()) - { + else if (state_.error) { + if (next_led_blink_ms_ < RF_.board_.clock_millis()) { RF_.board_.led1_toggle(); next_led_blink_ms_ = RF_.board_.clock_millis() + 500; } diff --git a/test/command_manager_test.cpp b/test/command_manager_test.cpp index d5be3369..d1c0b419 100644 --- a/test/command_manager_test.cpp +++ b/test/command_manager_test.cpp @@ -35,18 +35,19 @@ class CommandManagerTest : public ::testing::Test {true, RATE, OFFBOARD_Z}, {true, THROTTLE, OFFBOARD_F}}; - CommandManagerTest() : mavlink(board), rf(board, mavlink) {} + CommandManagerTest() + : mavlink(board) + , rf(board, mavlink) + {} void SetUp() override { rf.init(); - rf.state_manager_.clear_error(rf.state_manager_.state().error_codes); // Clear All Errors to Start + rf.state_manager_.clear_error( + rf.state_manager_.state().error_codes); // Clear All Errors to Start rf.params_.set_param_int(PARAM_CALIBRATE_GYRO_ON_ARM, false); - for (int i = 0; i < 8; i++) - { - rc_values[i] = 1500; - } + for (int i = 0; i < 8; i++) { rc_values[i] = 1500; } rc_values[2] = 1000; rf.params_.set_param_int(PARAM_MIXER, Mixer::PASSTHROUGH); @@ -56,7 +57,7 @@ class CommandManagerTest : public ::testing::Test max_yawrate = rf.params_.get_param_float(PARAM_RC_MAX_YAWRATE); } - void setOffboard(control_t& command) + void setOffboard(control_t & command) { command.stamp_ms = rf.board_.clock_millis(); rf.command_manager_.set_new_offboard_command(command); @@ -67,8 +68,7 @@ class CommandManagerTest : public ::testing::Test uint64_t start_time_us = board.clock_micros(); float dummy_acc[3] = {0, 0, -9.80665}; float dummy_gyro[3] = {0, 0, 0}; - while (board.clock_micros() < start_time_us + us) - { + while (board.clock_micros() < start_time_us + us) { board.set_imu(dummy_acc, dummy_gyro, board.clock_micros() + 1000); rf.run(); } diff --git a/test/common.cpp b/test/common.cpp index 5f5de157..0acc4ab7 100644 --- a/test/common.cpp +++ b/test/common.cpp @@ -4,11 +4,10 @@ double quaternion_error(Eigen::Quaternionf q_eig, turbomath::Quaternion q) { Eigen::Quaternionf est_quat(q.w, q.x, q.y, q.z); Eigen::Quaternionf q_tilde = q_eig * est_quat.inverse(); - if (q_tilde.vec().norm() < 0.000001) - return 0; - else - { - Eigen::Vector3f v_tilde = atan2(q_tilde.vec().norm(), q_tilde.w()) * q_tilde.vec() / q_tilde.vec().norm(); + if (q_tilde.vec().norm() < 0.000001) return 0; + else { + Eigen::Vector3f v_tilde = + atan2(q_tilde.vec().norm(), q_tilde.w()) * q_tilde.vec() / q_tilde.vec().norm(); return v_tilde.norm(); } } @@ -18,28 +17,27 @@ double quaternion_error(turbomath::Quaternion q0, turbomath::Quaternion q) Eigen::Quaternionf est_quat(q.w, q.x, q.y, q.z); Eigen::Quaternionf q_eig(q0.w, q0.x, q0.y, q0.z); Eigen::Quaternionf q_tilde = q_eig * est_quat.inverse(); - if (q_tilde.vec().norm() < 0.000001) - return 0; - else - { - Eigen::Vector3f v_tilde = atan2(q_tilde.vec().norm(), q_tilde.w()) * q_tilde.vec() / q_tilde.vec().norm(); + if (q_tilde.vec().norm() < 0.000001) return 0; + else { + Eigen::Vector3f v_tilde = + atan2(q_tilde.vec().norm(), q_tilde.w()) * q_tilde.vec() / q_tilde.vec().norm(); return v_tilde.norm(); } } -void step_firmware(rosflight_firmware::ROSflight &rf, rosflight_firmware::testBoard &board, uint32_t us) +void step_firmware(rosflight_firmware::ROSflight & rf, rosflight_firmware::testBoard & board, + uint32_t us) { uint64_t start_time_us = board.clock_micros(); float dummy_acc[3] = {0, 0, -9.80665}; float dummy_gyro[3] = {0, 0, 0}; - while (board.clock_micros() < start_time_us + us) - { + while (board.clock_micros() < start_time_us + us) { board.set_imu(dummy_acc, dummy_gyro, board.clock_micros() + 1000); rf.run(); } } -int main(int argc, char **argv) +int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); diff --git a/test/common.h b/test/common.h index 6eb3d024..2fb36da3 100644 --- a/test/common.h +++ b/test/common.h @@ -11,40 +11,40 @@ #include -#define EXPECT_VEC3_SUPERCLOSE(vec, eig) \ - EXPECT_NEAR((vec).x, (eig).x(), 0.0001); \ - EXPECT_NEAR((vec).y, (eig).y(), 0.0001); \ +#define EXPECT_VEC3_SUPERCLOSE(vec, eig) \ + EXPECT_NEAR((vec).x, (eig).x(), 0.0001); \ + EXPECT_NEAR((vec).y, (eig).y(), 0.0001); \ EXPECT_NEAR((vec).z, (eig).z(), 0.0001) -#define EXPECT_QUAT_SUPERCLOSE(q, q_eig) \ - { \ - double e1 = quaternion_error((q_eig), (q)); \ - Eigen::Quaternionf q_eig_neg = q_eig; \ - q_eig_neg.coeffs() *= -1.0; \ - double e2 = quaternion_error(q_eig_neg, (q)); \ - double error = (e1 < e2) ? e1 : e2; \ - EXPECT_LE(error, 0.0001); \ +#define EXPECT_QUAT_SUPERCLOSE(q, q_eig) \ + { \ + double e1 = quaternion_error((q_eig), (q)); \ + Eigen::Quaternionf q_eig_neg = q_eig; \ + q_eig_neg.coeffs() *= -1.0; \ + double e2 = quaternion_error(q_eig_neg, (q)); \ + double error = (e1 < e2) ? e1 : e2; \ + EXPECT_LE(error, 0.0001); \ } -#define ASSERT_QUAT_SUPERCLOSE(q, q_eig) \ - { \ - double e1 = quaternion_error((q_eig), (q)); \ - Eigen::Quaternionf q_eig_neg = q_eig; \ - q_eig_neg.coeffs() *= -1.0; \ - double e2 = quaternion_error(q_eig_neg, (q)); \ - double error = (e1 < e2) ? e1 : e2; \ - EXPECT_LE(error, 0.0001); \ +#define ASSERT_QUAT_SUPERCLOSE(q, q_eig) \ + { \ + double e1 = quaternion_error((q_eig), (q)); \ + Eigen::Quaternionf q_eig_neg = q_eig; \ + q_eig_neg.coeffs() *= -1.0; \ + double e2 = quaternion_error(q_eig_neg, (q)); \ + double error = (e1 < e2) ? e1 : e2; \ + EXPECT_LE(error, 0.0001); \ } -#define ASSERT_TURBOQUAT_SUPERCLOSE(q, q2) \ - { \ - double e1 = quaternion_error(q, q2); \ - turbomath::Quaternion q2_neg = q2; \ - q2_neg.w *= -1.0; \ - q2_neg.x *= -1.0; \ - q2_neg.y *= -1.0; \ - q2_neg.z *= -1.0; \ - double e2 = quaternion_error(q, q2_neg); \ - double error = (e1 < e2) ? e1 : e2; \ - ASSERT_LE(error, 0.0001); \ +#define ASSERT_TURBOQUAT_SUPERCLOSE(q, q2) \ + { \ + double e1 = quaternion_error(q, q2); \ + turbomath::Quaternion q2_neg = q2; \ + q2_neg.w *= -1.0; \ + q2_neg.x *= -1.0; \ + q2_neg.y *= -1.0; \ + q2_neg.z *= -1.0; \ + double e2 = quaternion_error(q, q2_neg); \ + double error = (e1 < e2) ? e1 : e2; \ + ASSERT_LE(error, 0.0001); \ } #define EXPECT_BASICALLYTHESAME(x, y) EXPECT_NEAR(x, y, 0.00001) @@ -60,4 +60,5 @@ double quaternion_error(turbomath::Quaternion q0, turbomath::Quaternion q); double quaternion_error(Eigen::Quaternionf q_eig, turbomath::Quaternion q); -void step_firmware(rosflight_firmware::ROSflight &rf, rosflight_firmware::testBoard &board, uint32_t us); +void step_firmware(rosflight_firmware::ROSflight & rf, rosflight_firmware::testBoard & board, + uint32_t us); diff --git a/test/estimator_test.cpp b/test/estimator_test.cpp index 03ee2b07..092a0325 100644 --- a/test/estimator_test.cpp +++ b/test/estimator_test.cpp @@ -37,7 +37,10 @@ class EstimatorTest : public ::testing::Test int ext_att_update_rate_; int ext_att_count_; - EstimatorTest() : mavlink(board), rf(board, mavlink) {} + EstimatorTest() + : mavlink(board) + , rf(board, mavlink) + {} void SetUp() override { @@ -69,7 +72,7 @@ class EstimatorTest : public ::testing::Test rf.init(); } - void initFile(const std::string& filename) { file_.open(filename); } + void initFile(const std::string & filename) { file_.open(filename); } double run() { @@ -77,10 +80,8 @@ class EstimatorTest : public ::testing::Test double max_error = 0.0; t_ = 0.0; - while (t_ < tmax_) - { - for (int i = 0; i < oversampling_factor_; i++) - { + while (t_ < tmax_) { + for (int i = 0; i < oversampling_factor_; i++) { Vector3d w; w[0] = x_amp_ * sin(x_freq_ / (2.0 * M_PI) * t_); w[1] = y_amp_ * sin(y_freq_ / (2.0 * M_PI) * t_); @@ -97,8 +98,7 @@ class EstimatorTest : public ::testing::Test Vector3d err = computeError(); double err_norm = err.norm(); - if (std::abs(err_norm - 2.0 * M_PI) < err_norm) - { + if (std::abs(err_norm - 2.0 * M_PI) < err_norm) { err_norm = std::abs(err_norm - 2.0 * M_PI); } max_error = (err_norm > max_error) ? err_norm : max_error; @@ -106,23 +106,20 @@ class EstimatorTest : public ::testing::Test return max_error; } - void integrate(Quaterniond& q, const Vector3d& _w, double dt) + void integrate(Quaterniond & q, const Vector3d & _w, double dt) { Vector3d w = _w * dt; Quaterniond w_exp; double w_norm = w.norm(); - if (w_norm > 1e-4) - { + if (w_norm > 1e-4) { w_exp.w() = std::cos(w_norm / 2.0); double scale = std::sin(w_norm / 2.0) / w_norm; w_exp.x() = scale * w(0); w_exp.y() = scale * w(1); w_exp.z() = scale * w(2); w_exp.normalize(); - } - else - { + } else { w_exp.w() = 1.0; w_exp.x() = w(0) / 2.0; w_exp.y() = w(1) / 2.0; @@ -133,7 +130,7 @@ class EstimatorTest : public ::testing::Test q.coeffs() *= sign(q.w()); } - void simulateIMU(float* acc, float* gyro) + void simulateIMU(float * acc, float * gyro) { Vector3d y_acc = q_.inverse() * gravity; acc[0] = y_acc.x(); @@ -148,8 +145,7 @@ class EstimatorTest : public ::testing::Test void extAttUpdate() { - if (ext_att_update_rate_ && ++ext_att_count_ >= ext_att_update_rate_) - { + if (ext_att_update_rate_ && ++ext_att_count_ >= ext_att_update_rate_) { ext_att_count_ = 0; turbomath::Quaternion q_ext; q_ext.w = q_.w(); @@ -173,30 +169,25 @@ class EstimatorTest : public ::testing::Test float w = err.w(); double norm_v = v.norm(); - if (t_ > 20.698) - { + if (t_ > 20.698) { int debug = 1; - (void)debug; + (void) debug; } Vector3d log_err; - if (norm_v > 1e-4) - { + if (norm_v > 1e-4) { log_err = 2.0 * std::atan(norm_v / w) * v / norm_v; - } - else - { + } else { log_err = 2.0 * sign(w) * v; } - if (file_.is_open()) - { - file_.write(reinterpret_cast(&t_), sizeof(t_)); - file_.write(reinterpret_cast(&q_), sizeof(double) * 4); - file_.write(reinterpret_cast(&rf_quat), sizeof(double) * 4); - file_.write(reinterpret_cast(log_err.data()), sizeof(double) * 3); - file_.write(reinterpret_cast(eulerError().data()), sizeof(double) * 3); - file_.write(reinterpret_cast(&rf.estimator_.bias().x), sizeof(float) * 3); + if (file_.is_open()) { + file_.write(reinterpret_cast(&t_), sizeof(t_)); + file_.write(reinterpret_cast(&q_), sizeof(double) * 4); + file_.write(reinterpret_cast(&rf_quat), sizeof(double) * 4); + file_.write(reinterpret_cast(log_err.data()), sizeof(double) * 3); + file_.write(reinterpret_cast(eulerError().data()), sizeof(double) * 3); + file_.write(reinterpret_cast(&rf.estimator_.bias().x), sizeof(float) * 3); } return log_err; @@ -225,9 +216,11 @@ class EstimatorTest : public ::testing::Test Vector3d getTrueRPY() { Vector3d rpy; - rpy(0) = std::atan2(2.0f * (q_.w() * q_.x() + q_.y() * q_.z()), 1.0f - 2.0f * (q_.x() * q_.x() + q_.y() * q_.y())); + rpy(0) = std::atan2(2.0f * (q_.w() * q_.x() + q_.y() * q_.z()), + 1.0f - 2.0f * (q_.x() * q_.x() + q_.y() * q_.y())); rpy(1) = std::asin(2.0f * (q_.w() * q_.y() - q_.z() * q_.x())); - rpy(2) = std::atan2(2.0f * (q_.w() * q_.z() + q_.x() * q_.y()), 1.0f - 2.0f * (q_.y() * q_.y() + q_.z() * q_.z())); + rpy(2) = std::atan2(2.0f * (q_.w() * q_.z() + q_.x() * q_.y()), + 1.0f - 2.0f * (q_.y() * q_.y() + q_.z() * q_.z())); return rpy; } }; diff --git a/test/state_machine_test.cpp b/test/state_machine_test.cpp index 0b86a0e9..8c103200 100644 --- a/test/state_machine_test.cpp +++ b/test/state_machine_test.cpp @@ -14,13 +14,17 @@ class StateMachineTest : public ::testing::Test Mavlink mavlink; ROSflight rf; - StateMachineTest() : mavlink(board), rf(board, mavlink) {} + StateMachineTest() + : mavlink(board) + , rf(board, mavlink) + {} void SetUp() override { board.backup_memory_clear(); rf.init(); - rf.state_manager_.clear_error(rf.state_manager_.state().error_codes); // Clear All Errors to Start + rf.state_manager_.clear_error( + rf.state_manager_.state().error_codes); // Clear All Errors to Start rf.params_.set_param_int(PARAM_MIXER, 10); rf.params_.set_param_int(PARAM_CALIBRATE_GYRO_ON_ARM, false); // default to turning this off rf.params_.set_param_float(PARAM_FAILSAFE_THROTTLE, 0.0f); @@ -32,8 +36,7 @@ class StateMachineTest : public ::testing::Test uint64_t start_time_us = board.clock_micros(); float dummy_acc[3] = {0, 0, -9.80665}; float dummy_gyro[3] = {0, 0, 0}; - while (board.clock_micros() < start_time_us + us) - { + while (board.clock_micros() < start_time_us + us) { board.set_imu(dummy_acc, dummy_gyro, board.clock_micros() + 1000); rf.run(); } @@ -52,8 +55,7 @@ TEST_F(StateMachineTest, Init) TEST_F(StateMachineTest, SetAndClearAllErrors) { // Try setting and clearing all the errors - for (int error = 0x0001; error <= StateManager::ERROR_INVALID_FAILSAFE; error *= 2) - { + for (int error = 0x0001; error <= StateManager::ERROR_INVALID_FAILSAFE; error *= 2) { // set the error rf.state_manager_.set_error(error); EXPECT_EQ(rf.state_manager_.state().armed, false); @@ -73,7 +75,7 @@ TEST_F(StateMachineTest, SetAndClearAllErrors) TEST_F(StateMachineTest, SetAndClearComboErrors) { uint32_t error = StateManager::ERROR_IMU_NOT_RESPONDING | StateManager::ERROR_TIME_GOING_BACKWARDS - | StateManager::ERROR_UNCALIBRATED_IMU; + | StateManager::ERROR_UNCALIBRATED_IMU; rf.state_manager_.set_error(error); EXPECT_EQ(rf.state_manager_.state().armed, false); EXPECT_EQ(rf.state_manager_.state().failsafe, false); @@ -84,7 +86,7 @@ TEST_F(StateMachineTest, SetAndClearComboErrors) TEST_F(StateMachineTest, AddErrorAfterPreviousError) { uint32_t error = StateManager::ERROR_IMU_NOT_RESPONDING | StateManager::ERROR_TIME_GOING_BACKWARDS - | StateManager::ERROR_UNCALIBRATED_IMU; + | StateManager::ERROR_UNCALIBRATED_IMU; rf.state_manager_.set_error(error); rf.state_manager_.set_error(StateManager::ERROR_INVALID_MIXER); EXPECT_EQ(rf.state_manager_.state().armed, false); @@ -96,7 +98,7 @@ TEST_F(StateMachineTest, AddErrorAfterPreviousError) TEST_F(StateMachineTest, ClearOneErrorOutOfMany) { uint32_t error = StateManager::ERROR_IMU_NOT_RESPONDING | StateManager::ERROR_TIME_GOING_BACKWARDS - | StateManager::ERROR_UNCALIBRATED_IMU; + | StateManager::ERROR_UNCALIBRATED_IMU; rf.state_manager_.set_error(error); rf.state_manager_.clear_error(StateManager::ERROR_UNCALIBRATED_IMU); EXPECT_EQ(rf.state_manager_.state().armed, false); @@ -109,9 +111,10 @@ TEST_F(StateMachineTest, ClearOneErrorOutOfMany) TEST_F(StateMachineTest, ClearMultipleErrorsAtOnce) { uint32_t error = StateManager::ERROR_IMU_NOT_RESPONDING | StateManager::ERROR_TIME_GOING_BACKWARDS - | StateManager::ERROR_UNCALIBRATED_IMU; + | StateManager::ERROR_UNCALIBRATED_IMU; rf.state_manager_.set_error(error); - rf.state_manager_.clear_error(StateManager::ERROR_IMU_NOT_RESPONDING | StateManager::ERROR_TIME_GOING_BACKWARDS); + rf.state_manager_.clear_error(StateManager::ERROR_IMU_NOT_RESPONDING + | StateManager::ERROR_TIME_GOING_BACKWARDS); EXPECT_EQ(rf.state_manager_.state().armed, false); EXPECT_EQ(rf.state_manager_.state().failsafe, false); EXPECT_EQ(rf.state_manager_.state().error_codes, StateManager::ERROR_UNCALIBRATED_IMU); @@ -121,7 +124,7 @@ TEST_F(StateMachineTest, ClearMultipleErrorsAtOnce) TEST_F(StateMachineTest, ClearAllErrors) { uint32_t error = StateManager::ERROR_IMU_NOT_RESPONDING | StateManager::ERROR_TIME_GOING_BACKWARDS - | StateManager::ERROR_UNCALIBRATED_IMU; + | StateManager::ERROR_UNCALIBRATED_IMU; rf.state_manager_.set_error(error); rf.state_manager_.clear_error(error); EXPECT_EQ(rf.state_manager_.state().armed, false); @@ -280,10 +283,7 @@ TEST_F(StateMachineTest, UnableToArmWithPersistentErrors) TEST_F(StateMachineTest, ArmIfThrottleLow) { uint16_t rc_values[8]; - for (int i = 0; i < 8; i++) - { - rc_values[i] = (i > 3) ? 1000 : 1500; - } + for (int i = 0; i < 8; i++) { rc_values[i] = (i > 3) ? 1000 : 1500; } rc_values[2] = 1000; board.set_rc(rc_values); step_firmware(rf, board, 100); @@ -295,10 +295,7 @@ TEST_F(StateMachineTest, ArmIfThrottleHighWithMinThrottle) { rf.params_.set_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE, true); uint16_t rc_values[8]; - for (int i = 0; i < 8; i++) - { - rc_values[i] = (i > 3) ? 1000 : 1500; - } + for (int i = 0; i < 8; i++) { rc_values[i] = (i > 3) ? 1000 : 1500; } rc_values[2] = 1500; board.set_rc(rc_values); step_firmware(rf, board, 100000); @@ -312,10 +309,7 @@ TEST_F(StateMachineTest, DontArmIfThrottleHighWithoutMinThrottle) { rf.params_.set_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE, false); uint16_t rc_values[8]; - for (int i = 0; i < 8; i++) - { - rc_values[i] = (i > 3) ? 1000 : 1500; - } + for (int i = 0; i < 8; i++) { rc_values[i] = (i > 3) ? 1000 : 1500; } rc_values[2] = 1500; board.set_rc(rc_values); step_firmware(rf, board, 100000); diff --git a/test/test_board.cpp b/test/test_board.cpp index 304b29a0..7a5bedca 100644 --- a/test/test_board.cpp +++ b/test/test_board.cpp @@ -36,29 +36,19 @@ namespace rosflight_firmware { -void testBoard::set_rc(uint16_t *values) +void testBoard::set_rc(uint16_t * values) { - for (int i = 0; i < 8; i++) - { - rc_values[i] = values[i]; - } + for (int i = 0; i < 8; i++) { rc_values[i] = values[i]; } } -void testBoard::set_time(uint64_t time_us) -{ - time_us_ = time_us; -} +void testBoard::set_time(uint64_t time_us) { time_us_ = time_us; } -void testBoard::set_pwm_lost(bool lost) -{ - rc_lost_ = lost; -} +void testBoard::set_pwm_lost(bool lost) { rc_lost_ = lost; } -void testBoard::set_imu(float *acc, float *gyro, uint64_t time_us) +void testBoard::set_imu(float * acc, float * gyro, uint64_t time_us) { time_us_ = time_us; - for (int i = 0; i < 3; i++) - { + for (int i = 0; i < 3; i++) { acc_[i] = acc[i]; gyro_[i] = gyro[i]; } @@ -66,57 +56,37 @@ void testBoard::set_imu(float *acc, float *gyro, uint64_t time_us) } // setup -void testBoard::init_board() -{ - backup_memory_clear(); -} +void testBoard::init_board() { backup_memory_clear(); } void testBoard::board_reset(bool bootloader) {} // clock -uint32_t testBoard::clock_millis() -{ - return time_us_ / 1000; -} -uint64_t testBoard::clock_micros() -{ - return time_us_; -} +uint32_t testBoard::clock_millis() { return time_us_ / 1000; } +uint64_t testBoard::clock_micros() { return time_us_; } void testBoard::clock_delay(uint32_t milliseconds) {} // serial void testBoard::serial_init(uint32_t baud_rate, uint32_t dev) {} -void testBoard::serial_write(const uint8_t *src, size_t len) {} -uint16_t testBoard::serial_bytes_available() -{ - return 0; -} -uint8_t testBoard::serial_read() -{ - return 0; -} +void testBoard::serial_write(const uint8_t * src, size_t len) {} +uint16_t testBoard::serial_bytes_available() { return 0; } +uint8_t testBoard::serial_read() { return 0; } void testBoard::serial_flush() {} // sensors void testBoard::sensors_init() {} -uint16_t testBoard::num_sensor_errors() -{ - return 0; -} +uint16_t testBoard::num_sensor_errors() { return 0; } bool testBoard::new_imu_data() { - if (new_imu_) - { + if (new_imu_) { new_imu_ = false; return true; } return false; } -bool testBoard::imu_read(float accel[3], float *temperature, float gyro[3], uint64_t *time) +bool testBoard::imu_read(float accel[3], float * temperature, float gyro[3], uint64_t * time) { - for (int i = 0; i < 3; i++) - { + for (int i = 0; i < 3; i++) { accel[i] = acc_[i]; gyro[i] = gyro_[i]; } @@ -125,11 +95,10 @@ bool testBoard::imu_read(float accel[3], float *temperature, float gyro[3], uint return true; } -bool testBoard::backup_memory_read(void *dest, size_t len) +bool testBoard::backup_memory_read(void * dest, size_t len) { bool success = true; - if (len > BACKUP_MEMORY_SIZE) - { + if (len > BACKUP_MEMORY_SIZE) { len = BACKUP_MEMORY_SIZE; success = false; } @@ -137,105 +106,53 @@ bool testBoard::backup_memory_read(void *dest, size_t len) return success; } -void testBoard::backup_memory_write(const void *src, size_t len) +void testBoard::backup_memory_write(const void * src, size_t len) { - if (len > BACKUP_MEMORY_SIZE) - len = BACKUP_MEMORY_SIZE; + if (len > BACKUP_MEMORY_SIZE) len = BACKUP_MEMORY_SIZE; memcpy(backup_memory_, src, len); } -void testBoard::backup_memory_clear(size_t len) -{ - memset(backup_memory_, 0, len); -} -void testBoard::backup_memory_clear() -{ - backup_memory_clear(BACKUP_MEMORY_SIZE); -} +void testBoard::backup_memory_clear(size_t len) { memset(backup_memory_, 0, len); } +void testBoard::backup_memory_clear() { backup_memory_clear(BACKUP_MEMORY_SIZE); } void testBoard::imu_not_responding_error() {} -bool testBoard::mag_present() -{ - return false; -} +bool testBoard::mag_present() { return false; } void testBoard::mag_update() {} void testBoard::mag_read(float mag[3]) {} -bool testBoard::baro_present() -{ - return false; -} +bool testBoard::baro_present() { return false; } void testBoard::baro_update() {} -void testBoard::baro_read(float *pressure, float *temperature) {} +void testBoard::baro_read(float * pressure, float * temperature) {} -bool testBoard::diff_pressure_present() -{ - return false; -} +bool testBoard::diff_pressure_present() { return false; } void testBoard::diff_pressure_update() {} -void testBoard::diff_pressure_read(float *diff_pressure, float *temperature) {} +void testBoard::diff_pressure_read(float * diff_pressure, float * temperature) {} -bool testBoard::sonar_present() -{ - return false; -} +bool testBoard::sonar_present() { return false; } void testBoard::sonar_update() {} -float testBoard::sonar_read() -{ - return 0; -} +float testBoard::sonar_read() { return 0; } -bool testBoard::battery_voltage_present() const -{ - return false; -} -float testBoard::battery_voltage_read() const -{ - return 0; -} -void testBoard::battery_voltage_set_multiplier(double multiplier) -{ - (void)multiplier; -} +bool testBoard::battery_voltage_present() const { return false; } +float testBoard::battery_voltage_read() const { return 0; } +void testBoard::battery_voltage_set_multiplier(double multiplier) { (void) multiplier; } -bool testBoard::battery_current_present() const -{ - return false; -} -float testBoard::battery_current_read() const -{ - return 0; -} -void testBoard::battery_current_set_multiplier(double multiplier) -{ - (void)multiplier; -} +bool testBoard::battery_current_present() const { return false; } +float testBoard::battery_current_read() const { return 0; } +void testBoard::battery_current_set_multiplier(double multiplier) { (void) multiplier; } // GNSS is not supported on the test board -GNSSData testBoard::gnss_read() -{ - return {}; -} +GNSSData testBoard::gnss_read() { return {}; } // GNSS is not supported on the test board -GNSSFull testBoard::gnss_full_read() -{ - return {}; -} +GNSSFull testBoard::gnss_full_read() { return {}; } // GNSS is not supported on the test board -bool testBoard::gnss_has_new_data() -{ - return false; -} +bool testBoard::gnss_has_new_data() { return false; } // PWM // TODO make these deal in normalized (-1 to 1 or 0 to 1) values (not pwm-specific) void testBoard::rc_init(rc_type_t rc_type) {} -bool testBoard::rc_lost() -{ - return rc_lost_; -} +bool testBoard::rc_lost() { return rc_lost_; } float testBoard::rc_read(uint8_t channel) { return static_cast(rc_values[channel] - 1000) / 1000.0; @@ -246,14 +163,8 @@ void testBoard::pwm_disable() {} // non-volatile memory void testBoard::memory_init() {} -bool testBoard::memory_read(void *dest, size_t len) -{ - return false; -} -bool testBoard::memory_write(const void *src, size_t len) -{ - return false; -} +bool testBoard::memory_read(void * dest, size_t len) { return false; } +bool testBoard::memory_write(const void * src, size_t len) { return false; } // LEDs void testBoard::led0_on() {} diff --git a/test/test_board.h b/test/test_board.h index 35840019..d3b74842 100644 --- a/test/test_board.h +++ b/test/test_board.h @@ -61,7 +61,7 @@ class testBoard : public Board // serial void serial_init(uint32_t baud_rate, uint32_t dev) override; - void serial_write(const uint8_t *src, size_t len) override; + void serial_write(const uint8_t * src, size_t len) override; uint16_t serial_bytes_available() override; uint8_t serial_read() override; void serial_flush() override; @@ -71,7 +71,7 @@ class testBoard : public Board uint16_t num_sensor_errors(); bool new_imu_data() override; - bool imu_read(float accel[3], float *temperature, float gyro[3], uint64_t *time) override; + bool imu_read(float accel[3], float * temperature, float gyro[3], uint64_t * time) override; void imu_not_responding_error() override; bool mag_present() override; @@ -80,11 +80,11 @@ class testBoard : public Board bool baro_present() override; void baro_update() override; - void baro_read(float *pressure, float *temperature) override; + void baro_read(float * pressure, float * temperature) override; bool diff_pressure_present() override; void diff_pressure_update() override; - void diff_pressure_read(float *diff_pressure, float *temperature) override; + void diff_pressure_read(float * diff_pressure, float * temperature) override; bool sonar_present() override; void sonar_update() override; @@ -116,8 +116,8 @@ class testBoard : public Board // non-volatile memory void memory_init() override; - bool memory_read(void *dest, size_t len) override; - bool memory_write(const void *src, size_t len) override; + bool memory_read(void * dest, size_t len) override; + bool memory_write(const void * src, size_t len) override; // LEDs void led0_on() override; @@ -130,13 +130,13 @@ class testBoard : public Board // Backup memory void backup_memory_init() override {} - bool backup_memory_read(void *dest, size_t len) override; - void backup_memory_write(const void *src, size_t len) override; + bool backup_memory_read(void * dest, size_t len) override; + void backup_memory_write(const void * src, size_t len) override; void backup_memory_clear(size_t len) override; void backup_memory_clear(); // Not an override - void set_imu(float *acc, float *gyro, uint64_t time_us); - void set_rc(uint16_t *values); + void set_imu(float * acc, float * gyro, uint64_t time_us); + void set_rc(uint16_t * values); void set_time(uint64_t time_us); void set_pwm_lost(bool lost); }; diff --git a/test/turbotrig_test.cpp b/test/turbotrig_test.cpp index 7fd7d23b..95a84971 100644 --- a/test/turbotrig_test.cpp +++ b/test/turbotrig_test.cpp @@ -36,71 +36,70 @@ #include #include -turbomath::Vector random_vectors[25] = {turbomath::Vector(-0.0376278050814, 0.471775699711, -0.336572370974), - turbomath::Vector(0.842139998851, -0.113277302409, -0.435361598132), - turbomath::Vector(0.402876930871, -0.998517068538, 0.956603957591), - turbomath::Vector(0.366004030077, -0.966554559399, 0.236455814495), - turbomath::Vector(0.170963581611, -0.892193316086, -0.360102936987), - turbomath::Vector(-0.675191763273, -0.794118513048, 0.561367212903), - turbomath::Vector(-0.0299477253533, 0.0938177650483, 0.525814272724), - turbomath::Vector(-0.676191678521, -0.0780862208203, -0.272955681219), - turbomath::Vector(-0.435749833209, -0.673810649938, -0.896559097382), - turbomath::Vector(0.709083915552, -0.135067363969, -0.385492450532), - turbomath::Vector(-0.38728558039, -0.502219301225, 0.323557018529), - turbomath::Vector(-0.186870345154, 0.554827454101, 0.921567682061), - turbomath::Vector(-0.142106787605, -0.764876359963, 0.00303689980819), - turbomath::Vector(-0.677798963582, -0.664595954482, 0.339274533414), - turbomath::Vector(-0.700464041114, 0.325731535871, -0.621492014391), - turbomath::Vector(-0.604865828708, 0.270639620454, 0.188624833185), - turbomath::Vector(0.464205180183, -0.461504601245, -0.578708441515), - turbomath::Vector(0.498899172115, -0.582342366402, -0.694758083436), - turbomath::Vector(0.0710544604541, -0.63603887083, -0.521799692437), - turbomath::Vector(-0.372025413205, 0.83531212357, 0.232484576742), - turbomath::Vector(0.790872496361, -0.89600683592, 0.783984438621), - turbomath::Vector(0.236462609786, -0.636362560394, 0.203951290805), - turbomath::Vector(0.831924307534, -0.482532468579, 0.0600026189612), - turbomath::Vector(0.0562194856302, -0.605799189029, -0.556494338297), - turbomath::Vector(-0.85014432598, 0.0632157037573, 0.0272188414114)}; +turbomath::Vector random_vectors[25] = { + turbomath::Vector(-0.0376278050814, 0.471775699711, -0.336572370974), + turbomath::Vector(0.842139998851, -0.113277302409, -0.435361598132), + turbomath::Vector(0.402876930871, -0.998517068538, 0.956603957591), + turbomath::Vector(0.366004030077, -0.966554559399, 0.236455814495), + turbomath::Vector(0.170963581611, -0.892193316086, -0.360102936987), + turbomath::Vector(-0.675191763273, -0.794118513048, 0.561367212903), + turbomath::Vector(-0.0299477253533, 0.0938177650483, 0.525814272724), + turbomath::Vector(-0.676191678521, -0.0780862208203, -0.272955681219), + turbomath::Vector(-0.435749833209, -0.673810649938, -0.896559097382), + turbomath::Vector(0.709083915552, -0.135067363969, -0.385492450532), + turbomath::Vector(-0.38728558039, -0.502219301225, 0.323557018529), + turbomath::Vector(-0.186870345154, 0.554827454101, 0.921567682061), + turbomath::Vector(-0.142106787605, -0.764876359963, 0.00303689980819), + turbomath::Vector(-0.677798963582, -0.664595954482, 0.339274533414), + turbomath::Vector(-0.700464041114, 0.325731535871, -0.621492014391), + turbomath::Vector(-0.604865828708, 0.270639620454, 0.188624833185), + turbomath::Vector(0.464205180183, -0.461504601245, -0.578708441515), + turbomath::Vector(0.498899172115, -0.582342366402, -0.694758083436), + turbomath::Vector(0.0710544604541, -0.63603887083, -0.521799692437), + turbomath::Vector(-0.372025413205, 0.83531212357, 0.232484576742), + turbomath::Vector(0.790872496361, -0.89600683592, 0.783984438621), + turbomath::Vector(0.236462609786, -0.636362560394, 0.203951290805), + turbomath::Vector(0.831924307534, -0.482532468579, 0.0600026189612), + turbomath::Vector(0.0562194856302, -0.605799189029, -0.556494338297), + turbomath::Vector(-0.85014432598, 0.0632157037573, 0.0272188414114)}; turbomath::Quaternion random_quaternions[25] = { - turbomath::Quaternion(0.10377420365, -0.583993115868, -0.731526280531, -0.0530049846003), - turbomath::Quaternion(-0.00228103177408, -0.506936771567, 0.976002181169, 0.90368722061), - turbomath::Quaternion(-0.280191704748, 0.141235897077, 0.770363502952, 0.306427689307), - turbomath::Quaternion(0.964538929753, -0.849755381903, 0.36374459234, 0.694507794584), - turbomath::Quaternion(0.0176390041681, -0.960155080148, 0.340078582124, -0.119639355159), - turbomath::Quaternion(-0.213139865459, -0.91618752978, -0.192746623826, -0.761937711418), - turbomath::Quaternion(-0.491440057128, -0.468120646081, -0.0682240789086, -0.779728041272), - turbomath::Quaternion(0.00414757516987, -0.980357614738, 0.243315557667, 0.487816606638), - turbomath::Quaternion(-0.593742280674, 0.245648066311, 0.682367014935, -0.0659175648814), - turbomath::Quaternion(-0.322464011587, 0.706588950729, -0.966024250287, -0.50354344519), - turbomath::Quaternion(-0.537023302971, -0.496355850419, -0.326843736039, 0.456606813517), - turbomath::Quaternion(-0.581585485434, 0.225766708322, -0.121402082687, 0.160333514827), - turbomath::Quaternion(-0.422711480811, 0.894994456476, 0.392582496229, 0.0035135659771), - turbomath::Quaternion(0.326380783544, 0.551413227108, 0.89489801397, 0.87883243747), - turbomath::Quaternion(0.83500683695, -0.263875030319, -0.1783391105, 0.453727091163), - turbomath::Quaternion(-0.30389938019, -0.0744317276089, -0.436917072268, 0.907173926266), - turbomath::Quaternion(-0.320066655494, -0.349065285706, 0.0336903431161, 0.573906603454), - turbomath::Quaternion(-0.103624452083, -0.82874783662, -0.635967208274, 0.562138574765), - turbomath::Quaternion(0.90735669209, -0.611711092446, 0.732474120503, 0.866697480004), - turbomath::Quaternion(0.626137839218, 0.41320663394, -0.821473642241, -0.344696411875), - turbomath::Quaternion(0.266650461152, -0.784707647527, 0.324347257562, -0.724904312141), - turbomath::Quaternion(0.964177603528, -0.378173605577, 0.767349174766, 0.560290218637), - turbomath::Quaternion(0.0812716046369, 0.745067180353, -0.476875959113, -0.245887902551), - turbomath::Quaternion(-0.177027678376, 0.214558558928, -0.992910369554, 0.592964390132), - turbomath::Quaternion(0.0979109306209, 0.121890109199, 0.126418158551, 0.242200145606)}; + turbomath::Quaternion(0.10377420365, -0.583993115868, -0.731526280531, -0.0530049846003), + turbomath::Quaternion(-0.00228103177408, -0.506936771567, 0.976002181169, 0.90368722061), + turbomath::Quaternion(-0.280191704748, 0.141235897077, 0.770363502952, 0.306427689307), + turbomath::Quaternion(0.964538929753, -0.849755381903, 0.36374459234, 0.694507794584), + turbomath::Quaternion(0.0176390041681, -0.960155080148, 0.340078582124, -0.119639355159), + turbomath::Quaternion(-0.213139865459, -0.91618752978, -0.192746623826, -0.761937711418), + turbomath::Quaternion(-0.491440057128, -0.468120646081, -0.0682240789086, -0.779728041272), + turbomath::Quaternion(0.00414757516987, -0.980357614738, 0.243315557667, 0.487816606638), + turbomath::Quaternion(-0.593742280674, 0.245648066311, 0.682367014935, -0.0659175648814), + turbomath::Quaternion(-0.322464011587, 0.706588950729, -0.966024250287, -0.50354344519), + turbomath::Quaternion(-0.537023302971, -0.496355850419, -0.326843736039, 0.456606813517), + turbomath::Quaternion(-0.581585485434, 0.225766708322, -0.121402082687, 0.160333514827), + turbomath::Quaternion(-0.422711480811, 0.894994456476, 0.392582496229, 0.0035135659771), + turbomath::Quaternion(0.326380783544, 0.551413227108, 0.89489801397, 0.87883243747), + turbomath::Quaternion(0.83500683695, -0.263875030319, -0.1783391105, 0.453727091163), + turbomath::Quaternion(-0.30389938019, -0.0744317276089, -0.436917072268, 0.907173926266), + turbomath::Quaternion(-0.320066655494, -0.349065285706, 0.0336903431161, 0.573906603454), + turbomath::Quaternion(-0.103624452083, -0.82874783662, -0.635967208274, 0.562138574765), + turbomath::Quaternion(0.90735669209, -0.611711092446, 0.732474120503, 0.866697480004), + turbomath::Quaternion(0.626137839218, 0.41320663394, -0.821473642241, -0.344696411875), + turbomath::Quaternion(0.266650461152, -0.784707647527, 0.324347257562, -0.724904312141), + turbomath::Quaternion(0.964177603528, -0.378173605577, 0.767349174766, 0.560290218637), + turbomath::Quaternion(0.0812716046369, 0.745067180353, -0.476875959113, -0.245887902551), + turbomath::Quaternion(-0.177027678376, 0.214558558928, -0.992910369554, 0.592964390132), + turbomath::Quaternion(0.0979109306209, 0.121890109199, 0.126418158551, 0.242200145606)}; TEST(TurboMath, atan) { - for (float i = -200.0; i <= 200.0; i += 0.001) - { + for (float i = -200.0; i <= 200.0; i += 0.001) { EXPECT_NEAR(turbomath::atan(i), atan(i), 0.0001); } } TEST(TurboMath, sin_cos) { - for (float i = -200.0; i <= 200.0; i += 0.001) - { + for (float i = -200.0; i <= 200.0; i += 0.001) { EXPECT_NEAR(turbomath::sin(i), sin(i), 0.0002); EXPECT_NEAR(turbomath::cos(i), cos(i), 0.0002); } @@ -108,24 +107,17 @@ TEST(TurboMath, sin_cos) TEST(TurboMath, atan2) { - for (float i = -100.0; i <= 100.0; i += 0.1) - { - for (float j = -1.0; j <= 1.0; j += 0.001) - { - if (fabs(j) > 0.0001) - { - EXPECT_NEAR(turbomath::atan2(i, j), atan2(i, j), 0.001); - } + for (float i = -100.0; i <= 100.0; i += 0.1) { + for (float j = -1.0; j <= 1.0; j += 0.001) { + if (fabs(j) > 0.0001) { EXPECT_NEAR(turbomath::atan2(i, j), atan2(i, j), 0.001); } } } } TEST(TurboMath, asin) { - for (float i = -1.0; i <= 1.0; i += 0.001) - { - if (fabs(i) < 0.95) - EXPECT_NEAR(turbomath::asin(i), asin(i), 0.0001); + for (float i = -1.0; i <= 1.0; i += 0.001) { + if (fabs(i) < 0.95) EXPECT_NEAR(turbomath::asin(i), asin(i), 0.0001); else EXPECT_NEAR(turbomath::asin(i), asin(i), 0.2); } @@ -139,10 +131,10 @@ TEST(TurboMath, fastAlt) // all valid int values float trueResult = 0.0; - for (int i = 69682; i < 106597; i++) - { - trueResult = static_cast((1.0 - pow(static_cast(i) / 101325, 0.190284)) * 145366.45) - * static_cast(0.3048); + for (int i = 69682; i < 106597; i++) { + trueResult = + static_cast((1.0 - pow(static_cast(i) / 101325, 0.190284)) * 145366.45) + * static_cast(0.3048); EXPECT_NEAR(turbomath::alt(i), trueResult, .15); // arbitrarily chose <= .15m since fast_alt isn't accurate enough for EXPECT_CLOSE, // but being within .15 meters of the correct result seems pretty good to me @@ -151,8 +143,7 @@ TEST(TurboMath, fastAlt) TEST(TurboMath, Vector) { - for (int i = 0; i < 24; i++) - { + for (int i = 0; i < 24; i++) { turbomath::Vector vec1 = random_vectors[i]; turbomath::Vector vec2 = random_vectors[i + 1]; Eigen::Vector3f eig2, eig1; @@ -202,8 +193,7 @@ TEST(TurboMath, Vector) TEST(TurboMath, Quaternion) { - for (int i = 0; i < 24; i++) - { + for (int i = 0; i < 24; i++) { turbomath::Quaternion quat1 = random_quaternions[i].normalize(); turbomath::Quaternion quat2 = random_quaternions[i + 1].normalize(); @@ -214,8 +204,10 @@ TEST(TurboMath, Quaternion) EXPECT_QUAT_SUPERCLOSE(quat2, eig2); // Check normalization - EXPECT_SUPERCLOSE(quat1.x * quat1.x + quat1.y * quat1.y + quat1.z * quat1.z + quat1.w * quat1.w, 1.0f); - EXPECT_SUPERCLOSE(quat2.x * quat2.x + quat2.y * quat2.y + quat2.z * quat2.z + quat2.w * quat2.w, 1.0f); + EXPECT_SUPERCLOSE(quat1.x * quat1.x + quat1.y * quat1.y + quat1.z * quat1.z + quat1.w * quat1.w, + 1.0f); + EXPECT_SUPERCLOSE(quat2.x * quat2.x + quat2.y * quat2.y + quat2.z * quat2.z + quat2.w * quat2.w, + 1.0f); // Test Quaternion Operators ASSERT_QUAT_SUPERCLOSE((quat1 * quat2), (eig2 * eig1)); @@ -244,7 +236,8 @@ TEST(TurboMath, Quaternion) Eigen::Vector3f jhat(0, 1, 0); Eigen::Vector3f khat(0, 0, 1); - Eigen::Quaternionf eig3 = Eigen::AngleAxisf(s, khat) * Eigen::AngleAxisf(t, jhat) * Eigen::AngleAxisf(p, ihat); + Eigen::Quaternionf eig3 = + Eigen::AngleAxisf(s, khat) * Eigen::AngleAxisf(t, jhat) * Eigen::AngleAxisf(p, ihat); EXPECT_QUAT_SUPERCLOSE(quat3, eig3); EXPECT_SUPERCLOSE(quat1.w, quat3.w); @@ -266,8 +259,7 @@ TEST(TurboMath, QuatFromTwoVectors) ASSERT_TURBOQUAT_SUPERCLOSE(quat1, quat_test); // A bunch of random (off-axes tests) - for (int i = 0; i < 25; i++) - { + for (int i = 0; i < 25; i++) { turbomath::Vector vec3 = random_vectors[i].normalize(); turbomath::Quaternion quat2 = random_quaternions[i].normalize();