From 4a25193238ed875ee92e20fe4ad43f706aa00e61 Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Tue, 26 Nov 2019 14:40:04 -0700 Subject: [PATCH 01/76] Basic setup for device management --- boards/airbourne/airbourne_board.cpp | 66 +++++++++++++++++++++------- boards/airbourne/airbourne_board.h | 6 ++- include/board.h | 7 ++- include/configuration_enum.h | 33 ++++++++++++++ include/device_manager.h | 19 ++++++++ include/rosflight.h | 2 + src/device_manager.cpp | 10 +++++ src/rosflight.cpp | 6 ++- 8 files changed, 131 insertions(+), 18 deletions(-) create mode 100644 include/configuration_enum.h create mode 100644 include/device_manager.h create mode 100644 src/device_manager.cpp diff --git a/boards/airbourne/airbourne_board.cpp b/boards/airbourne/airbourne_board.cpp index 2d67bd17..2ac7dc8f 100644 --- a/boards/airbourne/airbourne_board.cpp +++ b/boards/airbourne/airbourne_board.cpp @@ -45,15 +45,13 @@ void AirbourneBoard::init_board() led1_.init(LED1_GPIO, LED1_PIN); int_i2c_.init(&i2c_config[BARO_I2C]); - ext_i2c_.init(&i2c_config[EXTERNAL_I2C]); + //ext_i2c_.init(&i2c_config[EXTERNAL_I2C]); spi1_.init(&spi_config[MPU6000_SPI]); spi3_.init(&spi_config[FLASH_SPI]); - uart1_.init(&uart_config[UART1], 115200, UART::MODE_8N1); - uart3_.init(&uart_config[UART3], 115200, UART::MODE_8N1); + //uart1_.init(&uart_config[UART1], 115200, UART::MODE_8N1); + //uart3_.init(&uart_config[UART3], 115200, UART::MODE_8N1); backup_sram_init(); - - current_serial_ = &vcp_; //uncomment this to switch to VCP as the main output } void AirbourneBoard::board_reset(bool bootloader) @@ -79,19 +77,27 @@ void AirbourneBoard::clock_delay(uint32_t milliseconds) } // serial -void AirbourneBoard::serial_init(uint32_t baud_rate, uint32_t dev) +void AirbourneBoard::serial_init(uint32_t baud_rate, hardware_config_t configuration) { - vcp_.init(); - switch (dev) + switch(configuration) { - case SERIAL_DEVICE_UART3: - uart3_.init(&uart_config[UART3], baud_rate); - current_serial_ = &uart3_; - secondary_serial_device_ = SERIAL_DEVICE_UART3; - break; default: + case 1: // VCP current_serial_ = &vcp_; - secondary_serial_device_ = SERIAL_DEVICE_VCP; + vcp_.init(); + break; + case 2: // UART 1 + current_serial_ = &uart1_; + uart1_.init(&uart_config[UART1], baud_rate); + break; + case 3: // UART 2 + current_serial_ = &uart2_; + uart2_.init(&uart_config[UART2], baud_rate); + break; + case 4: // UART 3 + current_serial_ = &uart3_; + uart3_.init(&uart_config[UART3], baud_rate); + break; } } @@ -133,7 +139,37 @@ void AirbourneBoard::serial_flush() current_serial_->flush(); } - +// Resources +bool AirbourneBoard::enable_device(device_t device, hardware_config_t configuration) +{ + switch(device) + { + case serial: + return true; // This is handled by serial_init + case rc: + switch(configuration) + { + case 1: + rc_init(RC_TYPE_PPM); + break; + case 2: + rc_init(RC_TYPE_SBUS); + break; + } + return true; + case airspeed: + break; + case gnss: + break; + case sonar: + break; + case battery_monitor: + break; + default: + return false; + } + return false; +} // sensors void AirbourneBoard::sensors_init() { diff --git a/boards/airbourne/airbourne_board.h b/boards/airbourne/airbourne_board.h index 69163945..9925b083 100644 --- a/boards/airbourne/airbourne_board.h +++ b/boards/airbourne/airbourne_board.h @@ -73,6 +73,7 @@ class AirbourneBoard : public Board private: VCP vcp_; UART uart1_; + UART uart2_; UART uart3_; Serial *current_serial_;//A pointer to the serial stream currently in use. I2C int_i2c_; @@ -135,12 +136,15 @@ class AirbourneBoard : public Board void clock_delay(uint32_t milliseconds) override; // serial - void serial_init(uint32_t baud_rate, uint32_t dev) override; + void serial_init(uint32_t baud_rate, hardware_config_t config) 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; + // resource management + void enable_device(device_t device, hardware_config_t configuration) override; + // sensors void sensors_init() override; uint16_t num_sensor_errors() override; diff --git a/include/board.h b/include/board.h index 6746231a..eb2e5756 100644 --- a/include/board.h +++ b/include/board.h @@ -38,6 +38,7 @@ #include "sensors.h" #include "state_manager.h" +#include "configuration_enum.h" namespace rosflight_firmware { @@ -62,12 +63,16 @@ class Board virtual void clock_delay(uint32_t milliseconds) = 0; // serial - virtual void serial_init(uint32_t baud_rate, uint32_t dev) = 0; + virtual void serial_init(uint32_t baud_rate, hardware_config_t configuration) = 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; +// hardware config + //virtual void enable_resource(resource_t resource); + virtual void enable_device(device_t device, hardware_config_t configuration); + // sensors virtual void sensors_init() = 0; virtual uint16_t num_sensor_errors() = 0; diff --git a/include/configuration_enum.h b/include/configuration_enum.h new file mode 100644 index 00000000..9d2c1ec1 --- /dev/null +++ b/include/configuration_enum.h @@ -0,0 +1,33 @@ +#ifndef CONFIGURATION_ENUM_H +#define CONFIGURATION_ENUM_H + +#include + +enum device_t +{ + serial, + rc, + airspeed, + gnss, + sonar, + battery_monitor, + device_count +}; + +/* +enum resource_t +{ + vcp, + uart, + sbus, + i2c, + spi, + ppm, + adc +}; +*/ + +typedef uint8_t hardware_config_t; + + +#endif // CONFIGURATION_ENUM_H diff --git a/include/device_manager.h b/include/device_manager.h new file mode 100644 index 00000000..d98343be --- /dev/null +++ b/include/device_manager.h @@ -0,0 +1,19 @@ +#ifndef HARDWARE_CONFIG_H +#define HARDWARE_CONFIG_H + +#include "rosflight.h" +#include "configuration_enum.h" + +namespace rosflight_firmware +{ +class DeviceManager +{ +public: + DeviceManager(ROSflight &rf); + bool init(); +private: + ROSflight *rf_; + +}; +} +#endif // HARDWARE_CONFIG_H diff --git a/include/rosflight.h b/include/rosflight.h index f4a85b65..0906488c 100644 --- a/include/rosflight.h +++ b/include/rosflight.h @@ -47,6 +47,7 @@ #include "mixer.h" #include "state_manager.h" #include "command_manager.h" +#include "device_manager.h" namespace rosflight_firmware { @@ -69,6 +70,7 @@ class ROSflight RC rc_; Sensors sensors_; StateManager state_manager_; + DeviceManager device_manager_; uint32_t loop_time_us; diff --git a/src/device_manager.cpp b/src/device_manager.cpp new file mode 100644 index 00000000..c3344927 --- /dev/null +++ b/src/device_manager.cpp @@ -0,0 +1,10 @@ +#include "device_manager.h" + +namespace rosflight_firmware +{ +DeviceManager::DeviceManager(ROSflight &rf): rf_{&rf} +{ + +} + +} diff --git a/src/rosflight.cpp b/src/rosflight.cpp index ea1c6ef4..aba7a0e8 100644 --- a/src/rosflight.cpp +++ b/src/rosflight.cpp @@ -45,7 +45,8 @@ ROSflight::ROSflight(Board& board, CommLinkInterface& comm_link) : mixer_(*this), rc_(*this), sensors_(*this), - state_manager_(*this) + state_manager_(*this), + device_manager_(*this) { comm_link.set_listener(&comm_manager_); params_.set_listeners(param_listeners_, num_param_listeners_); @@ -60,6 +61,9 @@ void ROSflight::init() // Read EEPROM to get initial params params_.init(); + //Initialize devices + device_manager_.init(); + // Initialize Mixer mixer_.init(); From 10ce42def249c610f452a1676f41597b801723c8 Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Mon, 2 Dec 2019 13:01:18 -0700 Subject: [PATCH 02/76] Rename DeviceManager to ConfigManager --- include/{device_manager.h => config_manager.h} | 4 ++-- include/rosflight.h | 4 ++-- src/config_manager.cpp | 13 +++++++++++++ src/device_manager.cpp | 10 ---------- 4 files changed, 17 insertions(+), 14 deletions(-) rename include/{device_manager.h => config_manager.h} (81%) create mode 100644 src/config_manager.cpp delete mode 100644 src/device_manager.cpp diff --git a/include/device_manager.h b/include/config_manager.h similarity index 81% rename from include/device_manager.h rename to include/config_manager.h index d98343be..38e5aa4e 100644 --- a/include/device_manager.h +++ b/include/config_manager.h @@ -6,10 +6,10 @@ namespace rosflight_firmware { -class DeviceManager +class ConfigManager { public: - DeviceManager(ROSflight &rf); + ConfigManager(ROSflight &rf); bool init(); private: ROSflight *rf_; diff --git a/include/rosflight.h b/include/rosflight.h index 0906488c..2d49996e 100644 --- a/include/rosflight.h +++ b/include/rosflight.h @@ -47,7 +47,7 @@ #include "mixer.h" #include "state_manager.h" #include "command_manager.h" -#include "device_manager.h" +#include "config_manager.h" namespace rosflight_firmware { @@ -70,7 +70,7 @@ class ROSflight RC rc_; Sensors sensors_; StateManager state_manager_; - DeviceManager device_manager_; + ConfigManager device_manager_; uint32_t loop_time_us; diff --git a/src/config_manager.cpp b/src/config_manager.cpp new file mode 100644 index 00000000..a7aedd4b --- /dev/null +++ b/src/config_manager.cpp @@ -0,0 +1,13 @@ +#include "config_manager.h" + +namespace rosflight_firmware +{ +ConfigManager::ConfigManager(ROSflight &rf): rf_{&rf} +{} + +bool ConfigManager::init() +{ + +} + +} diff --git a/src/device_manager.cpp b/src/device_manager.cpp deleted file mode 100644 index c3344927..00000000 --- a/src/device_manager.cpp +++ /dev/null @@ -1,10 +0,0 @@ -#include "device_manager.h" - -namespace rosflight_firmware -{ -DeviceManager::DeviceManager(ROSflight &rf): rf_{&rf} -{ - -} - -} From 4cf3023d8eba61b791d0c1f0c3235aab366f1a9e Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Mon, 2 Dec 2019 13:05:30 -0700 Subject: [PATCH 03/76] Define config struct --- include/config_manager.h | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/include/config_manager.h b/include/config_manager.h index 38e5aa4e..86f4bc2f 100644 --- a/include/config_manager.h +++ b/include/config_manager.h @@ -11,6 +11,21 @@ class ConfigManager public: ConfigManager(ROSflight &rf); bool init(); + enum: uint8_t + { + CONFIG_SERIAL, + CONFIG_RC, + CONFIG_AIRSPEED, + CONFIG_GNSS, + CONFIG_SONAR, + CONFIG_BATTERY_MONITOR, + CONFIG_COUNT + }; + typedef struct + { + uint8_t config[CONFIG_COUNT]; + } config_t; + private: ROSflight *rf_; From d79af1dd3a3ec148bd9884ebf420202d5d0991e8 Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Tue, 3 Dec 2019 16:47:53 -0700 Subject: [PATCH 04/76] Basic memory manager functionality --- boards/airbourne/airbourne_board.h | 4 +-- include/board.h | 3 +- include/config_manager.h | 33 ++++++++++--------- include/configuration_enum.h | 2 +- include/param.h | 12 +++---- include/rosflight.h | 4 ++- scripts/rosflight.mk | 4 ++- src/comm_manager.cpp | 2 +- src/config_manager.cpp | 51 +++++++++++++++++++++++++++++- src/param.cpp | 15 +++------ src/rosflight.cpp | 8 +++-- 11 files changed, 92 insertions(+), 46 deletions(-) diff --git a/boards/airbourne/airbourne_board.h b/boards/airbourne/airbourne_board.h index 9925b083..45f2837e 100644 --- a/boards/airbourne/airbourne_board.h +++ b/boards/airbourne/airbourne_board.h @@ -136,14 +136,14 @@ class AirbourneBoard : public Board void clock_delay(uint32_t milliseconds) override; // serial - void serial_init(uint32_t baud_rate, hardware_config_t config) override; + void serial_init(uint32_t baud_rate, hardware_config_t configuration) 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; // resource management - void enable_device(device_t device, hardware_config_t configuration) override; + bool enable_device(device_t device, hardware_config_t configuration) override; // sensors void sensors_init() override; diff --git a/include/board.h b/include/board.h index eb2e5756..3a47e2a7 100644 --- a/include/board.h +++ b/include/board.h @@ -70,8 +70,7 @@ class Board virtual void serial_flush() = 0; // hardware config - //virtual void enable_resource(resource_t resource); - virtual void enable_device(device_t device, hardware_config_t configuration); + virtual bool enable_device(device_t device, hardware_config_t configuration)=0; // sensors virtual void sensors_init() = 0; diff --git a/include/config_manager.h b/include/config_manager.h index 86f4bc2f..114a4e8f 100644 --- a/include/config_manager.h +++ b/include/config_manager.h @@ -1,33 +1,32 @@ -#ifndef HARDWARE_CONFIG_H +#ifndef HARDWARE_CONFIG_H #define HARDWARE_CONFIG_H -#include "rosflight.h" #include "configuration_enum.h" namespace rosflight_firmware { +class ROSflight; class ConfigManager { public: - ConfigManager(ROSflight &rf); - bool init(); - enum: uint8_t - { - CONFIG_SERIAL, - CONFIG_RC, - CONFIG_AIRSPEED, - CONFIG_GNSS, - CONFIG_SONAR, - CONFIG_BATTERY_MONITOR, - CONFIG_COUNT - }; - typedef struct + typedef struct __attribute__ ((packed)) { - uint8_t config[CONFIG_COUNT]; + uint32_t checksum; + hardware_config_t config[device_t::device_count]; } config_t; + ConfigManager(ROSflight &RF, config_t &config); + bool init(); + void set_configuration(device_t device, uint8_t config); + uint8_t get_configuration(device_t device); + void prepare_write(); + private: - ROSflight *rf_; + ROSflight &RF_; + config_t &config_; + bool read(); + void fill_defaults(); + uint32_t generate_checksum(); }; } diff --git a/include/configuration_enum.h b/include/configuration_enum.h index 9d2c1ec1..7616e72e 100644 --- a/include/configuration_enum.h +++ b/include/configuration_enum.h @@ -3,7 +3,7 @@ #include -enum device_t +enum device_t: uint8_t { serial, rc, diff --git a/include/param.h b/include/param.h index 433fbbd8..65876015 100644 --- a/include/param.h +++ b/include/param.h @@ -234,8 +234,6 @@ class Params public: static constexpr uint8_t PARAMS_NAME_LENGTH = 16; - -private: union param_value_t { float fvalue; @@ -256,8 +254,9 @@ class Params uint8_t chk; // XOR checksum } params_t; - params_t params; +private: ROSflight &RF_; + params_t ¶ms; 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); @@ -268,7 +267,7 @@ class Params public: - Params(ROSflight &_rf); + Params(ROSflight &_rf, params_t ¶m_struct); // function declarations @@ -296,10 +295,9 @@ class Params bool read(void); /** - * @brief Write current parameter values to non-volatile memory - * @return True if successful, false otherwise + * @brief Prepare the parameter struct to be written to non-volatile memory */ - bool write(void); + void prepare_write(void); /** * @brief Callback for executing actions that need to be taken when a parameter value changes diff --git a/include/rosflight.h b/include/rosflight.h index 2d49996e..4c195a55 100644 --- a/include/rosflight.h +++ b/include/rosflight.h @@ -48,6 +48,7 @@ #include "state_manager.h" #include "command_manager.h" #include "config_manager.h" +#include "memory_manager.h" namespace rosflight_firmware { @@ -59,6 +60,7 @@ class ROSflight ROSflight(Board& board, CommLinkInterface& comm_link); Board &board_; + MemoryManager memory_manager_; CommManager comm_manager_; Params params_; @@ -70,7 +72,7 @@ class ROSflight RC rc_; Sensors sensors_; StateManager state_manager_; - ConfigManager device_manager_; + ConfigManager config_manager_; uint32_t loop_time_us; diff --git a/scripts/rosflight.mk b/scripts/rosflight.mk index af1a6bb1..e91e5307 100644 --- a/scripts/rosflight.mk +++ b/scripts/rosflight.mk @@ -39,7 +39,9 @@ ROSFLIGHT_SRC = rosflight.cpp \ command_manager.cpp \ rc.cpp \ mixer.cpp \ - nanoprintf.cpp + nanoprintf.cpp \ + config_manager.cpp \ + memory_manager.cpp \ # Math Source Files VPATH := $(VPATH):$(TURBOMATH_DIR) diff --git a/src/comm_manager.cpp b/src/comm_manager.cpp index 83534e55..92fa7843 100644 --- a/src/comm_manager.cpp +++ b/src/comm_manager.cpp @@ -261,7 +261,7 @@ void CommManager::command_callback(CommLinkInterface::Command command) result = RF_.params_.read(); break; case CommLinkInterface::Command::COMMAND_WRITE_PARAMS: - result = RF_.params_.write(); + result = RF_.memory_manager_.write_memory(); break; case CommLinkInterface::Command::COMMAND_SET_PARAM_DEFAULTS: RF_.params_.set_defaults(); diff --git a/src/config_manager.cpp b/src/config_manager.cpp index a7aedd4b..acb3aec5 100644 --- a/src/config_manager.cpp +++ b/src/config_manager.cpp @@ -1,13 +1,62 @@ #include "config_manager.h" +#include "rosflight.h" namespace rosflight_firmware { -ConfigManager::ConfigManager(ROSflight &rf): rf_{&rf} +ConfigManager::ConfigManager(ROSflight &RF, config_t &config): + RF_{RF}, + config_{config} {} bool ConfigManager::init() { + if(!read()) + fill_defaults(); + bool success = true; + for(uint8_t device{0}; device < device_t::device_count; device++) + success = success &&RF_.board_.enable_device(static_cast(device), config_.config[device]); + return success; +} +void ConfigManager::set_configuration(device_t device, uint8_t config) +{ + config_.config[device] = config; + // TODO consider deinitializing and changing the config +} +uint8_t ConfigManager::get_configuration(device_t device) +{ + return config_.config[device]; +} + +void ConfigManager::prepare_write() +{ + config_.checksum = generate_checksum(); +} + +bool ConfigManager::read() +{ + if(!RF_.memory_manager_.is_ready()) + return false; + if(generate_checksum() != config_.checksum) + return false; + return true; +} +void ConfigManager::fill_defaults() +{ + memset(config_.config, 0, sizeof(config_.config)); +} +uint32_t ConfigManager::generate_checksum() +{ + //8 bit fletcher algorithm + const uint8_t *config_data = reinterpret_cast(config_.config); + uint8_t check_a{0}; + uint8_t check_b{0}; + for(size_t index{0}; index< sizeof(config_.config); index++ ) + { + check_a += config_data[index]; + check_b += check_a; + } + return check_a & (check_b<<8); } } diff --git a/src/param.cpp b/src/param.cpp index 993fffa4..1ae8b1d4 100644 --- a/src/param.cpp +++ b/src/param.cpp @@ -57,8 +57,9 @@ namespace rosflight_firmware { -Params::Params(ROSflight& _rf) : +Params::Params(ROSflight& _rf, params_t ¶m_struct) : RF_(_rf), + params(param_struct), listeners_(nullptr), num_listeners_(0) { @@ -102,12 +103,11 @@ uint8_t Params::compute_checksum(void) // function definitions void Params::init() { - RF_.board_.memory_init(); if (!read()) { RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_WARNING, "Unable to load parameters; using default values"); set_defaults(); - write(); + RF_.memory_manager_.write_memory(); } } @@ -298,9 +298,8 @@ void Params::set_listeners(ParamListenerInterface * const listeners[], size_t nu bool Params::read(void) { - if (!RF_.board_.memory_read(¶ms, sizeof(params_t))) + if (!RF_.memory_manager_.is_ready()) return false; - if (params.version != GIT_VERSION_HASH) return false; @@ -313,17 +312,13 @@ bool Params::read(void) return true; } -bool Params::write(void) +void Params::prepare_write(void) { params.version = GIT_VERSION_HASH; params.size = sizeof(params_t); params.magic_be = 0xBE; params.magic_ef = 0xEF; params.chk = compute_checksum(); - - if (!RF_.board_.memory_write(¶ms, sizeof(params_t))) - return false; - return true; } void Params::change_callback(uint16_t id) diff --git a/src/rosflight.cpp b/src/rosflight.cpp index aba7a0e8..01f89a8a 100644 --- a/src/rosflight.cpp +++ b/src/rosflight.cpp @@ -37,8 +37,9 @@ namespace rosflight_firmware ROSflight::ROSflight(Board& board, CommLinkInterface& comm_link) : board_(board), + memory_manager_(*this), comm_manager_(*this, comm_link), - params_(*this), + params_(*this, memory_manager_.get_params()), command_manager_(*this), controller_(*this), estimator_(*this), @@ -46,7 +47,7 @@ ROSflight::ROSflight(Board& board, CommLinkInterface& comm_link) : rc_(*this), sensors_(*this), state_manager_(*this), - device_manager_(*this) + config_manager_(*this, memory_manager_.get_config()) { comm_link.set_listener(&comm_manager_); params_.set_listeners(param_listeners_, num_param_listeners_); @@ -59,10 +60,11 @@ void ROSflight::init() state_manager_.init(); // Read EEPROM to get initial params + memory_manager_.read_memory(); params_.init(); //Initialize devices - device_manager_.init(); + config_manager_.init(); // Initialize Mixer mixer_.init(); From 1f6e21f35bce5d81a29819329ec92fc46369b9ce Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Wed, 4 Dec 2019 13:41:35 -0700 Subject: [PATCH 05/76] Add memory manager --- include/memory_manager.h | 35 +++++++++++++++++++++++++++++++++++ src/memory_manager.cpp | 24 ++++++++++++++++++++++++ 2 files changed, 59 insertions(+) create mode 100644 include/memory_manager.h create mode 100644 src/memory_manager.cpp diff --git a/include/memory_manager.h b/include/memory_manager.h new file mode 100644 index 00000000..16c6f6de --- /dev/null +++ b/include/memory_manager.h @@ -0,0 +1,35 @@ +#ifndef MEMORY_MANAGER_H +#define MEMORY_MANAGER_H + +#include "param.h" +#include "config_manager.h" + +namespace rosflight_firmware +{ +class ROSflight; +class MemoryManager +{ +public: + typedef struct + { + Params::params_t params; + ConfigManager::config_t config; + } persistent_memory_t; + + MemoryManager(ROSflight &rf); + bool read_memory(); + bool write_memory(); + inline bool is_ready() {return ready_;} + + inline Params::params_t &get_params(){return memory_.params;} + inline ConfigManager::config_t &get_config(){return memory_.config;} + +private: + ROSflight &RF_; + persistent_memory_t memory_; + bool ready_{false}; + +}; +} // namespace rosflight_firmware + +#endif // MEMORY_MANAGER_H diff --git a/src/memory_manager.cpp b/src/memory_manager.cpp new file mode 100644 index 00000000..fd0a2c4e --- /dev/null +++ b/src/memory_manager.cpp @@ -0,0 +1,24 @@ +#include "memory_manager.h" +#include "rosflight.h" + +namespace rosflight_firmware +{ + +MemoryManager::MemoryManager(ROSflight &rf): + RF_{rf} +{ +} +bool MemoryManager::read_memory() +{ + RF_.board_.memory_init(); + ready_ = RF_.board_.memory_read(&memory_, sizeof(persistent_memory_t)); + return ready_; +} +bool MemoryManager::write_memory() +{ + RF_.params_.prepare_write(); + RF_.config_manager_.prepare_write(); + return RF_.board_.memory_write(&memory_, sizeof(persistent_memory_t)); +} + +} // namespace rosflight_firmware From 4f3023188ff9badeb7e25e395b0ba76a6dc6f37b Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Wed, 4 Dec 2019 13:44:10 -0700 Subject: [PATCH 06/76] Config manager working for serial and airspeed --- boards/airbourne/airbourne_board.cpp | 49 ++++++++++++---------------- comms/mavlink/mavlink.cpp | 4 ++- src/config_manager.cpp | 2 +- 3 files changed, 24 insertions(+), 31 deletions(-) diff --git a/boards/airbourne/airbourne_board.cpp b/boards/airbourne/airbourne_board.cpp index 2ac7dc8f..efe13c1c 100644 --- a/boards/airbourne/airbourne_board.cpp +++ b/boards/airbourne/airbourne_board.cpp @@ -82,19 +82,19 @@ void AirbourneBoard::serial_init(uint32_t baud_rate, hardware_config_t configura switch(configuration) { default: - case 1: // VCP + case 0: // VCP current_serial_ = &vcp_; vcp_.init(); break; - case 2: // UART 1 + case 1: // UART 1 current_serial_ = &uart1_; uart1_.init(&uart_config[UART1], baud_rate); break; - case 3: // UART 2 + case 2: // UART 2 current_serial_ = &uart2_; uart2_.init(&uart_config[UART2], baud_rate); break; - case 4: // UART 3 + case 3: // UART 3 current_serial_ = &uart3_; uart3_.init(&uart_config[UART3], baud_rate); break; @@ -108,23 +108,6 @@ void AirbourneBoard::serial_write(const uint8_t *src, size_t len) uint16_t AirbourneBoard::serial_bytes_available() { - 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; - } - } - return current_serial_->rx_bytes_waiting(); } @@ -145,20 +128,31 @@ bool AirbourneBoard::enable_device(device_t device, hardware_config_t configurat switch(device) { case serial: - return true; // This is handled by serial_init + serial_init(921600, configuration); // TODO configurable baud rate + return true; // TODO serial_init success check + break; case rc: switch(configuration) { - case 1: + case 0: rc_init(RC_TYPE_PPM); break; - case 2: + case 1: rc_init(RC_TYPE_SBUS); break; + default: + return false; } return true; case airspeed: + if(configuration==1) + { + ext_i2c_.init(&i2c_config[EXTERNAL_I2C]); + //TODO check if I2C is initialized and only initialize if it is not + airspeed_.init(&ext_i2c_); + } break; + //TODO other config options case gnss: break; case sonar: @@ -178,11 +172,8 @@ void AirbourneBoard::sensors_init() baro_.init(&int_i2c_); mag_.init(&int_i2c_); - sonar_.init(&ext_i2c_); - airspeed_.init(&ext_i2c_); - // gnss_.init(&uart1_); - battery_adc_.init(battery_monitor_config.adc); - battery_monitor_.init(battery_monitor_config, &battery_adc_, 0,0); + + // Most sensors are set up through the configuration manager } uint16_t AirbourneBoard::num_sensor_errors() diff --git a/comms/mavlink/mavlink.cpp b/comms/mavlink/mavlink.cpp index e030eba6..4dfa6ac5 100644 --- a/comms/mavlink/mavlink.cpp +++ b/comms/mavlink/mavlink.cpp @@ -43,7 +43,9 @@ Mavlink::Mavlink(Board &board) : void Mavlink::init(uint32_t baud_rate, uint32_t dev) { - board_.serial_init(baud_rate, dev); + // TODO remove unneeded parameters + (void)baud_rate; + (void)dev; initialized_ = true; } diff --git a/src/config_manager.cpp b/src/config_manager.cpp index acb3aec5..14254385 100644 --- a/src/config_manager.cpp +++ b/src/config_manager.cpp @@ -56,7 +56,7 @@ uint32_t ConfigManager::generate_checksum() check_a += config_data[index]; check_b += check_a; } - return check_a & (check_b<<8); + return check_a & (check_b<<8) & (~check_a<<16) & (~check_b<<24); } } From ffefe1a50e6379edb2cf842284ca1f7e52e18f14 Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Fri, 6 Dec 2019 17:35:43 -0700 Subject: [PATCH 07/76] Basic functionality working for config_manager --- comms/mavlink/mavlink.cpp | 32 ++++++++++++++++++++++++++++++++ comms/mavlink/mavlink.h | 3 +++ comms/mavlink/v1.0 | 2 +- include/comm_manager.h | 3 +++ include/interface/comm_link.h | 3 +++ src/comm_manager.cpp | 20 ++++++++++++++++++++ 6 files changed, 62 insertions(+), 1 deletion(-) diff --git a/comms/mavlink/mavlink.cpp b/comms/mavlink/mavlink.cpp index 4dfa6ac5..60d84af1 100644 --- a/comms/mavlink/mavlink.cpp +++ b/comms/mavlink/mavlink.cpp @@ -288,6 +288,13 @@ void Mavlink::send_param_value_float(uint8_t system_id, send_message(msg); } +void Mavlink::send_config_value(uint8_t system_id, uint8_t device, uint8_t config) +{ + mavlink_message_t msg; + mavlink_msg_rosflight_config_pack(system_id, 0, &msg, device, config); + 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; @@ -575,6 +582,25 @@ void Mavlink::handle_msg_heartbeat(const mavlink_message_t *const msg) listener_->heartbeat_callback(); } +void Mavlink::handle_msg_config(const mavlink_message_t *const msg) +{ + mavlink_rosflight_config_t config_msg; + mavlink_msg_rosflight_config_decode(msg, &config_msg); + uint8_t device = config_msg.device; + uint8_t config = config_msg.config; + if(listener_ != nullptr) + listener_->config_set_callback(device, config); +} + +void Mavlink::handle_msg_config_request(const mavlink_message_t *const msg) +{ + mavlink_rosflight_config_request_t request_msg; + mavlink_msg_rosflight_config_request_decode(msg, &request_msg); + uint8_t device = request_msg.device; + if(listener_ != nullptr) + listener_->config_request_callback(device); +} + void Mavlink::handle_mavlink_message() { switch (in_buf_.msgid) @@ -606,6 +632,12 @@ void Mavlink::handle_mavlink_message() case MAVLINK_MSG_ID_HEARTBEAT: handle_msg_heartbeat(&in_buf_); break; + case MAVLINK_MSG_ID_ROSFLIGHT_CONFIG: + handle_msg_config(&in_buf_); + break; + case MAVLINK_MSG_ID_ROSFLIGHT_CONFIG_REQUEST: + handle_msg_config_request(&in_buf_); + break; default: break; } diff --git a/comms/mavlink/mavlink.h b/comms/mavlink/mavlink.h index 4b71dc65..a2bc6c0d 100644 --- a/comms/mavlink/mavlink.h +++ b/comms/mavlink/mavlink.h @@ -84,6 +84,7 @@ class Mavlink : public CommLinkInterface const char *const name, float value, uint16_t param_count) override; + void send_config_value(uint8_t system_id, uint8_t device, uint8_t config) 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, float min_range) override; void send_status(uint8_t system_id, @@ -116,6 +117,8 @@ class Mavlink : public CommLinkInterface 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_config(const mavlink_message_t *const msg); + void handle_msg_config_request(const mavlink_message_t *const msg); void handle_mavlink_message(); Board& board_; diff --git a/comms/mavlink/v1.0 b/comms/mavlink/v1.0 index 872592e9..8077e3cb 160000 --- a/comms/mavlink/v1.0 +++ b/comms/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit 872592e9281826de5f62dcd44b85c83e4726b986 +Subproject commit 8077e3cba558d20b8097d235074c41eb224bc94c diff --git a/include/comm_manager.h b/include/comm_manager.h index f54fa196..976fdafd 100644 --- a/include/comm_manager.h +++ b/include/comm_manager.h @@ -142,6 +142,8 @@ class CommManager : public CommLinkInterface::ListenerInterface, public ParamLis void aux_command_callback(const CommLinkInterface::AuxCommand &command) override; void external_attitude_callback(const turbomath::Quaternion &q) override; void heartbeat_callback() override; + void config_set_callback(uint8_t device, uint8_t configuration) override; + void config_request_callback(uint8_t device) override; void send_heartbeat(void); void send_status(void); @@ -194,6 +196,7 @@ class CommManager : public CommLinkInterface::ListenerInterface, public ParamLis void receive(void); void stream(); void send_param_value(uint16_t param_id); + void send_config_value(device_t device); void set_streaming_rate(uint8_t stream_id, int16_t param_id); void update_status(); void log(CommLinkInterface::LogSeverity severity, const char *fmt, ...); diff --git a/include/interface/comm_link.h b/include/interface/comm_link.h index c7e97acb..3457e06b 100644 --- a/include/interface/comm_link.h +++ b/include/interface/comm_link.h @@ -124,6 +124,8 @@ class CommLinkInterface virtual void aux_command_callback(const AuxCommand &command) = 0; virtual void external_attitude_callback(const turbomath::Quaternion &q) = 0; virtual void heartbeat_callback() = 0; + virtual void config_set_callback(uint8_t device, uint8_t configuration) = 0; + virtual void config_request_callback(uint8_t device) = 0; }; virtual void init(uint32_t baud_rate, uint32_t dev) = 0; @@ -159,6 +161,7 @@ class CommLinkInterface const char *const name, float value, uint16_t param_count) = 0; + virtual void send_config_value(uint8_t system_id, uint8_t device, uint8_t config) = 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, float min_range) = 0; virtual void send_status(uint8_t system_id, diff --git a/src/comm_manager.cpp b/src/comm_manager.cpp index 92fa7843..d840ff02 100644 --- a/src/comm_manager.cpp +++ b/src/comm_manager.cpp @@ -407,6 +407,26 @@ void CommManager::heartbeat_callback(void) this->send_heartbeat(); } +void CommManager::config_set_callback(uint8_t device, uint8_t configuration) +{ + //TODO consider checking a system ID to see if the message is intended for this device + if(device < device_count) + RF_.config_manager_.set_configuration(static_cast(device), configuration); +} + +void CommManager::config_request_callback(uint8_t device) +{ + //TODO consider checking a system ID to see if the message is intended for this device + if(device < device_count) + send_config_value(static_cast(device)); +} + +void CommManager::send_config_value(device_t device) +{ + uint8_t config = RF_.config_manager_.get_configuration(device); + comm_link_.send_config_value(sysid_, device, config); +} + // function definitions void CommManager::receive(void) { From fba096c15d17ae763736ceffba9d009d02edd1ee Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Mon, 9 Dec 2019 14:08:23 -0700 Subject: [PATCH 08/76] Add support for more devices in config_manager --- boards/airbourne/airbourne | 2 +- boards/airbourne/airbourne_board.cpp | 19 +++++++++++++++---- boards/airbourne/airbourne_board.h | 2 +- comms/mavlink/v1.0 | 2 +- include/board.h | 3 ++- include/config_manager.h | 1 + src/config_manager.cpp | 6 ++++++ src/rosflight.cpp | 3 ++- 8 files changed, 29 insertions(+), 9 deletions(-) diff --git a/boards/airbourne/airbourne b/boards/airbourne/airbourne index 2e18392f..d14a2331 160000 --- a/boards/airbourne/airbourne +++ b/boards/airbourne/airbourne @@ -1 +1 @@ -Subproject commit 2e18392f62926d1f83cc97f2073e8867166f5313 +Subproject commit d14a2331531ae959fa72164549c841144725ff77 diff --git a/boards/airbourne/airbourne_board.cpp b/boards/airbourne/airbourne_board.cpp index efe13c1c..6027c1b1 100644 --- a/boards/airbourne/airbourne_board.cpp +++ b/boards/airbourne/airbourne_board.cpp @@ -123,12 +123,13 @@ void AirbourneBoard::serial_flush() } // Resources -bool AirbourneBoard::enable_device(device_t device, hardware_config_t configuration) +bool AirbourneBoard::enable_device(device_t device, hardware_config_t configuration, const Params *params) { switch(device) { case serial: - serial_init(921600, configuration); // TODO configurable baud rate + uint32_t baud_rate = params->get_param_int(PARAM_BAUD_RATE); + serial_init(baud_rate, configuration); return true; // TODO serial_init success check break; case rc: @@ -147,17 +148,27 @@ bool AirbourneBoard::enable_device(device_t device, hardware_config_t configurat case airspeed: if(configuration==1) { - ext_i2c_.init(&i2c_config[EXTERNAL_I2C]); - //TODO check if I2C is initialized and only initialize if it is not + if(!ext_i2c_.is_initialized()) + ext_i2c_.init(&i2c_config[EXTERNAL_I2C]); airspeed_.init(&ext_i2c_); } break; //TODO other config options case gnss: + // GNSS is currently disabled break; case sonar: + if(!ext_i2c_.is_initialized()) + ext_i2c_.init(&i2c_config[EXTERNAL_I2C]); + sonar_.init(&ext_i2c_); break; case battery_monitor: + if(configuration == 1) + { + float voltage_multiplier = params->get_param_float(PARAM_BATTERY_VOLTAGE_MULTIPLIER); + float current_multiplier = params->get_param_float(PARAM_BATTERY_CURRENT_MULTIPLIER); + battery_adc_.init(&adc_config[]); + } break; default: return false; diff --git a/boards/airbourne/airbourne_board.h b/boards/airbourne/airbourne_board.h index 45f2837e..5a45d70d 100644 --- a/boards/airbourne/airbourne_board.h +++ b/boards/airbourne/airbourne_board.h @@ -143,7 +143,7 @@ class AirbourneBoard : public Board void serial_flush() override; // resource management - bool enable_device(device_t device, hardware_config_t configuration) override; + bool enable_device(device_t device, hardware_config_t configuration, const Params *params) override; // sensors void sensors_init() override; diff --git a/comms/mavlink/v1.0 b/comms/mavlink/v1.0 index 8077e3cb..872592e9 160000 --- a/comms/mavlink/v1.0 +++ b/comms/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit 8077e3cba558d20b8097d235074c41eb224bc94c +Subproject commit 872592e9281826de5f62dcd44b85c83e4726b986 diff --git a/include/board.h b/include/board.h index 3a47e2a7..78ae3909 100644 --- a/include/board.h +++ b/include/board.h @@ -39,6 +39,7 @@ #include "sensors.h" #include "state_manager.h" #include "configuration_enum.h" +#include "param.h" namespace rosflight_firmware { @@ -70,7 +71,7 @@ class Board virtual void serial_flush() = 0; // hardware config - virtual bool enable_device(device_t device, hardware_config_t configuration)=0; + virtual bool enable_device(device_t device, hardware_config_t configuration, const Params *params)=0; // sensors virtual void sensors_init() = 0; diff --git a/include/config_manager.h b/include/config_manager.h index 114a4e8f..a4ed9a8b 100644 --- a/include/config_manager.h +++ b/include/config_manager.h @@ -17,6 +17,7 @@ class ConfigManager ConfigManager(ROSflight &RF, config_t &config); bool init(); + bool configure_devices(); void set_configuration(device_t device, uint8_t config); uint8_t get_configuration(device_t device); void prepare_write(); diff --git a/src/config_manager.cpp b/src/config_manager.cpp index 14254385..ac213866 100644 --- a/src/config_manager.cpp +++ b/src/config_manager.cpp @@ -12,11 +12,17 @@ bool ConfigManager::init() { if(!read()) fill_defaults(); + return true; +} + +bool ConfigManager::configure_devices() +{ bool success = true; for(uint8_t device{0}; device < device_t::device_count; device++) success = success &&RF_.board_.enable_device(static_cast(device), config_.config[device]); return success; } + void ConfigManager::set_configuration(device_t device, uint8_t config) { config_.config[device] = config; diff --git a/src/rosflight.cpp b/src/rosflight.cpp index 01f89a8a..f096b628 100644 --- a/src/rosflight.cpp +++ b/src/rosflight.cpp @@ -62,9 +62,10 @@ void ROSflight::init() // Read EEPROM to get initial params memory_manager_.read_memory(); params_.init(); + config_manager_.init(); //Initialize devices - config_manager_.init(); + config_manager_.configure_devices(); // Initialize Mixer mixer_.init(); From 52cb10213ad3410e4875b5c19b252e06acb7264e Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Wed, 11 Dec 2019 14:22:13 -0700 Subject: [PATCH 09/76] Finish battery monitor initialization via config manager --- boards/airbourne/airbourne | 2 +- boards/airbourne/airbourne_board.cpp | 13 ++++++++----- boards/airbourne/airbourne_board.h | 2 +- comms/mavlink/v1.0 | 2 +- include/board.h | 2 +- src/config_manager.cpp | 2 +- 6 files changed, 13 insertions(+), 10 deletions(-) diff --git a/boards/airbourne/airbourne b/boards/airbourne/airbourne index d14a2331..27955055 160000 --- a/boards/airbourne/airbourne +++ b/boards/airbourne/airbourne @@ -1 +1 @@ -Subproject commit d14a2331531ae959fa72164549c841144725ff77 +Subproject commit 279550559cd568540d0fc3bcdd3e0ff297d0835c diff --git a/boards/airbourne/airbourne_board.cpp b/boards/airbourne/airbourne_board.cpp index 6027c1b1..60219303 100644 --- a/boards/airbourne/airbourne_board.cpp +++ b/boards/airbourne/airbourne_board.cpp @@ -123,15 +123,17 @@ void AirbourneBoard::serial_flush() } // Resources -bool AirbourneBoard::enable_device(device_t device, hardware_config_t configuration, const Params *params) +bool AirbourneBoard::enable_device(device_t device, hardware_config_t configuration, const Params ¶ms) { switch(device) { case serial: - uint32_t baud_rate = params->get_param_int(PARAM_BAUD_RATE); + { + uint32_t baud_rate = params.get_param_int(PARAM_BAUD_RATE); serial_init(baud_rate, configuration); return true; // TODO serial_init success check break; + } case rc: switch(configuration) { @@ -165,9 +167,10 @@ bool AirbourneBoard::enable_device(device_t device, hardware_config_t configurat case battery_monitor: if(configuration == 1) { - float voltage_multiplier = params->get_param_float(PARAM_BATTERY_VOLTAGE_MULTIPLIER); - float current_multiplier = params->get_param_float(PARAM_BATTERY_CURRENT_MULTIPLIER); - battery_adc_.init(&adc_config[]); + float voltage_multiplier = params.get_param_float(PARAM_BATTERY_VOLTAGE_MULTIPLIER); + float current_multiplier = params.get_param_float(PARAM_BATTERY_CURRENT_MULTIPLIER); + battery_adc_.init(battery_monitor_config.adc); + battery_monitor_.init(battery_monitor_config, &battery_adc_, voltage_multiplier, current_multiplier); } break; default: diff --git a/boards/airbourne/airbourne_board.h b/boards/airbourne/airbourne_board.h index 5a45d70d..05d7ba1f 100644 --- a/boards/airbourne/airbourne_board.h +++ b/boards/airbourne/airbourne_board.h @@ -143,7 +143,7 @@ class AirbourneBoard : public Board void serial_flush() override; // resource management - bool enable_device(device_t device, hardware_config_t configuration, const Params *params) override; + bool enable_device(device_t device, hardware_config_t configuration, const Params ¶ms) override; // sensors void sensors_init() override; diff --git a/comms/mavlink/v1.0 b/comms/mavlink/v1.0 index 872592e9..e2cb75ea 160000 --- a/comms/mavlink/v1.0 +++ b/comms/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit 872592e9281826de5f62dcd44b85c83e4726b986 +Subproject commit e2cb75ea4edb736ab26d8ece1229ac393a33350a diff --git a/include/board.h b/include/board.h index 78ae3909..2d1ba9f3 100644 --- a/include/board.h +++ b/include/board.h @@ -71,7 +71,7 @@ class Board virtual void serial_flush() = 0; // hardware config - virtual bool enable_device(device_t device, hardware_config_t configuration, const Params *params)=0; + virtual bool enable_device(device_t device, hardware_config_t configuration, const Params ¶ms)=0; // sensors virtual void sensors_init() = 0; diff --git a/src/config_manager.cpp b/src/config_manager.cpp index ac213866..108318dd 100644 --- a/src/config_manager.cpp +++ b/src/config_manager.cpp @@ -19,7 +19,7 @@ bool ConfigManager::configure_devices() { bool success = true; for(uint8_t device{0}; device < device_t::device_count; device++) - success = success &&RF_.board_.enable_device(static_cast(device), config_.config[device]); + success = success &&RF_.board_.enable_device(static_cast(device), config_.config[device], RF_.params_); return success; } From 3833594ca207278dc7308e8c9b9c44ab9c596906 Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Fri, 13 Dec 2019 15:09:48 -0700 Subject: [PATCH 10/76] Add enums for baro and mag --- include/configuration_enum.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/include/configuration_enum.h b/include/configuration_enum.h index 7616e72e..4b9677eb 100644 --- a/include/configuration_enum.h +++ b/include/configuration_enum.h @@ -11,7 +11,9 @@ enum device_t: uint8_t gnss, sonar, battery_monitor, - device_count + barometer, + magnetometer, + device_count // make sure this is last }; /* From bea06d948869f281d4311cb08b6cd37535f0dadf Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Fri, 13 Dec 2019 16:22:09 -0700 Subject: [PATCH 11/76] Add BoardConfigManager and AirbourneBoardConfigManager classes --- boards/airbourne/Makefile | 1 + boards/airbourne/airbourne | 2 +- boards/airbourne/airbourne_board.cpp | 11 +++ boards/airbourne/airbourne_board.h | 7 +- .../airbourne_board_config_manager.cpp | 72 +++++++++++++++++++ .../airbourne_board_config_manager.h | 27 +++++++ include/board.h | 3 + include/board_config_manager.h | 29 ++++++++ 8 files changed, 150 insertions(+), 2 deletions(-) create mode 100644 boards/airbourne/airbourne_board_config_manager.cpp create mode 100644 boards/airbourne/airbourne_board_config_manager.h create mode 100644 include/board_config_manager.h diff --git a/boards/airbourne/Makefile b/boards/airbourne/Makefile index 7e8bf89a..a8e05919 100644 --- a/boards/airbourne/Makefile +++ b/boards/airbourne/Makefile @@ -137,6 +137,7 @@ AIRBOURNE_SRCS = led.cpp \ # board-specific source files VPATH := $(VPATH):$(BOARD_DIR) BOARD_CXX_SRC = airbourne_board.cpp \ + airbourne_board_config_manager.cpp \ main.cpp # Make a list of source files and includes diff --git a/boards/airbourne/airbourne b/boards/airbourne/airbourne index 27955055..ce8c0ab2 160000 --- a/boards/airbourne/airbourne +++ b/boards/airbourne/airbourne @@ -1 +1 @@ -Subproject commit 279550559cd568540d0fc3bcdd3e0ff297d0835c +Subproject commit ce8c0ab25670bd63a53652a04a150ec30e6b970a diff --git a/boards/airbourne/airbourne_board.cpp b/boards/airbourne/airbourne_board.cpp index 60219303..be4a6c99 100644 --- a/boards/airbourne/airbourne_board.cpp +++ b/boards/airbourne/airbourne_board.cpp @@ -178,6 +178,17 @@ bool AirbourneBoard::enable_device(device_t device, hardware_config_t configurat } return false; } + +void AirbourneBoard::init_board_config_manager(ROSflight *rf) +{ + board_config_manager_.init(rf, this); +} + +AirbourneBoardConfigManager *AirbourneBoard::get_board_config_manager() +{ + return &board_config_manager_; +} + // sensors void AirbourneBoard::sensors_init() { diff --git a/boards/airbourne/airbourne_board.h b/boards/airbourne/airbourne_board.h index 05d7ba1f..2018a453 100644 --- a/boards/airbourne/airbourne_board.h +++ b/boards/airbourne/airbourne_board.h @@ -60,6 +60,7 @@ #include "analog_digital_converter.h" #include "analog_pin.h" #include "battery_monitor.h" +#include "airbourne_board_config_manager.h" // #include "ublox.h" #include "board.h" @@ -71,6 +72,8 @@ class AirbourneBoard : public Board { private: + AirbourneBoardConfigManager board_config_manager_; + VCP vcp_; UART uart1_; UART uart2_; @@ -142,8 +145,10 @@ class AirbourneBoard : public Board uint8_t serial_read() override; void serial_flush() override; - // resource management + // hardware config bool enable_device(device_t device, hardware_config_t configuration, const Params ¶ms) override; + void init_board_config_manager(ROSflight *rf) override; + AirbourneBoardConfigManager *get_board_config_manager() override; // sensors void sensors_init() override; diff --git a/boards/airbourne/airbourne_board_config_manager.cpp b/boards/airbourne/airbourne_board_config_manager.cpp new file mode 100644 index 00000000..364c101c --- /dev/null +++ b/boards/airbourne/airbourne_board_config_manager.cpp @@ -0,0 +1,72 @@ +#include "airbourne_board_config_manager.h" +#include + +namespace rosflight_firmware +{ +AirbourneBoardConfigManager::AirbourneBoardConfigManager() +{ +} +void AirbourneBoardConfigManager::init(ROSflight *rf, AirbourneBoard *board) +{ + this->RF_ = rf; + this->board_ = board; + is_initialized_ = true; +} +hardware_config_t AirbourneBoardConfigManager::get_max_config(device_t device) +{ + if(device >= device_count) + return 0; + else + return max_configs[device]; +} +BoardConfigManager::config_response AirbourneBoardConfigManager::check_config_change(device_t device, hardware_config_t config) +{ + BoardConfigManager::config_response resp; + // TODO actual checks + (void)device; + (void)config; + resp.successful = true; + resp.reboot_required = true; + return resp; +} +void AirbourneBoardConfigManager::get_device_name(device_t device, char (&name)[20]) +{ + switch(device) + { + case serial: + strcpy(name, "Serial"); + break; + case rc: + strcpy(name, "RC"); + break; + case airspeed: + strcpy(name, "Airspeed"); + break; + case gnss: + strcpy(name, "GNSS"); + break; + case sonar: + strcpy(name, "Sonar"); + break; + case battery_monitor: + strcpy(name, "Battery Monitor"); + break; + case barometer: + strcpy(name, "Barometer"); + break; + case magnetometer: + strcpy(name, "Magnetometer"); + break; + default: + strcpy(name, "Error/Unsupported"); + break; + } +} +void AirbourneBoardConfigManager::get_config_name(device_t device, hardware_config_t config, char (&name)[20]) +{ + //TODO + (void)device; + (void)config; + strcpy(name, "TODO"); +} +} //rosflight_firmware diff --git a/boards/airbourne/airbourne_board_config_manager.h b/boards/airbourne/airbourne_board_config_manager.h new file mode 100644 index 00000000..d84f97a8 --- /dev/null +++ b/boards/airbourne/airbourne_board_config_manager.h @@ -0,0 +1,27 @@ +#ifndef AIRBOURNEBOARDCONFIGMANAGER_H +#define AIRBOURNEBOARDCONFIGMANAGER_H + +#include "board_config_manager.h" + +namespace rosflight_firmware{ +class ROSflight; +class AirbourneBoard; +class AirbourneBoardConfigManager : public BoardConfigManager +{ +public: + AirbourneBoardConfigManager(); + void init(ROSflight *rf, AirbourneBoard *board); + hardware_config_t get_max_config(device_t device) override; + config_response check_config_change(device_t device, hardware_config_t config) override; + void get_device_name(device_t device, char (&name)[20]) override; + void get_config_name(device_t device, hardware_config_t config, char (&name)[20]) override; + inline bool is_initialized(){return is_initialized_;} +private: + bool is_initialized_{false}; + AirbourneBoard *board_; + ROSflight *RF_; + const hardware_config_t max_configs[device_count]{3, 1, 1, 3, 1, 1, 0, 0}; +}; +} // namespace rosflight_firmware + +#endif // AIRBOURNEBOARDCONFIGMANAGER_H diff --git a/include/board.h b/include/board.h index 2d1ba9f3..42ecf216 100644 --- a/include/board.h +++ b/include/board.h @@ -39,6 +39,7 @@ #include "sensors.h" #include "state_manager.h" #include "configuration_enum.h" +#include "board_config_manager.h" #include "param.h" namespace rosflight_firmware @@ -72,6 +73,8 @@ class Board // hardware config virtual bool enable_device(device_t device, hardware_config_t configuration, const Params ¶ms)=0; + virtual void init_board_config_manager(ROSflight *rf) = 0; + virtual BoardConfigManager *get_board_config_manager() = 0; // sensors virtual void sensors_init() = 0; diff --git a/include/board_config_manager.h b/include/board_config_manager.h new file mode 100644 index 00000000..1a8d5ef6 --- /dev/null +++ b/include/board_config_manager.h @@ -0,0 +1,29 @@ +#ifndef BOARD_CONFIG_MANAGER_H +#define BOARD_CONFIG_MANAGER_H + +#include "configuration_enum.h" + +namespace rosflight_firmware +{ +class BoardConfigManager +{ +public: + typedef struct + { + bool successful; + bool reboot_required; + char error_message[50]; + }config_response; + + // Get the largest number that is valid for the configuration of a given device + virtual hardware_config_t get_max_config(device_t device) = 0; + // Check if a config change is allowed. + // If the response indicates success, then the config manager accepts the change + // If not, the config manager returns the error indicated + virtual config_response check_config_change(device_t device, hardware_config_t config) = 0; + virtual void get_device_name(device_t device, char (&name)[20])=0; + virtual void get_config_name(device_t device, hardware_config_t config, char (&name)[20]) = 0; +}; +} // namespace rosflight_firmware + +#endif // BOARD_CONFIG_MANAGER_H From d4b22ead3f8c7d40b9162672ac8a498bb42f7f01 Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Wed, 18 Dec 2019 15:41:41 -0700 Subject: [PATCH 12/76] Transmitting device and config names on request --- boards/airbourne/airbourne_board.cpp | 4 +- boards/airbourne/airbourne_board.h | 2 +- .../airbourne_board_config_manager.cpp | 103 +++++++++++++++--- .../airbourne_board_config_manager.h | 4 +- comms/mavlink/mavlink.cpp | 14 +++ comms/mavlink/mavlink.h | 2 + comms/mavlink/v1.0 | 2 +- include/board.h | 2 +- include/board_config_manager.h | 6 +- include/comm_manager.h | 6 + include/configuration_enum.h | 20 ++-- include/interface/comm_link.h | 2 + src/comm_manager.cpp | 42 +++++++ src/rosflight.cpp | 3 + 14 files changed, 176 insertions(+), 36 deletions(-) diff --git a/boards/airbourne/airbourne_board.cpp b/boards/airbourne/airbourne_board.cpp index be4a6c99..3d500da0 100644 --- a/boards/airbourne/airbourne_board.cpp +++ b/boards/airbourne/airbourne_board.cpp @@ -184,9 +184,9 @@ void AirbourneBoard::init_board_config_manager(ROSflight *rf) board_config_manager_.init(rf, this); } -AirbourneBoardConfigManager *AirbourneBoard::get_board_config_manager() +AirbourneBoardConfigManager &AirbourneBoard::get_board_config_manager() { - return &board_config_manager_; + return board_config_manager_; } // sensors diff --git a/boards/airbourne/airbourne_board.h b/boards/airbourne/airbourne_board.h index 2018a453..9eaed7da 100644 --- a/boards/airbourne/airbourne_board.h +++ b/boards/airbourne/airbourne_board.h @@ -148,7 +148,7 @@ class AirbourneBoard : public Board // hardware config bool enable_device(device_t device, hardware_config_t configuration, const Params ¶ms) override; void init_board_config_manager(ROSflight *rf) override; - AirbourneBoardConfigManager *get_board_config_manager() override; + AirbourneBoardConfigManager &get_board_config_manager() override; // sensors void sensors_init() override; diff --git a/boards/airbourne/airbourne_board_config_manager.cpp b/boards/airbourne/airbourne_board_config_manager.cpp index 364c101c..8c17e831 100644 --- a/boards/airbourne/airbourne_board_config_manager.cpp +++ b/boards/airbourne/airbourne_board_config_manager.cpp @@ -29,44 +29,117 @@ BoardConfigManager::config_response AirbourneBoardConfigManager::check_config_ch resp.reboot_required = true; return resp; } -void AirbourneBoardConfigManager::get_device_name(device_t device, char (&name)[20]) +void AirbourneBoardConfigManager::get_device_name(device_t device, uint8_t (&name)[20]) { + char *name_char = reinterpret_cast(name); switch(device) { case serial: - strcpy(name, "Serial"); + strcpy(name_char, "Serial"); break; case rc: - strcpy(name, "RC"); + strcpy(name_char, "RC"); break; case airspeed: - strcpy(name, "Airspeed"); + strcpy(name_char, "Airspeed"); break; case gnss: - strcpy(name, "GNSS"); + strcpy(name_char, "GNSS"); break; case sonar: - strcpy(name, "Sonar"); + strcpy(name_char, "Sonar"); break; case battery_monitor: - strcpy(name, "Battery Monitor"); + strcpy(name_char, "Battery Monitor"); break; case barometer: - strcpy(name, "Barometer"); + strcpy(name_char, "Barometer"); break; case magnetometer: - strcpy(name, "Magnetometer"); + strcpy(name_char, "Magnetometer"); break; default: - strcpy(name, "Error/Unsupported"); + strcpy(name_char, "Error/Unsupported"); break; } } -void AirbourneBoardConfigManager::get_config_name(device_t device, hardware_config_t config, char (&name)[20]) +void AirbourneBoardConfigManager::get_config_name(device_t device, hardware_config_t config, uint8_t (&name)[20]) { - //TODO - (void)device; - (void)config; - strcpy(name, "TODO"); + char *name_char = reinterpret_cast(name); + const char *name_str; + switch(device) + { + case serial: + switch(config) + { + case 0: + name_str="VCP over USB"; + break; + case 1: + name_str="UART1 on Main"; + break; + case 2: + name_str="UART2 on Flex-IO"; + break; + case 3: + name_str="UART3 on Flexi"; + break; + } + break; + case rc: + if(config==0) + name_str = "PPM on Flex-IO"; + else + name_str = "SBUS on Main"; + break; + case airspeed: + if(config==0) + name_str = "Disabled"; + else + name_str = "I2C2 on Flexi"; + break; + case gnss: + switch(config) + { + case 0: + name_str = "Disabled"; + break; + case 1: + name_str = "UART1 on main"; + break; + case 2: + name_str = "UART2 on Flex-Io"; + break; + case 3: + name_str = "UART3 on Flexi"; + break; + } + break; + case sonar: + if(config ==0) + name_str = "Disabled"; + else + name_str = "I2C2 on Flexi"; + break; + case battery_monitor: + if(config==0) + name_str = "Disabled"; + else + name_str = "ADC3 on Power"; + break; + case barometer: + if(config==0) + name_str = "Onboard barometer"; + break; + case magnetometer: + if(config ==0) + name_str = "Onboard magnetometer"; + break; + default: + name_str = "Invalid device"; + } + if(config > max_configs[device]) + name_str = "Invalid config"; + strcpy(name_char, name_str); } } //rosflight_firmware diff --git a/boards/airbourne/airbourne_board_config_manager.h b/boards/airbourne/airbourne_board_config_manager.h index d84f97a8..3d056357 100644 --- a/boards/airbourne/airbourne_board_config_manager.h +++ b/boards/airbourne/airbourne_board_config_manager.h @@ -13,8 +13,8 @@ class AirbourneBoardConfigManager : public BoardConfigManager void init(ROSflight *rf, AirbourneBoard *board); hardware_config_t get_max_config(device_t device) override; config_response check_config_change(device_t device, hardware_config_t config) override; - void get_device_name(device_t device, char (&name)[20]) override; - void get_config_name(device_t device, hardware_config_t config, char (&name)[20]) override; + void get_device_name(device_t device, uint8_t (&name)[20]) override; + void get_config_name(device_t device, hardware_config_t config, uint8_t (&name)[20]) override; inline bool is_initialized(){return is_initialized_;} private: bool is_initialized_{false}; diff --git a/comms/mavlink/mavlink.cpp b/comms/mavlink/mavlink.cpp index 60d84af1..a20925da 100644 --- a/comms/mavlink/mavlink.cpp +++ b/comms/mavlink/mavlink.cpp @@ -295,6 +295,20 @@ void Mavlink::send_config_value(uint8_t system_id, uint8_t device, uint8_t confi send_message(msg); } +void Mavlink::send_device_info(uint8_t system_id, uint8_t device, uint8_t max_config, uint8_t (&name)[20]) +{ + mavlink_message_t msg; + mavlink_msg_rosflight_device_info_pack(system_id, 0, &msg, device, max_config, name); + send_message(msg); +} + +void Mavlink::send_config_info(uint8_t system_id, uint8_t device, uint8_t config, uint8_t (&name)[20]) +{ + mavlink_message_t msg; + mavlink_msg_rosflight_config_info_pack(system_id, 0, &msg, device, config, name); + 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; diff --git a/comms/mavlink/mavlink.h b/comms/mavlink/mavlink.h index a2bc6c0d..8da4cec8 100644 --- a/comms/mavlink/mavlink.h +++ b/comms/mavlink/mavlink.h @@ -85,6 +85,8 @@ class Mavlink : public CommLinkInterface float value, uint16_t param_count) override; void send_config_value(uint8_t system_id, uint8_t device, uint8_t config) override; + void send_device_info(uint8_t system_id, uint8_t device, uint8_t max_config, uint8_t (&name)[20]) override; + void send_config_info(uint8_t system_id, uint8_t device, uint8_t config, uint8_t (&name)[20]) 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, float min_range) override; void send_status(uint8_t system_id, diff --git a/comms/mavlink/v1.0 b/comms/mavlink/v1.0 index e2cb75ea..d6ac54b0 160000 --- a/comms/mavlink/v1.0 +++ b/comms/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit e2cb75ea4edb736ab26d8ece1229ac393a33350a +Subproject commit d6ac54b03ee6dbfe114a9cce6346920dc35bbd42 diff --git a/include/board.h b/include/board.h index 42ecf216..14e240df 100644 --- a/include/board.h +++ b/include/board.h @@ -74,7 +74,7 @@ class Board // hardware config virtual bool enable_device(device_t device, hardware_config_t configuration, const Params ¶ms)=0; virtual void init_board_config_manager(ROSflight *rf) = 0; - virtual BoardConfigManager *get_board_config_manager() = 0; + virtual BoardConfigManager &get_board_config_manager() = 0; // sensors virtual void sensors_init() = 0; diff --git a/include/board_config_manager.h b/include/board_config_manager.h index 1a8d5ef6..807cc3cd 100644 --- a/include/board_config_manager.h +++ b/include/board_config_manager.h @@ -16,13 +16,15 @@ class BoardConfigManager }config_response; // Get the largest number that is valid for the configuration of a given device + // This number is inclusive, i.e. a value of 2 means 0, 1, or 2 are valid + // For devices that are not configurable, returns 0 virtual hardware_config_t get_max_config(device_t device) = 0; // Check if a config change is allowed. // If the response indicates success, then the config manager accepts the change // If not, the config manager returns the error indicated virtual config_response check_config_change(device_t device, hardware_config_t config) = 0; - virtual void get_device_name(device_t device, char (&name)[20])=0; - virtual void get_config_name(device_t device, hardware_config_t config, char (&name)[20]) = 0; + virtual void get_device_name(device_t device, uint8_t (&name)[20])=0; + virtual void get_config_name(device_t device, hardware_config_t config, uint8_t (&name)[20]) = 0; }; } // namespace rosflight_firmware diff --git a/include/comm_manager.h b/include/comm_manager.h index 976fdafd..b74855e2 100644 --- a/include/comm_manager.h +++ b/include/comm_manager.h @@ -83,6 +83,8 @@ class CommManager : public CommLinkInterface::ListenerInterface, public ParamLis ROSflight& RF_; CommLinkInterface& comm_link_; uint8_t send_params_index_; + device_t send_device_info_index_; + hardware_config_t send_config_info_index_; bool initialized_ = false; bool connected_ = false; @@ -165,6 +167,7 @@ class CommManager : public CommLinkInterface::ListenerInterface, public ParamLis // void send_named_command_struct(const char *const name, control_t command_struct); void send_next_param(void); + void send_next_config_info(void); Stream streams_[STREAM_COUNT] = { Stream(0, [this]{this->send_heartbeat();}), @@ -197,6 +200,9 @@ class CommManager : public CommLinkInterface::ListenerInterface, public ParamLis void stream(); void send_param_value(uint16_t param_id); void send_config_value(device_t device); + void send_all_config_info(); // Sends all device and configuration names + void send_device_info(device_t device); + void send_config_info(device_t device, hardware_config_t config); void set_streaming_rate(uint8_t stream_id, int16_t param_id); void update_status(); void log(CommLinkInterface::LogSeverity severity, const char *fmt, ...); diff --git a/include/configuration_enum.h b/include/configuration_enum.h index 4b9677eb..3f98ba21 100644 --- a/include/configuration_enum.h +++ b/include/configuration_enum.h @@ -16,19 +16,15 @@ enum device_t: uint8_t device_count // make sure this is last }; -/* -enum resource_t +inline device_t& operator++(device_t &dev) { - vcp, - uart, - sbus, - i2c, - spi, - ppm, - adc -}; -*/ - + uint8_t return_value = dev; + return_value++; + if (return_value > device_count) + return_value--; + dev = static_cast(return_value); + return dev; +} typedef uint8_t hardware_config_t; diff --git a/include/interface/comm_link.h b/include/interface/comm_link.h index 3457e06b..b12287ed 100644 --- a/include/interface/comm_link.h +++ b/include/interface/comm_link.h @@ -162,6 +162,8 @@ class CommLinkInterface float value, uint16_t param_count) = 0; virtual void send_config_value(uint8_t system_id, uint8_t device, uint8_t config) = 0; + virtual void send_device_info(uint8_t system_id, uint8_t device, uint8_t max_config, uint8_t (&name)[20]) = 0; + virtual void send_config_info(uint8_t system_id, uint8_t device, uint8_t config, uint8_t (&name)[20]) = 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, float min_range) = 0; virtual void send_status(uint8_t system_id, diff --git a/src/comm_manager.cpp b/src/comm_manager.cpp index d840ff02..78c514e1 100644 --- a/src/comm_manager.cpp +++ b/src/comm_manager.cpp @@ -86,6 +86,8 @@ void CommManager::init() offboard_control_time_ = 0; send_params_index_ = PARAMS_COUNT; + send_device_info_index_ = device_count; + send_config_info_index_ = 0; update_system_id(PARAM_SYSTEM_ID); set_streaming_rate(STREAM_ID_HEARTBEAT, PARAM_STREAM_HEARTBEAT_RATE); @@ -419,6 +421,28 @@ void CommManager::config_request_callback(uint8_t device) //TODO consider checking a system ID to see if the message is intended for this device if(device < device_count) send_config_value(static_cast(device)); + if(device == 0xff) + send_all_config_info(); +} +void CommManager::send_all_config_info() +{ + send_config_info_index_ = 0; + send_device_info_index_ = static_cast(0); +} + +void CommManager::send_device_info(device_t device) +{ + uint8_t device_name[20]; + RF_.board_.get_board_config_manager().get_device_name(device, device_name); + uint8_t max_config = RF_.board_.get_board_config_manager().get_max_config(device); + comm_link_.send_device_info(sysid_, device, max_config, device_name); +} + +void CommManager::send_config_info(device_t device, hardware_config_t config) +{ + uint8_t config_name[20]; + RF_.board_.get_board_config_manager().get_config_name(device, config, config_name); + comm_link_.send_config_info(sysid_, device, config, config_name); } void CommManager::send_config_value(device_t device) @@ -615,6 +639,7 @@ void CommManager::send_gnss_raw() void CommManager::send_low_priority(void) { send_next_param(); + send_next_config_info(); // send buffered log messages if (connected_ && !log_buffer_.empty()) @@ -660,6 +685,23 @@ void CommManager::send_next_param(void) } } +void CommManager::send_next_config_info(void) +{ + if (send_device_info_index_ < device_count) + { + if (send_config_info_index_ == 0) + send_device_info(send_device_info_index_); + else + send_config_info(send_device_info_index_, send_config_info_index_-1); + send_config_info_index_++; + if(send_config_info_index_ -1 > RF_.board_.get_board_config_manager().get_max_config(send_device_info_index_)) + { + ++send_device_info_index_; + send_config_info_index_ = 0; + } + } +} + CommManager::Stream::Stream(uint32_t period_us, std::function send_function) : period_us_(period_us), next_time_us_(0), diff --git a/src/rosflight.cpp b/src/rosflight.cpp index f096b628..5104b4f6 100644 --- a/src/rosflight.cpp +++ b/src/rosflight.cpp @@ -61,7 +61,10 @@ void ROSflight::init() // Read EEPROM to get initial params memory_manager_.read_memory(); + + // Prepare to initialize devices params_.init(); + board_.init_board_config_manager(this); config_manager_.init(); //Initialize devices From 465e2f614606761b7a49b29ec128667315d41b8a Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Thu, 19 Dec 2019 15:44:03 -0700 Subject: [PATCH 13/76] Add error messages to AirbourneBoardConfigManager --- .../airbourne_board_config_manager.cpp | 111 +++++++++++++++++- .../airbourne_board_config_manager.h | 10 ++ include/config_manager.h | 2 + src/config_manager.cpp | 4 + 4 files changed, 124 insertions(+), 3 deletions(-) diff --git a/boards/airbourne/airbourne_board_config_manager.cpp b/boards/airbourne/airbourne_board_config_manager.cpp index 8c17e831..2962b28e 100644 --- a/boards/airbourne/airbourne_board_config_manager.cpp +++ b/boards/airbourne/airbourne_board_config_manager.cpp @@ -1,6 +1,9 @@ #include "airbourne_board_config_manager.h" + #include +#include "rosflight.h" + namespace rosflight_firmware { AirbourneBoardConfigManager::AirbourneBoardConfigManager() @@ -22,11 +25,86 @@ hardware_config_t AirbourneBoardConfigManager::get_max_config(device_t device) BoardConfigManager::config_response AirbourneBoardConfigManager::check_config_change(device_t device, hardware_config_t config) { BoardConfigManager::config_response resp; - // TODO actual checks - (void)device; - (void)config; + resp.reboot_required = false; + resp.successful = false; + + if(device > device_count) + { + strcpy(resp.error_message, "Device not found"); + return resp; + } + + if(config > max_configs[device]) + { + strcpy(resp.error_message, "Configuration not found"); + return resp; + } + revo_port port = get_port(device, config); + ConfigManager &cm = RF_->config_manager_; + device_t conflict_device = device_count; + + switch(device) + { + case rc: + if(config ==0) + break; + case serial: + case gnss: + for(device_t other_device{0}; other_device != device_count; ++other_device) + if(other_device != rc && port == get_port(other_device, cm[other_device])) + { + conflict_device = other_device; + break; + } + break; + case airspeed: + case sonar: + if(cm[gnss] == 3) + conflict_device = gnss; + if(cm[serial] == 3) + conflict_device = serial; + break; + default: + break; + } + if(conflict_device != device_count) + { + switch(conflict_device) + { + case serial: + strcpy(resp.error_message, "Port is used by serial."); + break; + case rc: + strcpy(resp.error_message, "Port is used by RC."); + break; + case airspeed: + strcpy(resp.error_message, "Port is used by airspeed sensor."); + break; + case gnss: + strcpy(resp.error_message, "Port is used by GNSS receiver."); + break; + case sonar: + strcpy(resp.error_message, "Port is used by sonar sensor."); + break; + // At the time of this writing, the below should not cause issues + case battery_monitor: + strcpy(resp.error_message, "Port is used by battery monitor."); + break; + case barometer: + strcpy(resp.error_message, "Port is used by barometer."); + break; + case magnetometer: + strcpy(resp.error_message, "Port is used by magnetometer."); + break; + default: + strcpy(resp.error_message, "Other error."); + break; + } + return resp; + } resp.successful = true; resp.reboot_required = true; + resp.error_message[0]=0; return resp; } void AirbourneBoardConfigManager::get_device_name(device_t device, uint8_t (&name)[20]) @@ -142,4 +220,31 @@ void AirbourneBoardConfigManager::get_config_name(device_t device, hardware_conf name_str = "Invalid config"; strcpy(name_char, name_str); } +AirbourneBoardConfigManager::revo_port AirbourneBoardConfigManager::get_port(uint8_t device, uint8_t config) +{ + switch(device) + { + case serial: + if(config == 0) + return usb; + // break intentionally ommitted + case gnss: + return static_cast(device); + case rc: + if(config == 0) + return flex_io; + if(config == 1) + return main; + break; + case airspeed: + case sonar: + if(config==1) + return flexi; + break; + case battery_monitor: + if(config == 1) + return power; + } + return none; +} } //rosflight_firmware diff --git a/boards/airbourne/airbourne_board_config_manager.h b/boards/airbourne/airbourne_board_config_manager.h index 3d056357..df3f72d1 100644 --- a/boards/airbourne/airbourne_board_config_manager.h +++ b/boards/airbourne/airbourne_board_config_manager.h @@ -17,6 +17,16 @@ class AirbourneBoardConfigManager : public BoardConfigManager void get_config_name(device_t device, hardware_config_t config, uint8_t (&name)[20]) override; inline bool is_initialized(){return is_initialized_;} private: + enum revo_port + { + none, + main, + flex_io, + flexi, + usb, + power + }; + revo_port get_port(uint8_t device, uint8_t config); bool is_initialized_{false}; AirbourneBoard *board_; ROSflight *RF_; diff --git a/include/config_manager.h b/include/config_manager.h index a4ed9a8b..24d78e29 100644 --- a/include/config_manager.h +++ b/include/config_manager.h @@ -20,6 +20,8 @@ class ConfigManager bool configure_devices(); void set_configuration(device_t device, uint8_t config); uint8_t get_configuration(device_t device); + uint8_t operator[](device_t device); // same as get_configuration, for convenience + void prepare_write(); private: diff --git a/src/config_manager.cpp b/src/config_manager.cpp index 108318dd..ef14a966 100644 --- a/src/config_manager.cpp +++ b/src/config_manager.cpp @@ -32,6 +32,10 @@ uint8_t ConfigManager::get_configuration(device_t device) { return config_.config[device]; } +uint8_t ConfigManager::operator[](device_t device) +{ + return get_configuration(device); +} void ConfigManager::prepare_write() { From 525334b894bcdceac82cdc1fb47999fead7fbb44 Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Thu, 19 Dec 2019 15:45:40 -0700 Subject: [PATCH 14/76] Add error messages to AirbourneBoardConfigManager --- boards/airbourne/airbourne_board_config_manager.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/boards/airbourne/airbourne_board_config_manager.cpp b/boards/airbourne/airbourne_board_config_manager.cpp index 2962b28e..c196e3dc 100644 --- a/boards/airbourne/airbourne_board_config_manager.cpp +++ b/boards/airbourne/airbourne_board_config_manager.cpp @@ -50,7 +50,7 @@ BoardConfigManager::config_response AirbourneBoardConfigManager::check_config_ch break; case serial: case gnss: - for(device_t other_device{0}; other_device != device_count; ++other_device) + for(device_t other_device{static_cast(0)}; other_device != device_count; ++other_device) if(other_device != rc && port == get_port(other_device, cm[other_device])) { conflict_device = other_device; From 95a69db1787065f3d1753ae1940e3fdd3fef28f8 Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Tue, 7 Jan 2020 15:33:53 -0700 Subject: [PATCH 15/76] Working responses to setting configs --- .../airbourne_board_config_manager.cpp | 44 ++++++++++--------- .../airbourne_board_config_manager.h | 2 +- comms/mavlink/mavlink.cpp | 6 +++ comms/mavlink/mavlink.h | 1 + include/board_config_manager.h | 9 +--- include/config_manager.h | 11 ++++- include/interface/comm_link.h | 1 + src/comm_manager.cpp | 9 ++-- src/config_manager.cpp | 7 +++ 9 files changed, 56 insertions(+), 34 deletions(-) diff --git a/boards/airbourne/airbourne_board_config_manager.cpp b/boards/airbourne/airbourne_board_config_manager.cpp index c196e3dc..687a60cb 100644 --- a/boards/airbourne/airbourne_board_config_manager.cpp +++ b/boards/airbourne/airbourne_board_config_manager.cpp @@ -22,21 +22,22 @@ hardware_config_t AirbourneBoardConfigManager::get_max_config(device_t device) else return max_configs[device]; } -BoardConfigManager::config_response AirbourneBoardConfigManager::check_config_change(device_t device, hardware_config_t config) +ConfigManager::config_response AirbourneBoardConfigManager::check_config_change(device_t device, hardware_config_t config) { - BoardConfigManager::config_response resp; + ConfigManager::config_response resp; resp.reboot_required = false; resp.successful = false; + char *error_message = reinterpret_cast(resp.error_message); - if(device > device_count) + if(device >= device_count) { - strcpy(resp.error_message, "Device not found"); + strcpy(error_message, "Device not found"); return resp; } if(config > max_configs[device]) { - strcpy(resp.error_message, "Configuration not found"); + strcpy(error_message, "Configuration not found"); return resp; } revo_port port = get_port(device, config); @@ -50,12 +51,13 @@ BoardConfigManager::config_response AirbourneBoardConfigManager::check_config_ch break; case serial: case gnss: - for(device_t other_device{static_cast(0)}; other_device != device_count; ++other_device) - if(other_device != rc && port == get_port(other_device, cm[other_device])) - { - conflict_device = other_device; - break; - } + if(port != none) + for(device_t other_device{static_cast(0)}; other_device != device_count; ++other_device) + if(other_device != rc && port == get_port(other_device, cm[other_device])) + { + conflict_device = other_device; + break; + } break; case airspeed: case sonar: @@ -72,32 +74,32 @@ BoardConfigManager::config_response AirbourneBoardConfigManager::check_config_ch switch(conflict_device) { case serial: - strcpy(resp.error_message, "Port is used by serial."); + strcpy(error_message, "Port is used by serial."); break; case rc: - strcpy(resp.error_message, "Port is used by RC."); + strcpy(error_message, "Port is used by RC."); break; case airspeed: - strcpy(resp.error_message, "Port is used by airspeed sensor."); + strcpy(error_message, "Port is used by airspeed sensor."); break; case gnss: - strcpy(resp.error_message, "Port is used by GNSS receiver."); + strcpy(error_message, "Port is used by GNSS receiver."); break; case sonar: - strcpy(resp.error_message, "Port is used by sonar sensor."); + strcpy(error_message, "Port is used by sonar sensor."); break; // At the time of this writing, the below should not cause issues case battery_monitor: - strcpy(resp.error_message, "Port is used by battery monitor."); + strcpy(error_message, "Port is used by battery monitor."); break; case barometer: - strcpy(resp.error_message, "Port is used by barometer."); + strcpy(error_message, "Port is used by barometer."); break; case magnetometer: - strcpy(resp.error_message, "Port is used by magnetometer."); + strcpy(error_message, "Port is used by magnetometer."); break; default: - strcpy(resp.error_message, "Other error."); + strcpy(error_message, "Other error."); break; } return resp; @@ -229,7 +231,7 @@ AirbourneBoardConfigManager::revo_port AirbourneBoardConfigManager::get_port(uin return usb; // break intentionally ommitted case gnss: - return static_cast(device); + return static_cast(config); case rc: if(config == 0) return flex_io; diff --git a/boards/airbourne/airbourne_board_config_manager.h b/boards/airbourne/airbourne_board_config_manager.h index df3f72d1..b5a4eb78 100644 --- a/boards/airbourne/airbourne_board_config_manager.h +++ b/boards/airbourne/airbourne_board_config_manager.h @@ -12,7 +12,7 @@ class AirbourneBoardConfigManager : public BoardConfigManager AirbourneBoardConfigManager(); void init(ROSflight *rf, AirbourneBoard *board); hardware_config_t get_max_config(device_t device) override; - config_response check_config_change(device_t device, hardware_config_t config) override; + ConfigManager::config_response check_config_change(device_t device, hardware_config_t config) override; void get_device_name(device_t device, uint8_t (&name)[20]) override; void get_config_name(device_t device, hardware_config_t config, uint8_t (&name)[20]) override; inline bool is_initialized(){return is_initialized_;} diff --git a/comms/mavlink/mavlink.cpp b/comms/mavlink/mavlink.cpp index a20925da..87e96d37 100644 --- a/comms/mavlink/mavlink.cpp +++ b/comms/mavlink/mavlink.cpp @@ -308,6 +308,12 @@ void Mavlink::send_config_info(uint8_t system_id, uint8_t device, uint8_t config mavlink_msg_rosflight_config_info_pack(system_id, 0, &msg, device, config, name); send_message(msg); } +void Mavlink::send_config_status(uint8_t system_id, uint8_t device, bool success, bool reboot_required, uint8_t (&error_message)[50]) +{ + mavlink_message_t msg; + mavlink_msg_rosflight_config_status_pack(system_id, 0, &msg, device, success, reboot_required, error_message); + send_message(msg); +} void Mavlink::send_rc_raw(uint8_t system_id, uint32_t timestamp_ms, const uint16_t channels[8]) { diff --git a/comms/mavlink/mavlink.h b/comms/mavlink/mavlink.h index 8da4cec8..4c478c1b 100644 --- a/comms/mavlink/mavlink.h +++ b/comms/mavlink/mavlink.h @@ -87,6 +87,7 @@ class Mavlink : public CommLinkInterface void send_config_value(uint8_t system_id, uint8_t device, uint8_t config) override; void send_device_info(uint8_t system_id, uint8_t device, uint8_t max_config, uint8_t (&name)[20]) override; void send_config_info(uint8_t system_id, uint8_t device, uint8_t config, uint8_t (&name)[20]) override; + void send_config_status(uint8_t system_id, uint8_t device, bool success, bool reboot_required, uint8_t (&error_message)[50]) 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, float min_range) override; void send_status(uint8_t system_id, diff --git a/include/board_config_manager.h b/include/board_config_manager.h index 807cc3cd..e31736eb 100644 --- a/include/board_config_manager.h +++ b/include/board_config_manager.h @@ -2,18 +2,13 @@ #define BOARD_CONFIG_MANAGER_H #include "configuration_enum.h" +#include "config_manager.h" namespace rosflight_firmware { class BoardConfigManager { public: - typedef struct - { - bool successful; - bool reboot_required; - char error_message[50]; - }config_response; // Get the largest number that is valid for the configuration of a given device // This number is inclusive, i.e. a value of 2 means 0, 1, or 2 are valid @@ -22,7 +17,7 @@ class BoardConfigManager // Check if a config change is allowed. // If the response indicates success, then the config manager accepts the change // If not, the config manager returns the error indicated - virtual config_response check_config_change(device_t device, hardware_config_t config) = 0; + virtual ConfigManager::config_response check_config_change(device_t device, hardware_config_t config) = 0; virtual void get_device_name(device_t device, uint8_t (&name)[20])=0; virtual void get_config_name(device_t device, hardware_config_t config, uint8_t (&name)[20]) = 0; }; diff --git a/include/config_manager.h b/include/config_manager.h index 24d78e29..fec35af1 100644 --- a/include/config_manager.h +++ b/include/config_manager.h @@ -15,10 +15,19 @@ class ConfigManager hardware_config_t config[device_t::device_count]; } config_t; + typedef struct + { + bool successful; + bool reboot_required; + uint8_t error_message[50]; + }config_response; + ConfigManager(ROSflight &RF, config_t &config); bool init(); bool configure_devices(); - void set_configuration(device_t device, uint8_t config); + // Attempts to set a configuration, failing if the board config manager rejects it + config_response attempt_set_configuration(device_t device, uint8_t config); + void set_configuration(device_t device, uint8_t config); // Directly sets a configuration uint8_t get_configuration(device_t device); uint8_t operator[](device_t device); // same as get_configuration, for convenience diff --git a/include/interface/comm_link.h b/include/interface/comm_link.h index b12287ed..1f0043d4 100644 --- a/include/interface/comm_link.h +++ b/include/interface/comm_link.h @@ -164,6 +164,7 @@ class CommLinkInterface virtual void send_config_value(uint8_t system_id, uint8_t device, uint8_t config) = 0; virtual void send_device_info(uint8_t system_id, uint8_t device, uint8_t max_config, uint8_t (&name)[20]) = 0; virtual void send_config_info(uint8_t system_id, uint8_t device, uint8_t config, uint8_t (&name)[20]) = 0; + virtual void send_config_status(uint8_t system_id, uint8_t device, bool success, bool reboot_required, uint8_t (&error_message)[50]) = 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, float min_range) = 0; virtual void send_status(uint8_t system_id, diff --git a/src/comm_manager.cpp b/src/comm_manager.cpp index 78c514e1..ede06b5a 100644 --- a/src/comm_manager.cpp +++ b/src/comm_manager.cpp @@ -411,14 +411,15 @@ void CommManager::heartbeat_callback(void) void CommManager::config_set_callback(uint8_t device, uint8_t configuration) { - //TODO consider checking a system ID to see if the message is intended for this device - if(device < device_count) - RF_.config_manager_.set_configuration(static_cast(device), configuration); + uint8_t requested_device{device}; + if(device >=device_count) + device = device_count; + ConfigManager::config_response resp = RF_.config_manager_.attempt_set_configuration(static_cast(device), configuration); + comm_link_.send_config_status(sysid_, requested_device, resp.successful, resp.reboot_required, resp.error_message); } void CommManager::config_request_callback(uint8_t device) { - //TODO consider checking a system ID to see if the message is intended for this device if(device < device_count) send_config_value(static_cast(device)); if(device == 0xff) diff --git a/src/config_manager.cpp b/src/config_manager.cpp index ef14a966..7d3bd3a0 100644 --- a/src/config_manager.cpp +++ b/src/config_manager.cpp @@ -23,6 +23,13 @@ bool ConfigManager::configure_devices() return success; } +ConfigManager::config_response ConfigManager::attempt_set_configuration(device_t device, uint8_t config) +{ + config_response resp = RF_.board_.get_board_config_manager().check_config_change(device, config); + if(resp.successful) + set_configuration(device, config); + return resp; +} void ConfigManager::set_configuration(device_t device, uint8_t config) { config_.config[device] = config; From 22c8ca8fd0ff3ddadc458836ba1ca7002dffff69 Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Wed, 8 Jan 2020 15:42:16 -0700 Subject: [PATCH 16/76] Add namespace for device enum. Fix some configuration checking --- boards/airbourne/airbourne_board.cpp | 12 +-- .../airbourne_board_config_manager.cpp | 102 ++++++++++-------- .../airbourne_board_config_manager.h | 2 +- include/config_manager.h | 8 +- include/configuration_enum.h | 26 +++-- src/comm_manager.cpp | 10 +- src/config_manager.cpp | 7 +- 7 files changed, 90 insertions(+), 77 deletions(-) diff --git a/boards/airbourne/airbourne_board.cpp b/boards/airbourne/airbourne_board.cpp index 3d500da0..bf88830c 100644 --- a/boards/airbourne/airbourne_board.cpp +++ b/boards/airbourne/airbourne_board.cpp @@ -127,14 +127,14 @@ bool AirbourneBoard::enable_device(device_t device, hardware_config_t configurat { switch(device) { - case serial: + case Configuration::SERIAL: { uint32_t baud_rate = params.get_param_int(PARAM_BAUD_RATE); serial_init(baud_rate, configuration); return true; // TODO serial_init success check break; } - case rc: + case Configuration::RC: switch(configuration) { case 0: @@ -147,7 +147,7 @@ bool AirbourneBoard::enable_device(device_t device, hardware_config_t configurat return false; } return true; - case airspeed: + case Configuration::AIRSPEED: if(configuration==1) { if(!ext_i2c_.is_initialized()) @@ -156,15 +156,15 @@ bool AirbourneBoard::enable_device(device_t device, hardware_config_t configurat } break; //TODO other config options - case gnss: + case Configuration::GNSS: // GNSS is currently disabled break; - case sonar: + case Configuration::SONAR: if(!ext_i2c_.is_initialized()) ext_i2c_.init(&i2c_config[EXTERNAL_I2C]); sonar_.init(&ext_i2c_); break; - case battery_monitor: + case Configuration::BATTERY_MONITOR: if(configuration == 1) { float voltage_multiplier = params.get_param_float(PARAM_BATTERY_VOLTAGE_MULTIPLIER); diff --git a/boards/airbourne/airbourne_board_config_manager.cpp b/boards/airbourne/airbourne_board_config_manager.cpp index 687a60cb..2e21afae 100644 --- a/boards/airbourne/airbourne_board_config_manager.cpp +++ b/boards/airbourne/airbourne_board_config_manager.cpp @@ -17,7 +17,7 @@ void AirbourneBoardConfigManager::init(ROSflight *rf, AirbourneBoard *board) } hardware_config_t AirbourneBoardConfigManager::get_max_config(device_t device) { - if(device >= device_count) + if(device >= Configuration::DEVICE_COUNT) return 0; else return max_configs[device]; @@ -25,11 +25,12 @@ hardware_config_t AirbourneBoardConfigManager::get_max_config(device_t device) ConfigManager::config_response AirbourneBoardConfigManager::check_config_change(device_t device, hardware_config_t config) { ConfigManager::config_response resp; + ConfigManager &cm = RF_->config_manager_; resp.reboot_required = false; resp.successful = false; char *error_message = reinterpret_cast(resp.error_message); - if(device >= device_count) + if(device >= Configuration::DEVICE_COUNT) { strcpy(error_message, "Device not found"); return resp; @@ -40,62 +41,69 @@ ConfigManager::config_response AirbourneBoardConfigManager::check_config_change( strcpy(error_message, "Configuration not found"); return resp; } + if(config == cm[device]) + { + strcpy(error_message, "Configuration already set. No change required"); + resp.successful = true; + resp.reboot_required = false; + return resp; + } + revo_port port = get_port(device, config); - ConfigManager &cm = RF_->config_manager_; - device_t conflict_device = device_count; + device_t conflict_device = Configuration::DEVICE_COUNT; switch(device) { - case rc: - if(config ==0) + case Configuration::RC: + if(config ==0) // PPM is not known to conflict with anything break; - case serial: - case gnss: + case Configuration::SERIAL: + case Configuration::GNSS: if(port != none) - for(device_t other_device{static_cast(0)}; other_device != device_count; ++other_device) - if(other_device != rc && port == get_port(other_device, cm[other_device])) + for(device_t other_device{static_cast(0)}; other_device != Configuration::DEVICE_COUNT; ++other_device) + if(port == get_port(other_device, cm[other_device]) && (other_device !=Configuration::RC || cm[Configuration::RC]!=0)) // RC over PPM does not conflict with UART, even though both use the same port { conflict_device = other_device; break; } break; - case airspeed: - case sonar: - if(cm[gnss] == 3) - conflict_device = gnss; - if(cm[serial] == 3) - conflict_device = serial; + case Configuration::AIRSPEED: + case Configuration::SONAR: + if(cm[Configuration::GNSS] == 3) + conflict_device = Configuration::GNSS; + if(cm[Configuration::SERIAL] == 3) + conflict_device = Configuration::SERIAL; break; default: break; } - if(conflict_device != device_count) + if(conflict_device != Configuration::DEVICE_COUNT) { switch(conflict_device) { - case serial: + case Configuration::SERIAL: strcpy(error_message, "Port is used by serial."); break; - case rc: + case Configuration::RC: strcpy(error_message, "Port is used by RC."); break; - case airspeed: + case Configuration::AIRSPEED: strcpy(error_message, "Port is used by airspeed sensor."); break; - case gnss: + case Configuration::GNSS: strcpy(error_message, "Port is used by GNSS receiver."); break; - case sonar: + case Configuration::SONAR: strcpy(error_message, "Port is used by sonar sensor."); break; // At the time of this writing, the below should not cause issues - case battery_monitor: + case Configuration::BATTERY_MONITOR: strcpy(error_message, "Port is used by battery monitor."); break; - case barometer: + case Configuration::BAROMETER: strcpy(error_message, "Port is used by barometer."); break; - case magnetometer: + case Configuration::MAGNETOMETER: strcpy(error_message, "Port is used by magnetometer."); break; default: @@ -114,28 +122,28 @@ void AirbourneBoardConfigManager::get_device_name(device_t device, uint8_t (&nam char *name_char = reinterpret_cast(name); switch(device) { - case serial: + case Configuration::SERIAL: strcpy(name_char, "Serial"); break; - case rc: + case Configuration::RC: strcpy(name_char, "RC"); break; - case airspeed: + case Configuration::AIRSPEED: strcpy(name_char, "Airspeed"); break; - case gnss: + case Configuration::GNSS: strcpy(name_char, "GNSS"); break; - case sonar: + case Configuration::SONAR: strcpy(name_char, "Sonar"); break; - case battery_monitor: + case Configuration::BATTERY_MONITOR: strcpy(name_char, "Battery Monitor"); break; - case barometer: + case Configuration::BAROMETER: strcpy(name_char, "Barometer"); break; - case magnetometer: + case Configuration::MAGNETOMETER: strcpy(name_char, "Magnetometer"); break; default: @@ -149,7 +157,7 @@ void AirbourneBoardConfigManager::get_config_name(device_t device, hardware_conf const char *name_str; switch(device) { - case serial: + case Configuration::SERIAL: switch(config) { case 0: @@ -166,19 +174,19 @@ void AirbourneBoardConfigManager::get_config_name(device_t device, hardware_conf break; } break; - case rc: + case Configuration::RC: if(config==0) name_str = "PPM on Flex-IO"; else name_str = "SBUS on Main"; break; - case airspeed: + case Configuration::AIRSPEED: if(config==0) name_str = "Disabled"; else name_str = "I2C2 on Flexi"; break; - case gnss: + case Configuration::GNSS: switch(config) { case 0: @@ -195,23 +203,23 @@ void AirbourneBoardConfigManager::get_config_name(device_t device, hardware_conf break; } break; - case sonar: + case Configuration::SONAR: if(config ==0) name_str = "Disabled"; else name_str = "I2C2 on Flexi"; break; - case battery_monitor: + case Configuration::BATTERY_MONITOR: if(config==0) name_str = "Disabled"; else name_str = "ADC3 on Power"; break; - case barometer: + case Configuration::BAROMETER: if(config==0) name_str = "Onboard barometer"; break; - case magnetometer: + case Configuration::MAGNETOMETER: if(config ==0) name_str = "Onboard magnetometer"; break; @@ -226,24 +234,24 @@ AirbourneBoardConfigManager::revo_port AirbourneBoardConfigManager::get_port(uin { switch(device) { - case serial: + case Configuration::SERIAL: if(config == 0) return usb; // break intentionally ommitted - case gnss: + case Configuration::GNSS: return static_cast(config); - case rc: + case Configuration::RC: if(config == 0) return flex_io; if(config == 1) return main; break; - case airspeed: - case sonar: + case Configuration::AIRSPEED: + case Configuration::SONAR: if(config==1) return flexi; break; - case battery_monitor: + case Configuration::BATTERY_MONITOR: if(config == 1) return power; } diff --git a/boards/airbourne/airbourne_board_config_manager.h b/boards/airbourne/airbourne_board_config_manager.h index b5a4eb78..aa9e4025 100644 --- a/boards/airbourne/airbourne_board_config_manager.h +++ b/boards/airbourne/airbourne_board_config_manager.h @@ -30,7 +30,7 @@ class AirbourneBoardConfigManager : public BoardConfigManager bool is_initialized_{false}; AirbourneBoard *board_; ROSflight *RF_; - const hardware_config_t max_configs[device_count]{3, 1, 1, 3, 1, 1, 0, 0}; + const hardware_config_t max_configs[Configuration::DEVICE_COUNT]{3, 1, 1, 3, 1, 1, 0, 0}; }; } // namespace rosflight_firmware diff --git a/include/config_manager.h b/include/config_manager.h index fec35af1..fed847db 100644 --- a/include/config_manager.h +++ b/include/config_manager.h @@ -12,7 +12,7 @@ class ConfigManager typedef struct __attribute__ ((packed)) { uint32_t checksum; - hardware_config_t config[device_t::device_count]; + hardware_config_t config[Configuration::DEVICE_COUNT]; } config_t; typedef struct @@ -24,14 +24,14 @@ class ConfigManager ConfigManager(ROSflight &RF, config_t &config); bool init(); - bool configure_devices(); + bool configure_devices(); // Sends configurations to the board via the enable_device method // Attempts to set a configuration, failing if the board config manager rejects it config_response attempt_set_configuration(device_t device, uint8_t config); - void set_configuration(device_t device, uint8_t config); // Directly sets a configuration + void set_configuration(device_t device, uint8_t config); // Sets a config without checks uint8_t get_configuration(device_t device); uint8_t operator[](device_t device); // same as get_configuration, for convenience - void prepare_write(); + void prepare_write(); // prepares a checksum, so that the config struct can be saved private: ROSflight &RF_; diff --git a/include/configuration_enum.h b/include/configuration_enum.h index 3f98ba21..eba70456 100644 --- a/include/configuration_enum.h +++ b/include/configuration_enum.h @@ -3,29 +3,33 @@ #include +namespace Configuration +{ + enum device_t: uint8_t { - serial, - rc, - airspeed, - gnss, - sonar, - battery_monitor, - barometer, - magnetometer, - device_count // make sure this is last + SERIAL, + RC, + AIRSPEED, + GNSS, + SONAR, + BATTERY_MONITOR, + BAROMETER, + MAGNETOMETER, + DEVICE_COUNT // make sure this is last }; inline device_t& operator++(device_t &dev) { uint8_t return_value = dev; return_value++; - if (return_value > device_count) + if (return_value > DEVICE_COUNT) return_value--; dev = static_cast(return_value); return dev; } +} typedef uint8_t hardware_config_t; - +typedef Configuration::device_t device_t; #endif // CONFIGURATION_ENUM_H diff --git a/src/comm_manager.cpp b/src/comm_manager.cpp index ede06b5a..55049f4b 100644 --- a/src/comm_manager.cpp +++ b/src/comm_manager.cpp @@ -86,7 +86,7 @@ void CommManager::init() offboard_control_time_ = 0; send_params_index_ = PARAMS_COUNT; - send_device_info_index_ = device_count; + send_device_info_index_ = Configuration::DEVICE_COUNT; send_config_info_index_ = 0; update_system_id(PARAM_SYSTEM_ID); @@ -412,15 +412,15 @@ void CommManager::heartbeat_callback(void) void CommManager::config_set_callback(uint8_t device, uint8_t configuration) { uint8_t requested_device{device}; - if(device >=device_count) - device = device_count; + if(device >=Configuration::DEVICE_COUNT) + device = Configuration::DEVICE_COUNT; ConfigManager::config_response resp = RF_.config_manager_.attempt_set_configuration(static_cast(device), configuration); comm_link_.send_config_status(sysid_, requested_device, resp.successful, resp.reboot_required, resp.error_message); } void CommManager::config_request_callback(uint8_t device) { - if(device < device_count) + if(device < Configuration::DEVICE_COUNT) send_config_value(static_cast(device)); if(device == 0xff) send_all_config_info(); @@ -688,7 +688,7 @@ void CommManager::send_next_param(void) void CommManager::send_next_config_info(void) { - if (send_device_info_index_ < device_count) + if (send_device_info_index_ < Configuration::DEVICE_COUNT) { if (send_config_info_index_ == 0) send_device_info(send_device_info_index_); diff --git a/src/config_manager.cpp b/src/config_manager.cpp index 7d3bd3a0..afc1319a 100644 --- a/src/config_manager.cpp +++ b/src/config_manager.cpp @@ -18,8 +18,9 @@ bool ConfigManager::init() bool ConfigManager::configure_devices() { bool success = true; - for(uint8_t device{0}; device < device_t::device_count; device++) - success = success &&RF_.board_.enable_device(static_cast(device), config_.config[device], RF_.params_); + //for(uint8_t device{0}; device < device_t::device_count; device++) + for(device_t device{static_cast(0)}; device < Configuration::DEVICE_COUNT; ++device) + success = success &&RF_.board_.enable_device(device, config_.config[device], RF_.params_); return success; } @@ -64,7 +65,7 @@ void ConfigManager::fill_defaults() } uint32_t ConfigManager::generate_checksum() { - //8 bit fletcher algorithm + //8 bit fletcher algorithm, because we can't assume that the struct is a multiple of 16 bits const uint8_t *config_data = reinterpret_cast(config_.config); uint8_t check_a{0}; uint8_t check_b{0}; From d31b3d3e6115581e57f24209554b6216294ac661 Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Wed, 8 Jan 2020 16:21:53 -0700 Subject: [PATCH 17/76] Code cleanup --- boards/airbourne/airbourne_board.cpp | 1 - .../airbourne_board_config_manager.cpp | 25 ++++++++++--------- .../airbourne_board_config_manager.h | 18 ++++++------- include/board_config_manager.h | 8 ++++-- include/config_manager.h | 11 +++++--- src/comm_manager.cpp | 2 +- src/config_manager.cpp | 1 - 7 files changed, 36 insertions(+), 30 deletions(-) diff --git a/boards/airbourne/airbourne_board.cpp b/boards/airbourne/airbourne_board.cpp index bf88830c..91b0e3c7 100644 --- a/boards/airbourne/airbourne_board.cpp +++ b/boards/airbourne/airbourne_board.cpp @@ -155,7 +155,6 @@ bool AirbourneBoard::enable_device(device_t device, hardware_config_t configurat airspeed_.init(&ext_i2c_); } break; - //TODO other config options case Configuration::GNSS: // GNSS is currently disabled break; diff --git a/boards/airbourne/airbourne_board_config_manager.cpp b/boards/airbourne/airbourne_board_config_manager.cpp index 2e21afae..ba2f2e7e 100644 --- a/boards/airbourne/airbourne_board_config_manager.cpp +++ b/boards/airbourne/airbourne_board_config_manager.cpp @@ -28,7 +28,7 @@ ConfigManager::config_response AirbourneBoardConfigManager::check_config_change( ConfigManager &cm = RF_->config_manager_; resp.reboot_required = false; resp.successful = false; - char *error_message = reinterpret_cast(resp.error_message); + char *error_message = reinterpret_cast(resp.message); // Because it wasn't converting implicitly if(device >= Configuration::DEVICE_COUNT) { @@ -52,6 +52,7 @@ ConfigManager::config_response AirbourneBoardConfigManager::check_config_change( revo_port port = get_port(device, config); device_t conflict_device = Configuration::DEVICE_COUNT; + // While a config may conflict with multiple devices, this will only report one switch(device) { case Configuration::RC: @@ -59,7 +60,7 @@ ConfigManager::config_response AirbourneBoardConfigManager::check_config_change( break; case Configuration::SERIAL: case Configuration::GNSS: - if(port != none) + if(port != NO_PORT) for(device_t other_device{static_cast(0)}; other_device != Configuration::DEVICE_COUNT; ++other_device) if(port == get_port(other_device, cm[other_device]) && (other_device !=Configuration::RC || cm[Configuration::RC]!=0)) // RC over PPM does not conflict with UART, even though both use the same port { @@ -96,7 +97,7 @@ ConfigManager::config_response AirbourneBoardConfigManager::check_config_change( case Configuration::SONAR: strcpy(error_message, "Port is used by sonar sensor."); break; - // At the time of this writing, the below should not cause issues + // At the time of this writing, the below are incapable of conflicts case Configuration::BATTERY_MONITOR: strcpy(error_message, "Port is used by battery monitor."); break; @@ -114,10 +115,10 @@ ConfigManager::config_response AirbourneBoardConfigManager::check_config_change( } resp.successful = true; resp.reboot_required = true; - resp.error_message[0]=0; + resp.message[0]=0; return resp; } -void AirbourneBoardConfigManager::get_device_name(device_t device, uint8_t (&name)[20]) +void AirbourneBoardConfigManager::get_device_name(device_t device, uint8_t (&name)[DEVICE_NAME_LENGTH]) { char *name_char = reinterpret_cast(name); switch(device) @@ -151,7 +152,7 @@ void AirbourneBoardConfigManager::get_device_name(device_t device, uint8_t (&nam break; } } -void AirbourneBoardConfigManager::get_config_name(device_t device, hardware_config_t config, uint8_t (&name)[20]) +void AirbourneBoardConfigManager::get_config_name(device_t device, hardware_config_t config, uint8_t (&name)[CONFIG_NAME_LENGTH]) { char *name_char = reinterpret_cast(name); const char *name_str; @@ -236,25 +237,25 @@ AirbourneBoardConfigManager::revo_port AirbourneBoardConfigManager::get_port(uin { case Configuration::SERIAL: if(config == 0) - return usb; + return USB_PORT; // break intentionally ommitted case Configuration::GNSS: return static_cast(config); case Configuration::RC: if(config == 0) - return flex_io; + return FLEX_IO_PORT; if(config == 1) - return main; + return MAIN_PORT; break; case Configuration::AIRSPEED: case Configuration::SONAR: if(config==1) - return flexi; + return FLEXI_PORT; break; case Configuration::BATTERY_MONITOR: if(config == 1) - return power; + return POWER_PORT; } - return none; + return NO_PORT; } } //rosflight_firmware diff --git a/boards/airbourne/airbourne_board_config_manager.h b/boards/airbourne/airbourne_board_config_manager.h index aa9e4025..49d7a15c 100644 --- a/boards/airbourne/airbourne_board_config_manager.h +++ b/boards/airbourne/airbourne_board_config_manager.h @@ -13,20 +13,20 @@ class AirbourneBoardConfigManager : public BoardConfigManager void init(ROSflight *rf, AirbourneBoard *board); hardware_config_t get_max_config(device_t device) override; ConfigManager::config_response check_config_change(device_t device, hardware_config_t config) override; - void get_device_name(device_t device, uint8_t (&name)[20]) override; - void get_config_name(device_t device, hardware_config_t config, uint8_t (&name)[20]) override; + void get_device_name(device_t device, uint8_t (&name)[DEVICE_NAME_LENGTH]) override; + void get_config_name(device_t device, hardware_config_t config, uint8_t (&name)[CONFIG_NAME_LENGTH]) override; inline bool is_initialized(){return is_initialized_;} private: enum revo_port { - none, - main, - flex_io, - flexi, - usb, - power + NO_PORT, + MAIN_PORT, + FLEX_IO_PORT, + FLEXI_PORT, + USB_PORT, + POWER_PORT // the port labeled "PWR / SONAR" is not to provide power, but rather for a battery monitor }; - revo_port get_port(uint8_t device, uint8_t config); + revo_port get_port(uint8_t device, uint8_t config); // Get the port used by a given configuration bool is_initialized_{false}; AirbourneBoard *board_; ROSflight *RF_; diff --git a/include/board_config_manager.h b/include/board_config_manager.h index e31736eb..ffb16a7d 100644 --- a/include/board_config_manager.h +++ b/include/board_config_manager.h @@ -17,9 +17,13 @@ class BoardConfigManager // Check if a config change is allowed. // If the response indicates success, then the config manager accepts the change // If not, the config manager returns the error indicated + // Implementations should not assume that either the device or the config are valid virtual ConfigManager::config_response check_config_change(device_t device, hardware_config_t config) = 0; - virtual void get_device_name(device_t device, uint8_t (&name)[20])=0; - virtual void get_config_name(device_t device, hardware_config_t config, uint8_t (&name)[20]) = 0; + static const int DEVICE_NAME_LENGTH{20}; + static const int CONFIG_NAME_LENGTH{20}; + // Do not assume that the device or config are valid numbers + virtual void get_device_name(device_t device, uint8_t (&name)[DEVICE_NAME_LENGTH])=0; + virtual void get_config_name(device_t device, hardware_config_t config, uint8_t (&name)[CONFIG_NAME_LENGTH]) = 0; }; } // namespace rosflight_firmware diff --git a/include/config_manager.h b/include/config_manager.h index fed847db..37e437bf 100644 --- a/include/config_manager.h +++ b/include/config_manager.h @@ -23,11 +23,14 @@ class ConfigManager }config_response; ConfigManager(ROSflight &RF, config_t &config); + // Reads the memory, and loads defaults if it is invalid. Call after the memory manager is ready bool init(); bool configure_devices(); // Sends configurations to the board via the enable_device method // Attempts to set a configuration, failing if the board config manager rejects it config_response attempt_set_configuration(device_t device, uint8_t config); - void set_configuration(device_t device, uint8_t config); // Sets a config without checks + // Sets a config without checks. This may cause an invalid configuration combo, + // so attempt_set_configuration is recommended + void set_configuration(device_t device, uint8_t config); uint8_t get_configuration(device_t device); uint8_t operator[](device_t device); // same as get_configuration, for convenience @@ -36,9 +39,9 @@ class ConfigManager private: ROSflight &RF_; config_t &config_; - bool read(); - void fill_defaults(); - uint32_t generate_checksum(); + bool read(); // currently just checks that the memory manager is ready and the checksum is correct + void fill_defaults(); // Default values are 0, by convention + uint32_t generate_checksum(); // Based off of fletcher algorithm }; } diff --git a/src/comm_manager.cpp b/src/comm_manager.cpp index 55049f4b..4df85a2e 100644 --- a/src/comm_manager.cpp +++ b/src/comm_manager.cpp @@ -415,7 +415,7 @@ void CommManager::config_set_callback(uint8_t device, uint8_t configuration) if(device >=Configuration::DEVICE_COUNT) device = Configuration::DEVICE_COUNT; ConfigManager::config_response resp = RF_.config_manager_.attempt_set_configuration(static_cast(device), configuration); - comm_link_.send_config_status(sysid_, requested_device, resp.successful, resp.reboot_required, resp.error_message); + comm_link_.send_config_status(sysid_, requested_device, resp.successful, resp.reboot_required, resp.message); } void CommManager::config_request_callback(uint8_t device) diff --git a/src/config_manager.cpp b/src/config_manager.cpp index afc1319a..065be0ca 100644 --- a/src/config_manager.cpp +++ b/src/config_manager.cpp @@ -18,7 +18,6 @@ bool ConfigManager::init() bool ConfigManager::configure_devices() { bool success = true; - //for(uint8_t device{0}; device < device_t::device_count; device++) for(device_t device{static_cast(0)}; device < Configuration::DEVICE_COUNT; ++device) success = success &&RF_.board_.enable_device(device, config_.config[device], RF_.params_); return success; From f8bda6b6de1b47a5ba642c5a1d407ebca158660b Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Wed, 8 Jan 2020 17:31:35 -0700 Subject: [PATCH 18/76] Start documentation for configurations --- include/board_config_manager.h | 2 ++ include/config_manager.h | 3 ++- mkdocs.yml | 1 + 3 files changed, 5 insertions(+), 1 deletion(-) diff --git a/include/board_config_manager.h b/include/board_config_manager.h index ffb16a7d..9021475b 100644 --- a/include/board_config_manager.h +++ b/include/board_config_manager.h @@ -22,6 +22,8 @@ class BoardConfigManager static const int DEVICE_NAME_LENGTH{20}; static const int CONFIG_NAME_LENGTH{20}; // Do not assume that the device or config are valid numbers + // When passed an invalid argument, it is better to return a string so indicating than an + // empty string virtual void get_device_name(device_t device, uint8_t (&name)[DEVICE_NAME_LENGTH])=0; virtual void get_config_name(device_t device, hardware_config_t config, uint8_t (&name)[CONFIG_NAME_LENGTH]) = 0; }; diff --git a/include/config_manager.h b/include/config_manager.h index 37e437bf..a704b082 100644 --- a/include/config_manager.h +++ b/include/config_manager.h @@ -15,11 +15,12 @@ class ConfigManager hardware_config_t config[Configuration::DEVICE_COUNT]; } config_t; + static const int CONFIG_RESPONSE_MESSAGE_LENGTH{50}; typedef struct { bool successful; bool reboot_required; - uint8_t error_message[50]; + uint8_t message[CONFIG_RESPONSE_MESSAGE_LENGTH]; // Primarily for error messages }config_response; ConfigManager(ROSflight &RF, config_t &config); diff --git a/mkdocs.yml b/mkdocs.yml index 3a9015e5..ee71fe10 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -52,6 +52,7 @@ nav: - Flight Controller Setup: user-guide/flight-controller-setup.md - RC Configuration: user-guide/rc-configuration.md - ROS Setup: user-guide/ros-setup.md + - Firmware Configuration: user-guide/firmware-configuration.md - Parameter Configuration: user-guide/parameter-configuration.md - Pre-Flight Checks: user-guide/preflight-checks.md - Improving Performance: user-guide/performance.md From 97ffe7882218373c807a5f928181e7b627f55f0d Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Thu, 9 Jan 2020 10:38:09 -0700 Subject: [PATCH 19/76] add documentation for configurations --- docs/user-guide/firmware-configuration.md | 104 +++++++++++++++++++++ docs/user-guide/parameter-configuration.md | 7 +- 2 files changed, 109 insertions(+), 2 deletions(-) create mode 100644 docs/user-guide/firmware-configuration.md diff --git a/docs/user-guide/firmware-configuration.md b/docs/user-guide/firmware-configuration.md new file mode 100644 index 00000000..215fe295 --- /dev/null +++ b/docs/user-guide/firmware-configuration.md @@ -0,0 +1,104 @@ +# Firmware Configuration + +The most recent versions of ROSflight allow you to specify the hardware setup of your aircraft in greater detail. These settings are dependant on your choice of flight controller. ROSflight does not support this feature on the Naze/Flip32. + +For each of a number of devices, there are several choices of configuration. Such configurations may specify the port, the protocol, or other settings for using the device. Many devices can be disabled entirely. Some devices may also use parameters. + +## Getting Current Configurations +The current configuration for a device can be read through the ROS service `config_get`, which requires `rosflight_io` to be running. The only parameter for the service is the name of the device. The name is case insensitive, and underscores may be used for spaces. For example, all of the following are valid: +``` +rosservice call /config_get gnss +rosservice call /config_get "Battery Monitor" +rosservice call /config_get battery_monitor +``` +Here is an example of a command and the response +``` +$ rosservice call /config_get battery_monitor +successful: True +configuration: "ADC3 on Power" +message: '' +``` + +##Setting Configurations +Setting configurations is done similarly to getting them, but using the `config_set` service, which also requires `rosflight_io` to be running. The service takes the name of the device and the name of the configuration as parameters. The service is flexible in the name of the configuration. It is case insensitive and will recognize configurations from most single words in the name. For example the configuration `ADC3 on Power` may be called `adc3` or `power`. The keyword `default` refers to the default configuration for a device. In addition, the number for the configuration may be used. However, due to a quirk in rosservice, this must be prefaced with `!!str` and enclosed in single quotes when calling from the command line. All of the following examples are valid, and the first four are equivalent. + +``` +rosservice call /config_set gnss "UART1 on Main" +rosservice call /config_set gnss uart1 +rosservice call /config_set gnss main +rosservice call /config_set gnss '!!str 1' +rosservice call /config_set airspeed disabled +rosservice call /config_set serial default +``` + +The service response indicates whether the configuration was successfully set and if a reboot is required. A short message may also be included. When the service fails to set a configuration, it is usually because the firmware found a conflict with another, existing configuration (e.g. putting two things on the same port). The service may also fail if a configuration or device does not exist. An example of the response from setting a configuration is shown below. + +``` +$ rosservice call /config_set gnss flexi +successful: False +reboot_required: False +message: "Port is used by airspeed sensor." +``` +This response indicates that the configuration could not be set because the airspeed sensor uses the same port. It also indicates that no reboot is needed (in this case, because the configuration was not changed). + +##Saving Configurations +Configurations are saved by calling the `settings_write` service. This service also saves parameters. +##Configurations +Available devices and configurations are dependent on the flight controller. The Naze/Flip32 does not support changing configurations. +###OpenPilot Revolution (Revo) Configurations +####Serial +The serial connection is used to communicate with `rosflight_io`. Change with caution, because doing so may make it difficult to change back. + +| Configuration | Number | port | Notes | +| ------------- | ------ | ---- | ----- | +| VCP over USB | 0 |Micro USB| +| UART1 on Main|1| Main| +| UART2 on Flex-IO|2| Flex-IO| +| UART3 on Flexi|3| Flexi| + +####RC +| Configuration | Number | port | Notes | +| ------------- | ------ | ---- | ----- | +| PPM on Flex-IO|0|Flex-io|Does not conflict with UART on the Flex-IO port| +|SBUS on Main|1|Main|| + +####Airspeed +| Configuration | Number | port | Notes | +| ------------- | ------ | ---- | ----- | +|Disabled|0|None|| +|I2C2 on Flexi|1|Flexi|| + +####GNSS + +| Configuration | Number | port | Notes | +| ------------- | ------ | ---- | ----- | +|Disabled|0|None|| +|UART1 on Main|1|Main|| +|UART2 on Flexi-IO|2|Flex-IO|| +|UART3 on Flexi|3|Flexi|| + +####Sonar +| Configuration | Number | port | Notes | +| ------------- | ------ | ---- | ----- | +|Disabled|0|None|| +|I2C2 on Flexi|1|Flexi|| + +####Battery Montior +| Configuration | Number | port | Notes | +| ------------- | ------ | ---- | ----- | +|Disabled|0|None|| +|ADC3 on Power|1|PWR / Sonar|| + +####Barometer +Future versions of ROSflight may support more options with barometers + +| Configuration | Number | port | Notes | +| ------------- | ------ | ---- | ----- | +|Onboard Barometer|0|None|| + +####Magnetometer +Future versions of ROSflight may support more options with magnetometers + +| Configuration | Number | port | Notes | +| ------------- | ------ | ---- | ----- | +|Onboard Magnetometer|0|None|| diff --git a/docs/user-guide/parameter-configuration.md b/docs/user-guide/parameter-configuration.md index 62d223ea..ee3ae408 100644 --- a/docs/user-guide/parameter-configuration.md +++ b/docs/user-guide/parameter-configuration.md @@ -45,17 +45,20 @@ Notice that the parameters have been set, but not saved. Parameter changes take ### Writing Parameters -To ensure that parameter values persist between reboots, you must write the parameters to the non-volatile memory. This is done by calling `param_write` +To ensure that parameter values persist between reboots, you must write the parameters to the non-volatile memory. This is done by calling `settings_write`. This also saves firmware configurations. ``` -rosservice call /param_write +rosservice call /settings_write ``` + `rosflight_io` should then respond with + ``` [ INFO] [1491672597.123201952]: Param write succeeded [ INFO] [1491672597.123452908]: Onboard parameters have been saved ``` + !!! error Parameter writing can only happen if the flight controller is disarmed. If the param write failed for some reason, you may want to make sure your FC is disarmed and try again. From b5f82bb0f607e962bd8e4dc9cdb90fe8930b51f2 Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Thu, 9 Jan 2020 14:43:01 -0700 Subject: [PATCH 20/76] Add implementations for breezy board and test board to pass tests --- .../airbourne_board_config_manager.cpp | 2 +- boards/breezy/Makefile | 3 +- boards/breezy/breezy_board.cpp | 22 ++++++++++- boards/breezy/breezy_board.h | 10 ++++- boards/breezy/breezy_board_config_manager.cpp | 36 ++++++++++++++++++ boards/breezy/breezy_board_config_manager.h | 18 +++++++++ test/CMakeLists.txt | 3 ++ test/test_board.cpp | 20 +++++++++- test/test_board.h | 9 ++++- test/test_board_config_manager.cpp | 37 +++++++++++++++++++ test/test_board_config_manager.h | 18 +++++++++ 11 files changed, 171 insertions(+), 7 deletions(-) create mode 100644 boards/breezy/breezy_board_config_manager.cpp create mode 100644 boards/breezy/breezy_board_config_manager.h create mode 100644 test/test_board_config_manager.cpp create mode 100644 test/test_board_config_manager.h diff --git a/boards/airbourne/airbourne_board_config_manager.cpp b/boards/airbourne/airbourne_board_config_manager.cpp index ba2f2e7e..25deabfd 100644 --- a/boards/airbourne/airbourne_board_config_manager.cpp +++ b/boards/airbourne/airbourne_board_config_manager.cpp @@ -155,7 +155,7 @@ void AirbourneBoardConfigManager::get_device_name(device_t device, uint8_t (&nam void AirbourneBoardConfigManager::get_config_name(device_t device, hardware_config_t config, uint8_t (&name)[CONFIG_NAME_LENGTH]) { char *name_char = reinterpret_cast(name); - const char *name_str; + const char *name_str = "Invalid Config"; switch(device) { case Configuration::SERIAL: diff --git a/boards/breezy/Makefile b/boards/breezy/Makefile index de797f03..c64a2f69 100644 --- a/boards/breezy/Makefile +++ b/boards/breezy/Makefile @@ -72,7 +72,8 @@ STDPERIPH_SRC = $(notdir $(wildcard $(STDPERIPH_DIR)/src/*.c)) VPATH := $(VPATH):$(BOARD_DIR) BOARD_C_SRC = flash.c BOARD_CXX_SRC = breezy_board.cpp \ - main.cpp + main.cpp \ + breezy_board_config_manager.cpp # Hardware Driver Source Files BREEZY_SRC = drv_gpio.c \ diff --git a/boards/breezy/breezy_board.cpp b/boards/breezy/breezy_board.cpp index e405a2b6..99714463 100644 --- a/boards/breezy/breezy_board.cpp +++ b/boards/breezy/breezy_board.cpp @@ -81,9 +81,9 @@ void BreezyBoard::clock_delay(uint32_t milliseconds) // serial -void BreezyBoard::serial_init(uint32_t baud_rate, uint32_t dev) +void BreezyBoard::serial_init(uint32_t baud_rate, hardware_config_t configuration) { - (void)dev; + (void)configuration; Serial1 = uartOpen(USART1, NULL, baud_rate, MODE_RXTX); } @@ -110,6 +110,24 @@ void BreezyBoard::serial_flush() return; } +bool BreezyBoard::enable_device(device_t device, hardware_config_t configuration, const Params ¶ms) +{ + (void)device; + (void)configuration; + (void)params; + return true; +} + +void BreezyBoard::init_board_config_manager(ROSflight *rf) +{ + (void)rf; +} + +BreezyBoardConfigManager &BreezyBoard::get_board_config_manager() +{ + return config_manager_; +} + // sensors void BreezyBoard::sensors_init() diff --git a/boards/breezy/breezy_board.h b/boards/breezy/breezy_board.h index ec7f0a66..7085b478 100644 --- a/boards/breezy/breezy_board.h +++ b/boards/breezy/breezy_board.h @@ -43,6 +43,8 @@ extern "C" #include "board.h" #include "sensors.h" +#include "configuration_enum.h" +#include "breezy_board_config_manager.h" namespace rosflight_firmware { @@ -79,6 +81,7 @@ class BreezyBoard : public Board bool new_imu_data_; uint64_t imu_time_us_; + BreezyBoardConfigManager config_manager_; public: BreezyBoard(); @@ -93,12 +96,17 @@ class BreezyBoard : public Board void clock_delay(uint32_t milliseconds) override; // serial - void serial_init(uint32_t baud_rate, uint32_t dev) override; + void serial_init(uint32_t baud_rate, hardware_config_t configuration) 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; + // hardware config + bool enable_device(device_t device, hardware_config_t configuration, const Params ¶ms) override; + void init_board_config_manager(ROSflight *rf) override; + BreezyBoardConfigManager & get_board_config_manager() override; + // sensors void sensors_init() override; uint16_t num_sensor_errors() override; diff --git a/boards/breezy/breezy_board_config_manager.cpp b/boards/breezy/breezy_board_config_manager.cpp new file mode 100644 index 00000000..b6be8570 --- /dev/null +++ b/boards/breezy/breezy_board_config_manager.cpp @@ -0,0 +1,36 @@ +#include "breezy_board_config_manager.h" +#include + +namespace rosflight_firmware +{ +hardware_config_t BreezyBoardConfigManager::get_max_config(device_t device) +{ + (void)device; + return 0; +} + +ConfigManager::config_response BreezyBoardConfigManager::check_config_change(device_t device, hardware_config_t config) +{ + (void)device; + (void)config; + ConfigManager::config_response response; + response.successful = false; + response.reboot_required = false; + strcpy(reinterpret_cast(response.message), "Feature unsupported on naze"); + return response; +} + +void BreezyBoardConfigManager::get_device_name(device_t device, uint8_t (&name)[20]) +{ + (void)device; + strcpy(reinterpret_cast(name), "Unsupported"); +} + +void BreezyBoardConfigManager::get_config_name(device_t device, hardware_config_t config, uint8_t (&name)[20]) +{ + (void)device; + (void)config; + strcpy(reinterpret_cast(name), "Unsupported"); +} + +} // namespace rosflight_firmware diff --git a/boards/breezy/breezy_board_config_manager.h b/boards/breezy/breezy_board_config_manager.h new file mode 100644 index 00000000..dcbd00ce --- /dev/null +++ b/boards/breezy/breezy_board_config_manager.h @@ -0,0 +1,18 @@ +#ifndef BREEZYBOARDCONFIGMANAGER_H +#define BREEZYBOARDCONFIGMANAGER_H + +#include "board_config_manager.h" + +namespace rosflight_firmware +{ +class BreezyBoardConfigManager : public BoardConfigManager +{ +public: + hardware_config_t get_max_config(device_t device) override; + ConfigManager::config_response check_config_change(device_t device, hardware_config_t config) override; + void get_device_name(device_t device, uint8_t (&name)[20]) override; + void get_config_name(device_t device, hardware_config_t config, uint8_t (&name)[20]) override; +}; +} // namespace rosflight_firmware + +#endif // BREEZYBOARDCONFIGMANAGER_H diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 57486408..2a90589e 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -39,6 +39,8 @@ set(ROSFLIGHT_SRC ../src/command_manager.cpp ../src/rc.cpp ../src/mixer.cpp + ../src/memory_manager.cpp + ../src/config_manager.cpp ../comms/mavlink/mavlink.cpp ../lib/turbomath/turbomath.cpp ) @@ -51,6 +53,7 @@ add_executable(unit_tests common.cpp command_manager_test.cpp test_board.cpp + test_board_config_manager.cpp turbotrig_test.cpp state_machine_test.cpp command_manager_test.cpp diff --git a/test/test_board.cpp b/test/test_board.cpp index 298f43ca..969b3bc2 100644 --- a/test/test_board.cpp +++ b/test/test_board.cpp @@ -80,12 +80,30 @@ 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_init(uint32_t baud_rate, hardware_config_t configuration) {} 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() {} +// Hardware config +bool testBoard::enable_device(device_t device, hardware_config_t configuration, const Params ¶ms) +{ + (void)device; + (void)configuration; + (void)params; + return true; +} + +void testBoard::init_board_config_manager(ROSflight *rf) +{ + (void)rf; +} + +TestBoardConfigManager &testBoard::get_board_config_manager() +{ + return config_manager_; +} // sensors void testBoard::sensors_init() {} uint16_t testBoard::num_sensor_errors() { return 0; } diff --git a/test/test_board.h b/test/test_board.h index 21a968d5..733c883e 100644 --- a/test/test_board.h +++ b/test/test_board.h @@ -34,6 +34,7 @@ #include "board.h" #include "sensors.h" +#include "test_board_config_manager.h" namespace rosflight_firmware { @@ -50,6 +51,7 @@ class testBoard : public Board bool new_imu_ = false; static constexpr size_t BACKUP_MEMORY_SIZE{1024}; uint8_t backup_memory_[BACKUP_MEMORY_SIZE]; + TestBoardConfigManager config_manager_; public: // setup @@ -62,12 +64,17 @@ class testBoard : public Board void clock_delay(uint32_t milliseconds) override; // serial - void serial_init(uint32_t baud_rate, uint32_t dev) override; + void serial_init(uint32_t baud_rate, hardware_config_t configuration) 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; +// Hardware config + bool enable_device(device_t device, hardware_config_t configuration, const Params ¶ms) override; + void init_board_config_manager(ROSflight *rf) override; + TestBoardConfigManager & get_board_config_manager() override; + // sensors void sensors_init() override; uint16_t num_sensor_errors() ; diff --git a/test/test_board_config_manager.cpp b/test/test_board_config_manager.cpp new file mode 100644 index 00000000..e11a9a73 --- /dev/null +++ b/test/test_board_config_manager.cpp @@ -0,0 +1,37 @@ +#include "test_board_config_manager.h" +#include +#include + +namespace rosflight_firmware +{ +hardware_config_t TestBoardConfigManager::get_max_config(device_t device) +{ + (void)device; + return 0; // This is not needed to test other software +} +ConfigManager::config_response TestBoardConfigManager::check_config_change(device_t device, hardware_config_t config) +{ + // A couple variations are given for testing + ConfigManager::config_response response; + response.successful = true; + response.reboot_required = true; + if(device == Configuration::SERIAL && config == 1) + { + response.successful = false; + strcpy(reinterpret_cast(response.message), "Fail for testing"); + return response; + } + strcpy(reinterpret_cast(response.message), "Succeed for testing"); + return response; +} +void TestBoardConfigManager::get_device_name(device_t device, uint8_t (&name)[20]) +{ + std::string device_name = "device #" + std::to_string(static_cast(device)); + strcpy(reinterpret_cast(name), device_name.c_str()); +} +void TestBoardConfigManager::get_config_name(device_t device, hardware_config_t config, uint8_t (&name)[20]) +{ + std::string config_name = "config " + std::to_string(static_cast(device)) + ","+std::to_string(static_cast(config)); + strcpy(reinterpret_cast(name), config_name.c_str()); +} +} diff --git a/test/test_board_config_manager.h b/test/test_board_config_manager.h new file mode 100644 index 00000000..d59e44c6 --- /dev/null +++ b/test/test_board_config_manager.h @@ -0,0 +1,18 @@ +#ifndef TESTBOARDCONFIGMANAGER_H +#define TESTBOARDCONFIGMANAGER_H + +#include "board_config_manager.h" + +namespace rosflight_firmware +{ +class TestBoardConfigManager : public BoardConfigManager +{ +public: + hardware_config_t get_max_config(device_t device) override; + ConfigManager::config_response check_config_change(device_t device, hardware_config_t config) override; + void get_device_name(device_t device, uint8_t (&name)[20]) override; + void get_config_name(device_t device, hardware_config_t config, uint8_t (&name)[20]) override; +}; + +} // rosflight_firmware +#endif // TESTBOARDCONFIGMANAGER_H From f7bde6f7c80a62fd050f82c468e6b4eee4d335ba Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Thu, 9 Jan 2020 15:22:25 -0700 Subject: [PATCH 21/76] Add test for config manager --- test/CMakeLists.txt | 1 + test/config_manager_test.cpp | 59 ++++++++++++++++++++++++++++++ test/test_board.cpp | 5 ++- test/test_board_config_manager.cpp | 1 + 4 files changed, 65 insertions(+), 1 deletion(-) create mode 100644 test/config_manager_test.cpp diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 2a90589e..09c80725 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -59,5 +59,6 @@ add_executable(unit_tests command_manager_test.cpp estimator_test.cpp parameters_test.cpp + config_manager_test.cpp ) target_link_libraries(unit_tests ${GTEST_LIBRARIES} pthread) diff --git a/test/config_manager_test.cpp b/test/config_manager_test.cpp new file mode 100644 index 00000000..8b65bef2 --- /dev/null +++ b/test/config_manager_test.cpp @@ -0,0 +1,59 @@ +#include +#include + +#include "rosflight.h" +#include "mavlink.h" +#include "test_board.h" +#include "configuration_enum.h" +#include "config_manager.h" + +using namespace rosflight_firmware; + +class ConfigManagerTest : public ::testing::Test +{ +public: + testBoard board; + Mavlink mavlink; + ROSflight rf; + + ConfigManagerTest() : + mavlink(board), + rf(board, mavlink) {} + void SetUp() override + { + rf.init(); + } +}; + +TEST_F(ConfigManagerTest, DefaultValues) +{ + for(device_t device{static_cast(0)}; device < Configuration::DEVICE_COUNT; ++device) + EXPECT_EQ(rf.config_manager_[device], 0); +} + +TEST_F(ConfigManagerTest, SetValid) +{ + device_t changed_device = Configuration::SERIAL; + hardware_config_t config = 27; + ConfigManager::config_response response = rf.config_manager_.attempt_set_configuration(changed_device, config); + EXPECT_TRUE(response.successful); + EXPECT_TRUE(response.reboot_required); + EXPECT_EQ(std::string(reinterpret_cast(response.message)), "Succeed for testing"); + for(device_t device{static_cast(0)}; device < Configuration::DEVICE_COUNT; ++device) + if(device == changed_device) + EXPECT_EQ(rf.config_manager_[device], config); + else + EXPECT_EQ(rf.config_manager_[device], 0); +} + +TEST_F(ConfigManagerTest, SetInvalid) +{ + device_t changed_device = Configuration::SERIAL; + hardware_config_t config = 1; + ConfigManager::config_response response = rf.config_manager_.attempt_set_configuration(changed_device, config); + EXPECT_FALSE(response.successful); + EXPECT_FALSE(response.reboot_required); + EXPECT_EQ(std::string(reinterpret_cast(response.message)), "Fail for testing"); + for(device_t device{static_cast(0)}; device < Configuration::DEVICE_COUNT; ++device) + EXPECT_EQ(rf.config_manager_[device], 0); +} diff --git a/test/test_board.cpp b/test/test_board.cpp index 969b3bc2..2b58968a 100644 --- a/test/test_board.cpp +++ b/test/test_board.cpp @@ -226,7 +226,10 @@ 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_read(void *dest, size_t len) { + memset(dest, 0, len); + return true; +} bool testBoard::memory_write(const void *src, size_t len) { return false; } // LEDs diff --git a/test/test_board_config_manager.cpp b/test/test_board_config_manager.cpp index e11a9a73..94d41ee7 100644 --- a/test/test_board_config_manager.cpp +++ b/test/test_board_config_manager.cpp @@ -18,6 +18,7 @@ ConfigManager::config_response TestBoardConfigManager::check_config_change(devic if(device == Configuration::SERIAL && config == 1) { response.successful = false; + response.reboot_required = false; strcpy(reinterpret_cast(response.message), "Fail for testing"); return response; } From 0f76e4c8e49a4a1f0b7e2cd79f5b0fb0fddd136e Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Thu, 9 Jan 2020 15:51:47 -0700 Subject: [PATCH 22/76] Update astylerc --- .astylerc | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/.astylerc b/.astylerc index b19af1a5..db8bb785 100644 --- a/.astylerc +++ b/.astylerc @@ -7,3 +7,9 @@ align-pointer=name align-reference=name max-code-length=120 suffix=none +keep-one-line-blocks +lineend=linux +exclude=boards/airbourne/airbourne +exclude=boards/breezy/breezystm32 +exclude=comms/mavlink/v1.0 +exclude=lib From 9baf8e14056e9c2fdfa41401b6bccedef36b0843 Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Fri, 10 Jan 2020 14:33:15 -0700 Subject: [PATCH 23/76] Add support for no mag or baro --- boards/airbourne/airbourne_board.cpp | 23 ++++++++++++------- .../airbourne_board_config_manager.cpp | 9 ++++++++ .../airbourne_board_config_manager.h | 5 ++-- docs/user-guide/firmware-configuration.md | 8 +++++-- src/config_manager.cpp | 2 +- 5 files changed, 34 insertions(+), 13 deletions(-) diff --git a/boards/airbourne/airbourne_board.cpp b/boards/airbourne/airbourne_board.cpp index 91b0e3c7..f327e65b 100644 --- a/boards/airbourne/airbourne_board.cpp +++ b/boards/airbourne/airbourne_board.cpp @@ -44,12 +44,8 @@ void AirbourneBoard::init_board() led2_.init(LED2_GPIO, LED2_PIN); led1_.init(LED1_GPIO, LED1_PIN); - int_i2c_.init(&i2c_config[BARO_I2C]); - //ext_i2c_.init(&i2c_config[EXTERNAL_I2C]); spi1_.init(&spi_config[MPU6000_SPI]); spi3_.init(&spi_config[FLASH_SPI]); - //uart1_.init(&uart_config[UART1], 115200, UART::MODE_8N1); - //uart3_.init(&uart_config[UART3], 115200, UART::MODE_8N1); backup_sram_init(); } @@ -172,6 +168,21 @@ bool AirbourneBoard::enable_device(device_t device, hardware_config_t configurat battery_monitor_.init(battery_monitor_config, &battery_adc_, voltage_multiplier, current_multiplier); } break; + case Configuration::BAROMETER: + if(configuration == 1) + { + if(!int_i2c_.is_initialized()) + int_i2c_.init(&i2c_config[BARO_I2C]); + baro_.init(&int_i2c_); + } + break; + case Configuration::MAGNETOMETER: + if(configuration == 1) + { + if(!int_i2c_.is_initialized()) + int_i2c_.init(&i2c_config[BARO_I2C]); + mag_.init(&int_i2c_); + } default: return false; } @@ -193,10 +204,6 @@ void AirbourneBoard::sensors_init() { while (millis() < 50) {} // wait for sensors to boot up imu_.init(&spi1_); - - baro_.init(&int_i2c_); - mag_.init(&int_i2c_); - // Most sensors are set up through the configuration manager } diff --git a/boards/airbourne/airbourne_board_config_manager.cpp b/boards/airbourne/airbourne_board_config_manager.cpp index 25deabfd..ac61d99c 100644 --- a/boards/airbourne/airbourne_board_config_manager.cpp +++ b/boards/airbourne/airbourne_board_config_manager.cpp @@ -218,10 +218,14 @@ void AirbourneBoardConfigManager::get_config_name(device_t device, hardware_conf break; case Configuration::BAROMETER: if(config==0) + name_str = "Disabled"; + else name_str = "Onboard barometer"; break; case Configuration::MAGNETOMETER: if(config ==0) + name_str = "Disabled"; + else name_str = "Onboard magnetometer"; break; default: @@ -255,6 +259,11 @@ AirbourneBoardConfigManager::revo_port AirbourneBoardConfigManager::get_port(uin case Configuration::BATTERY_MONITOR: if(config == 1) return POWER_PORT; + break; + case Configuration::MAGNETOMETER: + case Configuration::BAROMETER: + if(config == 1) + return INTERNAL_I2C; } return NO_PORT; } diff --git a/boards/airbourne/airbourne_board_config_manager.h b/boards/airbourne/airbourne_board_config_manager.h index 49d7a15c..8e605956 100644 --- a/boards/airbourne/airbourne_board_config_manager.h +++ b/boards/airbourne/airbourne_board_config_manager.h @@ -24,13 +24,14 @@ class AirbourneBoardConfigManager : public BoardConfigManager FLEX_IO_PORT, FLEXI_PORT, USB_PORT, - POWER_PORT // the port labeled "PWR / SONAR" is not to provide power, but rather for a battery monitor + POWER_PORT, // the port labeled "PWR / SONAR" is not to provide power, but rather for a battery monitor + INTERNAL_I2C }; revo_port get_port(uint8_t device, uint8_t config); // Get the port used by a given configuration bool is_initialized_{false}; AirbourneBoard *board_; ROSflight *RF_; - const hardware_config_t max_configs[Configuration::DEVICE_COUNT]{3, 1, 1, 3, 1, 1, 0, 0}; + const hardware_config_t max_configs[Configuration::DEVICE_COUNT]{3, 1, 1, 3, 1, 1, 1, 1}; }; } // namespace rosflight_firmware diff --git a/docs/user-guide/firmware-configuration.md b/docs/user-guide/firmware-configuration.md index 215fe295..acdcf9b1 100644 --- a/docs/user-guide/firmware-configuration.md +++ b/docs/user-guide/firmware-configuration.md @@ -45,6 +45,8 @@ This response indicates that the configuration could not be set because the airs Configurations are saved by calling the `settings_write` service. This service also saves parameters. ##Configurations Available devices and configurations are dependent on the flight controller. The Naze/Flip32 does not support changing configurations. + +For all boards, the default configuration is number 0. ###OpenPilot Revolution (Revo) Configurations ####Serial The serial connection is used to communicate with `rosflight_io`. Change with caution, because doing so may make it difficult to change back. @@ -94,11 +96,13 @@ Future versions of ROSflight may support more options with barometers | Configuration | Number | port | Notes | | ------------- | ------ | ---- | ----- | -|Onboard Barometer|0|None|| +|Disabled|0|None|| +|Onboard Barometer|1|None|| ####Magnetometer Future versions of ROSflight may support more options with magnetometers | Configuration | Number | port | Notes | | ------------- | ------ | ---- | ----- | -|Onboard Magnetometer|0|None|| +|Disabled|0|None|| +|Onboard Magnetometer|1|None|| diff --git a/src/config_manager.cpp b/src/config_manager.cpp index 065be0ca..b1095490 100644 --- a/src/config_manager.cpp +++ b/src/config_manager.cpp @@ -19,7 +19,7 @@ bool ConfigManager::configure_devices() { bool success = true; for(device_t device{static_cast(0)}; device < Configuration::DEVICE_COUNT; ++device) - success = success &&RF_.board_.enable_device(device, config_.config[device], RF_.params_); + success = RF_.board_.enable_device(device, config_.config[device], RF_.params_) && success; return success; } From 0ff9180ee2af408e3752fa4332ab115352ea4d5a Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Fri, 10 Jan 2020 14:51:18 -0700 Subject: [PATCH 24/76] Simplify BoardConfigurationManager interface --- boards/airbourne/airbourne_board.cpp | 5 ----- boards/airbourne/airbourne_board.h | 1 - boards/airbourne/airbourne_board_config_manager.cpp | 10 ++-------- boards/airbourne/airbourne_board_config_manager.h | 6 +----- boards/breezy/breezy_board.cpp | 5 ----- boards/breezy/breezy_board.h | 1 - include/board.h | 1 - include/board_config_manager.h | 2 +- include/config_manager.h | 8 ++++---- src/config_manager.cpp | 10 +++++----- src/rosflight.cpp | 1 - test/test_board.cpp | 5 ----- test/test_board.h | 1 - 13 files changed, 13 insertions(+), 43 deletions(-) diff --git a/boards/airbourne/airbourne_board.cpp b/boards/airbourne/airbourne_board.cpp index f327e65b..b1e225f1 100644 --- a/boards/airbourne/airbourne_board.cpp +++ b/boards/airbourne/airbourne_board.cpp @@ -189,11 +189,6 @@ bool AirbourneBoard::enable_device(device_t device, hardware_config_t configurat return false; } -void AirbourneBoard::init_board_config_manager(ROSflight *rf) -{ - board_config_manager_.init(rf, this); -} - AirbourneBoardConfigManager &AirbourneBoard::get_board_config_manager() { return board_config_manager_; diff --git a/boards/airbourne/airbourne_board.h b/boards/airbourne/airbourne_board.h index 9eaed7da..ffa107b3 100644 --- a/boards/airbourne/airbourne_board.h +++ b/boards/airbourne/airbourne_board.h @@ -147,7 +147,6 @@ class AirbourneBoard : public Board // hardware config bool enable_device(device_t device, hardware_config_t configuration, const Params ¶ms) override; - void init_board_config_manager(ROSflight *rf) override; AirbourneBoardConfigManager &get_board_config_manager() override; // sensors diff --git a/boards/airbourne/airbourne_board_config_manager.cpp b/boards/airbourne/airbourne_board_config_manager.cpp index ac61d99c..c2f45168 100644 --- a/boards/airbourne/airbourne_board_config_manager.cpp +++ b/boards/airbourne/airbourne_board_config_manager.cpp @@ -9,12 +9,7 @@ namespace rosflight_firmware AirbourneBoardConfigManager::AirbourneBoardConfigManager() { } -void AirbourneBoardConfigManager::init(ROSflight *rf, AirbourneBoard *board) -{ - this->RF_ = rf; - this->board_ = board; - is_initialized_ = true; -} + hardware_config_t AirbourneBoardConfigManager::get_max_config(device_t device) { if(device >= Configuration::DEVICE_COUNT) @@ -22,10 +17,9 @@ hardware_config_t AirbourneBoardConfigManager::get_max_config(device_t device) else return max_configs[device]; } -ConfigManager::config_response AirbourneBoardConfigManager::check_config_change(device_t device, hardware_config_t config) +ConfigManager::config_response AirbourneBoardConfigManager::check_config_change(device_t device, hardware_config_t config, const ConfigManager &cm) { ConfigManager::config_response resp; - ConfigManager &cm = RF_->config_manager_; resp.reboot_required = false; resp.successful = false; char *error_message = reinterpret_cast(resp.message); // Because it wasn't converting implicitly diff --git a/boards/airbourne/airbourne_board_config_manager.h b/boards/airbourne/airbourne_board_config_manager.h index 8e605956..0a8932d9 100644 --- a/boards/airbourne/airbourne_board_config_manager.h +++ b/boards/airbourne/airbourne_board_config_manager.h @@ -10,12 +10,10 @@ class AirbourneBoardConfigManager : public BoardConfigManager { public: AirbourneBoardConfigManager(); - void init(ROSflight *rf, AirbourneBoard *board); hardware_config_t get_max_config(device_t device) override; - ConfigManager::config_response check_config_change(device_t device, hardware_config_t config) override; + ConfigManager::config_response check_config_change(device_t device, hardware_config_t config, const ConfigManager &cm) override; void get_device_name(device_t device, uint8_t (&name)[DEVICE_NAME_LENGTH]) override; void get_config_name(device_t device, hardware_config_t config, uint8_t (&name)[CONFIG_NAME_LENGTH]) override; - inline bool is_initialized(){return is_initialized_;} private: enum revo_port { @@ -28,8 +26,6 @@ class AirbourneBoardConfigManager : public BoardConfigManager INTERNAL_I2C }; revo_port get_port(uint8_t device, uint8_t config); // Get the port used by a given configuration - bool is_initialized_{false}; - AirbourneBoard *board_; ROSflight *RF_; const hardware_config_t max_configs[Configuration::DEVICE_COUNT]{3, 1, 1, 3, 1, 1, 1, 1}; }; diff --git a/boards/breezy/breezy_board.cpp b/boards/breezy/breezy_board.cpp index 99714463..6d3661ff 100644 --- a/boards/breezy/breezy_board.cpp +++ b/boards/breezy/breezy_board.cpp @@ -118,11 +118,6 @@ bool BreezyBoard::enable_device(device_t device, hardware_config_t configuration return true; } -void BreezyBoard::init_board_config_manager(ROSflight *rf) -{ - (void)rf; -} - BreezyBoardConfigManager &BreezyBoard::get_board_config_manager() { return config_manager_; diff --git a/boards/breezy/breezy_board.h b/boards/breezy/breezy_board.h index 7085b478..b73eceb5 100644 --- a/boards/breezy/breezy_board.h +++ b/boards/breezy/breezy_board.h @@ -104,7 +104,6 @@ class BreezyBoard : public Board // hardware config bool enable_device(device_t device, hardware_config_t configuration, const Params ¶ms) override; - void init_board_config_manager(ROSflight *rf) override; BreezyBoardConfigManager & get_board_config_manager() override; // sensors diff --git a/include/board.h b/include/board.h index 14e240df..a4a43a1e 100644 --- a/include/board.h +++ b/include/board.h @@ -73,7 +73,6 @@ class Board // hardware config virtual bool enable_device(device_t device, hardware_config_t configuration, const Params ¶ms)=0; - virtual void init_board_config_manager(ROSflight *rf) = 0; virtual BoardConfigManager &get_board_config_manager() = 0; // sensors diff --git a/include/board_config_manager.h b/include/board_config_manager.h index 9021475b..d0968af0 100644 --- a/include/board_config_manager.h +++ b/include/board_config_manager.h @@ -18,7 +18,7 @@ class BoardConfigManager // If the response indicates success, then the config manager accepts the change // If not, the config manager returns the error indicated // Implementations should not assume that either the device or the config are valid - virtual ConfigManager::config_response check_config_change(device_t device, hardware_config_t config) = 0; + virtual ConfigManager::config_response check_config_change(device_t device, hardware_config_t config, const ConfigManager &cm) = 0; static const int DEVICE_NAME_LENGTH{20}; static const int CONFIG_NAME_LENGTH{20}; // Do not assume that the device or config are valid numbers diff --git a/include/config_manager.h b/include/config_manager.h index a704b082..aabfb963 100644 --- a/include/config_manager.h +++ b/include/config_manager.h @@ -26,14 +26,14 @@ class ConfigManager ConfigManager(ROSflight &RF, config_t &config); // Reads the memory, and loads defaults if it is invalid. Call after the memory manager is ready bool init(); - bool configure_devices(); // Sends configurations to the board via the enable_device method + bool configure_devices() const; // Sends configurations to the board via the enable_device method // Attempts to set a configuration, failing if the board config manager rejects it config_response attempt_set_configuration(device_t device, uint8_t config); // Sets a config without checks. This may cause an invalid configuration combo, // so attempt_set_configuration is recommended void set_configuration(device_t device, uint8_t config); - uint8_t get_configuration(device_t device); - uint8_t operator[](device_t device); // same as get_configuration, for convenience + uint8_t get_configuration(device_t device) const; + uint8_t operator[](device_t device) const; // same as get_configuration, for convenience void prepare_write(); // prepares a checksum, so that the config struct can be saved @@ -42,7 +42,7 @@ class ConfigManager config_t &config_; bool read(); // currently just checks that the memory manager is ready and the checksum is correct void fill_defaults(); // Default values are 0, by convention - uint32_t generate_checksum(); // Based off of fletcher algorithm + uint32_t generate_checksum() const; // Based off of fletcher algorithm }; } diff --git a/src/config_manager.cpp b/src/config_manager.cpp index b1095490..d240c7bd 100644 --- a/src/config_manager.cpp +++ b/src/config_manager.cpp @@ -15,7 +15,7 @@ bool ConfigManager::init() return true; } -bool ConfigManager::configure_devices() +bool ConfigManager::configure_devices() const { bool success = true; for(device_t device{static_cast(0)}; device < Configuration::DEVICE_COUNT; ++device) @@ -25,7 +25,7 @@ bool ConfigManager::configure_devices() ConfigManager::config_response ConfigManager::attempt_set_configuration(device_t device, uint8_t config) { - config_response resp = RF_.board_.get_board_config_manager().check_config_change(device, config); + config_response resp = RF_.board_.get_board_config_manager().check_config_change(device, config, *this); if(resp.successful) set_configuration(device, config); return resp; @@ -35,11 +35,11 @@ void ConfigManager::set_configuration(device_t device, uint8_t config) config_.config[device] = config; // TODO consider deinitializing and changing the config } -uint8_t ConfigManager::get_configuration(device_t device) +uint8_t ConfigManager::get_configuration(device_t device) const { return config_.config[device]; } -uint8_t ConfigManager::operator[](device_t device) +uint8_t ConfigManager::operator[](device_t device) const { return get_configuration(device); } @@ -62,7 +62,7 @@ void ConfigManager::fill_defaults() { memset(config_.config, 0, sizeof(config_.config)); } -uint32_t ConfigManager::generate_checksum() +uint32_t ConfigManager::generate_checksum() const { //8 bit fletcher algorithm, because we can't assume that the struct is a multiple of 16 bits const uint8_t *config_data = reinterpret_cast(config_.config); diff --git a/src/rosflight.cpp b/src/rosflight.cpp index 5104b4f6..088e59bc 100644 --- a/src/rosflight.cpp +++ b/src/rosflight.cpp @@ -64,7 +64,6 @@ void ROSflight::init() // Prepare to initialize devices params_.init(); - board_.init_board_config_manager(this); config_manager_.init(); //Initialize devices diff --git a/test/test_board.cpp b/test/test_board.cpp index 2b58968a..f2602c9b 100644 --- a/test/test_board.cpp +++ b/test/test_board.cpp @@ -95,11 +95,6 @@ bool testBoard::enable_device(device_t device, hardware_config_t configuration, return true; } -void testBoard::init_board_config_manager(ROSflight *rf) -{ - (void)rf; -} - TestBoardConfigManager &testBoard::get_board_config_manager() { return config_manager_; diff --git a/test/test_board.h b/test/test_board.h index 733c883e..0f3e2d82 100644 --- a/test/test_board.h +++ b/test/test_board.h @@ -72,7 +72,6 @@ class testBoard : public Board // Hardware config bool enable_device(device_t device, hardware_config_t configuration, const Params ¶ms) override; - void init_board_config_manager(ROSflight *rf) override; TestBoardConfigManager & get_board_config_manager() override; // sensors From 8f4d481d81271a32680d7eab74414da6ad6e07c3 Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Fri, 10 Jan 2020 15:05:44 -0700 Subject: [PATCH 25/76] Change to chars for strings until needed for transmission --- .../airbourne_board_config_manager.cpp | 186 +++++++++--------- .../airbourne_board_config_manager.h | 4 +- comms/mavlink/mavlink.cpp | 12 +- comms/mavlink/mavlink.h | 6 +- include/board_config_manager.h | 4 +- include/config_manager.h | 2 +- include/interface/comm_link.h | 6 +- src/comm_manager.cpp | 4 +- 8 files changed, 111 insertions(+), 113 deletions(-) diff --git a/boards/airbourne/airbourne_board_config_manager.cpp b/boards/airbourne/airbourne_board_config_manager.cpp index c2f45168..e5b4d438 100644 --- a/boards/airbourne/airbourne_board_config_manager.cpp +++ b/boards/airbourne/airbourne_board_config_manager.cpp @@ -22,22 +22,21 @@ ConfigManager::config_response AirbourneBoardConfigManager::check_config_change( ConfigManager::config_response resp; resp.reboot_required = false; resp.successful = false; - char *error_message = reinterpret_cast(resp.message); // Because it wasn't converting implicitly if(device >= Configuration::DEVICE_COUNT) { - strcpy(error_message, "Device not found"); + strcpy(resp.message, "Device not found"); return resp; } if(config > max_configs[device]) { - strcpy(error_message, "Configuration not found"); + strcpy(resp.message, "Configuration not found"); return resp; } if(config == cm[device]) { - strcpy(error_message, "Configuration already set. No change required"); + strcpy(resp.message, "Configuration already set. No change required"); resp.successful = true; resp.reboot_required = false; return resp; @@ -77,32 +76,32 @@ ConfigManager::config_response AirbourneBoardConfigManager::check_config_change( switch(conflict_device) { case Configuration::SERIAL: - strcpy(error_message, "Port is used by serial."); + strcpy(resp.message, "Port is used by serial."); break; case Configuration::RC: - strcpy(error_message, "Port is used by RC."); + strcpy(resp.message, "Port is used by RC."); break; case Configuration::AIRSPEED: - strcpy(error_message, "Port is used by airspeed sensor."); + strcpy(resp.message, "Port is used by airspeed sensor."); break; case Configuration::GNSS: - strcpy(error_message, "Port is used by GNSS receiver."); + strcpy(resp.message, "Port is used by GNSS receiver."); break; case Configuration::SONAR: - strcpy(error_message, "Port is used by sonar sensor."); + strcpy(resp.message, "Port is used by sonar sensor."); break; // At the time of this writing, the below are incapable of conflicts case Configuration::BATTERY_MONITOR: - strcpy(error_message, "Port is used by battery monitor."); + strcpy(resp.message, "Port is used by battery monitor."); break; case Configuration::BAROMETER: - strcpy(error_message, "Port is used by barometer."); + strcpy(resp.message, "Port is used by barometer."); break; case Configuration::MAGNETOMETER: - strcpy(error_message, "Port is used by magnetometer."); + strcpy(resp.message, "Port is used by magnetometer."); break; default: - strcpy(error_message, "Other error."); + strcpy(resp.message, "Other error."); break; } return resp; @@ -112,122 +111,121 @@ ConfigManager::config_response AirbourneBoardConfigManager::check_config_change( resp.message[0]=0; return resp; } -void AirbourneBoardConfigManager::get_device_name(device_t device, uint8_t (&name)[DEVICE_NAME_LENGTH]) +void AirbourneBoardConfigManager::get_device_name(device_t device, char (&name)[DEVICE_NAME_LENGTH]) { - char *name_char = reinterpret_cast(name); switch(device) { case Configuration::SERIAL: - strcpy(name_char, "Serial"); + strcpy(name, "Serial"); break; case Configuration::RC: - strcpy(name_char, "RC"); + strcpy(name, "RC"); break; case Configuration::AIRSPEED: - strcpy(name_char, "Airspeed"); + strcpy(name, "Airspeed"); break; case Configuration::GNSS: - strcpy(name_char, "GNSS"); + strcpy(name, "GNSS"); break; case Configuration::SONAR: - strcpy(name_char, "Sonar"); + strcpy(name, "Sonar"); break; case Configuration::BATTERY_MONITOR: - strcpy(name_char, "Battery Monitor"); + strcpy(name, "Battery Monitor"); break; case Configuration::BAROMETER: - strcpy(name_char, "Barometer"); + strcpy(name, "Barometer"); break; case Configuration::MAGNETOMETER: - strcpy(name_char, "Magnetometer"); + strcpy(name, "Magnetometer"); break; default: - strcpy(name_char, "Error/Unsupported"); + strcpy(name, "Error/Unsupported"); break; } } -void AirbourneBoardConfigManager::get_config_name(device_t device, hardware_config_t config, uint8_t (&name)[CONFIG_NAME_LENGTH]) +void AirbourneBoardConfigManager::get_config_name(device_t device, hardware_config_t config, char (&name)[CONFIG_NAME_LENGTH]) { - char *name_char = reinterpret_cast(name); const char *name_str = "Invalid Config"; - switch(device) - { - case Configuration::SERIAL: - switch(config) + if(config > max_configs[device]) + name_str = "Invalid config"; + else + switch(device) { - case 0: - name_str="VCP over USB"; + case Configuration::SERIAL: + switch(config) + { + case 0: + name_str="VCP over USB"; + break; + case 1: + name_str="UART1 on Main"; + break; + case 2: + name_str="UART2 on Flex-IO"; + break; + case 3: + name_str="UART3 on Flexi"; + break; + } break; - case 1: - name_str="UART1 on Main"; + case Configuration::RC: + if(config==0) + name_str = "PPM on Flex-IO"; + else + name_str = "SBUS on Main"; break; - case 2: - name_str="UART2 on Flex-IO"; + case Configuration::AIRSPEED: + if(config==0) + name_str = "Disabled"; + else + name_str = "I2C2 on Flexi"; break; - case 3: - name_str="UART3 on Flexi"; + case Configuration::GNSS: + switch(config) + { + case 0: + name_str = "Disabled"; + break; + case 1: + name_str = "UART1 on main"; + break; + case 2: + name_str = "UART2 on Flex-Io"; + break; + case 3: + name_str = "UART3 on Flexi"; + break; + } break; - } - break; - case Configuration::RC: - if(config==0) - name_str = "PPM on Flex-IO"; - else - name_str = "SBUS on Main"; - break; - case Configuration::AIRSPEED: - if(config==0) - name_str = "Disabled"; - else - name_str = "I2C2 on Flexi"; - break; - case Configuration::GNSS: - switch(config) - { - case 0: - name_str = "Disabled"; + case Configuration::SONAR: + if(config ==0) + name_str = "Disabled"; + else + name_str = "I2C2 on Flexi"; break; - case 1: - name_str = "UART1 on main"; + case Configuration::BATTERY_MONITOR: + if(config==0) + name_str = "Disabled"; + else + name_str = "ADC3 on Power"; break; - case 2: - name_str = "UART2 on Flex-Io"; + case Configuration::BAROMETER: + if(config==0) + name_str = "Disabled"; + else + name_str = "Onboard barometer"; break; - case 3: - name_str = "UART3 on Flexi"; + case Configuration::MAGNETOMETER: + if(config ==0) + name_str = "Disabled"; + else + name_str = "Onboard magnetometer"; break; + default: + name_str = "Invalid device"; } - break; - case Configuration::SONAR: - if(config ==0) - name_str = "Disabled"; - else - name_str = "I2C2 on Flexi"; - break; - case Configuration::BATTERY_MONITOR: - if(config==0) - name_str = "Disabled"; - else - name_str = "ADC3 on Power"; - break; - case Configuration::BAROMETER: - if(config==0) - name_str = "Disabled"; - else - name_str = "Onboard barometer"; - break; - case Configuration::MAGNETOMETER: - if(config ==0) - name_str = "Disabled"; - else - name_str = "Onboard magnetometer"; - break; - default: - name_str = "Invalid device"; - } - if(config > max_configs[device]) - name_str = "Invalid config"; - strcpy(name_char, name_str); + strcpy(name, name_str); } AirbourneBoardConfigManager::revo_port AirbourneBoardConfigManager::get_port(uint8_t device, uint8_t config) { diff --git a/boards/airbourne/airbourne_board_config_manager.h b/boards/airbourne/airbourne_board_config_manager.h index 0a8932d9..79ade73f 100644 --- a/boards/airbourne/airbourne_board_config_manager.h +++ b/boards/airbourne/airbourne_board_config_manager.h @@ -12,8 +12,8 @@ class AirbourneBoardConfigManager : public BoardConfigManager AirbourneBoardConfigManager(); hardware_config_t get_max_config(device_t device) override; ConfigManager::config_response check_config_change(device_t device, hardware_config_t config, const ConfigManager &cm) override; - void get_device_name(device_t device, uint8_t (&name)[DEVICE_NAME_LENGTH]) override; - void get_config_name(device_t device, hardware_config_t config, uint8_t (&name)[CONFIG_NAME_LENGTH]) override; + void get_device_name(device_t device, char (&name)[DEVICE_NAME_LENGTH]) override; + void get_config_name(device_t device, hardware_config_t config, char (&name)[CONFIG_NAME_LENGTH]) override; private: enum revo_port { diff --git a/comms/mavlink/mavlink.cpp b/comms/mavlink/mavlink.cpp index 87e96d37..c31acf14 100644 --- a/comms/mavlink/mavlink.cpp +++ b/comms/mavlink/mavlink.cpp @@ -295,23 +295,23 @@ void Mavlink::send_config_value(uint8_t system_id, uint8_t device, uint8_t confi send_message(msg); } -void Mavlink::send_device_info(uint8_t system_id, uint8_t device, uint8_t max_config, uint8_t (&name)[20]) +void Mavlink::send_device_info(uint8_t system_id, uint8_t device, uint8_t max_config, char (&name)[BoardConfigManager::DEVICE_NAME_LENGTH]) { mavlink_message_t msg; - mavlink_msg_rosflight_device_info_pack(system_id, 0, &msg, device, max_config, name); + mavlink_msg_rosflight_device_info_pack(system_id, 0, &msg, device, max_config, reinterpret_cast(name)); send_message(msg); } -void Mavlink::send_config_info(uint8_t system_id, uint8_t device, uint8_t config, uint8_t (&name)[20]) +void Mavlink::send_config_info(uint8_t system_id, uint8_t device, uint8_t config, char (&name)[BoardConfigManager::CONFIG_NAME_LENGTH]) { mavlink_message_t msg; - mavlink_msg_rosflight_config_info_pack(system_id, 0, &msg, device, config, name); + mavlink_msg_rosflight_config_info_pack(system_id, 0, &msg, device, config, reinterpret_cast(name)); send_message(msg); } -void Mavlink::send_config_status(uint8_t system_id, uint8_t device, bool success, bool reboot_required, uint8_t (&error_message)[50]) +void Mavlink::send_config_status(uint8_t system_id, uint8_t device, bool success, bool reboot_required, char (&error_message)[ConfigManager::CONFIG_RESPONSE_MESSAGE_LENGTH]) { mavlink_message_t msg; - mavlink_msg_rosflight_config_status_pack(system_id, 0, &msg, device, success, reboot_required, error_message); + mavlink_msg_rosflight_config_status_pack(system_id, 0, &msg, device, success, reboot_required, reinterpret_cast(error_message)); send_message(msg); } diff --git a/comms/mavlink/mavlink.h b/comms/mavlink/mavlink.h index 4c478c1b..adb4e51a 100644 --- a/comms/mavlink/mavlink.h +++ b/comms/mavlink/mavlink.h @@ -85,9 +85,9 @@ class Mavlink : public CommLinkInterface float value, uint16_t param_count) override; void send_config_value(uint8_t system_id, uint8_t device, uint8_t config) override; - void send_device_info(uint8_t system_id, uint8_t device, uint8_t max_config, uint8_t (&name)[20]) override; - void send_config_info(uint8_t system_id, uint8_t device, uint8_t config, uint8_t (&name)[20]) override; - void send_config_status(uint8_t system_id, uint8_t device, bool success, bool reboot_required, uint8_t (&error_message)[50]) override; + void send_device_info(uint8_t system_id, uint8_t device, uint8_t max_config, char (&name)[BoardConfigManager::DEVICE_NAME_LENGTH]) override; + void send_config_info(uint8_t system_id, uint8_t device, uint8_t config, char (&name)[BoardConfigManager::CONFIG_NAME_LENGTH]) override; + void send_config_status(uint8_t system_id, uint8_t device, bool success, bool reboot_required, char (&error_message)[ConfigManager::CONFIG_RESPONSE_MESSAGE_LENGTH]) 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, float min_range) override; void send_status(uint8_t system_id, diff --git a/include/board_config_manager.h b/include/board_config_manager.h index d0968af0..160dce22 100644 --- a/include/board_config_manager.h +++ b/include/board_config_manager.h @@ -24,8 +24,8 @@ class BoardConfigManager // Do not assume that the device or config are valid numbers // When passed an invalid argument, it is better to return a string so indicating than an // empty string - virtual void get_device_name(device_t device, uint8_t (&name)[DEVICE_NAME_LENGTH])=0; - virtual void get_config_name(device_t device, hardware_config_t config, uint8_t (&name)[CONFIG_NAME_LENGTH]) = 0; + virtual void get_device_name(device_t device, char (&name)[DEVICE_NAME_LENGTH])=0; + virtual void get_config_name(device_t device, hardware_config_t config, char (&name)[CONFIG_NAME_LENGTH]) = 0; }; } // namespace rosflight_firmware diff --git a/include/config_manager.h b/include/config_manager.h index aabfb963..69dfd5fb 100644 --- a/include/config_manager.h +++ b/include/config_manager.h @@ -20,7 +20,7 @@ class ConfigManager { bool successful; bool reboot_required; - uint8_t message[CONFIG_RESPONSE_MESSAGE_LENGTH]; // Primarily for error messages + char message[CONFIG_RESPONSE_MESSAGE_LENGTH]; // Primarily for error messages }config_response; ConfigManager(ROSflight &RF, config_t &config); diff --git a/include/interface/comm_link.h b/include/interface/comm_link.h index 1f0043d4..bd60e91c 100644 --- a/include/interface/comm_link.h +++ b/include/interface/comm_link.h @@ -162,9 +162,9 @@ class CommLinkInterface float value, uint16_t param_count) = 0; virtual void send_config_value(uint8_t system_id, uint8_t device, uint8_t config) = 0; - virtual void send_device_info(uint8_t system_id, uint8_t device, uint8_t max_config, uint8_t (&name)[20]) = 0; - virtual void send_config_info(uint8_t system_id, uint8_t device, uint8_t config, uint8_t (&name)[20]) = 0; - virtual void send_config_status(uint8_t system_id, uint8_t device, bool success, bool reboot_required, uint8_t (&error_message)[50]) = 0; + virtual void send_device_info(uint8_t system_id, uint8_t device, uint8_t max_config, char (&name)[BoardConfigManager::DEVICE_NAME_LENGTH]) = 0; + virtual void send_config_info(uint8_t system_id, uint8_t device, uint8_t config, char (&name)[BoardConfigManager::CONFIG_NAME_LENGTH]) = 0; + virtual void send_config_status(uint8_t system_id, uint8_t device, bool success, bool reboot_required, char (&error_message)[ConfigManager::CONFIG_RESPONSE_MESSAGE_LENGTH]) = 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, float min_range) = 0; virtual void send_status(uint8_t system_id, diff --git a/src/comm_manager.cpp b/src/comm_manager.cpp index 4df85a2e..ee5e1e56 100644 --- a/src/comm_manager.cpp +++ b/src/comm_manager.cpp @@ -433,7 +433,7 @@ void CommManager::send_all_config_info() void CommManager::send_device_info(device_t device) { - uint8_t device_name[20]; + char device_name[BoardConfigManager::DEVICE_NAME_LENGTH]; RF_.board_.get_board_config_manager().get_device_name(device, device_name); uint8_t max_config = RF_.board_.get_board_config_manager().get_max_config(device); comm_link_.send_device_info(sysid_, device, max_config, device_name); @@ -441,7 +441,7 @@ void CommManager::send_device_info(device_t device) void CommManager::send_config_info(device_t device, hardware_config_t config) { - uint8_t config_name[20]; + char config_name[BoardConfigManager::CONFIG_NAME_LENGTH]; RF_.board_.get_board_config_manager().get_config_name(device, config, config_name); comm_link_.send_config_info(sysid_, device, config, config_name); } From 3771453d2c12c07aeb9c9b0b67dadab4d2c291ef Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Fri, 10 Jan 2020 15:34:54 -0700 Subject: [PATCH 26/76] Clean up AirbourneBoardConfigManager --- .../airbourne_board_config_manager.cpp | 95 +++++++++++-------- .../airbourne_board_config_manager.h | 21 ++-- include/board_config_manager.h | 8 +- 3 files changed, 73 insertions(+), 51 deletions(-) diff --git a/boards/airbourne/airbourne_board_config_manager.cpp b/boards/airbourne/airbourne_board_config_manager.cpp index e5b4d438..bdb46501 100644 --- a/boards/airbourne/airbourne_board_config_manager.cpp +++ b/boards/airbourne/airbourne_board_config_manager.cpp @@ -6,18 +6,19 @@ namespace rosflight_firmware { +constexpr hardware_config_t AirbourneBoardConfigManager::max_configs[]; // I can't wait for c++ 17 so that this is optional AirbourneBoardConfigManager::AirbourneBoardConfigManager() { } -hardware_config_t AirbourneBoardConfigManager::get_max_config(device_t device) +hardware_config_t AirbourneBoardConfigManager::get_max_config(device_t device) const { if(device >= Configuration::DEVICE_COUNT) return 0; else - return max_configs[device]; + return AirbourneBoardConfigManager::max_configs[device]; } -ConfigManager::config_response AirbourneBoardConfigManager::check_config_change(device_t device, hardware_config_t config, const ConfigManager &cm) +ConfigManager::config_response AirbourneBoardConfigManager::check_config_change(device_t device, hardware_config_t config, const ConfigManager &cm) const { ConfigManager::config_response resp; resp.reboot_required = false; @@ -29,7 +30,7 @@ ConfigManager::config_response AirbourneBoardConfigManager::check_config_change( return resp; } - if(config > max_configs[device]) + if(config > AirbourneBoardConfigManager::max_configs[device]) { strcpy(resp.message, "Configuration not found"); return resp; @@ -42,7 +43,7 @@ ConfigManager::config_response AirbourneBoardConfigManager::check_config_change( return resp; } - revo_port port = get_port(device, config); + Port port = get_port(device, config); device_t conflict_device = Configuration::DEVICE_COUNT; // While a config may conflict with multiple devices, this will only report one @@ -55,11 +56,15 @@ ConfigManager::config_response AirbourneBoardConfigManager::check_config_change( case Configuration::GNSS: if(port != NO_PORT) for(device_t other_device{static_cast(0)}; other_device != Configuration::DEVICE_COUNT; ++other_device) - if(port == get_port(other_device, cm[other_device]) && (other_device !=Configuration::RC || cm[Configuration::RC]!=0)) // RC over PPM does not conflict with UART, even though both use the same port + { + if(other_device == Configuration::RC && cm[Configuration::RC] == 0) // RC over PPM does not conflict with UART, even though both use the same port + continue; + if(port == get_port(other_device, cm[other_device])) { conflict_device = other_device; break; } + } break; case Configuration::AIRSPEED: case Configuration::SONAR: @@ -111,7 +116,7 @@ ConfigManager::config_response AirbourneBoardConfigManager::check_config_change( resp.message[0]=0; return resp; } -void AirbourneBoardConfigManager::get_device_name(device_t device, char (&name)[DEVICE_NAME_LENGTH]) +void AirbourneBoardConfigManager::get_device_name(device_t device, char (&name)[DEVICE_NAME_LENGTH]) const { switch(device) { @@ -144,11 +149,10 @@ void AirbourneBoardConfigManager::get_device_name(device_t device, char (&name)[ break; } } -void AirbourneBoardConfigManager::get_config_name(device_t device, hardware_config_t config, char (&name)[CONFIG_NAME_LENGTH]) +void AirbourneBoardConfigManager::get_config_name(device_t device, hardware_config_t config, char (&name)[CONFIG_NAME_LENGTH]) const { - const char *name_str = "Invalid Config"; - if(config > max_configs[device]) - name_str = "Invalid config"; + if(config > AirbourneBoardConfigManager::max_configs[device]) + strcpy(name, "Invalid config"); else switch(device) { @@ -156,87 +160,104 @@ void AirbourneBoardConfigManager::get_config_name(device_t device, hardware_conf switch(config) { case 0: - name_str="VCP over USB"; + strcpy(name, "VCP over USB"); break; case 1: - name_str="UART1 on Main"; + strcpy(name, "UART1 on Main"); break; case 2: - name_str="UART2 on Flex-IO"; + strcpy(name, "UART2 on Flex-IO"); break; case 3: - name_str="UART3 on Flexi"; + strcpy(name, "UART3 on Flexi"); break; } break; case Configuration::RC: if(config==0) - name_str = "PPM on Flex-IO"; + strcpy(name, "PPM on Flex-IO"); else - name_str = "SBUS on Main"; + strcpy(name, "SBUS on Main"); break; case Configuration::AIRSPEED: if(config==0) - name_str = "Disabled"; + strcpy(name, "Disabled"); else - name_str = "I2C2 on Flexi"; + strcpy(name, "I2C2 on Flexi"); break; case Configuration::GNSS: switch(config) { case 0: - name_str = "Disabled"; + strcpy(name, "Disabled"); break; case 1: - name_str = "UART1 on main"; + strcpy(name, "UART1 on main"); break; case 2: - name_str = "UART2 on Flex-Io"; + strcpy(name, "UART2 on Flex-Io"); break; case 3: - name_str = "UART3 on Flexi"; + strcpy(name, "UART3 on Flexi"); break; } break; case Configuration::SONAR: if(config ==0) - name_str = "Disabled"; + strcpy(name, "Disabled"); else - name_str = "I2C2 on Flexi"; + strcpy(name, "I2C2 on Flexi"); break; case Configuration::BATTERY_MONITOR: if(config==0) - name_str = "Disabled"; + strcpy(name, "Disabled"); else - name_str = "ADC3 on Power"; + strcpy(name, "ADC3 on Power"); break; case Configuration::BAROMETER: if(config==0) - name_str = "Disabled"; + strcpy(name, "Disabled"); else - name_str = "Onboard barometer"; + strcpy(name, "Onboard barometer"); break; case Configuration::MAGNETOMETER: if(config ==0) - name_str = "Disabled"; + strcpy(name, "Disabled"); else - name_str = "Onboard magnetometer"; + strcpy(name, "Onboard magnetometer"); break; default: - name_str = "Invalid device"; + strcpy(name, "Invalid device"); } - strcpy(name, name_str); } -AirbourneBoardConfigManager::revo_port AirbourneBoardConfigManager::get_port(uint8_t device, uint8_t config) +AirbourneBoardConfigManager::Port AirbourneBoardConfigManager::get_port(uint8_t device, uint8_t config) const { switch(device) { case Configuration::SERIAL: - if(config == 0) + switch(config) + { + case 0: return USB_PORT; - // break intentionally ommitted + case 1: + return MAIN_PORT; + case 2: + return FLEX_IO_PORT; + case 3: + return FLEXI_PORT; + } case Configuration::GNSS: - return static_cast(config); + switch(config) + { + case 0: + return USB_PORT; + case 1: + return MAIN_PORT; + case 2: + return FLEX_IO_PORT; + case 3: + return FLEXI_PORT; + } case Configuration::RC: if(config == 0) return FLEX_IO_PORT; diff --git a/boards/airbourne/airbourne_board_config_manager.h b/boards/airbourne/airbourne_board_config_manager.h index 79ade73f..8c592363 100644 --- a/boards/airbourne/airbourne_board_config_manager.h +++ b/boards/airbourne/airbourne_board_config_manager.h @@ -1,5 +1,5 @@ -#ifndef AIRBOURNEBOARDCONFIGMANAGER_H -#define AIRBOURNEBOARDCONFIGMANAGER_H +#ifndef AIRBOURNE_BOARD_CONFIG_MANAGER_H +#define AIRBOURNE_BOARD_CONFIG_MANAGER_H #include "board_config_manager.h" @@ -10,12 +10,12 @@ class AirbourneBoardConfigManager : public BoardConfigManager { public: AirbourneBoardConfigManager(); - hardware_config_t get_max_config(device_t device) override; - ConfigManager::config_response check_config_change(device_t device, hardware_config_t config, const ConfigManager &cm) override; - void get_device_name(device_t device, char (&name)[DEVICE_NAME_LENGTH]) override; - void get_config_name(device_t device, hardware_config_t config, char (&name)[CONFIG_NAME_LENGTH]) override; + hardware_config_t get_max_config(device_t device) const override; + ConfigManager::config_response check_config_change(device_t device, hardware_config_t config, const ConfigManager &cm) const override; + void get_device_name(device_t device, char (&name)[DEVICE_NAME_LENGTH]) const override; + void get_config_name(device_t device, hardware_config_t config, char (&name)[CONFIG_NAME_LENGTH]) const override; private: - enum revo_port + enum Port { NO_PORT, MAIN_PORT, @@ -25,10 +25,11 @@ class AirbourneBoardConfigManager : public BoardConfigManager POWER_PORT, // the port labeled "PWR / SONAR" is not to provide power, but rather for a battery monitor INTERNAL_I2C }; - revo_port get_port(uint8_t device, uint8_t config); // Get the port used by a given configuration + Port get_port(uint8_t device, uint8_t config) const; // Get the port used by a given configuration + ROSflight *RF_; - const hardware_config_t max_configs[Configuration::DEVICE_COUNT]{3, 1, 1, 3, 1, 1, 1, 1}; + static constexpr hardware_config_t max_configs[Configuration::DEVICE_COUNT]{3, 1, 1, 3, 1, 1, 1, 1}; }; } // namespace rosflight_firmware -#endif // AIRBOURNEBOARDCONFIGMANAGER_H +#endif // AIRBOURNE_BOARD_CONFIG_MANAGER_H diff --git a/include/board_config_manager.h b/include/board_config_manager.h index 160dce22..8505058c 100644 --- a/include/board_config_manager.h +++ b/include/board_config_manager.h @@ -13,19 +13,19 @@ class BoardConfigManager // Get the largest number that is valid for the configuration of a given device // This number is inclusive, i.e. a value of 2 means 0, 1, or 2 are valid // For devices that are not configurable, returns 0 - virtual hardware_config_t get_max_config(device_t device) = 0; + virtual hardware_config_t get_max_config(device_t device) const = 0; // Check if a config change is allowed. // If the response indicates success, then the config manager accepts the change // If not, the config manager returns the error indicated // Implementations should not assume that either the device or the config are valid - virtual ConfigManager::config_response check_config_change(device_t device, hardware_config_t config, const ConfigManager &cm) = 0; + virtual ConfigManager::config_response check_config_change(device_t device, hardware_config_t config, const ConfigManager &cm) const = 0; static const int DEVICE_NAME_LENGTH{20}; static const int CONFIG_NAME_LENGTH{20}; // Do not assume that the device or config are valid numbers // When passed an invalid argument, it is better to return a string so indicating than an // empty string - virtual void get_device_name(device_t device, char (&name)[DEVICE_NAME_LENGTH])=0; - virtual void get_config_name(device_t device, hardware_config_t config, char (&name)[CONFIG_NAME_LENGTH]) = 0; + virtual void get_device_name(device_t device, char (&name)[DEVICE_NAME_LENGTH]) const=0; + virtual void get_config_name(device_t device, hardware_config_t config, char (&name)[CONFIG_NAME_LENGTH]) const = 0; }; } // namespace rosflight_firmware From d3a98053a212ef3f03199853f461bf4e05052cbc Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Fri, 10 Jan 2020 15:49:32 -0700 Subject: [PATCH 27/76] Add consts to functions in board relating to BoardConfigManagers --- boards/airbourne/airbourne_board.cpp | 2 +- boards/airbourne/airbourne_board.h | 2 +- include/board.h | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/boards/airbourne/airbourne_board.cpp b/boards/airbourne/airbourne_board.cpp index b1e225f1..e198248b 100644 --- a/boards/airbourne/airbourne_board.cpp +++ b/boards/airbourne/airbourne_board.cpp @@ -189,7 +189,7 @@ bool AirbourneBoard::enable_device(device_t device, hardware_config_t configurat return false; } -AirbourneBoardConfigManager &AirbourneBoard::get_board_config_manager() +AirbourneBoardConfigManager const &AirbourneBoard::get_board_config_manager() const { return board_config_manager_; } diff --git a/boards/airbourne/airbourne_board.h b/boards/airbourne/airbourne_board.h index ffa107b3..7ed8a751 100644 --- a/boards/airbourne/airbourne_board.h +++ b/boards/airbourne/airbourne_board.h @@ -147,7 +147,7 @@ class AirbourneBoard : public Board // hardware config bool enable_device(device_t device, hardware_config_t configuration, const Params ¶ms) override; - AirbourneBoardConfigManager &get_board_config_manager() override; + AirbourneBoardConfigManager const &get_board_config_manager() const override; // sensors void sensors_init() override; diff --git a/include/board.h b/include/board.h index a4a43a1e..d4b51573 100644 --- a/include/board.h +++ b/include/board.h @@ -73,7 +73,7 @@ class Board // hardware config virtual bool enable_device(device_t device, hardware_config_t configuration, const Params ¶ms)=0; - virtual BoardConfigManager &get_board_config_manager() = 0; + virtual const BoardConfigManager &get_board_config_manager() const = 0; // sensors virtual void sensors_init() = 0; From c4f5201d327099ee176cf0fb5bbe6c7e98188534 Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Fri, 10 Jan 2020 15:54:36 -0700 Subject: [PATCH 28/76] Style changes --- .../airbourne/airbourne_board_config_manager.cpp | 4 ++-- .../airbourne/airbourne_board_config_manager.h | 2 +- boards/breezy/breezy_board_config_manager.h | 2 +- include/board_config_manager.h | 6 +++--- include/config_manager.h | 16 ++++++++-------- include/memory_manager.h | 4 ++-- src/comm_manager.cpp | 2 +- src/config_manager.cpp | 6 +++--- test/test_board_config_manager.h | 2 +- 9 files changed, 22 insertions(+), 22 deletions(-) diff --git a/boards/airbourne/airbourne_board_config_manager.cpp b/boards/airbourne/airbourne_board_config_manager.cpp index bdb46501..f524c311 100644 --- a/boards/airbourne/airbourne_board_config_manager.cpp +++ b/boards/airbourne/airbourne_board_config_manager.cpp @@ -18,9 +18,9 @@ hardware_config_t AirbourneBoardConfigManager::get_max_config(device_t device) c else return AirbourneBoardConfigManager::max_configs[device]; } -ConfigManager::config_response AirbourneBoardConfigManager::check_config_change(device_t device, hardware_config_t config, const ConfigManager &cm) const +ConfigManager::ConfigResponse AirbourneBoardConfigManager::check_config_change(device_t device, hardware_config_t config, const ConfigManager &cm) const { - ConfigManager::config_response resp; + ConfigManager::ConfigResponse resp; resp.reboot_required = false; resp.successful = false; diff --git a/boards/airbourne/airbourne_board_config_manager.h b/boards/airbourne/airbourne_board_config_manager.h index 8c592363..6e20f992 100644 --- a/boards/airbourne/airbourne_board_config_manager.h +++ b/boards/airbourne/airbourne_board_config_manager.h @@ -11,7 +11,7 @@ class AirbourneBoardConfigManager : public BoardConfigManager public: AirbourneBoardConfigManager(); hardware_config_t get_max_config(device_t device) const override; - ConfigManager::config_response check_config_change(device_t device, hardware_config_t config, const ConfigManager &cm) const override; + ConfigManager::ConfigResponse check_config_change(device_t device, hardware_config_t config, const ConfigManager &cm) const override; void get_device_name(device_t device, char (&name)[DEVICE_NAME_LENGTH]) const override; void get_config_name(device_t device, hardware_config_t config, char (&name)[CONFIG_NAME_LENGTH]) const override; private: diff --git a/boards/breezy/breezy_board_config_manager.h b/boards/breezy/breezy_board_config_manager.h index dcbd00ce..3e1e396a 100644 --- a/boards/breezy/breezy_board_config_manager.h +++ b/boards/breezy/breezy_board_config_manager.h @@ -9,7 +9,7 @@ class BreezyBoardConfigManager : public BoardConfigManager { public: hardware_config_t get_max_config(device_t device) override; - ConfigManager::config_response check_config_change(device_t device, hardware_config_t config) override; + ConfigManager::ConfigResponse check_config_change(device_t device, hardware_config_t config) override; void get_device_name(device_t device, uint8_t (&name)[20]) override; void get_config_name(device_t device, hardware_config_t config, uint8_t (&name)[20]) override; }; diff --git a/include/board_config_manager.h b/include/board_config_manager.h index 8505058c..80efb90d 100644 --- a/include/board_config_manager.h +++ b/include/board_config_manager.h @@ -18,9 +18,9 @@ class BoardConfigManager // If the response indicates success, then the config manager accepts the change // If not, the config manager returns the error indicated // Implementations should not assume that either the device or the config are valid - virtual ConfigManager::config_response check_config_change(device_t device, hardware_config_t config, const ConfigManager &cm) const = 0; - static const int DEVICE_NAME_LENGTH{20}; - static const int CONFIG_NAME_LENGTH{20}; + virtual ConfigManager::ConfigResponse check_config_change(device_t device, hardware_config_t config, const ConfigManager &cm) const = 0; + static constexpr int DEVICE_NAME_LENGTH = 20; + static constexpr int CONFIG_NAME_LENGTH = 20; // Do not assume that the device or config are valid numbers // When passed an invalid argument, it is better to return a string so indicating than an // empty string diff --git a/include/config_manager.h b/include/config_manager.h index 69dfd5fb..aa98d9b6 100644 --- a/include/config_manager.h +++ b/include/config_manager.h @@ -9,26 +9,26 @@ class ROSflight; class ConfigManager { public: - typedef struct __attribute__ ((packed)) + struct __attribute__ ((packed)) Config { uint32_t checksum; hardware_config_t config[Configuration::DEVICE_COUNT]; - } config_t; + }; - static const int CONFIG_RESPONSE_MESSAGE_LENGTH{50}; - typedef struct + static constexpr int CONFIG_RESPONSE_MESSAGE_LENGTH = 50; + struct ConfigResponse { bool successful; bool reboot_required; char message[CONFIG_RESPONSE_MESSAGE_LENGTH]; // Primarily for error messages - }config_response; + }; - ConfigManager(ROSflight &RF, config_t &config); + ConfigManager(ROSflight &RF, Config &config); // Reads the memory, and loads defaults if it is invalid. Call after the memory manager is ready bool init(); bool configure_devices() const; // Sends configurations to the board via the enable_device method // Attempts to set a configuration, failing if the board config manager rejects it - config_response attempt_set_configuration(device_t device, uint8_t config); + ConfigResponse attempt_set_configuration(device_t device, uint8_t config); // Sets a config without checks. This may cause an invalid configuration combo, // so attempt_set_configuration is recommended void set_configuration(device_t device, uint8_t config); @@ -39,7 +39,7 @@ class ConfigManager private: ROSflight &RF_; - config_t &config_; + Config &config_; bool read(); // currently just checks that the memory manager is ready and the checksum is correct void fill_defaults(); // Default values are 0, by convention uint32_t generate_checksum() const; // Based off of fletcher algorithm diff --git a/include/memory_manager.h b/include/memory_manager.h index 16c6f6de..bd3be7d6 100644 --- a/include/memory_manager.h +++ b/include/memory_manager.h @@ -13,7 +13,7 @@ class MemoryManager typedef struct { Params::params_t params; - ConfigManager::config_t config; + ConfigManager::Config config; } persistent_memory_t; MemoryManager(ROSflight &rf); @@ -22,7 +22,7 @@ class MemoryManager inline bool is_ready() {return ready_;} inline Params::params_t &get_params(){return memory_.params;} - inline ConfigManager::config_t &get_config(){return memory_.config;} + inline ConfigManager::Config &get_config(){return memory_.config;} private: ROSflight &RF_; diff --git a/src/comm_manager.cpp b/src/comm_manager.cpp index ee5e1e56..e71d325e 100644 --- a/src/comm_manager.cpp +++ b/src/comm_manager.cpp @@ -414,7 +414,7 @@ void CommManager::config_set_callback(uint8_t device, uint8_t configuration) uint8_t requested_device{device}; if(device >=Configuration::DEVICE_COUNT) device = Configuration::DEVICE_COUNT; - ConfigManager::config_response resp = RF_.config_manager_.attempt_set_configuration(static_cast(device), configuration); + ConfigManager::ConfigResponse resp = RF_.config_manager_.attempt_set_configuration(static_cast(device), configuration); comm_link_.send_config_status(sysid_, requested_device, resp.successful, resp.reboot_required, resp.message); } diff --git a/src/config_manager.cpp b/src/config_manager.cpp index d240c7bd..5eb0ac36 100644 --- a/src/config_manager.cpp +++ b/src/config_manager.cpp @@ -3,7 +3,7 @@ namespace rosflight_firmware { -ConfigManager::ConfigManager(ROSflight &RF, config_t &config): +ConfigManager::ConfigManager(ROSflight &RF, Config &config): RF_{RF}, config_{config} {} @@ -23,9 +23,9 @@ bool ConfigManager::configure_devices() const return success; } -ConfigManager::config_response ConfigManager::attempt_set_configuration(device_t device, uint8_t config) +ConfigManager::ConfigResponse ConfigManager::attempt_set_configuration(device_t device, uint8_t config) { - config_response resp = RF_.board_.get_board_config_manager().check_config_change(device, config, *this); + ConfigResponse resp = RF_.board_.get_board_config_manager().check_config_change(device, config, *this); if(resp.successful) set_configuration(device, config); return resp; diff --git a/test/test_board_config_manager.h b/test/test_board_config_manager.h index d59e44c6..e89fa36f 100644 --- a/test/test_board_config_manager.h +++ b/test/test_board_config_manager.h @@ -9,7 +9,7 @@ class TestBoardConfigManager : public BoardConfigManager { public: hardware_config_t get_max_config(device_t device) override; - ConfigManager::config_response check_config_change(device_t device, hardware_config_t config) override; + ConfigManager::ConfigResponse check_config_change(device_t device, hardware_config_t config) override; void get_device_name(device_t device, uint8_t (&name)[20]) override; void get_config_name(device_t device, hardware_config_t config, uint8_t (&name)[20]) override; }; From 6b8ee4377861fe732dd2c2491ac62ef05ad54949 Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Fri, 10 Jan 2020 16:12:28 -0700 Subject: [PATCH 29/76] Style changes --- include/config_manager.h | 6 +++--- include/memory_manager.h | 6 +++--- include/param.h | 4 ++-- include/rosflight.h | 2 +- src/memory_manager.cpp | 4 ++-- 5 files changed, 11 insertions(+), 11 deletions(-) diff --git a/include/config_manager.h b/include/config_manager.h index aa98d9b6..df25d7da 100644 --- a/include/config_manager.h +++ b/include/config_manager.h @@ -29,9 +29,6 @@ class ConfigManager bool configure_devices() const; // Sends configurations to the board via the enable_device method // Attempts to set a configuration, failing if the board config manager rejects it ConfigResponse attempt_set_configuration(device_t device, uint8_t config); - // Sets a config without checks. This may cause an invalid configuration combo, - // so attempt_set_configuration is recommended - void set_configuration(device_t device, uint8_t config); uint8_t get_configuration(device_t device) const; uint8_t operator[](device_t device) const; // same as get_configuration, for convenience @@ -40,6 +37,9 @@ class ConfigManager private: ROSflight &RF_; Config &config_; + // Sets a config without checks. This may cause an invalid configuration combo, + // so attempt_set_configuration is recommended + void set_configuration(device_t device, uint8_t config); bool read(); // currently just checks that the memory manager is ready and the checksum is correct void fill_defaults(); // Default values are 0, by convention uint32_t generate_checksum() const; // Based off of fletcher algorithm diff --git a/include/memory_manager.h b/include/memory_manager.h index bd3be7d6..4ae01db2 100644 --- a/include/memory_manager.h +++ b/include/memory_manager.h @@ -10,11 +10,11 @@ class ROSflight; class MemoryManager { public: - typedef struct + struct PersistentMemory { Params::params_t params; ConfigManager::Config config; - } persistent_memory_t; + }; MemoryManager(ROSflight &rf); bool read_memory(); @@ -26,7 +26,7 @@ class MemoryManager private: ROSflight &RF_; - persistent_memory_t memory_; + PersistentMemory memory_; bool ready_{false}; }; diff --git a/include/param.h b/include/param.h index 65876015..c1024a16 100644 --- a/include/param.h +++ b/include/param.h @@ -234,12 +234,13 @@ class Params public: static constexpr uint8_t PARAMS_NAME_LENGTH = 16; +private: union param_value_t { float fvalue; int32_t ivalue; }; - +public: typedef struct { uint32_t version; @@ -253,7 +254,6 @@ class Params uint8_t magic_ef; // magic number, should be 0xEF uint8_t chk; // XOR checksum } params_t; - private: ROSflight &RF_; params_t ¶ms; diff --git a/include/rosflight.h b/include/rosflight.h index 4c195a55..579f4860 100644 --- a/include/rosflight.h +++ b/include/rosflight.h @@ -60,9 +60,9 @@ class ROSflight ROSflight(Board& board, CommLinkInterface& comm_link); Board &board_; + MemoryManager memory_manager_; CommManager comm_manager_; - Params params_; CommandManager command_manager_; diff --git a/src/memory_manager.cpp b/src/memory_manager.cpp index fd0a2c4e..2b6e4566 100644 --- a/src/memory_manager.cpp +++ b/src/memory_manager.cpp @@ -11,14 +11,14 @@ MemoryManager::MemoryManager(ROSflight &rf): bool MemoryManager::read_memory() { RF_.board_.memory_init(); - ready_ = RF_.board_.memory_read(&memory_, sizeof(persistent_memory_t)); + ready_ = RF_.board_.memory_read(&memory_, sizeof(PersistentMemory)); return ready_; } bool MemoryManager::write_memory() { RF_.params_.prepare_write(); RF_.config_manager_.prepare_write(); - return RF_.board_.memory_write(&memory_, sizeof(persistent_memory_t)); + return RF_.board_.memory_write(&memory_, sizeof(PersistentMemory)); } } // namespace rosflight_firmware From 705508956b86a53304ac6975029ec7a4bd3b057c Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Fri, 10 Jan 2020 16:21:50 -0700 Subject: [PATCH 30/76] Disallow config change while armed --- src/config_manager.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/src/config_manager.cpp b/src/config_manager.cpp index 5eb0ac36..fc25fc8f 100644 --- a/src/config_manager.cpp +++ b/src/config_manager.cpp @@ -25,7 +25,15 @@ bool ConfigManager::configure_devices() const ConfigManager::ConfigResponse ConfigManager::attempt_set_configuration(device_t device, uint8_t config) { - ConfigResponse resp = RF_.board_.get_board_config_manager().check_config_change(device, config, *this); + ConfigResponse resp; + if(RF_.state_manager_.state().armed) + { + resp.successful = false; + resp.reboot_required = false; + strcpy(resp.message, "Config changes while armed are not allowed."); + return resp; + } + resp = RF_.board_.get_board_config_manager().check_config_change(device, config, *this); if(resp.successful) set_configuration(device, config); return resp; From 0a12d69c694de3c55ed4088529afc2c00ee5eac1 Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Mon, 13 Jan 2020 12:36:34 -0700 Subject: [PATCH 31/76] Add command specifically for requesting configuration and deivce information --- comms/mavlink/mavlink.cpp | 6 ++++++ comms/mavlink/v1.0 | 2 +- include/interface/comm_link.h | 3 ++- src/comm_manager.cpp | 5 +++-- 4 files changed, 12 insertions(+), 4 deletions(-) diff --git a/comms/mavlink/mavlink.cpp b/comms/mavlink/mavlink.cpp index c31acf14..f6386de3 100644 --- a/comms/mavlink/mavlink.cpp +++ b/comms/mavlink/mavlink.cpp @@ -121,6 +121,9 @@ void Mavlink::send_command_ack(uint8_t system_id, Command command, bool success) case CommLinkInterface::Command::COMMAND_SEND_VERSION: rosflight_cmd = ROSFLIGHT_CMD_SEND_VERSION; break; + case CommLinkInterface::Command::COMMAND_SEND_ALL_CONFIG_INFOS: + rosflight_cmd = ROSFLIGHT_CMD_SEND_ALL_CONFIG_INFOS; + break; } mavlink_message_t msg; @@ -487,6 +490,9 @@ void Mavlink::handle_msg_rosflight_cmd(const mavlink_message_t *const msg) case ROSFLIGHT_CMD_SEND_VERSION: command = CommLinkInterface::Command::COMMAND_SEND_VERSION; break; + case ROSFLIGHT_CMD_SEND_ALL_CONFIG_INFOS: + command = CommLinkInterface::Command::COMMAND_SEND_ALL_CONFIG_INFOS; + 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); diff --git a/comms/mavlink/v1.0 b/comms/mavlink/v1.0 index d6ac54b0..4a8fd733 160000 --- a/comms/mavlink/v1.0 +++ b/comms/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit d6ac54b03ee6dbfe114a9cce6346920dc35bbd42 +Subproject commit 4a8fd73347a95b16488eec6f56d7a4ab93bb87b5 diff --git a/include/interface/comm_link.h b/include/interface/comm_link.h index bd60e91c..920da852 100644 --- a/include/interface/comm_link.h +++ b/include/interface/comm_link.h @@ -68,7 +68,8 @@ class CommLinkInterface COMMAND_RC_CALIBRATION, COMMAND_REBOOT, COMMAND_REBOOT_TO_BOOTLOADER, - COMMAND_SEND_VERSION + COMMAND_SEND_VERSION, + COMMAND_SEND_ALL_CONFIG_INFOS }; struct OffboardControl diff --git a/src/comm_manager.cpp b/src/comm_manager.cpp index e71d325e..8c087b33 100644 --- a/src/comm_manager.cpp +++ b/src/comm_manager.cpp @@ -292,6 +292,9 @@ void CommManager::command_callback(CommLinkInterface::Command command) case CommLinkInterface::Command::COMMAND_SEND_VERSION: comm_link_.send_version(sysid_, GIT_VERSION_STRING); break; + case CommLinkInterface::Command::COMMAND_SEND_ALL_CONFIG_INFOS: + send_all_config_info(); + break; } } @@ -422,8 +425,6 @@ void CommManager::config_request_callback(uint8_t device) { if(device < Configuration::DEVICE_COUNT) send_config_value(static_cast(device)); - if(device == 0xff) - send_all_config_info(); } void CommManager::send_all_config_info() { From 9cd54a30946f54280353c12827fd587450280393 Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Mon, 13 Jan 2020 12:43:11 -0700 Subject: [PATCH 32/76] Clean up sending configuration info --- src/comm_manager.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/comm_manager.cpp b/src/comm_manager.cpp index 8c087b33..b5fef93c 100644 --- a/src/comm_manager.cpp +++ b/src/comm_manager.cpp @@ -693,10 +693,9 @@ void CommManager::send_next_config_info(void) { if (send_config_info_index_ == 0) send_device_info(send_device_info_index_); - else - send_config_info(send_device_info_index_, send_config_info_index_-1); + send_config_info(send_device_info_index_, send_config_info_index_); send_config_info_index_++; - if(send_config_info_index_ -1 > RF_.board_.get_board_config_manager().get_max_config(send_device_info_index_)) + if(send_config_info_index_ > RF_.board_.get_board_config_manager().get_max_config(send_device_info_index_)) { ++send_device_info_index_; send_config_info_index_ = 0; From 3d9f3d711169da115f2ac2006ea5f796b050d2e4 Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Mon, 13 Jan 2020 13:37:43 -0700 Subject: [PATCH 33/76] Replace magic numbers for configurations with enums --- boards/airbourne/airbourne_board.cpp | 29 ++++--- .../airbourne_board_config_manager.cpp | 84 ++++++++++--------- .../airbourne_board_config_manager.h | 1 + .../airbourne/airbourne_configuration_enum.h | 62 ++++++++++++++ include/configuration_enum.h | 1 + 5 files changed, 125 insertions(+), 52 deletions(-) create mode 100644 boards/airbourne/airbourne_configuration_enum.h diff --git a/boards/airbourne/airbourne_board.cpp b/boards/airbourne/airbourne_board.cpp index e198248b..42f47aef 100644 --- a/boards/airbourne/airbourne_board.cpp +++ b/boards/airbourne/airbourne_board.cpp @@ -78,19 +78,19 @@ void AirbourneBoard::serial_init(uint32_t baud_rate, hardware_config_t configura switch(configuration) { default: - case 0: // VCP + case AirbourneConfiguration::SERIAL_VCP: current_serial_ = &vcp_; vcp_.init(); break; - case 1: // UART 1 + case AirbourneConfiguration::SERIAL_UART1: current_serial_ = &uart1_; uart1_.init(&uart_config[UART1], baud_rate); break; - case 2: // UART 2 + case AirbourneConfiguration::SERIAL_UART2: current_serial_ = &uart2_; uart2_.init(&uart_config[UART2], baud_rate); break; - case 3: // UART 3 + case AirbourneConfiguration::SERIAL_UART3: current_serial_ = &uart3_; uart3_.init(&uart_config[UART3], baud_rate); break; @@ -133,10 +133,10 @@ bool AirbourneBoard::enable_device(device_t device, hardware_config_t configurat case Configuration::RC: switch(configuration) { - case 0: + case AirbourneConfiguration::RC_PPM: rc_init(RC_TYPE_PPM); break; - case 1: + case AirbourneConfiguration::RC_SBUS: rc_init(RC_TYPE_SBUS); break; default: @@ -144,7 +144,7 @@ bool AirbourneBoard::enable_device(device_t device, hardware_config_t configurat } return true; case Configuration::AIRSPEED: - if(configuration==1) + if(configuration==AirbourneConfiguration::AIRSPEED_I2C2) { if(!ext_i2c_.is_initialized()) ext_i2c_.init(&i2c_config[EXTERNAL_I2C]); @@ -155,12 +155,15 @@ bool AirbourneBoard::enable_device(device_t device, hardware_config_t configurat // GNSS is currently disabled break; case Configuration::SONAR: - if(!ext_i2c_.is_initialized()) - ext_i2c_.init(&i2c_config[EXTERNAL_I2C]); - sonar_.init(&ext_i2c_); + if(configuration == AirbourneConfiguration::SONAR_I2C2) + { + if(!ext_i2c_.is_initialized()) + ext_i2c_.init(&i2c_config[EXTERNAL_I2C]); + sonar_.init(&ext_i2c_); + } break; case Configuration::BATTERY_MONITOR: - if(configuration == 1) + if(configuration == AirbourneConfiguration::BATTERY_MONITOR_ADC3) { float voltage_multiplier = params.get_param_float(PARAM_BATTERY_VOLTAGE_MULTIPLIER); float current_multiplier = params.get_param_float(PARAM_BATTERY_CURRENT_MULTIPLIER); @@ -169,7 +172,7 @@ bool AirbourneBoard::enable_device(device_t device, hardware_config_t configurat } break; case Configuration::BAROMETER: - if(configuration == 1) + if(configuration == AirbourneConfiguration::BAROMETER_ONBOARD) { if(!int_i2c_.is_initialized()) int_i2c_.init(&i2c_config[BARO_I2C]); @@ -177,7 +180,7 @@ bool AirbourneBoard::enable_device(device_t device, hardware_config_t configurat } break; case Configuration::MAGNETOMETER: - if(configuration == 1) + if(configuration == AirbourneConfiguration::MAGNETOMETER_ONBOARD) { if(!int_i2c_.is_initialized()) int_i2c_.init(&i2c_config[BARO_I2C]); diff --git a/boards/airbourne/airbourne_board_config_manager.cpp b/boards/airbourne/airbourne_board_config_manager.cpp index f524c311..cfe67c71 100644 --- a/boards/airbourne/airbourne_board_config_manager.cpp +++ b/boards/airbourne/airbourne_board_config_manager.cpp @@ -50,14 +50,14 @@ ConfigManager::ConfigResponse AirbourneBoardConfigManager::check_config_change(d switch(device) { case Configuration::RC: - if(config ==0) // PPM is not known to conflict with anything + if(config ==AirbourneConfiguration::RC_PPM) // PPM is not known to conflict with anything break; case Configuration::SERIAL: case Configuration::GNSS: if(port != NO_PORT) for(device_t other_device{static_cast(0)}; other_device != Configuration::DEVICE_COUNT; ++other_device) { - if(other_device == Configuration::RC && cm[Configuration::RC] == 0) // RC over PPM does not conflict with UART, even though both use the same port + if(other_device == Configuration::RC && cm[Configuration::RC] == AirbourneConfiguration::RC_PPM) // RC over PPM does not conflict with UART, even though both use the same port continue; if(port == get_port(other_device, cm[other_device])) { @@ -68,9 +68,9 @@ ConfigManager::ConfigResponse AirbourneBoardConfigManager::check_config_change(d break; case Configuration::AIRSPEED: case Configuration::SONAR: - if(cm[Configuration::GNSS] == 3) + if(cm[Configuration::GNSS] == AirbourneConfiguration::GNSS_UART3) conflict_device = Configuration::GNSS; - if(cm[Configuration::SERIAL] == 3) + if(cm[Configuration::SERIAL] == AirbourneConfiguration::GNSS_UART3) conflict_device = Configuration::SERIAL; break; default: @@ -113,7 +113,7 @@ ConfigManager::ConfigResponse AirbourneBoardConfigManager::check_config_change(d } resp.successful = true; resp.reboot_required = true; - resp.message[0]=0; + resp.message[0]=0; // Ensuring that the message is treated as a zero-length string return resp; } void AirbourneBoardConfigManager::get_device_name(device_t device, char (&name)[DEVICE_NAME_LENGTH]) const @@ -159,71 +159,71 @@ void AirbourneBoardConfigManager::get_config_name(device_t device, hardware_conf case Configuration::SERIAL: switch(config) { - case 0: + case AirbourneConfiguration::SERIAL_VCP: strcpy(name, "VCP over USB"); break; - case 1: + case AirbourneConfiguration::SERIAL_UART1: strcpy(name, "UART1 on Main"); break; - case 2: + case AirbourneConfiguration::SERIAL_UART2: strcpy(name, "UART2 on Flex-IO"); break; - case 3: + case AirbourneConfiguration::SERIAL_UART3: strcpy(name, "UART3 on Flexi"); break; } break; case Configuration::RC: - if(config==0) + if(config==AirbourneConfiguration::RC_PPM) strcpy(name, "PPM on Flex-IO"); - else + else if(config==AirbourneConfiguration::RC_SBUS) strcpy(name, "SBUS on Main"); break; case Configuration::AIRSPEED: - if(config==0) + if(config==AirbourneConfiguration::AIRSPEED_DISABLED) strcpy(name, "Disabled"); - else + else if(config == AirbourneConfiguration::AIRSPEED_I2C2) strcpy(name, "I2C2 on Flexi"); break; case Configuration::GNSS: switch(config) { - case 0: + case AirbourneConfiguration::GNSS_DISABLED: strcpy(name, "Disabled"); break; - case 1: + case AirbourneConfiguration::GNSS_UART1: strcpy(name, "UART1 on main"); break; - case 2: + case AirbourneConfiguration::GNSS_UART2: strcpy(name, "UART2 on Flex-Io"); break; - case 3: + case AirbourneConfiguration::GNSS_UART3: strcpy(name, "UART3 on Flexi"); break; } break; case Configuration::SONAR: - if(config ==0) + if(config ==AirbourneConfiguration::SONAR_DISABLED) strcpy(name, "Disabled"); - else + else if(config == AirbourneConfiguration::SONAR_I2C2) strcpy(name, "I2C2 on Flexi"); break; case Configuration::BATTERY_MONITOR: - if(config==0) + if(config==AirbourneConfiguration::BATTERY_MONITOR_DISABLED) strcpy(name, "Disabled"); - else + else if(config==AirbourneConfiguration::BATTERY_MONITOR_ADC3) strcpy(name, "ADC3 on Power"); break; case Configuration::BAROMETER: - if(config==0) + if(config==AirbourneConfiguration::BAROMETER_DISABLED) strcpy(name, "Disabled"); - else + else if(config == AirbourneConfiguration::BAROMETER_ONBOARD) strcpy(name, "Onboard barometer"); break; case Configuration::MAGNETOMETER: - if(config ==0) + if(config ==AirbourneConfiguration::MAGNETOMETER_DISABLED) strcpy(name, "Disabled"); - else + else if(config == AirbourneConfiguration::MAGNETOMETER_ONBOARD) strcpy(name, "Onboard magnetometer"); break; default: @@ -237,45 +237,51 @@ AirbourneBoardConfigManager::Port AirbourneBoardConfigManager::get_port(uint8_t case Configuration::SERIAL: switch(config) { - case 0: + case AirbourneConfiguration::SERIAL_VCP: return USB_PORT; - case 1: + case AirbourneConfiguration::SERIAL_UART1: return MAIN_PORT; - case 2: + case AirbourneConfiguration::SERIAL_UART2: return FLEX_IO_PORT; - case 3: + case AirbourneConfiguration::SERIAL_UART3: return FLEXI_PORT; } case Configuration::GNSS: switch(config) { - case 0: - return USB_PORT; - case 1: + case AirbourneConfiguration::GNSS_DISABLED: + return NO_PORT; + case AirbourneConfiguration::GNSS_UART1: return MAIN_PORT; - case 2: + case AirbourneConfiguration::GNSS_UART2: return FLEX_IO_PORT; - case 3: + case AirbourneConfiguration::GNSS_UART3: return FLEXI_PORT; } case Configuration::RC: - if(config == 0) + if(config == AirbourneConfiguration::RC_PPM) return FLEX_IO_PORT; - if(config == 1) + if(config == AirbourneConfiguration::RC_SBUS) return MAIN_PORT; break; case Configuration::AIRSPEED: + if(config==AirbourneConfiguration::AIRSPEED_I2C2) + return FLEXI_PORT; + break; case Configuration::SONAR: - if(config==1) + if(config==AirbourneConfiguration::SONAR_I2C2) return FLEXI_PORT; break; case Configuration::BATTERY_MONITOR: - if(config == 1) + if(config == AirbourneConfiguration::BATTERY_MONITOR_ADC3) return POWER_PORT; break; case Configuration::MAGNETOMETER: + if(config == AirbourneConfiguration::MAGNETOMETER_ONBOARD) + return INTERNAL_I2C; + break; case Configuration::BAROMETER: - if(config == 1) + if(config == AirbourneConfiguration::BAROMETER_ONBOARD) return INTERNAL_I2C; } return NO_PORT; diff --git a/boards/airbourne/airbourne_board_config_manager.h b/boards/airbourne/airbourne_board_config_manager.h index 6e20f992..a53dc3c7 100644 --- a/boards/airbourne/airbourne_board_config_manager.h +++ b/boards/airbourne/airbourne_board_config_manager.h @@ -2,6 +2,7 @@ #define AIRBOURNE_BOARD_CONFIG_MANAGER_H #include "board_config_manager.h" +#include "airbourne_configuration_enum.h" namespace rosflight_firmware{ class ROSflight; diff --git a/boards/airbourne/airbourne_configuration_enum.h b/boards/airbourne/airbourne_configuration_enum.h new file mode 100644 index 00000000..4cecfac7 --- /dev/null +++ b/boards/airbourne/airbourne_configuration_enum.h @@ -0,0 +1,62 @@ +#ifndef AIRBOURNE_CONFIGURATION_ENUM_H +#define AIRBOURNE_CONFIGURATION_ENUM_H +#include +namespace rosflight_firmware +{ +namespace AirbourneConfiguration +{ +enum serial_config_t: uint8_t +{ + SERIAL_VCP, + SERIAL_UART1, + SERIAL_UART2, + SERIAL_UART3 +}; + +enum rc_config_t: uint8_t +{ + RC_PPM, + RC_SBUS +}; + +enum airspeed_config_t: uint8_t +{ + AIRSPEED_DISABLED, + AIRSPEED_I2C2 +}; + +enum gnss_config_t: uint8_t +{ + GNSS_DISABLED, + GNSS_UART1, + GNSS_UART2, + GNSS_UART3 +}; + +enum sonar_config_t: uint8_t +{ + SONAR_DISABLED, + SONAR_I2C2 +}; + +enum battery_monitor_config_t: uint8_t +{ + BATTERY_MONITOR_DISABLED, + BATTERY_MONITOR_ADC3 +}; + +enum barometer_config_t: uint8_t +{ + BAROMETER_DISABLED, + BAROMETER_ONBOARD +}; + +enum magnetometer_config_t: uint8_t +{ + MAGNETOMETER_DISABLED, + MAGNETOMETER_ONBOARD +}; + +} +} // namespace rosflight_firmware +#endif // AIRBOURNE_CONFIGURATION_ENUM_H diff --git a/include/configuration_enum.h b/include/configuration_enum.h index eba70456..50e9ff10 100644 --- a/include/configuration_enum.h +++ b/include/configuration_enum.h @@ -28,6 +28,7 @@ inline device_t& operator++(device_t &dev) dev = static_cast(return_value); return dev; } + } typedef uint8_t hardware_config_t; typedef Configuration::device_t device_t; From d76dd7d33ab7a701cad1b6a302e7602e2d026103 Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Mon, 13 Jan 2020 13:42:42 -0700 Subject: [PATCH 34/76] Add delay to prevent cold boot issues with sensors --- boards/airbourne/airbourne_board.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/boards/airbourne/airbourne_board.cpp b/boards/airbourne/airbourne_board.cpp index 42f47aef..a57c8eed 100644 --- a/boards/airbourne/airbourne_board.cpp +++ b/boards/airbourne/airbourne_board.cpp @@ -174,6 +174,7 @@ bool AirbourneBoard::enable_device(device_t device, hardware_config_t configurat case Configuration::BAROMETER: if(configuration == AirbourneConfiguration::BAROMETER_ONBOARD) { + while (millis() < 50) {} // wait for sensors to boot up if(!int_i2c_.is_initialized()) int_i2c_.init(&i2c_config[BARO_I2C]); baro_.init(&int_i2c_); @@ -182,6 +183,7 @@ bool AirbourneBoard::enable_device(device_t device, hardware_config_t configurat case Configuration::MAGNETOMETER: if(configuration == AirbourneConfiguration::MAGNETOMETER_ONBOARD) { + while (millis() < 50) {} // wait for sensors to boot up if(!int_i2c_.is_initialized()) int_i2c_.init(&i2c_config[BARO_I2C]); mag_.init(&int_i2c_); From ff3540ec83fe96136862415cbb5c630c9262dfc0 Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Tue, 14 Jan 2020 15:14:36 -0700 Subject: [PATCH 35/76] Remove now unneeded methods and params --- boards/airbourne/airbourne_board.h | 4 ++-- boards/breezy/breezy_board.cpp | 17 +++++++++++++---- boards/breezy/breezy_board.h | 2 +- comms/mavlink/mavlink.cpp | 5 +---- comms/mavlink/mavlink.h | 2 +- include/board.h | 2 -- include/interface/comm_link.h | 2 +- include/param.h | 2 -- src/comm_manager.cpp | 3 +-- src/mixer.cpp | 1 - src/param.cpp | 2 -- src/rc.cpp | 4 ---- 12 files changed, 20 insertions(+), 26 deletions(-) diff --git a/boards/airbourne/airbourne_board.h b/boards/airbourne/airbourne_board.h index 7ed8a751..57418310 100644 --- a/boards/airbourne/airbourne_board.h +++ b/boards/airbourne/airbourne_board.h @@ -139,7 +139,7 @@ class AirbourneBoard : public Board void clock_delay(uint32_t milliseconds) override; // serial - void serial_init(uint32_t baud_rate, hardware_config_t configuration) override; + void serial_init(uint32_t baud_rate, hardware_config_t configuration); void serial_write(const uint8_t *src, size_t len) override; uint16_t serial_bytes_available() override; uint8_t serial_read() override; @@ -189,7 +189,7 @@ class AirbourneBoard : public Board bool gnss_has_new_data() override; GNSSRaw gnss_raw_read() override; // RC - void rc_init(rc_type_t rc_type) override; + void rc_init(rc_type_t rc_type); bool rc_lost() override; float rc_read(uint8_t channel) override; diff --git a/boards/breezy/breezy_board.cpp b/boards/breezy/breezy_board.cpp index 6d3661ff..1b096b51 100644 --- a/boards/breezy/breezy_board.cpp +++ b/boards/breezy/breezy_board.cpp @@ -112,9 +112,19 @@ void BreezyBoard::serial_flush() bool BreezyBoard::enable_device(device_t device, hardware_config_t configuration, const Params ¶ms) { - (void)device; (void)configuration; - (void)params; + switch(device) + { + case Configuration::RC: + rc_init(); + break; + case Configuration::SERIAL: + serial_init(params.get_param_int(PARAM_BAUD_RATE), 0); + break; + default: + break; + } + return true; } @@ -347,9 +357,8 @@ void BreezyBoard::battery_current_set_multiplier(double multiplier) } // PWM -void BreezyBoard::rc_init(rc_type_t rc_type) +void BreezyBoard::rc_init() { - (void) rc_type; // TODO SBUS is not supported on F1 pwmInit(true, false, false, pwm_refresh_rate_, pwm_idle_pwm_); } diff --git a/boards/breezy/breezy_board.h b/boards/breezy/breezy_board.h index b73eceb5..f14e846b 100644 --- a/boards/breezy/breezy_board.h +++ b/boards/breezy/breezy_board.h @@ -154,7 +154,7 @@ class BreezyBoard : public Board // PWM // TODO make these deal in normalized (-1 to 1 or 0 to 1) values (not pwm-specific) - void rc_init(rc_type_t rc_type) override; + void rc_init() override; bool rc_lost() override; float rc_read(uint8_t channel) override; diff --git a/comms/mavlink/mavlink.cpp b/comms/mavlink/mavlink.cpp index f6386de3..bec63522 100644 --- a/comms/mavlink/mavlink.cpp +++ b/comms/mavlink/mavlink.cpp @@ -41,11 +41,8 @@ Mavlink::Mavlink(Board &board) : board_(board) {} -void Mavlink::init(uint32_t baud_rate, uint32_t dev) +void Mavlink::init() { - // TODO remove unneeded parameters - (void)baud_rate; - (void)dev; initialized_ = true; } diff --git a/comms/mavlink/mavlink.h b/comms/mavlink/mavlink.h index adb4e51a..57586278 100644 --- a/comms/mavlink/mavlink.h +++ b/comms/mavlink/mavlink.h @@ -54,7 +54,7 @@ class Mavlink : public CommLinkInterface { public: Mavlink(Board& board); - void init(uint32_t baud_rate, uint32_t dev) override; + void init() override; void receive() override; void send_attitude_quaternion(uint8_t system_id, diff --git a/include/board.h b/include/board.h index d4b51573..a2484c87 100644 --- a/include/board.h +++ b/include/board.h @@ -65,7 +65,6 @@ class Board virtual void clock_delay(uint32_t milliseconds) = 0; // serial - virtual void serial_init(uint32_t baud_rate, hardware_config_t configuration) = 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; @@ -115,7 +114,6 @@ class Board virtual void battery_current_set_multiplier(double multiplier) = 0; // RC - virtual void rc_init(rc_type_t rc_type) = 0; virtual bool rc_lost() = 0; virtual float rc_read(uint8_t channel) = 0; diff --git a/include/interface/comm_link.h b/include/interface/comm_link.h index 920da852..f38754b0 100644 --- a/include/interface/comm_link.h +++ b/include/interface/comm_link.h @@ -129,7 +129,7 @@ class CommLinkInterface virtual void config_request_callback(uint8_t device) = 0; }; - virtual void init(uint32_t baud_rate, uint32_t dev) = 0; + virtual void init() = 0; virtual void receive() = 0; // send functions diff --git a/include/param.h b/include/param.h index c1024a16..6a871b1c 100644 --- a/include/param.h +++ b/include/param.h @@ -46,7 +46,6 @@ enum : uint16_t /*** HARDWARE CONFIGURATION ***/ /******************************/ PARAM_BAUD_RATE = 0, - PARAM_SERIAL_DEVICE, /*****************************/ /*** MAVLINK CONFIGURATION ***/ @@ -158,7 +157,6 @@ enum : uint16_t /************************/ /*** RC CONFIGURATION ***/ /************************/ - PARAM_RC_TYPE, PARAM_RC_X_CHANNEL, PARAM_RC_Y_CHANNEL, PARAM_RC_Z_CHANNEL, diff --git a/src/comm_manager.cpp b/src/comm_manager.cpp index b5fef93c..0cae2455 100644 --- a/src/comm_manager.cpp +++ b/src/comm_manager.cpp @@ -81,8 +81,7 @@ CommManager::CommManager(ROSflight& rf, CommLinkInterface& comm_link) : // function definitions void CommManager::init() { - comm_link_.init(static_cast(RF_.params_.get_param_int(PARAM_BAUD_RATE)), - static_cast(RF_.params_.get_param_int(PARAM_SERIAL_DEVICE))); + comm_link_.init(); offboard_control_time_ = 0; send_params_index_ = PARAMS_COUNT; diff --git a/src/mixer.cpp b/src/mixer.cpp index eb03c274..c07995f8 100644 --- a/src/mixer.cpp +++ b/src/mixer.cpp @@ -57,7 +57,6 @@ void Mixer::param_change_callback(uint16_t param_id) init_mixing(); break; case PARAM_MOTOR_PWM_SEND_RATE: - case PARAM_RC_TYPE: init_PWM(); break; default: diff --git a/src/param.cpp b/src/param.cpp index 1ae8b1d4..554af9d7 100644 --- a/src/param.cpp +++ b/src/param.cpp @@ -117,7 +117,6 @@ void Params::set_defaults(void) /*** HARDWARE CONFIGURATION ***/ /******************************/ init_param_int(PARAM_BAUD_RATE, "BAUD_RATE", 921600); // Baud rate of MAVlink communication with companion computer | 9600 | 921600 - init_param_int(PARAM_SERIAL_DEVICE, "SERIAL_DEVICE", 0); // Serial Port (for supported devices) | 0 | 3 /*****************************/ /*** MAVLINK CONFIGURATION ***/ @@ -229,7 +228,6 @@ void Params::set_defaults(void) /************************/ /*** RC CONFIGURATION ***/ /************************/ - init_param_int(PARAM_RC_TYPE, "RC_TYPE", 0); // Type of RC input 0 - PPM, 1 - SBUS | 0 | 1 init_param_int(PARAM_RC_X_CHANNEL, "RC_X_CHN", 0); // RC input channel mapped to x-axis commands [0 - indexed] | 0 | 3 init_param_int(PARAM_RC_Y_CHANNEL, "RC_Y_CHN", 1); // RC input channel mapped to y-axis commands [0 - indexed] | 0 | 3 init_param_int(PARAM_RC_Z_CHANNEL, "RC_Z_CHN", 3); // RC input channel mapped to z-axis commands [0 - indexed] | 0 | 3 diff --git a/src/rc.cpp b/src/rc.cpp index 41e19f6d..9e823310 100644 --- a/src/rc.cpp +++ b/src/rc.cpp @@ -50,7 +50,6 @@ void RC::init() void RC::init_rc() { - RF_.board_.rc_init(static_cast(RF_.params_.get_param_int(PARAM_RC_TYPE))); init_sticks(); init_switches(); } @@ -59,9 +58,6 @@ 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: From 0b989f93e11f09cc1202a661950a5acf9302399f Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Tue, 14 Jan 2020 15:21:53 -0700 Subject: [PATCH 36/76] Get breezy building with changes --- boards/breezy/breezy_board.cpp | 7 +++---- boards/breezy/breezy_board.h | 6 +++--- boards/breezy/breezy_board_config_manager.cpp | 15 ++++++++------- boards/breezy/breezy_board_config_manager.h | 8 ++++---- 4 files changed, 18 insertions(+), 18 deletions(-) diff --git a/boards/breezy/breezy_board.cpp b/boards/breezy/breezy_board.cpp index 1b096b51..59148d5a 100644 --- a/boards/breezy/breezy_board.cpp +++ b/boards/breezy/breezy_board.cpp @@ -81,9 +81,8 @@ void BreezyBoard::clock_delay(uint32_t milliseconds) // serial -void BreezyBoard::serial_init(uint32_t baud_rate, hardware_config_t configuration) +void BreezyBoard::serial_init(uint32_t baud_rate) { - (void)configuration; Serial1 = uartOpen(USART1, NULL, baud_rate, MODE_RXTX); } @@ -119,7 +118,7 @@ bool BreezyBoard::enable_device(device_t device, hardware_config_t configuration rc_init(); break; case Configuration::SERIAL: - serial_init(params.get_param_int(PARAM_BAUD_RATE), 0); + serial_init(params.get_param_int(PARAM_BAUD_RATE)); break; default: break; @@ -128,7 +127,7 @@ bool BreezyBoard::enable_device(device_t device, hardware_config_t configuration return true; } -BreezyBoardConfigManager &BreezyBoard::get_board_config_manager() +const BreezyBoardConfigManager &BreezyBoard::get_board_config_manager() const { return config_manager_; } diff --git a/boards/breezy/breezy_board.h b/boards/breezy/breezy_board.h index f14e846b..bc289396 100644 --- a/boards/breezy/breezy_board.h +++ b/boards/breezy/breezy_board.h @@ -96,7 +96,7 @@ class BreezyBoard : public Board void clock_delay(uint32_t milliseconds) override; // serial - void serial_init(uint32_t baud_rate, hardware_config_t configuration) override; + void serial_init(uint32_t baud_rate); void serial_write(const uint8_t *src, size_t len) override; uint16_t serial_bytes_available() override; uint8_t serial_read() override; @@ -104,7 +104,7 @@ class BreezyBoard : public Board // hardware config bool enable_device(device_t device, hardware_config_t configuration, const Params ¶ms) override; - BreezyBoardConfigManager & get_board_config_manager() override; + const BreezyBoardConfigManager & get_board_config_manager() const override; // sensors void sensors_init() override; @@ -154,7 +154,7 @@ class BreezyBoard : public Board // PWM // TODO make these deal in normalized (-1 to 1 or 0 to 1) values (not pwm-specific) - void rc_init() override; + void rc_init(); bool rc_lost() override; float rc_read(uint8_t channel) override; diff --git a/boards/breezy/breezy_board_config_manager.cpp b/boards/breezy/breezy_board_config_manager.cpp index b6be8570..48b77c86 100644 --- a/boards/breezy/breezy_board_config_manager.cpp +++ b/boards/breezy/breezy_board_config_manager.cpp @@ -3,34 +3,35 @@ namespace rosflight_firmware { -hardware_config_t BreezyBoardConfigManager::get_max_config(device_t device) +hardware_config_t BreezyBoardConfigManager::get_max_config(device_t device) const { (void)device; return 0; } -ConfigManager::config_response BreezyBoardConfigManager::check_config_change(device_t device, hardware_config_t config) +ConfigManager::ConfigResponse BreezyBoardConfigManager::check_config_change(device_t device, hardware_config_t config, const ConfigManager &cm) const { (void)device; (void)config; - ConfigManager::config_response response; + (void)cm; + ConfigManager::ConfigResponse response; response.successful = false; response.reboot_required = false; strcpy(reinterpret_cast(response.message), "Feature unsupported on naze"); return response; } -void BreezyBoardConfigManager::get_device_name(device_t device, uint8_t (&name)[20]) +void BreezyBoardConfigManager::get_device_name(device_t device, char (&name)[BoardConfigManager::DEVICE_NAME_LENGTH]) const { (void)device; - strcpy(reinterpret_cast(name), "Unsupported"); + strcpy(name, "Unsupported"); } -void BreezyBoardConfigManager::get_config_name(device_t device, hardware_config_t config, uint8_t (&name)[20]) +void BreezyBoardConfigManager::get_config_name(device_t device, hardware_config_t config, char (&name)[BoardConfigManager::CONFIG_NAME_LENGTH]) const { (void)device; (void)config; - strcpy(reinterpret_cast(name), "Unsupported"); + strcpy(name, "Unsupported"); } } // namespace rosflight_firmware diff --git a/boards/breezy/breezy_board_config_manager.h b/boards/breezy/breezy_board_config_manager.h index 3e1e396a..f375041a 100644 --- a/boards/breezy/breezy_board_config_manager.h +++ b/boards/breezy/breezy_board_config_manager.h @@ -8,10 +8,10 @@ namespace rosflight_firmware class BreezyBoardConfigManager : public BoardConfigManager { public: - hardware_config_t get_max_config(device_t device) override; - ConfigManager::ConfigResponse check_config_change(device_t device, hardware_config_t config) override; - void get_device_name(device_t device, uint8_t (&name)[20]) override; - void get_config_name(device_t device, hardware_config_t config, uint8_t (&name)[20]) override; + hardware_config_t get_max_config(device_t device) const override; + ConfigManager::ConfigResponse check_config_change(device_t device, hardware_config_t config, const ConfigManager &cm) const override; + void get_device_name(device_t device, char (&name)[BoardConfigManager::DEVICE_NAME_LENGTH]) const override; + void get_config_name(device_t device, hardware_config_t config, char (&name)[BoardConfigManager::CONFIG_NAME_LENGTH]) const override; }; } // namespace rosflight_firmware From 02407e3284d1a505856fb3e26939c35c9f2d5ef2 Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Tue, 14 Jan 2020 15:28:43 -0700 Subject: [PATCH 37/76] Update test and test board to work with changes --- test/config_manager_test.cpp | 4 ++-- test/parameters_test.cpp | 1 - test/test_board.cpp | 15 ++++++++++++--- test/test_board.h | 6 +++--- test/test_board_config_manager.cpp | 11 ++++++----- test/test_board_config_manager.h | 8 ++++---- 6 files changed, 27 insertions(+), 18 deletions(-) diff --git a/test/config_manager_test.cpp b/test/config_manager_test.cpp index 8b65bef2..11d3a94d 100644 --- a/test/config_manager_test.cpp +++ b/test/config_manager_test.cpp @@ -35,7 +35,7 @@ TEST_F(ConfigManagerTest, SetValid) { device_t changed_device = Configuration::SERIAL; hardware_config_t config = 27; - ConfigManager::config_response response = rf.config_manager_.attempt_set_configuration(changed_device, config); + ConfigManager::ConfigResponse response = rf.config_manager_.attempt_set_configuration(changed_device, config); EXPECT_TRUE(response.successful); EXPECT_TRUE(response.reboot_required); EXPECT_EQ(std::string(reinterpret_cast(response.message)), "Succeed for testing"); @@ -50,7 +50,7 @@ TEST_F(ConfigManagerTest, SetInvalid) { device_t changed_device = Configuration::SERIAL; hardware_config_t config = 1; - ConfigManager::config_response response = rf.config_manager_.attempt_set_configuration(changed_device, config); + ConfigManager::ConfigResponse response = rf.config_manager_.attempt_set_configuration(changed_device, config); EXPECT_FALSE(response.successful); EXPECT_FALSE(response.reboot_required); EXPECT_EQ(std::string(reinterpret_cast(response.message)), "Fail for testing"); diff --git a/test/parameters_test.cpp b/test/parameters_test.cpp index 61f91087..7a9ac617 100644 --- a/test/parameters_test.cpp +++ b/test/parameters_test.cpp @@ -47,7 +47,6 @@ TEST(Parameters, DefaultParameters) EXPECT_PARAM_EQ_FLOAT(PARAM_MAG_Y_BIAS, 0.0f); EXPECT_PARAM_EQ_FLOAT(PARAM_MAG_Z_BIAS, 0.0f); EXPECT_PARAM_EQ_FLOAT(PARAM_BARO_BIAS, 0.0f); - EXPECT_PARAM_EQ_INT(PARAM_RC_TYPE, 0); EXPECT_PARAM_EQ_INT(PARAM_RC_X_CHANNEL, 0); EXPECT_PARAM_EQ_INT(PARAM_RC_Y_CHANNEL, 1); EXPECT_PARAM_EQ_INT(PARAM_RC_Z_CHANNEL, 3); diff --git a/test/test_board.cpp b/test/test_board.cpp index f2602c9b..2509858e 100644 --- a/test/test_board.cpp +++ b/test/test_board.cpp @@ -89,13 +89,22 @@ void testBoard::serial_flush() {} // Hardware config bool testBoard::enable_device(device_t device, hardware_config_t configuration, const Params ¶ms) { - (void)device; (void)configuration; (void)params; + switch(configuration) + { + case Configuration::SERIAL: + serial_init(0,0); + break; + case Configuration::RC: + rc_init(); + break; + } + return true; } -TestBoardConfigManager &testBoard::get_board_config_manager() +const TestBoardConfigManager &testBoard::get_board_config_manager() const { return config_manager_; } @@ -209,7 +218,7 @@ 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) {} +void testBoard::rc_init() {} bool testBoard::rc_lost() { return rc_lost_; } float testBoard::rc_read(uint8_t channel) { diff --git a/test/test_board.h b/test/test_board.h index 0f3e2d82..08750ea5 100644 --- a/test/test_board.h +++ b/test/test_board.h @@ -64,7 +64,7 @@ class testBoard : public Board void clock_delay(uint32_t milliseconds) override; // serial - void serial_init(uint32_t baud_rate, hardware_config_t configuration) override; + void serial_init(uint32_t baud_rate, hardware_config_t configuration); void serial_write(const uint8_t *src, size_t len) override; uint16_t serial_bytes_available() override; uint8_t serial_read() override; @@ -72,7 +72,7 @@ class testBoard : public Board // Hardware config bool enable_device(device_t device, hardware_config_t configuration, const Params ¶ms) override; - TestBoardConfigManager & get_board_config_manager() override; + const TestBoardConfigManager & get_board_config_manager() const override; // sensors void sensors_init() override; @@ -113,7 +113,7 @@ class testBoard : public Board void battery_current_set_multiplier(double multiplier) override; // RC - void rc_init(rc_type_t rc_type) override; + void rc_init(); bool rc_lost() override; float rc_read(uint8_t channel) override; diff --git a/test/test_board_config_manager.cpp b/test/test_board_config_manager.cpp index 94d41ee7..2b8045dc 100644 --- a/test/test_board_config_manager.cpp +++ b/test/test_board_config_manager.cpp @@ -4,15 +4,16 @@ namespace rosflight_firmware { -hardware_config_t TestBoardConfigManager::get_max_config(device_t device) +hardware_config_t TestBoardConfigManager::get_max_config(device_t device) const { (void)device; return 0; // This is not needed to test other software } -ConfigManager::config_response TestBoardConfigManager::check_config_change(device_t device, hardware_config_t config) +ConfigManager::ConfigResponse TestBoardConfigManager::check_config_change(device_t device, hardware_config_t config, const ConfigManager &cm) const { + (void)cm; // A couple variations are given for testing - ConfigManager::config_response response; + ConfigManager::ConfigResponse response; response.successful = true; response.reboot_required = true; if(device == Configuration::SERIAL && config == 1) @@ -25,12 +26,12 @@ ConfigManager::config_response TestBoardConfigManager::check_config_change(devic strcpy(reinterpret_cast(response.message), "Succeed for testing"); return response; } -void TestBoardConfigManager::get_device_name(device_t device, uint8_t (&name)[20]) +void TestBoardConfigManager::get_device_name(device_t device, char (&name)[BoardConfigManager::DEVICE_NAME_LENGTH]) const { std::string device_name = "device #" + std::to_string(static_cast(device)); strcpy(reinterpret_cast(name), device_name.c_str()); } -void TestBoardConfigManager::get_config_name(device_t device, hardware_config_t config, uint8_t (&name)[20]) +void TestBoardConfigManager::get_config_name(device_t device, hardware_config_t config, char (&name)[BoardConfigManager::CONFIG_NAME_LENGTH]) const { std::string config_name = "config " + std::to_string(static_cast(device)) + ","+std::to_string(static_cast(config)); strcpy(reinterpret_cast(name), config_name.c_str()); diff --git a/test/test_board_config_manager.h b/test/test_board_config_manager.h index e89fa36f..edf57799 100644 --- a/test/test_board_config_manager.h +++ b/test/test_board_config_manager.h @@ -8,10 +8,10 @@ namespace rosflight_firmware class TestBoardConfigManager : public BoardConfigManager { public: - hardware_config_t get_max_config(device_t device) override; - ConfigManager::ConfigResponse check_config_change(device_t device, hardware_config_t config) override; - void get_device_name(device_t device, uint8_t (&name)[20]) override; - void get_config_name(device_t device, hardware_config_t config, uint8_t (&name)[20]) override; + hardware_config_t get_max_config(device_t device) const override; + ConfigManager::ConfigResponse check_config_change(device_t device, hardware_config_t config, const ConfigManager &cm) const override; + void get_device_name(device_t device, char (&name)[BoardConfigManager::DEVICE_NAME_LENGTH]) const override; + void get_config_name(device_t device, hardware_config_t config, char (&name)[BoardConfigManager::CONFIG_NAME_LENGTH]) const override; }; } // rosflight_firmware From d87e2f8179ab9fa9ba752b52fdeef1e56b14494d Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Tue, 14 Jan 2020 15:48:33 -0700 Subject: [PATCH 38/76] Fix behavior of using VCP when connected, even when UART is enabled --- boards/airbourne/airbourne_board.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/boards/airbourne/airbourne_board.cpp b/boards/airbourne/airbourne_board.cpp index a57c8eed..0b70f170 100644 --- a/boards/airbourne/airbourne_board.cpp +++ b/boards/airbourne/airbourne_board.cpp @@ -75,12 +75,12 @@ void AirbourneBoard::clock_delay(uint32_t milliseconds) // serial void AirbourneBoard::serial_init(uint32_t baud_rate, hardware_config_t configuration) { + vcp_.init(); // VCP is always initialized, so that if UART is mistakenly enabled, it can still be used switch(configuration) { default: case AirbourneConfiguration::SERIAL_VCP: current_serial_ = &vcp_; - vcp_.init(); break; case AirbourneConfiguration::SERIAL_UART1: current_serial_ = &uart1_; @@ -99,6 +99,8 @@ void AirbourneBoard::serial_init(uint32_t baud_rate, hardware_config_t configura void AirbourneBoard::serial_write(const uint8_t *src, size_t len) { + if(vcp_.connected()) + current_serial_ = &vcp; current_serial_->write(src, len); } From 573e118bf2e63b0c63422d0cbd7221304542a57d Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Wed, 15 Jan 2020 15:57:39 -0700 Subject: [PATCH 39/76] Small Documentation fixes --- docs/user-guide/firmware-configuration.md | 4 ++-- docs/user-guide/hardware-setup.md | 4 ++-- docs/user-guide/parameter-configuration.md | 1 - 3 files changed, 4 insertions(+), 5 deletions(-) diff --git a/docs/user-guide/firmware-configuration.md b/docs/user-guide/firmware-configuration.md index acdcf9b1..1fffa9b4 100644 --- a/docs/user-guide/firmware-configuration.md +++ b/docs/user-guide/firmware-configuration.md @@ -1,6 +1,6 @@ # Firmware Configuration -The most recent versions of ROSflight allow you to specify the hardware setup of your aircraft in greater detail. These settings are dependant on your choice of flight controller. ROSflight does not support this feature on the Naze/Flip32. +The most recent versions of ROSflight allow you to specify the hardware setup of your aircraft in greater detail. These settings are dependent on your choice of flight controller. ROSflight does not support this feature on the Naze/Flip32. For each of a number of devices, there are several choices of configuration. Such configurations may specify the port, the protocol, or other settings for using the device. Many devices can be disabled entirely. Some devices may also use parameters. @@ -61,7 +61,7 @@ The serial connection is used to communicate with `rosflight_io`. Change with ca ####RC | Configuration | Number | port | Notes | | ------------- | ------ | ---- | ----- | -| PPM on Flex-IO|0|Flex-io|Does not conflict with UART on the Flex-IO port| +| PPM on Flex-IO|0|Flex-IO|Does not conflict with UART on the Flex-IO port| |SBUS on Main|1|Main|| ####Airspeed diff --git a/docs/user-guide/hardware-setup.md b/docs/user-guide/hardware-setup.md index d042cb68..379255e0 100644 --- a/docs/user-guide/hardware-setup.md +++ b/docs/user-guide/hardware-setup.md @@ -148,9 +148,9 @@ The flight controller communicates with the companion computer over a serial lin ### Using Secondary Serial Links -In the case of an F4 flight controller, which has a USB peripheral, the highest bandwidth connection will be the USB connector. However, UART3 can also be used to communicate with the companion computer if you desire a more secure connection (micro USB connectors have been known to disconnect in high vibrations), or if you would like to use a telemetry radio for remote control. +In the case of an F4 flight controller, which has a USB peripheral, the highest bandwidth connection will be the USB connector. However, UART can also be used to communicate with the companion computer if you desire a more secure connection (micro USB connectors have been known to disconnect in high vibrations), or if you would like to use a telemetry radio for remote control. -If a USB connection is detected on the USB peripheral, ROSflight will direct all communication through this port. However, if the `PARAM_SERIAL_DEVICE` parameter is set to `3` and the `PARAM_BAUD_RATE` parameter is set properly, then UART3 will be enabled when the USB connection is absent. +If a USB connection is detected on the USB peripheral, ROSflight will direct all communication through this port. However, if the `Serial` device is configured to use UART and the `PARAM_BAUD_RATE` parameter is set properly, then the UART will be enabled when the USB connection is absent. For details on how to set the serial configuration, see [Firmware Configuration](firmware-configuration.md) We have had the most sucess with the SiK radios (AKA 3DR telemetry radios). These require a 5V supply and ground and connect directly to the UART3 pins. We like the SiK radios because they can be easily configured using the `AT-commands`, which are used by [MissionPlanner](http://ardupilot.org/planner/) (Windows only), [sikset.py](https://community.emlid.com/t/sikset-py-a-python-script-to-easily-control-your-rfd900-3dr-radio-from-the-command-line/3654) or with the [AT-commands](http://files.rfdesign.com.au/Files/documents/Software%20manual.pdf) directly on the command line. There are a number of configuration options available which should be used to optimize the radios for their intended usage. diff --git a/docs/user-guide/parameter-configuration.md b/docs/user-guide/parameter-configuration.md index ee3ae408..7c75c8ea 100644 --- a/docs/user-guide/parameter-configuration.md +++ b/docs/user-guide/parameter-configuration.md @@ -107,7 +107,6 @@ This is a list of all ROSflight parameters, including their types, default value | Parameter | Description | Type | Default Value | Min | Max | |-----------|-------------|------|---------------|-----|-----| | BAUD_RATE | Baud rate of MAVlink communication with companion computer | int | 921600 | 9600 | 921600 | -| SERIAL_DEVICE | Serial Port (for supported devices) | int | 0 | 0 | 3 | | SYS_ID | Mavlink System ID | int | 1 | 1 | 255 | | STRM_HRTBT | Rate of heartbeat stream (Hz) | int | 1 | 0 | 1000 | | STRM_STATUS | Rate of status stream (Hz) | int | 10 | 0 | 1000 | From d71be80ce3f9f641ffc64db3998f461fca217435 Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Thu, 16 Jan 2020 11:05:28 -0700 Subject: [PATCH 40/76] Update documentation after reading through all of it --- docs/developer-guide/code-architecture.md | 8 ++++++++ docs/user-guide/firmware-configuration.md | 18 +++++++++--------- docs/user-guide/getting-started.md | 6 +++--- docs/user-guide/overview.md | 2 +- docs/user-guide/parameter-configuration.md | 4 ++-- docs/user-guide/rc-configuration.md | 2 +- 6 files changed, 24 insertions(+), 16 deletions(-) diff --git a/docs/developer-guide/code-architecture.md b/docs/developer-guide/code-architecture.md index ccf1da77..649e61b3 100644 --- a/docs/developer-guide/code-architecture.md +++ b/docs/developer-guide/code-architecture.md @@ -84,12 +84,20 @@ The operation of the state manager is defined by the following finite state mach ![state manager FSM](images/arming-fsm.svg) +### Config Manager +This module handles the configurations for various devices, such as sensors and the serial connection. Each configuration is stored as an integer. Configurations can be set from the companion computer over the serial connection. On startup, the config manager sends configurations to the board support layer to initialize devices. + +The config manager also interacts with the board config manager, which is provided by the board support layer. The board config manager provides information on available configurations (such as name, number of options, etc). Additionally, the board config manager checks if a config change is valid. If the board config manager rejects a change, it explains why in an error message. + ### Parameter Server This module handles all parameters for the flight stack. It supports the getting and setting of integer and floating-point parameters, and the saving of these parameters to non-volatile memory. Setting and getting of parameters from the companion computer is done through the serial communication interface. While no other data flow lines are shown on the diagram, all of the other modules interact with the parameter server. +### Memory Manager +The memory manager interfaces with the board support layer to read from and write to non-volatile memory. This memory is used by the parameter server and the config manager. + ### Comm Manager This module handles all serial communication between the flight controller and companion computer. This includes streaming data and receiving offboard control setpoints and other commands from the computer. diff --git a/docs/user-guide/firmware-configuration.md b/docs/user-guide/firmware-configuration.md index 1fffa9b4..12d913d3 100644 --- a/docs/user-guide/firmware-configuration.md +++ b/docs/user-guide/firmware-configuration.md @@ -2,7 +2,7 @@ The most recent versions of ROSflight allow you to specify the hardware setup of your aircraft in greater detail. These settings are dependent on your choice of flight controller. ROSflight does not support this feature on the Naze/Flip32. -For each of a number of devices, there are several choices of configuration. Such configurations may specify the port, the protocol, or other settings for using the device. Many devices can be disabled entirely. Some devices may also use parameters. +For each of a number of devices, there are several choices of configuration. Such configurations may specify the port, the protocol, or other settings for using the device. Many devices can be disabled entirely. Some devices may also have settings via [parameters](parameter-configuration.md). ## Getting Current Configurations The current configuration for a device can be read through the ROS service `config_get`, which requires `rosflight_io` to be running. The only parameter for the service is the name of the device. The name is case insensitive, and underscores may be used for spaces. For example, all of the following are valid: @@ -20,7 +20,9 @@ message: '' ``` ##Setting Configurations -Setting configurations is done similarly to getting them, but using the `config_set` service, which also requires `rosflight_io` to be running. The service takes the name of the device and the name of the configuration as parameters. The service is flexible in the name of the configuration. It is case insensitive and will recognize configurations from most single words in the name. For example the configuration `ADC3 on Power` may be called `adc3` or `power`. The keyword `default` refers to the default configuration for a device. In addition, the number for the configuration may be used. However, due to a quirk in rosservice, this must be prefaced with `!!str` and enclosed in single quotes when calling from the command line. All of the following examples are valid, and the first four are equivalent. +Setting configurations is done similarly to getting them, but using the `config_set` service, which also requires `rosflight_io` to be running. The service takes the name of the device and the name of the configuration as parameters. + +The service is flexible in the name of the configuration. It is case insensitive and will recognize configurations from most single words in the name. For example the configuration `ADC3 on Power` may be called `adc3` or `power`. The keyword `default` refers to the default configuration for a device. In addition, the number for the configuration may be used. However, due to a quirk in rosservice, this must be prefaced with `!!str` and enclosed in single quotes when calling from the command line. All of the following examples are valid, and the first four are equivalent. ``` rosservice call /config_set gnss "UART1 on Main" @@ -42,14 +44,14 @@ message: "Port is used by airspeed sensor." This response indicates that the configuration could not be set because the airspeed sensor uses the same port. It also indicates that no reboot is needed (in this case, because the configuration was not changed). ##Saving Configurations -Configurations are saved by calling the `settings_write` service. This service also saves parameters. +Configurations are saved by calling the `memory_write` service. This service also saves parameters. ##Configurations Available devices and configurations are dependent on the flight controller. The Naze/Flip32 does not support changing configurations. -For all boards, the default configuration is number 0. +For all boards, the default configuration is configuration #0. ###OpenPilot Revolution (Revo) Configurations ####Serial -The serial connection is used to communicate with `rosflight_io`. Change with caution, because doing so may make it difficult to change back. +See [Using Secondary Serial Links](hardware-setup.md#using-secondary-serial-links) for more details on this setting. | Configuration | Number | port | Notes | | ------------- | ------ | ---- | ----- | @@ -68,7 +70,7 @@ The serial connection is used to communicate with `rosflight_io`. Change with ca | Configuration | Number | port | Notes | | ------------- | ------ | ---- | ----- | |Disabled|0|None|| -|I2C2 on Flexi|1|Flexi|| +|I2C2 on Flexi|1|Flexi|Multiple I2C devices can share the port.| ####GNSS @@ -83,7 +85,7 @@ The serial connection is used to communicate with `rosflight_io`. Change with ca | Configuration | Number | port | Notes | | ------------- | ------ | ---- | ----- | |Disabled|0|None|| -|I2C2 on Flexi|1|Flexi|| +|I2C2 on Flexi|1|Flexi|Multiple I2C devices can share the port.| ####Battery Montior | Configuration | Number | port | Notes | @@ -92,7 +94,6 @@ The serial connection is used to communicate with `rosflight_io`. Change with ca |ADC3 on Power|1|PWR / Sonar|| ####Barometer -Future versions of ROSflight may support more options with barometers | Configuration | Number | port | Notes | | ------------- | ------ | ---- | ----- | @@ -100,7 +101,6 @@ Future versions of ROSflight may support more options with barometers |Onboard Barometer|1|None|| ####Magnetometer -Future versions of ROSflight may support more options with magnetometers | Configuration | Number | port | Notes | | ------------- | ------ | ---- | ----- | diff --git a/docs/user-guide/getting-started.md b/docs/user-guide/getting-started.md index 94b22c1a..71fb8613 100644 --- a/docs/user-guide/getting-started.md +++ b/docs/user-guide/getting-started.md @@ -17,8 +17,8 @@ The following checklists should help you get a new vehicle set up for the first ### General Setup - 1. Set the `FIXED_WING` parameter (`1` if a fixed-wing, `0` if a multirotor) - 2. Set the `RC_TYPE` parameter (`0` if PPM, `1` if SBUS) + 1. [Configure the firmware](firmware-configuration.md) for your specific RC, serial, and sensor setup + 2. Set the `FIXED_WING` parameter (`1` if a fixed-wing, `0` if a multirotor) 3. Set the `MIXER` parameter to the appropriate value described in the [Hardware Setup](hardware-setup.md) page 4. Set the `MOTOR_PWM_UPDATE` parameter (typically `490` for SimonK ESCs, `50` for standard servos) 5. Make sure your [RC transmitter is set up correctly](rc-configuration.md) @@ -27,7 +27,7 @@ The following checklists should help you get a new vehicle set up for the first * If you want to use a switch to enable RC override, set the `RC_ATT_OVRD_CHN` and `RC_THR_OVRD_CHN` parameters to the appropriate channel(s) (0-indexed). If you want complete control (attitude and throttle) when you flip the switch, set both these parameters to the same channel. 7. Calibrate your IMU: start `rosflight_io`, then run `rosservice call /calibrate_imu` 8. Complete the multirotor-specific or fixed-wing-specific checklist below - 9. Save the parameters (`rosservice call /param_write`) + 9. Save the parameters (`rosservice call /memory_write`) 10. You'll probably want to save a backup of your parameters (call `rosservice call /param_save_to_file /path/to/file.yml`) 11. Make sure you run through the [Preflight Checklist](preflight-checks.md) before flying diff --git a/docs/user-guide/overview.md b/docs/user-guide/overview.md index 24b01e73..be3ee6ba 100644 --- a/docs/user-guide/overview.md +++ b/docs/user-guide/overview.md @@ -6,7 +6,7 @@ This page provides an overview of the basic operation of the ROSflight firmware ROSflight is primarily intended to be used with a companion computer running ROS. The ROS interface is provided by the [rosflight_io](http://wiki.ros.org/rosflight) node. -All configuration of the flight controller is done through the ROS service API provided by `rosflight_io` (see the [parameter configuration](parameter-configuration.md) documentation page). +All configuration of the flight controller is done through the ROS service API provided by `rosflight_io` (see the [parameter configuration](parameter-configuration.md) and [firmware configuration](firmware-configuration.md) documentation pages). Sensor data such as IMU measurements are streamed from the flight controller to the companion computer and published as ROS topics. Control setpoints can also be sent to the flight controller by publishing to the appropriate ROS topic (see the [autonomous flight](autonomous-flight.md) documentation page). diff --git a/docs/user-guide/parameter-configuration.md b/docs/user-guide/parameter-configuration.md index 7c75c8ea..25d3c622 100644 --- a/docs/user-guide/parameter-configuration.md +++ b/docs/user-guide/parameter-configuration.md @@ -45,10 +45,10 @@ Notice that the parameters have been set, but not saved. Parameter changes take ### Writing Parameters -To ensure that parameter values persist between reboots, you must write the parameters to the non-volatile memory. This is done by calling `settings_write`. This also saves firmware configurations. +To ensure that parameter values persist between reboots, you must write the parameters to the non-volatile memory. This is done by calling `memory_write`. This also saves firmware configurations. ``` -rosservice call /settings_write +rosservice call /memory_write ``` diff --git a/docs/user-guide/rc-configuration.md b/docs/user-guide/rc-configuration.md index 0bcd3042..1f8a3c11 100644 --- a/docs/user-guide/rc-configuration.md +++ b/docs/user-guide/rc-configuration.md @@ -2,7 +2,7 @@ ## Binding your Transmitter to your Receiver -As of version 1.0, ROSflight only supports PPM receivers on F1 controllers, while F4 controllers support SBUS and PPM. If your transmitter/receiver setup only supports PWM output, we recommend using a PPM encoder such as the one [here](https://www.getfpv.com/holybro-ppm-encoder-module.html). Be sure to set the `RC_TYPE` parameter to `0` for PPM, or `1` for SBUS. +As of version 1.0, ROSflight only supports PPM receivers on F1 controllers, while F4 controllers support SBUS and PPM. If your transmitter/receiver setup only supports PWM output, we recommend using a PPM encoder such as the one [here](https://www.getfpv.com/holybro-ppm-encoder-module.html). Be sure to set the `RC` device to the correct configuration, as explained in [Firmware Configuration](firmware-configuration.md). Follow the instructions in your user manual to bind your transmitter to your RC receiver. You may also be able to find a guide on YouTube with instructions; just search for your particular transmitter and receiver model. From ce21fce0246ae452a0c7b9ba5edd87276c920a34 Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Thu, 16 Jan 2020 11:10:24 -0700 Subject: [PATCH 41/76] Add note on memory manager simplicity --- docs/developer-guide/code-architecture.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/developer-guide/code-architecture.md b/docs/developer-guide/code-architecture.md index 649e61b3..15678a43 100644 --- a/docs/developer-guide/code-architecture.md +++ b/docs/developer-guide/code-architecture.md @@ -96,7 +96,7 @@ Setting and getting of parameters from the companion computer is done through th While no other data flow lines are shown on the diagram, all of the other modules interact with the parameter server. ### Memory Manager -The memory manager interfaces with the board support layer to read from and write to non-volatile memory. This memory is used by the parameter server and the config manager. +The memory manager interfaces with the board support layer to read from and write to non-volatile memory. This memory is used by the parameter server and the config manager. At the time of this writing, there is almost no logic in the memory manager. Logic such as checksums are handled by the parameter server and config manager. ### Comm Manager This module handles all serial communication between the flight controller and companion computer. From 29b42c57fceb3b1bfc03406ab312195d515b9021 Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Thu, 16 Jan 2020 14:42:04 -0700 Subject: [PATCH 42/76] Doxygen style comments --- include/board_config_manager.h | 44 +++++++++++++++++++++------- include/config_manager.h | 53 ++++++++++++++++++++++++++++++---- include/memory_manager.h | 21 ++++++++++++++ 3 files changed, 102 insertions(+), 16 deletions(-) diff --git a/include/board_config_manager.h b/include/board_config_manager.h index 80efb90d..16bfa1eb 100644 --- a/include/board_config_manager.h +++ b/include/board_config_manager.h @@ -10,21 +10,45 @@ class BoardConfigManager { public: - // Get the largest number that is valid for the configuration of a given device - // This number is inclusive, i.e. a value of 2 means 0, 1, or 2 are valid - // For devices that are not configurable, returns 0 + /** + * @brief Get the largest number that is valid for the configuration of a given device + * This number is inclusive, i.e. a value of 2 means 0, 1, or 2 are valid + * For devices that are not configurable, returns 0 + * @param device Any device + */ virtual hardware_config_t get_max_config(device_t device) const = 0; - // Check if a config change is allowed. - // If the response indicates success, then the config manager accepts the change - // If not, the config manager returns the error indicated - // Implementations should not assume that either the device or the config are valid + /** + * @brief Check if a config change is allowed. + * If the response indicates success, then the config manager accepts the change + * If not, the config manager returns the error indicated + * Implementations should not assume that either the device or the config are valid + * @param device The device whose configuration is being changed + * @param config The new configuration for the device + * @param cm The ConfigManager with the current configurations + */ virtual ConfigManager::ConfigResponse check_config_change(device_t device, hardware_config_t config, const ConfigManager &cm) const = 0; static constexpr int DEVICE_NAME_LENGTH = 20; static constexpr int CONFIG_NAME_LENGTH = 20; - // Do not assume that the device or config are valid numbers - // When passed an invalid argument, it is better to return a string so indicating than an - // empty string + /** + * Do not assume that the device or config are valid numbers + * When passed an invalid argument, it is better to return a string so indicating than an + * empty string + */ + /** + * @brief Returns the name of the device + * Do not assume that the device number is valid. + * When passed an invalid device, it is better to return a string so indicating than an empty string + * @param device Any device + */ virtual void get_device_name(device_t device, char (&name)[DEVICE_NAME_LENGTH]) const=0; + /** + * @brief Returns the name of a configuration + * Do not assume that the device number or configuration are valid. + * When passed an invalid device or configuration, it is better to return a string so indicating + * than an empty string. + * @param device Any device + * @param config Any configuration + */ virtual void get_config_name(device_t device, hardware_config_t config, char (&name)[CONFIG_NAME_LENGTH]) const = 0; }; } // namespace rosflight_firmware diff --git a/include/config_manager.h b/include/config_manager.h index df25d7da..62173060 100644 --- a/include/config_manager.h +++ b/include/config_manager.h @@ -6,6 +6,12 @@ namespace rosflight_firmware { class ROSflight; +/** + * @brief A class for managing the configuration of various devices. + * Devices include the serial connection, RC, and sensors. Devices are represented by + * @ref device_t and configurations by @ref hardware_config_t + * @sa Configuration::device_t + */ class ConfigManager { public: @@ -16,22 +22,57 @@ class ConfigManager }; static constexpr int CONFIG_RESPONSE_MESSAGE_LENGTH = 50; + /** + * @brief A struct to hold responses to attempts to change configurations + */ struct ConfigResponse { - bool successful; - bool reboot_required; - char message[CONFIG_RESPONSE_MESSAGE_LENGTH]; // Primarily for error messages + bool successful; /** If the change was successfully made **/ + bool reboot_required; /** If a reboot is required for the change to take effect */ + char message[CONFIG_RESPONSE_MESSAGE_LENGTH]; /** An optional message, often an error message */ }; ConfigManager(ROSflight &RF, Config &config); // Reads the memory, and loads defaults if it is invalid. Call after the memory manager is ready + /** + * @brief Reads from memory, and loads defaults if invalid + * @pre Memory manager is initialized + * @return if the initialization suceeded + */ bool init(); - bool configure_devices() const; // Sends configurations to the board via the enable_device method - // Attempts to set a configuration, failing if the board config manager rejects it + /** + * @brief Sends configurations to the board via the enable_device method + * @pre The config manager is initialized + * @return if all devices were configured successfully + */ + bool configure_devices() const; + /** + * @brief Attempts to set a configuration, failing if the board config manager rejects it. + * If the board config manager does not reject the change, the change is made in the config manager. + * This does not save the change, and currently does not change any configuration until reboot. + * @param device Any device + * @param config The new configuration for the device + * @return A response, indicating if the change was successful, if a reboot is required for the + * change to take effect, and optionally a message (often an error message) + */ ConfigResponse attempt_set_configuration(device_t device, uint8_t config); + /** + * @brief Get the current configuration for a device. + * This does not necessarily reflect how hardware is currently loaded, as changes via + * attempt_set_configuration may require a reboot to take effect, but not to be reflected here. + * @param device Any device + * @return The current configuration for the device + */ uint8_t get_configuration(device_t device) const; - uint8_t operator[](device_t device) const; // same as get_configuration, for convenience + /** + * @brief Get the current configuration for a device. Alias for ConfigManager::get_configuration + * @see ConfigManager::get_configuration + */ + uint8_t operator[](device_t device) const; + /** + * @brief Prepares the checksum, so that the config struct can be saved to non-volatile memory + */ void prepare_write(); // prepares a checksum, so that the config struct can be saved private: diff --git a/include/memory_manager.h b/include/memory_manager.h index 4ae01db2..a63550e4 100644 --- a/include/memory_manager.h +++ b/include/memory_manager.h @@ -17,11 +17,32 @@ class MemoryManager }; MemoryManager(ROSflight &rf); + /** + * @brief Reads all memory from the board's persistent memory + * @return if the read was succesful + */ bool read_memory(); + /** + * @brief Writes all memory to the board's persistent memory + * Beforehand, calls prepare_write on both the parameter server and config manager + * @return if the write was successful + */ bool write_memory(); + /** + * @brief Checks if memory has been read successfully at some point in the past + * @return if memory has been successfully loaded + */ inline bool is_ready() {return ready_;} + /** + * @brief Get a pointer to the parameter structure, which was stored in persistent memory + * @return A pointer to the parameter struct + */ inline Params::params_t &get_params(){return memory_.params;} + /** + * @brief Get a pointer to the config structure, which was stored in persistent memory + * @return A pointer to the config struct + */ inline ConfigManager::Config &get_config(){return memory_.config;} private: From 14e48f6949b120853aa12642dc0668d14fc2a5ca Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Thu, 16 Jan 2020 15:21:25 -0700 Subject: [PATCH 43/76] More doxygen comments --- include/board_config_manager.h | 22 +++++++++++----------- include/config_manager.h | 10 +++++----- include/configuration_enum.h | 13 +++++++++++-- 3 files changed, 27 insertions(+), 18 deletions(-) diff --git a/include/board_config_manager.h b/include/board_config_manager.h index 16bfa1eb..3991a7c4 100644 --- a/include/board_config_manager.h +++ b/include/board_config_manager.h @@ -6,22 +6,27 @@ namespace rosflight_firmware { +/** + * @brief Board Config Managers handle board specific details of configurations. + * @details Such details include the names of devices and configurations, as well as checking + * The validity of configuration changes before they take effect. + */ class BoardConfigManager { public: /** * @brief Get the largest number that is valid for the configuration of a given device - * This number is inclusive, i.e. a value of 2 means 0, 1, or 2 are valid + * @details This number is inclusive, i.e. a value of 2 means 0, 1, or 2 are valid * For devices that are not configurable, returns 0 * @param device Any device */ virtual hardware_config_t get_max_config(device_t device) const = 0; /** * @brief Check if a config change is allowed. - * If the response indicates success, then the config manager accepts the change - * If not, the config manager returns the error indicated - * Implementations should not assume that either the device or the config are valid + * @details If the response indicates success, then the config manager accepts the change. + * If not, the config manager returns the error indicated. + * Implementations should not assume that either the device or the config are valid. * @param device The device whose configuration is being changed * @param config The new configuration for the device * @param cm The ConfigManager with the current configurations @@ -29,21 +34,16 @@ class BoardConfigManager virtual ConfigManager::ConfigResponse check_config_change(device_t device, hardware_config_t config, const ConfigManager &cm) const = 0; static constexpr int DEVICE_NAME_LENGTH = 20; static constexpr int CONFIG_NAME_LENGTH = 20; - /** - * Do not assume that the device or config are valid numbers - * When passed an invalid argument, it is better to return a string so indicating than an - * empty string - */ /** * @brief Returns the name of the device - * Do not assume that the device number is valid. + * @details Do not assume that the device number is valid. * When passed an invalid device, it is better to return a string so indicating than an empty string * @param device Any device */ virtual void get_device_name(device_t device, char (&name)[DEVICE_NAME_LENGTH]) const=0; /** * @brief Returns the name of a configuration - * Do not assume that the device number or configuration are valid. + * @details Do not assume that the device number or configuration are valid. * When passed an invalid device or configuration, it is better to return a string so indicating * than an empty string. * @param device Any device diff --git a/include/config_manager.h b/include/config_manager.h index 62173060..15de29e6 100644 --- a/include/config_manager.h +++ b/include/config_manager.h @@ -8,8 +8,8 @@ namespace rosflight_firmware class ROSflight; /** * @brief A class for managing the configuration of various devices. - * Devices include the serial connection, RC, and sensors. Devices are represented by - * @ref device_t and configurations by @ref hardware_config_t + * @details Devices include the serial connection, %RC, and sensors. Devices are represented by + * @ref[device_t] and configurations by @ref hardware_config_t * @sa Configuration::device_t */ class ConfigManager @@ -27,9 +27,9 @@ class ConfigManager */ struct ConfigResponse { - bool successful; /** If the change was successfully made **/ - bool reboot_required; /** If a reboot is required for the change to take effect */ - char message[CONFIG_RESPONSE_MESSAGE_LENGTH]; /** An optional message, often an error message */ + bool successful; /**< If the change was successfully made **/ + bool reboot_required; /**< If a reboot is required for the change to take effect */ + char message[CONFIG_RESPONSE_MESSAGE_LENGTH]; /**< An optional message, often an error message */ }; ConfigManager(ROSflight &RF, Config &config); diff --git a/include/configuration_enum.h b/include/configuration_enum.h index 50e9ff10..2e957b7f 100644 --- a/include/configuration_enum.h +++ b/include/configuration_enum.h @@ -3,9 +3,15 @@ #include +/** + * The namespace for Configuration options + */ namespace Configuration { +/** + * @brief An enumeration of configurable devices + */ enum device_t: uint8_t { SERIAL, @@ -16,9 +22,12 @@ enum device_t: uint8_t BATTERY_MONITOR, BAROMETER, MAGNETOMETER, - DEVICE_COUNT // make sure this is last + DEVICE_COUNT /** Date: Thu, 16 Jan 2020 15:22:08 -0700 Subject: [PATCH 44/76] More doxygen comments --- include/config_manager.h | 2 +- include/configuration_enum.h | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/include/config_manager.h b/include/config_manager.h index 15de29e6..78109c3b 100644 --- a/include/config_manager.h +++ b/include/config_manager.h @@ -9,7 +9,7 @@ class ROSflight; /** * @brief A class for managing the configuration of various devices. * @details Devices include the serial connection, %RC, and sensors. Devices are represented by - * @ref[device_t] and configurations by @ref hardware_config_t + * @ref device_t and configurations by @ref hardware_config_t * @sa Configuration::device_t */ class ConfigManager diff --git a/include/configuration_enum.h b/include/configuration_enum.h index 2e957b7f..8a2861a0 100644 --- a/include/configuration_enum.h +++ b/include/configuration_enum.h @@ -25,6 +25,7 @@ enum device_t: uint8_t DEVICE_COUNT /**(0)}; /** * @brief Allows incrementing device_t's for use in for loops stops incrementing past DEVICE_COUNT */ From 77e8527cc64923fc053c580af573770b44adfb4f Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Thu, 16 Jan 2020 15:25:38 -0700 Subject: [PATCH 45/76] Add FIRST_DEVICE constant --- src/config_manager.cpp | 2 +- test/config_manager_test.cpp | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/config_manager.cpp b/src/config_manager.cpp index fc25fc8f..aa18f654 100644 --- a/src/config_manager.cpp +++ b/src/config_manager.cpp @@ -18,7 +18,7 @@ bool ConfigManager::init() bool ConfigManager::configure_devices() const { bool success = true; - for(device_t device{static_cast(0)}; device < Configuration::DEVICE_COUNT; ++device) + for(device_t device{Configuration::FIRST_DEVICE}; device < Configuration::DEVICE_COUNT; ++device) success = RF_.board_.enable_device(device, config_.config[device], RF_.params_) && success; return success; } diff --git a/test/config_manager_test.cpp b/test/config_manager_test.cpp index 11d3a94d..bd19137f 100644 --- a/test/config_manager_test.cpp +++ b/test/config_manager_test.cpp @@ -27,7 +27,7 @@ class ConfigManagerTest : public ::testing::Test TEST_F(ConfigManagerTest, DefaultValues) { - for(device_t device{static_cast(0)}; device < Configuration::DEVICE_COUNT; ++device) + for(device_t device{Configuration::FIRST_DEVICE}; device < Configuration::DEVICE_COUNT; ++device) EXPECT_EQ(rf.config_manager_[device], 0); } @@ -39,7 +39,7 @@ TEST_F(ConfigManagerTest, SetValid) EXPECT_TRUE(response.successful); EXPECT_TRUE(response.reboot_required); EXPECT_EQ(std::string(reinterpret_cast(response.message)), "Succeed for testing"); - for(device_t device{static_cast(0)}; device < Configuration::DEVICE_COUNT; ++device) + for(device_t device{Configuration::FIRST_DEVICE}; device < Configuration::DEVICE_COUNT; ++device) if(device == changed_device) EXPECT_EQ(rf.config_manager_[device], config); else @@ -54,6 +54,6 @@ TEST_F(ConfigManagerTest, SetInvalid) EXPECT_FALSE(response.successful); EXPECT_FALSE(response.reboot_required); EXPECT_EQ(std::string(reinterpret_cast(response.message)), "Fail for testing"); - for(device_t device{static_cast(0)}; device < Configuration::DEVICE_COUNT; ++device) + for(device_t device{Configuration::FIRST_DEVICE}; device < Configuration::DEVICE_COUNT; ++device) EXPECT_EQ(rf.config_manager_[device], 0); } From 71cd17f9779d0f780f888112e207e77d5f9cbc65 Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Thu, 16 Jan 2020 15:26:01 -0700 Subject: [PATCH 46/76] Fix simple typo --- boards/airbourne/airbourne_board.cpp | 2 +- boards/airbourne/airbourne_board_config_manager.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/boards/airbourne/airbourne_board.cpp b/boards/airbourne/airbourne_board.cpp index 0b70f170..9983b4ed 100644 --- a/boards/airbourne/airbourne_board.cpp +++ b/boards/airbourne/airbourne_board.cpp @@ -100,7 +100,7 @@ void AirbourneBoard::serial_init(uint32_t baud_rate, hardware_config_t configura void AirbourneBoard::serial_write(const uint8_t *src, size_t len) { if(vcp_.connected()) - current_serial_ = &vcp; + current_serial_ = &vcp_; current_serial_->write(src, len); } diff --git a/boards/airbourne/airbourne_board_config_manager.cpp b/boards/airbourne/airbourne_board_config_manager.cpp index cfe67c71..6e742e16 100644 --- a/boards/airbourne/airbourne_board_config_manager.cpp +++ b/boards/airbourne/airbourne_board_config_manager.cpp @@ -55,7 +55,7 @@ ConfigManager::ConfigResponse AirbourneBoardConfigManager::check_config_change(d case Configuration::SERIAL: case Configuration::GNSS: if(port != NO_PORT) - for(device_t other_device{static_cast(0)}; other_device != Configuration::DEVICE_COUNT; ++other_device) + for(device_t other_device{Configuration::FIRST_DEVICE}; other_device != Configuration::DEVICE_COUNT; ++other_device) { if(other_device == Configuration::RC && cm[Configuration::RC] == AirbourneConfiguration::RC_PPM) // RC over PPM does not conflict with UART, even though both use the same port continue; From b5362ac4dfa04f0fdf46639c6d1081d5cfa55d2c Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Fri, 17 Jan 2020 14:59:40 -0700 Subject: [PATCH 47/76] Comment clean up --- include/config_manager.h | 4 ++-- include/configuration_enum.h | 8 +++++++- 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/include/config_manager.h b/include/config_manager.h index 78109c3b..d58a07aa 100644 --- a/include/config_manager.h +++ b/include/config_manager.h @@ -33,9 +33,9 @@ class ConfigManager }; ConfigManager(ROSflight &RF, Config &config); - // Reads the memory, and loads defaults if it is invalid. Call after the memory manager is ready /** - * @brief Reads from memory, and loads defaults if invalid + * @brief Reads from memory, and loads defaults if invalid. + * @details By convention, all default values are 0. * @pre Memory manager is initialized * @return if the initialization suceeded */ diff --git a/include/configuration_enum.h b/include/configuration_enum.h index 8a2861a0..ff72d58b 100644 --- a/include/configuration_enum.h +++ b/include/configuration_enum.h @@ -3,6 +3,11 @@ #include +/** + * Test + */ +namespace rosflight_firmware +{ /** * The namespace for Configuration options */ @@ -27,7 +32,7 @@ enum device_t: uint8_t constexpr device_t FIRST_DEVICE{static_cast(0)}; /** - * @brief Allows incrementing device_t's for use in for loops stops incrementing past DEVICE_COUNT + * @brief Allows incrementing device_t's for use in for loops. Stops incrementing past DEVICE_COUNT */ inline device_t& operator++(device_t &dev) { @@ -42,5 +47,6 @@ inline device_t& operator++(device_t &dev) } typedef uint8_t hardware_config_t; typedef Configuration::device_t device_t; /**< typedef'd for your convenience */ +} #endif // CONFIGURATION_ENUM_H From 243d34bc3e5f8263dd2bdeef048f7950c9f136c7 Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Fri, 24 Jan 2020 14:12:20 -0700 Subject: [PATCH 48/76] fix checksum for ConfigManager --- src/config_manager.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/config_manager.cpp b/src/config_manager.cpp index aa18f654..248ad8cd 100644 --- a/src/config_manager.cpp +++ b/src/config_manager.cpp @@ -79,9 +79,11 @@ uint32_t ConfigManager::generate_checksum() const for(size_t index{0}; index< sizeof(config_.config); index++ ) { check_a += config_data[index]; + check_a %= (UINT8_MAX - 1); check_b += check_a; + check_b %= (UINT8_MAX - 1); } - return check_a & (check_b<<8) & (~check_a<<16) & (~check_b<<24); + return check_a | (check_b<<8) | (~check_a<<16) | (~check_b<<24); } } From 648d12f124b57ffdfbfcde6cced7484a68222fae Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Fri, 24 Jan 2020 14:13:42 -0700 Subject: [PATCH 49/76] Add checks to not update disabled sensors --- boards/airbourne/airbourne | 2 +- boards/airbourne/airbourne_board.cpp | 13 +++++++++---- 2 files changed, 10 insertions(+), 5 deletions(-) diff --git a/boards/airbourne/airbourne b/boards/airbourne/airbourne index ce8c0ab2..0c7f51c3 160000 --- a/boards/airbourne/airbourne +++ b/boards/airbourne/airbourne @@ -1 +1 @@ -Subproject commit ce8c0ab25670bd63a53652a04a150ec30e6b970a +Subproject commit 0c7f51c306dd073cf7d45889b5a343596472b1d9 diff --git a/boards/airbourne/airbourne_board.cpp b/boards/airbourne/airbourne_board.cpp index 9983b4ed..30aa6226 100644 --- a/boards/airbourne/airbourne_board.cpp +++ b/boards/airbourne/airbourne_board.cpp @@ -248,7 +248,8 @@ bool AirbourneBoard::mag_present() void AirbourneBoard::mag_update() { - mag_.update(); + if(mag_.is_initialized()) + mag_.update(); } void AirbourneBoard::mag_read(float mag[3]) @@ -264,7 +265,8 @@ bool AirbourneBoard::baro_present() void AirbourneBoard::baro_update() { - baro_.update(); + if(baro_.is_initialized()) + baro_.update(); } void AirbourneBoard::baro_read(float *pressure, float *temperature) @@ -275,7 +277,9 @@ void AirbourneBoard::baro_read(float *pressure, float *temperature) bool AirbourneBoard::diff_pressure_present() { - return airspeed_.present(); + if(airspeed_.is_initialized()) + return airspeed_.present(); + return false; } void AirbourneBoard::diff_pressure_update() @@ -299,7 +303,8 @@ bool AirbourneBoard::sonar_present() void AirbourneBoard::sonar_update() { - sonar_.update(); + if(sonar_.is_initialized()) + sonar_.update(); } float AirbourneBoard::sonar_read() From 93b9dd90710258ec06886bb6dfd9105957f1a93e Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Mon, 16 Mar 2020 17:21:25 -0600 Subject: [PATCH 50/76] Add rebased airbourne --- boards/airbourne/airbourne | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/boards/airbourne/airbourne b/boards/airbourne/airbourne index 0c7f51c3..30927111 160000 --- a/boards/airbourne/airbourne +++ b/boards/airbourne/airbourne @@ -1 +1 @@ -Subproject commit 0c7f51c306dd073cf7d45889b5a343596472b1d9 +Subproject commit 309271115644612cef46ae22a3a702f93d5c674a From 7fb180940f92eaa4318359ed17aea8427fc9952b Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Mon, 16 Mar 2020 17:21:39 -0600 Subject: [PATCH 51/76] Fix switch fallthrough --- boards/airbourne/airbourne_board.cpp | 1 + boards/airbourne/airbourne_board_config_manager.cpp | 1 + 2 files changed, 2 insertions(+) diff --git a/boards/airbourne/airbourne_board.cpp b/boards/airbourne/airbourne_board.cpp index 30aa6226..641fdd24 100644 --- a/boards/airbourne/airbourne_board.cpp +++ b/boards/airbourne/airbourne_board.cpp @@ -190,6 +190,7 @@ bool AirbourneBoard::enable_device(device_t device, hardware_config_t configurat int_i2c_.init(&i2c_config[BARO_I2C]); mag_.init(&int_i2c_); } + break; default: return false; } diff --git a/boards/airbourne/airbourne_board_config_manager.cpp b/boards/airbourne/airbourne_board_config_manager.cpp index 6e742e16..deddce76 100644 --- a/boards/airbourne/airbourne_board_config_manager.cpp +++ b/boards/airbourne/airbourne_board_config_manager.cpp @@ -52,6 +52,7 @@ ConfigManager::ConfigResponse AirbourneBoardConfigManager::check_config_change(d case Configuration::RC: if(config ==AirbourneConfiguration::RC_PPM) // PPM is not known to conflict with anything break; + [[gnu::fallthrough]]; case Configuration::SERIAL: case Configuration::GNSS: if(port != NO_PORT) From 5a519c4fa281e73701fd6ed1817f4ffb77cbc139 Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Wed, 18 Mar 2020 13:39:28 -0600 Subject: [PATCH 52/76] prevent disabled sensors from appearing --- boards/airbourne/airbourne_board.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/boards/airbourne/airbourne_board.cpp b/boards/airbourne/airbourne_board.cpp index 641fdd24..fe70dbf4 100644 --- a/boards/airbourne/airbourne_board.cpp +++ b/boards/airbourne/airbourne_board.cpp @@ -255,12 +255,12 @@ void AirbourneBoard::mag_update() void AirbourneBoard::mag_read(float mag[3]) { - mag_.update(); + mag_update(); mag_.read(mag); } bool AirbourneBoard::baro_present() { - baro_.update(); + baro_update(); return baro_.present(); } From 69340fe7fe8c74d13a66bcc442702d3ef15e56d0 Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Tue, 24 Mar 2020 13:58:03 -0600 Subject: [PATCH 53/76] Explicitly initialize some fields --- include/comm_manager.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/comm_manager.h b/include/comm_manager.h index b74855e2..2269f6b2 100644 --- a/include/comm_manager.h +++ b/include/comm_manager.h @@ -83,8 +83,8 @@ class CommManager : public CommLinkInterface::ListenerInterface, public ParamLis ROSflight& RF_; CommLinkInterface& comm_link_; uint8_t send_params_index_; - device_t send_device_info_index_; - hardware_config_t send_config_info_index_; + device_t send_device_info_index_{Configuration::DEVICE_COUNT}; + hardware_config_t send_config_info_index_{0}; bool initialized_ = false; bool connected_ = false; From 2b8bd6eba456efe08458a6c5c9f5a91ee3a6f38c Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Tue, 24 Mar 2020 14:18:09 -0600 Subject: [PATCH 54/76] Fix buffer overflow, add documentation to prevent, abbreviate baro and mag --- boards/airbourne/airbourne_board_config_manager.cpp | 8 ++++---- include/board_config_manager.h | 9 ++++++--- 2 files changed, 10 insertions(+), 7 deletions(-) diff --git a/boards/airbourne/airbourne_board_config_manager.cpp b/boards/airbourne/airbourne_board_config_manager.cpp index deddce76..90c59dee 100644 --- a/boards/airbourne/airbourne_board_config_manager.cpp +++ b/boards/airbourne/airbourne_board_config_manager.cpp @@ -140,10 +140,10 @@ void AirbourneBoardConfigManager::get_device_name(device_t device, char (&name)[ strcpy(name, "Battery Monitor"); break; case Configuration::BAROMETER: - strcpy(name, "Barometer"); + strcpy(name, "Baro"); break; case Configuration::MAGNETOMETER: - strcpy(name, "Magnetometer"); + strcpy(name, "Mag"); break; default: strcpy(name, "Error/Unsupported"); @@ -219,13 +219,13 @@ void AirbourneBoardConfigManager::get_config_name(device_t device, hardware_conf if(config==AirbourneConfiguration::BAROMETER_DISABLED) strcpy(name, "Disabled"); else if(config == AirbourneConfiguration::BAROMETER_ONBOARD) - strcpy(name, "Onboard barometer"); + strcpy(name, "Onboard baro"); break; case Configuration::MAGNETOMETER: if(config ==AirbourneConfiguration::MAGNETOMETER_DISABLED) strcpy(name, "Disabled"); else if(config == AirbourneConfiguration::MAGNETOMETER_ONBOARD) - strcpy(name, "Onboard magnetometer"); + strcpy(name, "Onboard mag"); break; default: strcpy(name, "Invalid device"); diff --git a/include/board_config_manager.h b/include/board_config_manager.h index 3991a7c4..3753442c 100644 --- a/include/board_config_manager.h +++ b/include/board_config_manager.h @@ -1,4 +1,5 @@ #ifndef BOARD_CONFIG_MANAGER_H + #define BOARD_CONFIG_MANAGER_H #include "configuration_enum.h" @@ -32,12 +33,13 @@ class BoardConfigManager * @param cm The ConfigManager with the current configurations */ virtual ConfigManager::ConfigResponse check_config_change(device_t device, hardware_config_t config, const ConfigManager &cm) const = 0; - static constexpr int DEVICE_NAME_LENGTH = 20; - static constexpr int CONFIG_NAME_LENGTH = 20; + static constexpr int DEVICE_NAME_LENGTH = 20; /**< This includes the null terminator, so 19 is the practical maximum.*/ + static constexpr int CONFIG_NAME_LENGTH = 20; /**< This includes the null terminator, so 19 is the practical maximum.*/ /** * @brief Returns the name of the device * @details Do not assume that the device number is valid. - * When passed an invalid device, it is better to return a string so indicating than an empty string + * When passed an invalid device, it is better to return a string so indicating than an empty string. + * Note the requirement for a null terminator at the end of the string. * @param device Any device */ virtual void get_device_name(device_t device, char (&name)[DEVICE_NAME_LENGTH]) const=0; @@ -46,6 +48,7 @@ class BoardConfigManager * @details Do not assume that the device number or configuration are valid. * When passed an invalid device or configuration, it is better to return a string so indicating * than an empty string. + * Note the requirement for a null terminator at the end of the string. * @param device Any device * @param config Any configuration */ From 57892ee61326eefa0d18c9c110470354fa3e749a Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Tue, 24 Mar 2020 15:07:49 -0600 Subject: [PATCH 55/76] Fix indentation --- scripts/rosflight.mk | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/scripts/rosflight.mk b/scripts/rosflight.mk index e91e5307..ca89f073 100644 --- a/scripts/rosflight.mk +++ b/scripts/rosflight.mk @@ -40,8 +40,8 @@ ROSFLIGHT_SRC = rosflight.cpp \ rc.cpp \ mixer.cpp \ nanoprintf.cpp \ - config_manager.cpp \ - memory_manager.cpp \ + config_manager.cpp \ + memory_manager.cpp \ # Math Source Files VPATH := $(VPATH):$(TURBOMATH_DIR) From aa60907d5424b7956b562b69b3c5ed5b5e64724a Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Tue, 24 Mar 2020 15:08:07 -0600 Subject: [PATCH 56/76] Update airbourne to avoid sending baro/mag before initialization --- boards/airbourne/airbourne | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/boards/airbourne/airbourne b/boards/airbourne/airbourne index 30927111..1d52f046 160000 --- a/boards/airbourne/airbourne +++ b/boards/airbourne/airbourne @@ -1 +1 @@ -Subproject commit 309271115644612cef46ae22a3a702f93d5c674a +Subproject commit 1d52f046b4ca328fda9b0dd047de237b43f1b128 From 4e097aa90ded1eb48f77b368d1c0843bacd40ecd Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Tue, 24 Mar 2020 15:45:09 -0600 Subject: [PATCH 57/76] Cleanup, as requested in review --- boards/airbourne/airbourne_board.h | 2 +- boards/airbourne/airbourne_board_config_manager.cpp | 13 +++++++++++-- boards/airbourne/airbourne_board_config_manager.h | 5 ++--- 3 files changed, 14 insertions(+), 6 deletions(-) diff --git a/boards/airbourne/airbourne_board.h b/boards/airbourne/airbourne_board.h index 57418310..15e3f248 100644 --- a/boards/airbourne/airbourne_board.h +++ b/boards/airbourne/airbourne_board.h @@ -60,10 +60,10 @@ #include "analog_digital_converter.h" #include "analog_pin.h" #include "battery_monitor.h" -#include "airbourne_board_config_manager.h" // #include "ublox.h" #include "board.h" +#include "airbourne_board_config_manager.h" namespace rosflight_firmware { diff --git a/boards/airbourne/airbourne_board_config_manager.cpp b/boards/airbourne/airbourne_board_config_manager.cpp index 90c59dee..d187e43e 100644 --- a/boards/airbourne/airbourne_board_config_manager.cpp +++ b/boards/airbourne/airbourne_board_config_manager.cpp @@ -23,6 +23,7 @@ ConfigManager::ConfigResponse AirbourneBoardConfigManager::check_config_change(d ConfigManager::ConfigResponse resp; resp.reboot_required = false; resp.successful = false; + resp.message[0] = 0; // Just in case, because a string without a null terminator causes problems if(device >= Configuration::DEVICE_COUNT) { @@ -52,20 +53,25 @@ ConfigManager::ConfigResponse AirbourneBoardConfigManager::check_config_change(d case Configuration::RC: if(config ==AirbourneConfiguration::RC_PPM) // PPM is not known to conflict with anything break; - [[gnu::fallthrough]]; + [[gnu::fallthrough]]; // Fallthrough is intentional case Configuration::SERIAL: case Configuration::GNSS: if(port != NO_PORT) + { for(device_t other_device{Configuration::FIRST_DEVICE}; other_device != Configuration::DEVICE_COUNT; ++other_device) { - if(other_device == Configuration::RC && cm[Configuration::RC] == AirbourneConfiguration::RC_PPM) // RC over PPM does not conflict with UART, even though both use the same port + if(other_device == device) continue; + if(other_device == Configuration::RC) + if(cm[Configuration::RC] == AirbourneConfiguration::RC_PPM) // RC over PPM does not conflict with UART, even though both use the same port + continue; if(port == get_port(other_device, cm[other_device])) { conflict_device = other_device; break; } } + } break; case Configuration::AIRSPEED: case Configuration::SONAR: @@ -152,6 +158,9 @@ void AirbourneBoardConfigManager::get_device_name(device_t device, char (&name)[ } void AirbourneBoardConfigManager::get_config_name(device_t device, hardware_config_t config, char (&name)[CONFIG_NAME_LENGTH]) const { + name[0] = 0; // Prevents a string without a terminator being sent in case of programmer error + if(device >= Configuration::DEVICE_COUNT) // Although this is handled in the switch statement, this is so that it doesn't attempt to overflow max_configs + strcpy(name, "Invalid device"); if(config > AirbourneBoardConfigManager::max_configs[device]) strcpy(name, "Invalid config"); else diff --git a/boards/airbourne/airbourne_board_config_manager.h b/boards/airbourne/airbourne_board_config_manager.h index a53dc3c7..2bbc3aa6 100644 --- a/boards/airbourne/airbourne_board_config_manager.h +++ b/boards/airbourne/airbourne_board_config_manager.h @@ -16,6 +16,8 @@ class AirbourneBoardConfigManager : public BoardConfigManager void get_device_name(device_t device, char (&name)[DEVICE_NAME_LENGTH]) const override; void get_config_name(device_t device, hardware_config_t config, char (&name)[CONFIG_NAME_LENGTH]) const override; private: + static constexpr hardware_config_t max_configs[Configuration::DEVICE_COUNT]{3, 1, 1, 3, 1, 1, 1, 1}; + enum Port { NO_PORT, @@ -27,9 +29,6 @@ class AirbourneBoardConfigManager : public BoardConfigManager INTERNAL_I2C }; Port get_port(uint8_t device, uint8_t config) const; // Get the port used by a given configuration - - ROSflight *RF_; - static constexpr hardware_config_t max_configs[Configuration::DEVICE_COUNT]{3, 1, 1, 3, 1, 1, 1, 1}; }; } // namespace rosflight_firmware From 1e07c86e81607e94a4293b990321ec4fede53fef Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Wed, 25 Mar 2020 16:28:48 -0600 Subject: [PATCH 58/76] Send number of devices --- comms/mavlink/mavlink.cpp | 4 ++-- comms/mavlink/mavlink.h | 2 +- comms/mavlink/v1.0 | 2 +- include/interface/comm_link.h | 2 +- src/comm_manager.cpp | 2 +- 5 files changed, 6 insertions(+), 6 deletions(-) diff --git a/comms/mavlink/mavlink.cpp b/comms/mavlink/mavlink.cpp index bec63522..8b9f0499 100644 --- a/comms/mavlink/mavlink.cpp +++ b/comms/mavlink/mavlink.cpp @@ -295,10 +295,10 @@ void Mavlink::send_config_value(uint8_t system_id, uint8_t device, uint8_t confi send_message(msg); } -void Mavlink::send_device_info(uint8_t system_id, uint8_t device, uint8_t max_config, char (&name)[BoardConfigManager::DEVICE_NAME_LENGTH]) +void Mavlink::send_device_info(uint8_t system_id, uint8_t device, uint8_t max_config, char (&name)[BoardConfigManager::DEVICE_NAME_LENGTH], uint8_t num_devices) { mavlink_message_t msg; - mavlink_msg_rosflight_device_info_pack(system_id, 0, &msg, device, max_config, reinterpret_cast(name)); + mavlink_msg_rosflight_device_info_pack(system_id, 0, &msg, device, max_config, reinterpret_cast(name), num_devices); send_message(msg); } diff --git a/comms/mavlink/mavlink.h b/comms/mavlink/mavlink.h index 57586278..434c0401 100644 --- a/comms/mavlink/mavlink.h +++ b/comms/mavlink/mavlink.h @@ -85,7 +85,7 @@ class Mavlink : public CommLinkInterface float value, uint16_t param_count) override; void send_config_value(uint8_t system_id, uint8_t device, uint8_t config) override; - void send_device_info(uint8_t system_id, uint8_t device, uint8_t max_config, char (&name)[BoardConfigManager::DEVICE_NAME_LENGTH]) override; + void send_device_info(uint8_t system_id, uint8_t device, uint8_t max_config, char (&name)[BoardConfigManager::DEVICE_NAME_LENGTH], uint8_t num_devices) override; void send_config_info(uint8_t system_id, uint8_t device, uint8_t config, char (&name)[BoardConfigManager::CONFIG_NAME_LENGTH]) override; void send_config_status(uint8_t system_id, uint8_t device, bool success, bool reboot_required, char (&error_message)[ConfigManager::CONFIG_RESPONSE_MESSAGE_LENGTH]) override; void send_rc_raw(uint8_t system_id, uint32_t timestamp_ms, const uint16_t channels[8]) override; diff --git a/comms/mavlink/v1.0 b/comms/mavlink/v1.0 index 4a8fd733..cefd4a71 160000 --- a/comms/mavlink/v1.0 +++ b/comms/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit 4a8fd73347a95b16488eec6f56d7a4ab93bb87b5 +Subproject commit cefd4a713882140fc010018f9c1b057a74334784 diff --git a/include/interface/comm_link.h b/include/interface/comm_link.h index f38754b0..fa4103a3 100644 --- a/include/interface/comm_link.h +++ b/include/interface/comm_link.h @@ -163,7 +163,7 @@ class CommLinkInterface float value, uint16_t param_count) = 0; virtual void send_config_value(uint8_t system_id, uint8_t device, uint8_t config) = 0; - virtual void send_device_info(uint8_t system_id, uint8_t device, uint8_t max_config, char (&name)[BoardConfigManager::DEVICE_NAME_LENGTH]) = 0; + virtual void send_device_info(uint8_t system_id, uint8_t device, uint8_t max_config, char (&name)[BoardConfigManager::DEVICE_NAME_LENGTH], uint8_t num_devices) = 0; virtual void send_config_info(uint8_t system_id, uint8_t device, uint8_t config, char (&name)[BoardConfigManager::CONFIG_NAME_LENGTH]) = 0; virtual void send_config_status(uint8_t system_id, uint8_t device, bool success, bool reboot_required, char (&error_message)[ConfigManager::CONFIG_RESPONSE_MESSAGE_LENGTH]) = 0; virtual void send_rc_raw(uint8_t system_id, uint32_t timestamp_ms, const uint16_t channels[8]) = 0; diff --git a/src/comm_manager.cpp b/src/comm_manager.cpp index 0cae2455..3805f330 100644 --- a/src/comm_manager.cpp +++ b/src/comm_manager.cpp @@ -436,7 +436,7 @@ void CommManager::send_device_info(device_t device) char device_name[BoardConfigManager::DEVICE_NAME_LENGTH]; RF_.board_.get_board_config_manager().get_device_name(device, device_name); uint8_t max_config = RF_.board_.get_board_config_manager().get_max_config(device); - comm_link_.send_device_info(sysid_, device, max_config, device_name); + comm_link_.send_device_info(sysid_, device, max_config, device_name, Configuration::DEVICE_COUNT); } void CommManager::send_config_info(device_t device, hardware_config_t config) From 2ed27e0001dfb30302b025ecc86a029baea71f94 Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Thu, 26 Mar 2020 16:19:01 -0600 Subject: [PATCH 59/76] Update to latest mavlink --- comms/mavlink/v1.0 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/comms/mavlink/v1.0 b/comms/mavlink/v1.0 index cefd4a71..83c03721 160000 --- a/comms/mavlink/v1.0 +++ b/comms/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit cefd4a713882140fc010018f9c1b057a74334784 +Subproject commit 83c0372131785a505994aabe688b4cef3ea40804 From 8da53006a4a726e015d595eb99a55d26763921f0 Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Wed, 1 Apr 2020 13:24:09 -0600 Subject: [PATCH 60/76] Add documentation on config_list --- docs/user-guide/firmware-configuration.md | 52 ++++++++++++++++++++--- 1 file changed, 47 insertions(+), 5 deletions(-) diff --git a/docs/user-guide/firmware-configuration.md b/docs/user-guide/firmware-configuration.md index 12d913d3..480e88cb 100644 --- a/docs/user-guide/firmware-configuration.md +++ b/docs/user-guide/firmware-configuration.md @@ -18,6 +18,48 @@ successful: True configuration: "ADC3 on Power" message: '' ``` +## Listing Available Configurations +The firmware reports to rosflight_io what configurations are available for each device. A list of all devices and configurations can be obtained through the `config_list` service while rosflight_io is running. This does not show which configurations are currently active. Rosservice displays the output with quirky formatting, so note that the first configuration option is displayed on the same line as "configuration_names". An example output of `config_list` is shown below. +``` +$ rosservice call /config_list +devices: + - + device_name: "Serial" + configuration_names: - VCP over USB +- UART1 on Main +- UART2 on Flex-IO +- UART3 on Flexi + - + device_name: "RC" + configuration_names: - PPM on Flex-IO +- SBUS on Main + - + device_name: "Airspeed" + configuration_names: - Disabled +- I2C2 on Flexi + - + device_name: "GNSS" + configuration_names: - Disabled +- UART1 on main +- UART2 on Flex-Io +- UART3 on Flexi + - + device_name: "Sonar" + configuration_names: - Disabled +- I2C2 on Flexi + - + device_name: "Battery Monitor" + configuration_names: - Disabled +- ADC3 on Power + - + device_name: "Baro" + configuration_names: - Disabled +- Onboard baro + - + device_name: "Mag" + configuration_names: - Disabled +- Onboard mag +``` ##Setting Configurations Setting configurations is done similarly to getting them, but using the `config_set` service, which also requires `rosflight_io` to be running. The service takes the name of the device and the name of the configuration as parameters. @@ -87,22 +129,22 @@ See [Using Secondary Serial Links](hardware-setup.md#using-secondary-serial-link |Disabled|0|None|| |I2C2 on Flexi|1|Flexi|Multiple I2C devices can share the port.| -####Battery Montior +####Battery Monitor | Configuration | Number | port | Notes | | ------------- | ------ | ---- | ----- | |Disabled|0|None|| |ADC3 on Power|1|PWR / Sonar|| -####Barometer +####Baro (Barometer) | Configuration | Number | port | Notes | | ------------- | ------ | ---- | ----- | |Disabled|0|None|| -|Onboard Barometer|1|None|| +|Onboard Baro|1|None|| -####Magnetometer +####Mag (Magnetometer) | Configuration | Number | port | Notes | | ------------- | ------ | ---- | ----- | |Disabled|0|None|| -|Onboard Magnetometer|1|None|| +|Onboard Mag|1|None|| From d8543e4590790a900c99b828d40c1a6e0be5066b Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Tue, 21 Jan 2020 11:01:29 -0700 Subject: [PATCH 61/76] basic setup, but tests fail --- include/command_manager.h | 51 +++++++++++++++++++---- src/command_manager.cpp | 88 ++++++++++++++------------------------- 2 files changed, 74 insertions(+), 65 deletions(-) diff --git a/include/command_manager.h b/include/command_manager.h index d2d84849..89d4c7c9 100644 --- a/include/command_manager.h +++ b/include/command_manager.h @@ -143,33 +143,66 @@ class CommandManager : public ParamListenerInterface MUX_Z, MUX_F, }; + enum RCOverrideReason: uint16_t + { + OVERRIDE_NO_OVERRIDE = 0x0, + OVERRIDE_ATT_SWITCH = 0x1, + OVERRIDE_THR_SWITCH = 0x2, + OVERRIDE_X = 0x4, + OVERRIDE_Y = 0x8, + OVERRIDE_Z = 0x10, + OVERRIDE_T = 0x20, + OVERRIDE_OFFBOARD_X_INACTIVE = 0x40, + OVERRIDE_OFFBOARD_Y_INACTIVE = 0x80, + OVERRIDE_OFFBOARD_Z_INACTIVE = 0x100, + OVERRIDE_OFFBOARD_T_INACTIVE = 0x200, + }; + static constexpr uint16_t X_OVERRIDDEN{OVERRIDE_ATT_SWITCH | OVERRIDE_X | OVERRIDE_OFFBOARD_X_INACTIVE}; + static constexpr uint16_t Y_OVERRIDDEN{OVERRIDE_ATT_SWITCH | OVERRIDE_Y | OVERRIDE_OFFBOARD_Y_INACTIVE}; + static constexpr uint16_t Z_OVERRIDDEN{OVERRIDE_ATT_SWITCH | OVERRIDE_Z | OVERRIDE_OFFBOARD_Z_INACTIVE}; + static constexpr uint16_t T_OVERRIDDEN{OVERRIDE_THR_SWITCH | OVERRIDE_T | OVERRIDE_OFFBOARD_T_INACTIVE}; typedef struct { RC::Stick rc_channel; uint32_t last_override_time; - } rc_stick_override_t; + RCOverrideReason stick_override_reason; + RCOverrideReason offboard_inactive_override_reason; + uint16_t override_mask; + } channel_override_t; - rc_stick_override_t rc_stick_override_[3] = + channel_override_t channel_override_[4] = { - { RC::STICK_X, 0 }, - { RC::STICK_Y, 0 }, - { RC::STICK_Z, 0 } + { RC::STICK_X, 0, OVERRIDE_X, OVERRIDE_OFFBOARD_X_INACTIVE, X_OVERRIDDEN}, + { RC::STICK_Y, 0, OVERRIDE_Y, OVERRIDE_OFFBOARD_Y_INACTIVE, Y_OVERRIDDEN}, + { RC::STICK_Z, 0, OVERRIDE_Z, OVERRIDE_OFFBOARD_Z_INACTIVE, Z_OVERRIDDEN}, + { RC::STICK_F, 0, OVERRIDE_T, OVERRIDE_OFFBOARD_T_INACTIVE, T_OVERRIDDEN} // Note that throttle overriding works a bit differently }; ROSflight &RF_; bool new_command_; - bool rc_override_; + uint16_t rc_override_; control_t &failsafe_command_; void param_change_callback(uint16_t param_id) override; void init_failsafe(); - bool do_roll_pitch_yaw_muxing(MuxChannel channel); - bool do_throttle_muxing(void); - void do_min_throttle_muxing(); + /** + * @brief Checks which channels are overridden + * @details There are many reasons that a channel could be overriden. These reasons include: + * - A stick is deviated + * - The commanded throttle is less than the RC throttle, and the MIN_THROTTLE parameter is set + * - The attitude or throttle override switch is flipped + * - The onboard computer has not sent any commands recently + * The returned bitfield indicates which reasons have caused an override. + * By anding with a constant such as X_OVERRIDDEN, you can check if a specific channel is overridden. + * @return A bitfield, with overriden reasons indicated + */ + uint16_t determine_override_status(); + void do_muxing(uint16_t rc_override); + void do_channel_muxing(MuxChannel channel, uint16_t rc_override); void interpret_rc(void); bool stick_deviated(MuxChannel channel); diff --git a/src/command_manager.cpp b/src/command_manager.cpp index df0973ef..acba7639 100644 --- a/src/command_manager.cpp +++ b/src/command_manager.cpp @@ -158,78 +158,56 @@ 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 < channel_override_[channel].last_override_time + RF_.params_.get_param_int(PARAM_OVERRIDE_LAG_TIME)) { return true; } else { - if (fabsf(RF_.rc_.stick(rc_stick_override_[channel].rc_channel)) + if (fabsf(RF_.rc_.stick(channel_override_[channel].rc_channel)) > RF_.params_.get_param_float(PARAM_RC_OVERRIDE_DEVIATION)) { - rc_stick_override_[channel].last_override_time = now; + channel_override_[channel].last_override_time = now; return true; } return false; } } - -bool CommandManager::do_roll_pitch_yaw_muxing(MuxChannel channel) +uint16_t CommandManager::determine_override_status() { - bool override_this_channel = false; - //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)) + uint16_t rc_override{OVERRIDE_NO_OVERRIDE}; + if(RF_.rc_.switch_mapped(RC::SWITCH_ATT_OVERRIDE) && RF_.rc_.switch_on(RC::SWITCH_ATT_OVERRIDE)) + rc_override |= OVERRIDE_ATT_SWITCH; + if(RF_.rc_.switch_mapped(RC::SWITCH_THROTTLE_OVERRIDE) && RF_.rc_.switch_on(RC::SWITCH_THROTTLE_OVERRIDE)) + rc_override |= OVERRIDE_THR_SWITCH; + for(uint8_t channel{0}; channel < MUX_F; channel++) { - override_this_channel = true; + if(stick_deviated(static_cast(channel))) + rc_override |= channel_override_[channel].stick_override_reason; + if(!(muxes[channel].onboard->active)) + rc_override |= channel_override_[channel].offboard_inactive_override_reason; } - else // Otherwise only have RC override if the offboard channel is inactive + if(muxes[MUX_F].onboard->active) // The throttle has unique override behavior { - if (muxes[channel].onboard->active) - { - override_this_channel = false; - } - else - { - override_this_channel = true; - } + if(RF_.params_.get_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE)) + if(muxes[MUX_F].rc->value < muxes[MUX_F].onboard->value) + rc_override |= OVERRIDE_T; } - // set the combined channel output depending on whether RC is overriding for this channel or not - *muxes[channel].combined = override_this_channel ? *muxes[channel].rc : *muxes[channel].onboard; - return override_this_channel; + else + rc_override |= OVERRIDE_OFFBOARD_T_INACTIVE; + return rc_override; } -bool CommandManager::do_throttle_muxing(void) +void CommandManager::do_muxing(uint16_t rc_override) { - 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)) - { - override_this_channel = true; - } - else // Otherwise check if the offboard throttle channel is active, if it isn't, have RC override - { - 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)) - { - override_this_channel = (muxes[MUX_F].rc->value < muxes[MUX_F].onboard->value); - } - else - { - override_this_channel = false; - } - } - else - { - override_this_channel = true; - } - } - - // Set the combined channel output depending on whether RC is overriding for this channel or not - *muxes[MUX_F].combined = override_this_channel ? *muxes[MUX_F].rc : *muxes[MUX_F].onboard; - return override_this_channel; + for(uint8_t channel{0}; channel <=MUX_F; channel++) + do_channel_muxing(static_cast(channel), rc_override); +} +void CommandManager::do_channel_muxing(MuxChannel channel, uint16_t rc_override) +{ + bool override_this_channel = (rc_override | channel_override_[channel].override_mask); + // set the combined channel output depending on whether RC is overriding for this channel or not + *muxes[channel].combined = override_this_channel ? *muxes[channel].rc : *muxes[channel].onboard; } bool CommandManager::rc_override_active() @@ -291,10 +269,8 @@ bool CommandManager::run() } // Perform muxing - rc_override_ = do_roll_pitch_yaw_muxing(MUX_X); - rc_override_ |= do_roll_pitch_yaw_muxing(MUX_Y); - rc_override_ |= do_roll_pitch_yaw_muxing(MUX_Z); - rc_override_ |= do_throttle_muxing(); + rc_override_ = determine_override_status(); + do_muxing(rc_override_); // Light to indicate override if (rc_override_) From 0ad0f432a97ff309df43f9dc60d750530dd0d9e5 Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Tue, 21 Jan 2020 11:46:36 -0700 Subject: [PATCH 62/76] Passing most tests, which now check RC value --- include/command_manager.h | 2 +- src/comm_manager.cpp | 2 +- src/command_manager.cpp | 4 ++-- test/command_manager_test.cpp | 28 ++++++++++++++++++++++++++++ 4 files changed, 32 insertions(+), 4 deletions(-) diff --git a/include/command_manager.h b/include/command_manager.h index 89d4c7c9..66a94fab 100644 --- a/include/command_manager.h +++ b/include/command_manager.h @@ -212,7 +212,7 @@ class CommandManager : public ParamListenerInterface CommandManager(ROSflight &_rf); void init(); bool run(); - bool rc_override_active(); + uint16_t get_rc_override(); bool offboard_control_active(); void set_new_offboard_command(control_t new_offboard_command); void set_new_rc_command(control_t new_rc_command); diff --git a/src/comm_manager.cpp b/src/comm_manager.cpp index 3805f330..989fd2c2 100644 --- a/src/comm_manager.cpp +++ b/src/comm_manager.cpp @@ -498,7 +498,7 @@ void CommManager::send_status(void) comm_link_.send_status(sysid_, RF_.state_manager_.state().armed, RF_.state_manager_.state().failsafe, - RF_.command_manager_.rc_override_active(), + RF_.command_manager_.get_rc_override(), RF_.command_manager_.offboard_control_active(), RF_.state_manager_.state().error_codes, control_mode, diff --git a/src/command_manager.cpp b/src/command_manager.cpp index acba7639..3b49b59a 100644 --- a/src/command_manager.cpp +++ b/src/command_manager.cpp @@ -205,12 +205,12 @@ void CommandManager::do_muxing(uint16_t rc_override) } void CommandManager::do_channel_muxing(MuxChannel channel, uint16_t rc_override) { - bool override_this_channel = (rc_override | channel_override_[channel].override_mask); + bool override_this_channel = (rc_override & channel_override_[channel].override_mask); // set the combined channel output depending on whether RC is overriding for this channel or not *muxes[channel].combined = override_this_channel ? *muxes[channel].rc : *muxes[channel].onboard; } -bool CommandManager::rc_override_active() +uint16_t CommandManager::get_rc_override() { return rc_override_; } diff --git a/test/command_manager_test.cpp b/test/command_manager_test.cpp index ff68a0ab..21ad3ce7 100644 --- a/test/command_manager_test.cpp +++ b/test/command_manager_test.cpp @@ -335,6 +335,10 @@ TEST_F (CommandManagerTest, OffboardCommandMuxNoMinThrottle) stepFirmware(20000); control_t output = rf.command_manager_.combined_control(); + uint16_t override = rf.command_manager_.get_rc_override(); + + EXPECT_EQ(override, 0x0); + EXPECT_CLOSE(output.x.value, OFFBOARD_X); EXPECT_CLOSE(output.y.value, OFFBOARD_Y); EXPECT_CLOSE(output.z.value, OFFBOARD_Z); @@ -352,6 +356,10 @@ TEST_F (CommandManagerTest, OffboardCommandMuxMinThrottle) stepFirmware(20000); control_t output = rf.command_manager_.combined_control(); + uint16_t override = rf.command_manager_.get_rc_override(); + + EXPECT_EQ(override, 0x20); + EXPECT_CLOSE(output.x.value, OFFBOARD_X); EXPECT_CLOSE(output.y.value, OFFBOARD_Y); EXPECT_CLOSE(output.z.value, OFFBOARD_Z); @@ -368,6 +376,10 @@ TEST_F (CommandManagerTest, OffboardCommandMuxRollDeviation) stepFirmware(40000); control_t output = rf.command_manager_.combined_control(); + uint16_t override = rf.command_manager_.get_rc_override(); + + EXPECT_EQ(override, 0x24); + EXPECT_CLOSE(output.x.value, -0.5 * rf.params_.get_param_float(PARAM_RC_MAX_ROLL)); EXPECT_CLOSE(output.y.value, OFFBOARD_Y); EXPECT_CLOSE(output.z.value, OFFBOARD_Z); @@ -384,6 +396,10 @@ TEST_F (CommandManagerTest, OffboardCommandMuxPitchDeviation) stepFirmware(40000); control_t output = rf.command_manager_.combined_control(); + uint16_t override = rf.command_manager_.get_rc_override(); + + EXPECT_EQ(override, 0x28); + EXPECT_CLOSE(output.x.value, OFFBOARD_X); EXPECT_CLOSE(output.y.value, 0.5 * rf.params_.get_param_float(PARAM_RC_MAX_PITCH)); EXPECT_CLOSE(output.z.value, OFFBOARD_Z); @@ -400,6 +416,10 @@ TEST_F (CommandManagerTest, OffboardCommandMuxYawrateDeviation) stepFirmware(40000); control_t output = rf.command_manager_.combined_control(); + uint16_t override = rf.command_manager_.get_rc_override(); + + EXPECT_EQ(override, 0x30); + EXPECT_CLOSE(output.x.value, OFFBOARD_X); EXPECT_CLOSE(output.y.value, OFFBOARD_Y); EXPECT_CLOSE(output.z.value, -0.5 * rf.params_.get_param_float(PARAM_RC_MAX_YAWRATE)); @@ -417,6 +437,8 @@ TEST_F (CommandManagerTest, OffboardCommandMuxLag) control_t output = rf.command_manager_.combined_control(); EXPECT_CLOSE(output.x.value, -0.5 * rf.params_.get_param_float(PARAM_RC_MAX_ROLL)); + uint16_t override = rf.command_manager_.get_rc_override(); + EXPECT_EQ(override, 0x20); rc_values[0] = 1500; // return stick to center board.set_rc(rc_values); @@ -425,16 +447,22 @@ TEST_F (CommandManagerTest, OffboardCommandMuxLag) setOffboard(offboard_command); output=rf.command_manager_.combined_control(); EXPECT_CLOSE(output.x.value, 0.0); // lag + override = rf.command_manager_.get_rc_override(); + EXPECT_EQ(override, 0x24); stepFirmware(600000); setOffboard(offboard_command); output=rf.command_manager_.combined_control(); EXPECT_CLOSE(output.x.value, 0.0); // lag + override = rf.command_manager_.get_rc_override(); + EXPECT_EQ(override, 0x24); setOffboard(offboard_command); stepFirmware(20000); output=rf.command_manager_.combined_control(); EXPECT_CLOSE(output.x.value, OFFBOARD_X); + override = rf.command_manager_.get_rc_override(); + EXPECT_EQ(override, 0x20); } TEST_F (CommandManagerTest, StaleOffboardCommand) From cd3306ad005e785ea9c4b6b9b7a5f46ddffd8288 Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Tue, 21 Jan 2020 15:11:03 -0700 Subject: [PATCH 63/76] Update tests to check the verbose rc override indicators --- src/command_manager.cpp | 11 ++++------- test/command_manager_test.cpp | 26 +++++++++++++++----------- 2 files changed, 19 insertions(+), 18 deletions(-) diff --git a/src/command_manager.cpp b/src/command_manager.cpp index 3b49b59a..5b761e8f 100644 --- a/src/command_manager.cpp +++ b/src/command_manager.cpp @@ -187,14 +187,11 @@ uint16_t CommandManager::determine_override_status() if(!(muxes[channel].onboard->active)) rc_override |= channel_override_[channel].offboard_inactive_override_reason; } - if(muxes[MUX_F].onboard->active) // The throttle has unique override behavior - { - if(RF_.params_.get_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE)) - if(muxes[MUX_F].rc->value < muxes[MUX_F].onboard->value) - rc_override |= OVERRIDE_T; - } - else + if(!(muxes[MUX_F].onboard->active)) // The throttle has unique override behavior rc_override |= OVERRIDE_OFFBOARD_T_INACTIVE; + if(RF_.params_.get_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE)) + if(muxes[MUX_F].rc->value < muxes[MUX_F].onboard->value) + rc_override |= OVERRIDE_T; return rc_override; } diff --git a/test/command_manager_test.cpp b/test/command_manager_test.cpp index 21ad3ce7..fc898960 100644 --- a/test/command_manager_test.cpp +++ b/test/command_manager_test.cpp @@ -81,7 +81,7 @@ class CommandManagerTest : public ::testing::Test TEST_F (CommandManagerTest, Default) { board.set_rc(rc_values); - stepFirmware(20000); + stepFirmware(20000); // 20 ms control_t output = rf.command_manager_.combined_control(); EXPECT_EQ(output.x.type, ANGLE); @@ -428,41 +428,41 @@ TEST_F (CommandManagerTest, OffboardCommandMuxYawrateDeviation) TEST_F (CommandManagerTest, OffboardCommandMuxLag) { - stepFirmware(1100000); // Get past LAG_TIME + stepFirmware(1100000); // 1.1s Get past LAG_TIME rf.params_.set_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE, true); - rc_values[0] = 1250; + rc_values[0] = 1250; // About halfway left board.set_rc(rc_values); setOffboard(offboard_command); - stepFirmware(40000); + stepFirmware(40000); // 40 ms control_t output = rf.command_manager_.combined_control(); EXPECT_CLOSE(output.x.value, -0.5 * rf.params_.get_param_float(PARAM_RC_MAX_ROLL)); uint16_t override = rf.command_manager_.get_rc_override(); - EXPECT_EQ(override, 0x20); + EXPECT_EQ(override, 0x24); // Throttle and X stick overrides rc_values[0] = 1500; // return stick to center board.set_rc(rc_values); - stepFirmware(500000); + stepFirmware(500000); // 500 ms setOffboard(offboard_command); output=rf.command_manager_.combined_control(); EXPECT_CLOSE(output.x.value, 0.0); // lag override = rf.command_manager_.get_rc_override(); - EXPECT_EQ(override, 0x24); + EXPECT_EQ(override, 0x3E4); // Throttle and X stick overrides, plus stale offboard - stepFirmware(600000); + stepFirmware(600000); // 600 ms setOffboard(offboard_command); output=rf.command_manager_.combined_control(); EXPECT_CLOSE(output.x.value, 0.0); // lag override = rf.command_manager_.get_rc_override(); - EXPECT_EQ(override, 0x24); + EXPECT_EQ(override, 0x3E0); // Throttle override and stale offboard setOffboard(offboard_command); - stepFirmware(20000); + stepFirmware(20000); // 20 ms output=rf.command_manager_.combined_control(); EXPECT_CLOSE(output.x.value, OFFBOARD_X); override = rf.command_manager_.get_rc_override(); - EXPECT_EQ(override, 0x20); + EXPECT_EQ(override, 0x20); // Throttle override only } TEST_F (CommandManagerTest, StaleOffboardCommand) @@ -475,6 +475,8 @@ TEST_F (CommandManagerTest, StaleOffboardCommand) stepFirmware(timeout_us + 40000); control_t output = rf.command_manager_.combined_control(); + uint16_t override = rf.command_manager_.get_rc_override(); + EXPECT_EQ(override, 0x3E0); // Offboard not present, plus throttle limited EXPECT_CLOSE(output.x.value, 0.0); } @@ -486,10 +488,12 @@ TEST_F (CommandManagerTest, PartialMux) stepFirmware(30000); control_t output = rf.command_manager_.combined_control(); + uint16_t override = rf.command_manager_.get_rc_override(); EXPECT_CLOSE(output.x.value, 0.0); EXPECT_CLOSE(output.y.value, OFFBOARD_Y); EXPECT_CLOSE(output.z.value, OFFBOARD_Z); EXPECT_CLOSE(output.F.value, 0.0); + EXPECT_EQ(override, 0x60); // X channel stale and Throttle override } TEST_F (CommandManagerTest, MixedTypes) From 1a595045557ef0dbab57f6fd8d2ac549b98a8939 Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Tue, 21 Jan 2020 15:21:11 -0700 Subject: [PATCH 64/76] Update mavlink to send uint16's for rc_override --- comms/mavlink/mavlink.cpp | 2 +- comms/mavlink/mavlink.h | 2 +- include/interface/comm_link.h | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/comms/mavlink/mavlink.cpp b/comms/mavlink/mavlink.cpp index 8b9f0499..b1cf1298 100644 --- a/comms/mavlink/mavlink.cpp +++ b/comms/mavlink/mavlink.cpp @@ -345,7 +345,7 @@ void Mavlink::send_sonar(uint8_t system_id, /* TODO enum type*/uint8_t type, flo void Mavlink::send_status(uint8_t system_id, bool armed, bool failsafe, - bool rc_override, + uint16_t rc_override, bool offboard, uint8_t error_code, uint8_t control_mode, diff --git a/comms/mavlink/mavlink.h b/comms/mavlink/mavlink.h index 434c0401..de54e8ea 100644 --- a/comms/mavlink/mavlink.h +++ b/comms/mavlink/mavlink.h @@ -93,7 +93,7 @@ class Mavlink : public CommLinkInterface void send_status(uint8_t system_id, bool armed, bool failsafe, - bool rc_override, + uint16_t rc_override, bool offboard, uint8_t error_code, uint8_t control_mode, diff --git a/include/interface/comm_link.h b/include/interface/comm_link.h index fa4103a3..d477b5d1 100644 --- a/include/interface/comm_link.h +++ b/include/interface/comm_link.h @@ -171,7 +171,7 @@ class CommLinkInterface virtual void send_status(uint8_t system_id, bool armed, bool failsafe, - bool rc_override, + uint16_t rc_override, bool offboard, uint8_t error_code, uint8_t control_mode, From 757cfcbf47e3a05a7c07d8a5244b249c6ee1ec81 Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Wed, 22 Jan 2020 15:31:57 -0700 Subject: [PATCH 65/76] Prevent CommandManager from sending a status update every cycle --- src/command_manager.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/command_manager.cpp b/src/command_manager.cpp index 5b761e8f..0c974cda 100644 --- a/src/command_manager.cpp +++ b/src/command_manager.cpp @@ -243,7 +243,7 @@ void CommandManager::override_combined_command_with_rc() bool CommandManager::run() { - bool last_rc_override = rc_override_; + uint16_t last_rc_override = rc_override_; // Check for and apply failsafe command if (RF_.state_manager_.state().failsafe) From 0db8a3ed5f4cea6ed01d5512b98ec761589c92e3 Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Wed, 22 Jan 2020 17:06:18 -0700 Subject: [PATCH 66/76] Add tests for behavior with override switches flipped --- test/command_manager_test.cpp | 48 ++++++++++++++++++++++++++++++++--- 1 file changed, 45 insertions(+), 3 deletions(-) diff --git a/test/command_manager_test.cpp b/test/command_manager_test.cpp index fc898960..031188de 100644 --- a/test/command_manager_test.cpp +++ b/test/command_manager_test.cpp @@ -47,10 +47,10 @@ class CommandManagerTest : public ::testing::Test 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++) - { + for (int i = 0; i < 4; i++) rc_values[i] = 1500; - } + for (int i=4;i<8;i++) + rc_values[i]=1000; rc_values[2] = 1000; rf.params_.set_param_int(PARAM_MIXER, Mixer::PASSTHROUGH); @@ -465,6 +465,48 @@ TEST_F (CommandManagerTest, OffboardCommandMuxLag) EXPECT_EQ(override, 0x20); // Throttle override only } +TEST_F (CommandManagerTest, RCAttitudeOverrideSwitch) +{ + stepFirmware(1100000); // 1.1s Get past LAG_TIME + rf.params_.set_param_int(PARAM_RC_ATTITUDE_OVERRIDE_CHANNEL, 4); + rf.params_.set_param_int(PARAM_RC_THROTTLE_OVERRIDE_CHANNEL, -1); + rf.params_.set_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE, false); + setOffboard(offboard_command); + stepFirmware(50000); // 50ms + + uint16_t override = rf.command_manager_.get_rc_override(); + EXPECT_EQ(override, 0); + + rc_values[4] = 2000; + board.set_rc(rc_values); + + stepFirmware(50000); // 50ms + override = rf.command_manager_.get_rc_override(); + EXPECT_EQ(override, 1); + +} + +TEST_F (CommandManagerTest, RCThrottleOverrideSwitch) +{ + stepFirmware(1100000); // 1.1s Get past LAG_TIME + rf.params_.set_param_int(PARAM_RC_ATTITUDE_OVERRIDE_CHANNEL, -1); + rf.params_.set_param_int(PARAM_RC_THROTTLE_OVERRIDE_CHANNEL, 4); + rf.params_.set_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE, false); + setOffboard(offboard_command); + stepFirmware(50000); // 50ms + + uint16_t override = rf.command_manager_.get_rc_override(); + EXPECT_EQ(override, 0); + + rc_values[4] = 2000; + board.set_rc(rc_values); + + stepFirmware(50000); // 50ms + override = rf.command_manager_.get_rc_override(); + EXPECT_EQ(override, 2); + +} + TEST_F (CommandManagerTest, StaleOffboardCommand) { stepFirmware(1100000); // Get past LAG_TIME From ad240feba7e520bd7ec820b2fee496f799a8a80a Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Thu, 23 Jan 2020 11:20:17 -0700 Subject: [PATCH 67/76] Update documentation --- include/command_manager.h | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/include/command_manager.h b/include/command_manager.h index 66a94fab..5e98b428 100644 --- a/include/command_manager.h +++ b/include/command_manager.h @@ -192,7 +192,7 @@ class CommandManager : public ParamListenerInterface /** * @brief Checks which channels are overridden * @details There are many reasons that a channel could be overriden. These reasons include: - * - A stick is deviated + * - A stick is deflected * - The commanded throttle is less than the RC throttle, and the MIN_THROTTLE parameter is set * - The attitude or throttle override switch is flipped * - The onboard computer has not sent any commands recently @@ -212,6 +212,18 @@ class CommandManager : public ParamListenerInterface CommandManager(ROSflight &_rf); void init(); bool run(); + /** + * @brief Checks which channels are overridden, and why + * @details There are many reasons that a channel could be overriden. These reasons include: + * - A stick is deflected + * - The commanded throttle is less than the RC throttle, and the MIN_THROTTLE parameter is set + * - The attitude or throttle override switch is flipped + * - The onboard computer has not sent any commands recently + * The returned bitfield indicates which reasons have caused an override. + * Because c++ can use integers as booleans, this function can be treated as providing a boolean + * This value is updated when run is called if a new RC command is available + * @return A bitfield, with overriden reasons indicated + */ uint16_t get_rc_override(); bool offboard_control_active(); void set_new_offboard_command(control_t new_offboard_command); From 56f0d751ce969ef0ce238f15ab8bcd23b9be45a7 Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Thu, 2 Apr 2020 14:37:34 -0600 Subject: [PATCH 68/76] Update to right version of mavlink --- comms/mavlink/v1.0 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/comms/mavlink/v1.0 b/comms/mavlink/v1.0 index 83c03721..95b44529 160000 --- a/comms/mavlink/v1.0 +++ b/comms/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit 83c0372131785a505994aabe688b4cef3ea40804 +Subproject commit 95b44529c2cd87d59b09b90ec22d976dfa6ecaaa From 1a38535aea670eb3204a4b1076a8c10da64835b9 Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Thu, 2 Apr 2020 15:46:13 -0600 Subject: [PATCH 69/76] Additional checks for invalid configurations --- src/config_manager.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/config_manager.cpp b/src/config_manager.cpp index 248ad8cd..cfcfa153 100644 --- a/src/config_manager.cpp +++ b/src/config_manager.cpp @@ -63,6 +63,9 @@ bool ConfigManager::read() return false; if(generate_checksum() != config_.checksum) return false; + for (device_t device=Configuration::FIRST_DEVICE; device < Configuration::DEVICE_COUNT; ++device) + if (config_.config[device] > RF_.board_.get_board_config_manager().get_max_config(device)) + return false; return true; } From 75929be78b438691ead314eb64398794078e5752 Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Thu, 2 Apr 2020 16:04:25 -0600 Subject: [PATCH 70/76] better rc override status when rc is disconnected --- src/command_manager.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/command_manager.cpp b/src/command_manager.cpp index 0c974cda..5cf64f20 100644 --- a/src/command_manager.cpp +++ b/src/command_manager.cpp @@ -72,6 +72,7 @@ CommandManager::CommandManager(ROSflight &_rf) : void CommandManager::init() { init_failsafe(); + rc_override_ = determine_override_status(); } void CommandManager::param_change_callback(uint16_t param_id) From b16201601c6d5b588e8292ab4303101c762a14cc Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Fri, 3 Apr 2020 14:21:37 -0600 Subject: [PATCH 71/76] Run clang-format --- .clang-format | 53 +++++++ include/board.h | 43 +++--- include/board_config_manager.h | 15 +- include/comm_manager.h | 45 +++--- include/command_manager.h | 107 +++++--------- include/config_manager.h | 11 +- include/configuration_enum.h | 9 +- include/controller.h | 11 +- include/estimator.h | 42 +++--- include/memory_manager.h | 9 +- include/mixer.h | 219 +++++++++++------------------ include/nanoprintf.h | 13 +- include/param.h | 50 +++---- include/rc.h | 8 +- include/rosflight.h | 41 +++--- include/sensors.h | 43 +++--- include/state_manager.h | 25 ++-- include/util.h | 4 +- src/comm_manager.cpp | 123 ++++++----------- src/command_manager.cpp | 32 ++--- src/config_manager.cpp | 30 ++-- src/controller.cpp | 61 ++++---- src/estimator.cpp | 99 +++++++------ src/memory_manager.cpp | 7 +- src/mixer.cpp | 26 ++-- src/nanoprintf.cpp | 222 ++++++++++++++--------------- src/param.cpp | 246 ++++++++++++++++++++------------- src/rc.cpp | 34 ++--- src/rosflight.cpp | 9 +- src/sensors.cpp | 117 +++++++--------- src/state_manager.cpp | 18 +-- 31 files changed, 829 insertions(+), 943 deletions(-) create mode 100644 .clang-format diff --git a/.clang-format b/.clang-format new file mode 100644 index 00000000..9c051a61 --- /dev/null +++ b/.clang-format @@ -0,0 +1,53 @@ +--- +Language: Cpp +BasedOnStyle: Google +IndentWidth: 2 +UseTab: Never +ColumnLimit: 120 +BreakBeforeBraces: Custom +BraceWrapping: + AfterClass: true + AfterControlStatement: true + AfterEnum: true + AfterFunction: true + AfterNamespace: true + AfterObjCDeclaration: true + AfterStruct: true + AfterUnion: true + AfterExternBlock: true + BeforeCatch: true + BeforeElse: true + IndentBraces: false +PointerAlignment: Left +AccessModifierOffset: -2 +IndentCaseLabels: false +AlignAfterOpenBracket: Align +BinPackArguments: true +BinPackParameters: false +ExperimentalAutoDetectBinPacking: false +AllowAllParametersOfDeclarationOnNextLine: false +AllowShortBlocksOnASingleLine: false +AllowShortIfStatementsOnASingleLine: false +AllowShortLoopsOnASingleLine: true +AllowShortFunctionsOnASingleLine: Inline +AlignTrailingComments: true +SpacesBeforeTrailingComments: 1 +FixNamespaceComments: true +BreakBeforeBinaryOperators: NonAssignment +BreakConstructorInitializers: AfterColon +ConstructorInitializerIndentWidth: 2 + +IncludeBlocks: Regroup +IncludeCategories: + - Regex: '"rosflight\.h"' + Priority: 3 + - Regex: '"interface\/[[:alnum:]_]+\.h"' + Priority: 1 + - Regex: '' + Priority: 4 + - Regex: '<[[:alpha:]]+>' + Priority: 6 + - Regex: '<.*>' + Priority: 5 + - Regex: '".*"' + Priority: 2 diff --git a/include/board.h b/include/board.h index a2484c87..16b1b3d2 100644 --- a/include/board.h +++ b/include/board.h @@ -32,22 +32,20 @@ #ifndef ROSFLIGHT_FIRMWARE_BOARD_H #define ROSFLIGHT_FIRMWARE_BOARD_H -#include -#include -#include - -#include "sensors.h" -#include "state_manager.h" -#include "configuration_enum.h" #include "board_config_manager.h" +#include "configuration_enum.h" #include "param.h" +#include "sensors.h" +#include "state_manager.h" + +#include +#include +#include namespace rosflight_firmware { - class Board { - public: typedef enum { @@ -55,28 +53,28 @@ class Board RC_TYPE_SBUS = 1 } rc_type_t; -// setup + // setup virtual void init_board() = 0; virtual void board_reset(bool bootloader) = 0; -// clock + // clock virtual uint32_t clock_millis() = 0; virtual uint64_t clock_micros() = 0; virtual void clock_delay(uint32_t milliseconds) = 0; -// serial + // serial 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; -// hardware config - virtual bool enable_device(device_t device, hardware_config_t configuration, const Params ¶ms)=0; + // hardware config + virtual bool enable_device(device_t device, hardware_config_t configuration, const Params ¶ms) = 0; virtual const BoardConfigManager &get_board_config_manager() const = 0; -// sensors + // sensors virtual void sensors_init() = 0; - virtual uint16_t num_sensor_errors() = 0; + 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; @@ -113,21 +111,21 @@ class Board virtual float battery_current_read() const = 0; virtual void battery_current_set_multiplier(double multiplier) = 0; -// RC + // RC virtual bool rc_lost() = 0; virtual float rc_read(uint8_t channel) = 0; -// PWM - virtual void pwm_init(uint32_t refresh_rate, uint16_t idle_pwm) = 0; + // PWM + virtual void pwm_init(uint32_t refresh_rate, uint16_t idle_pwm) = 0; virtual void pwm_disable() = 0; virtual void pwm_write(uint8_t channel, float value) = 0; -// non-volatile memory + // 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; -// LEDs + // LEDs virtual void led0_on() = 0; virtual void led0_off() = 0; virtual void led0_toggle() = 0; @@ -136,12 +134,11 @@ class Board virtual void led1_off() = 0; virtual void led1_toggle() = 0; -// Backup memory + // 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 void backup_memory_clear(size_t len) = 0; - }; } // namespace rosflight_firmware diff --git a/include/board_config_manager.h b/include/board_config_manager.h index 3753442c..db8ce331 100644 --- a/include/board_config_manager.h +++ b/include/board_config_manager.h @@ -2,8 +2,8 @@ #define BOARD_CONFIG_MANAGER_H -#include "configuration_enum.h" #include "config_manager.h" +#include "configuration_enum.h" namespace rosflight_firmware { @@ -15,7 +15,6 @@ namespace rosflight_firmware class BoardConfigManager { public: - /** * @brief Get the largest number that is valid for the configuration of a given device * @details This number is inclusive, i.e. a value of 2 means 0, 1, or 2 are valid @@ -32,9 +31,13 @@ class BoardConfigManager * @param config The new configuration for the device * @param cm The ConfigManager with the current configurations */ - virtual ConfigManager::ConfigResponse check_config_change(device_t device, hardware_config_t config, const ConfigManager &cm) const = 0; - static constexpr int DEVICE_NAME_LENGTH = 20; /**< This includes the null terminator, so 19 is the practical maximum.*/ - static constexpr int CONFIG_NAME_LENGTH = 20; /**< This includes the null terminator, so 19 is the practical maximum.*/ + virtual ConfigManager::ConfigResponse check_config_change(device_t device, + hardware_config_t config, + const ConfigManager &cm) const = 0; + static constexpr int DEVICE_NAME_LENGTH = + 20; /**< This includes the null terminator, so 19 is the practical maximum.*/ + static constexpr int CONFIG_NAME_LENGTH = + 20; /**< This includes the null terminator, so 19 is the practical maximum.*/ /** * @brief Returns the name of the device * @details Do not assume that the device number is valid. @@ -42,7 +45,7 @@ class BoardConfigManager * Note the requirement for a null terminator at the end of the string. * @param device Any device */ - virtual void get_device_name(device_t device, char (&name)[DEVICE_NAME_LENGTH]) const=0; + virtual void get_device_name(device_t device, char (&name)[DEVICE_NAME_LENGTH]) const = 0; /** * @brief Returns the name of a configuration * @details Do not assume that the device number or configuration are valid. diff --git a/include/comm_manager.h b/include/comm_manager.h index 2269f6b2..1dbef25f 100644 --- a/include/comm_manager.h +++ b/include/comm_manager.h @@ -32,17 +32,16 @@ #ifndef ROSFLIGHT_FIRMWARE_COMM_MANAGER_H #define ROSFLIGHT_FIRMWARE_COMM_MANAGER_H -#include -#include - #include "interface/comm_link.h" #include "interface/param_listener.h" #include "nanoprintf.h" +#include +#include + namespace rosflight_firmware { - class ROSflight; class CommManager : public CommLinkInterface::ListenerInterface, public ParamListenerInterface @@ -88,7 +87,6 @@ class CommManager : public CommLinkInterface::ListenerInterface, public ParamLis bool initialized_ = false; bool connected_ = false; - static constexpr int LOG_MSG_SIZE = 50; class LogMessageBuffer { @@ -141,8 +139,8 @@ class CommManager : public CommLinkInterface::ListenerInterface, public ParamLis 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 aux_command_callback(const CommLinkInterface::AuxCommand& command) override; + void external_attitude_callback(const turbomath::Quaternion& q) override; void heartbeat_callback() override; void config_set_callback(uint8_t device, uint8_t configuration) override; void config_request_callback(uint8_t device) override; @@ -163,35 +161,26 @@ 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_command_struct(const char *const name, control_t command_struct); + 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); void send_next_config_info(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_raw();}), - Stream(0, [this]{this->send_rc_raw();}), - Stream(20000, [this]{this->send_low_priority();}) - }; + 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_raw(); }), + 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_raw_tow_ = 0; public: - CommManager(ROSflight& rf, CommLinkInterface& comm_link); void init(); @@ -205,12 +194,12 @@ class CommManager : public CommLinkInterface::ListenerInterface, public ParamLis void send_config_info(device_t device, hardware_config_t config); 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 d2d84849..5f2aafa7 100644 --- a/include/command_manager.h +++ b/include/command_manager.h @@ -32,31 +32,30 @@ #ifndef ROSFLIGHT_FIRMWARE_COMMAND_MANAGER_H #define ROSFLIGHT_FIRMWARE_COMMAND_MANAGER_H -#include -#include - #include "interface/param_listener.h" #include "rc.h" +#include +#include + namespace rosflight_firmware { - class ROSflight; typedef enum { - RATE, // Channel is is in rate mode (mrad/s) - ANGLE, // Channel command is in angle mode (mrad) - THROTTLE, // Channel is direcly controlling throttle max/1000 - PASSTHROUGH, // Channel directly passes PWM input to the mixer + RATE, // Channel is is in rate mode (mrad/s) + ANGLE, // Channel command is in angle mode (mrad) + THROTTLE, // Channel is direcly controlling throttle max/1000 + PASSTHROUGH, // Channel directly passes PWM input to the mixer } control_type_t; typedef struct { - bool active; // Whether or not the channel is active - control_type_t type; // What type the channel is - float value; // The value of the channel + bool active; // Whether or not the channel is active + control_type_t type; // What type the channel is + float value; // The value of the channel } control_channel_t; typedef struct @@ -70,9 +69,7 @@ typedef struct class CommandManager : public ParamListenerInterface { - private: - typedef struct { control_channel_t *rc; @@ -80,55 +77,33 @@ class CommandManager : public ParamListenerInterface control_channel_t *combined; } mux_t; - mux_t muxes[4] = - { - {&rc_command_.x, &offboard_command_.x, &combined_command_.x}, - {&rc_command_.y, &offboard_command_.y, &combined_command_.y}, - {&rc_command_.z, &offboard_command_.z, &combined_command_.z}, - {&rc_command_.F, &offboard_command_.F, &combined_command_.F} - }; - - control_t rc_command_ = - { - 0, - {false, ANGLE, 0.0}, - {false, ANGLE, 0.0}, - {false, RATE, 0.0}, - {false, THROTTLE, 0.0} - }; - control_t offboard_command_ = - { - 0, - {false, ANGLE, 0.0}, - {false, ANGLE, 0.0}, - {false, RATE, 0.0}, - {false, THROTTLE, 0.0} - }; - control_t combined_command_ = - { - 0, - {false, ANGLE, 0.0}, - {false, ANGLE, 0.0}, - {false, RATE, 0.0}, - {false, THROTTLE, 0.0} - }; - - control_t multirotor_failsafe_command_ = - { - 0, - {true, ANGLE, 0.0}, - {true, ANGLE, 0.0}, - {true, RATE, 0.0}, - {true, THROTTLE, 0.0} - }; - control_t fixedwing_failsafe_command_ = - { - 0, - {true, PASSTHROUGH, 0.0}, - {true, PASSTHROUGH, 0.0}, - {true, PASSTHROUGH, 0.0}, - {true, THROTTLE, 0.0} - }; + mux_t muxes[4] = {{&rc_command_.x, &offboard_command_.x, &combined_command_.x}, + {&rc_command_.y, &offboard_command_.y, &combined_command_.y}, + {&rc_command_.z, &offboard_command_.z, &combined_command_.z}, + {&rc_command_.F, &offboard_command_.F, &combined_command_.F}}; + + control_t rc_command_ = {0, {false, ANGLE, 0.0}, {false, ANGLE, 0.0}, {false, RATE, 0.0}, {false, THROTTLE, 0.0}}; + control_t offboard_command_ = {0, + {false, ANGLE, 0.0}, + {false, ANGLE, 0.0}, + {false, RATE, 0.0}, + {false, THROTTLE, 0.0}}; + control_t combined_command_ = {0, + {false, ANGLE, 0.0}, + {false, ANGLE, 0.0}, + {false, RATE, 0.0}, + {false, THROTTLE, 0.0}}; + + control_t multirotor_failsafe_command_ = {0, + {true, ANGLE, 0.0}, + {true, ANGLE, 0.0}, + {true, RATE, 0.0}, + {true, THROTTLE, 0.0}}; + control_t fixedwing_failsafe_command_ = {0, + {true, PASSTHROUGH, 0.0}, + {true, PASSTHROUGH, 0.0}, + {true, PASSTHROUGH, 0.0}, + {true, THROTTLE, 0.0}}; typedef enum { @@ -150,12 +125,7 @@ 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_; @@ -175,7 +145,6 @@ class CommandManager : public ParamListenerInterface bool stick_deviated(MuxChannel channel); public: - CommandManager(ROSflight &_rf); void init(); bool run(); diff --git a/include/config_manager.h b/include/config_manager.h index d58a07aa..1058804d 100644 --- a/include/config_manager.h +++ b/include/config_manager.h @@ -15,7 +15,7 @@ class ROSflight; class ConfigManager { public: - struct __attribute__ ((packed)) Config + struct __attribute__((packed)) Config { uint32_t checksum; hardware_config_t config[Configuration::DEVICE_COUNT]; @@ -27,8 +27,8 @@ class ConfigManager */ struct ConfigResponse { - bool successful; /**< If the change was successfully made **/ - bool reboot_required; /**< If a reboot is required for the change to take effect */ + bool successful; /**< If the change was successfully made **/ + bool reboot_required; /**< If a reboot is required for the change to take effect */ char message[CONFIG_RESPONSE_MESSAGE_LENGTH]; /**< An optional message, often an error message */ }; @@ -81,10 +81,9 @@ class ConfigManager // Sets a config without checks. This may cause an invalid configuration combo, // so attempt_set_configuration is recommended void set_configuration(device_t device, uint8_t config); - bool read(); // currently just checks that the memory manager is ready and the checksum is correct + bool read(); // currently just checks that the memory manager is ready and the checksum is correct void fill_defaults(); // Default values are 0, by convention uint32_t generate_checksum() const; // Based off of fletcher algorithm - }; -} +} // namespace rosflight_firmware #endif // HARDWARE_CONFIG_H diff --git a/include/configuration_enum.h b/include/configuration_enum.h index ff72d58b..44420af1 100644 --- a/include/configuration_enum.h +++ b/include/configuration_enum.h @@ -13,11 +13,10 @@ namespace rosflight_firmware */ namespace Configuration { - /** * @brief An enumeration of configurable devices */ -enum device_t: uint8_t +enum device_t : uint8_t { SERIAL, RC, @@ -34,7 +33,7 @@ constexpr device_t FIRST_DEVICE{static_cast(0)}; /** * @brief Allows incrementing device_t's for use in for loops. Stops incrementing past DEVICE_COUNT */ -inline device_t& operator++(device_t &dev) +inline device_t& operator++(device_t& dev) { uint8_t return_value = dev; return_value++; @@ -44,9 +43,9 @@ inline device_t& operator++(device_t &dev) return dev; } -} +} // namespace Configuration typedef uint8_t hardware_config_t; typedef Configuration::device_t device_t; /**< typedef'd for your convenience */ -} +} // namespace rosflight_firmware #endif // CONFIGURATION_ENUM_H diff --git a/include/controller.h b/include/controller.h index a7f77f8c..2d139661 100644 --- a/include/controller.h +++ b/include/controller.h @@ -32,19 +32,18 @@ #ifndef ROSFLIGHT_FIRMWARE_CONTROLLER_H #define ROSFLIGHT_FIRMWARE_CONTROLLER_H -#include -#include - -#include - #include "interface/param_listener.h" #include "command_manager.h" #include "estimator.h" +#include + +#include +#include + namespace rosflight_firmware { - class ROSflight; class Controller : public ParamListenerInterface diff --git a/include/estimator.h b/include/estimator.h index a5341e99..68d2bf79 100644 --- a/include/estimator.h +++ b/include/estimator.h @@ -32,22 +32,20 @@ #ifndef ROSFLIGHT_FIRMWARE_ESTIMATOR_H #define ROSFLIGHT_FIRMWARE_ESTIMATOR_H -#include -#include -#include +#include "interface/param_listener.h" #include -#include "interface/param_listener.h" +#include +#include +#include namespace rosflight_firmware { - class ROSflight; class Estimator : public ParamListenerInterface { - public: struct State { @@ -59,36 +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_; @@ -115,10 +104,11 @@ 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/memory_manager.h b/include/memory_manager.h index a63550e4..ad55a970 100644 --- a/include/memory_manager.h +++ b/include/memory_manager.h @@ -1,8 +1,8 @@ #ifndef MEMORY_MANAGER_H #define MEMORY_MANAGER_H -#include "param.h" #include "config_manager.h" +#include "param.h" namespace rosflight_firmware { @@ -32,24 +32,23 @@ class MemoryManager * @brief Checks if memory has been read successfully at some point in the past * @return if memory has been successfully loaded */ - inline bool is_ready() {return ready_;} + inline bool is_ready() { return ready_; } /** * @brief Get a pointer to the parameter structure, which was stored in persistent memory * @return A pointer to the parameter struct */ - inline Params::params_t &get_params(){return memory_.params;} + inline Params::params_t &get_params() { return memory_.params; } /** * @brief Get a pointer to the config structure, which was stored in persistent memory * @return A pointer to the config struct */ - inline ConfigManager::Config &get_config(){return memory_.config;} + inline ConfigManager::Config &get_config() { return memory_.config; } private: ROSflight &RF_; PersistentMemory memory_; bool ready_{false}; - }; } // namespace rosflight_firmware diff --git a/include/mixer.h b/include/mixer.h index 348c385d..8db86886 100644 --- a/include/mixer.h +++ b/include/mixer.h @@ -29,25 +29,21 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ - #ifndef ROSFLIGHT_FIRMWARE_MIXER_H #define ROSFLIGHT_FIRMWARE_MIXER_H -#include -#include - #include "interface/param_listener.h" +#include +#include + namespace rosflight_firmware { - class ROSflight; class Mixer : public ParamListenerInterface { - public: - static constexpr uint8_t NUM_TOTAL_OUTPUTS = 14; static constexpr uint8_t NUM_MIXER_OUTPUTS = 8; @@ -72,9 +68,9 @@ class Mixer : public ParamListenerInterface typedef enum { NONE, // None - S, // Servo - M, // Motor - G // GPIO + S, // Servo + M, // Motor + G // GPIO } output_type_t; typedef struct @@ -109,154 +105,107 @@ class Mixer : public ParamListenerInterface void write_motor(uint8_t index, float value); void write_servo(uint8_t index, float value); - const mixer_t esc_calibration_mixing = - { - {M, M, M, M, M, M, NONE, NONE}, - { 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f}, // F Mix - { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix - { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix - { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix - 490 - }; + const mixer_t esc_calibration_mixing = {{M, M, M, M, M, M, NONE, NONE}, + {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f}, // F Mix + {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix + {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix + {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 + const mixer_t quadcopter_x_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 - {-1.0f, -1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix - { 1.0f, -1.0f,-1.0f, 1.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 + {-1.0f, -1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix + {1.0f, -1.0f, -1.0f, 1.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 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 + const mixer_t X8_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 - {-1.0f, -1.0f, -1.0f, -1.0f, 1.0f, 1.0f, 1.0f, 1.0f}, // X Mix - { 1.0f, 1.0f, -1.0f, -1.0f, -1.0f, -1.0f, 1.0f, 1.0f}, // 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 + {-1.0f, -1.0f, -1.0f, -1.0f, 1.0f, 1.0f, 1.0f, 1.0f}, // X Mix + {1.0f, 1.0f, -1.0f, -1.0f, -1.0f, -1.0f, 1.0f, 1.0f}, // Y Mix + {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}, + const mixer_t fixedwing_mixing = {{S, S, M, S, S, M, NONE, NONE}, - { 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix - { 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix - { 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix - { 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Z Mix - 50 - }; + {0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix + {1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix + {0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix + {0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Z Mix + 50}; - const mixer_t passthrough_mixing = - { - {NONE, NONE, NONE, NONE, NONE, NONE, NONE, NONE}, + const mixer_t passthrough_mixing = {{NONE, NONE, NONE, NONE, NONE, NONE, NONE, NONE}, - { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix - { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix - { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix - { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Z Mix - 50 - }; + {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix + {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix + {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix + {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_; - const mixer_t *array_of_mixers_[NUM_MIXERS] = - { - &esc_calibration_mixing, - &quadcopter_plus_mixing, - &quadcopter_x_mixing, - &hex_plus_mixing, - &hex_x_mixing, - &octocopter_plus_mixing, - &octocopter_x_mixing, - &Y6_mixing, - &X8_mixing, - &tricopter_mixing, - &fixedwing_mixing, - &passthrough_mixing - }; + const mixer_t* array_of_mixers_[NUM_MIXERS] = { + &esc_calibration_mixing, &quadcopter_plus_mixing, &quadcopter_x_mixing, &hex_plus_mixing, &hex_x_mixing, + &octocopter_plus_mixing, &octocopter_x_mixing, &Y6_mixing, &X8_mixing, &tricopter_mixing, + &fixedwing_mixing, &passthrough_mixing}; public: Mixer(ROSflight& _rf); @@ -266,7 +215,7 @@ class Mixer : public ParamListenerInterface 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/nanoprintf.h b/include/nanoprintf.h index 49ce3393..a4967847 100644 --- a/include/nanoprintf.h +++ b/include/nanoprintf.h @@ -66,10 +66,10 @@ To use the printf you need to supply your own character output function, something like : void putc ( void* p, char c) - { - while (!SERIAL_PORT_EMPTY) ; - SERIAL_PORT_TX_REGISTER = c; - } + { + while (!SERIAL_PORT_EMPTY) ; + SERIAL_PORT_TX_REGISTER = c; + } Before you can call printf you need to initialize it to use your character output function with something like: @@ -111,13 +111,12 @@ namespace rosflight_firmware { namespace nanoprintf { - -void init_printf(void *putp,void (*putf)(void *,char)); +void init_printf(void *putp, void (*putf)(void *, char)); void tfp_printf(const char *fmt, ...); void tfp_sprintf(char *s, const char *fmt, va_list va); -void tfp_format(void *putp, void (*putf)(void *,char), const char *fmt, va_list va); +void tfp_format(void *putp, void (*putf)(void *, char), const char *fmt, va_list va); } // namespace nanoprintf } // namespace rosflight_firmware diff --git a/include/param.h b/include/param.h index 6a871b1c..64dabacb 100644 --- a/include/param.h +++ b/include/param.h @@ -32,14 +32,13 @@ #ifndef ROSFLIGHT_FIRMWARE_PARAM_H #define ROSFLIGHT_FIRMWARE_PARAM_H +#include "interface/param_listener.h" + #include #include -#include "interface/param_listener.h" - namespace rosflight_firmware { - enum : uint16_t { /******************************/ @@ -67,7 +66,6 @@ enum : uint16_t PARAM_STREAM_OUTPUT_RAW_RATE, PARAM_STREAM_RC_RAW_RATE, - /********************************/ /*** CONTROLLER CONFIGURATION ***/ /********************************/ @@ -229,29 +227,31 @@ typedef enum class ROSflight; class Params { - public: static constexpr uint8_t PARAMS_NAME_LENGTH = 16; + private: union param_value_t { float fvalue; int32_t ivalue; }; + public: typedef struct { uint32_t version; uint16_t size; - uint8_t magic_be; // magic number, should be 0xBE + uint8_t magic_be; // magic number, should be 0xBE param_value_t values[PARAMS_COUNT]; char names[PARAMS_COUNT][PARAMS_NAME_LENGTH]; param_type_t types[PARAMS_COUNT]; - uint8_t magic_ef; // magic number, should be 0xEF - uint8_t chk; // XOR checksum + uint8_t magic_ef; // magic number, should be 0xEF + uint8_t chk; // XOR checksum } params_t; + private: ROSflight &RF_; params_t ¶ms; @@ -260,10 +260,9 @@ class Params 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_t ¶m_struct); @@ -280,11 +279,11 @@ class Params void set_defaults(void); /** - * @brief Specify listeners for parameter changes - * @param listeners An array of pointers to objects that implement the ParamListenerInterface interface - * @param num_listeners The length of the array passed as the listeners parameter - */ - void set_listeners(ParamListenerInterface * const listeners[], size_t num_listeners); + * @brief Specify listeners for parameter changes + * @param listeners An array of pointers to objects that implement the ParamListenerInterface interface + * @param num_listeners The length of the array passed as the listeners parameter + */ + void set_listeners(ParamListenerInterface *const listeners[], size_t num_listeners); /** * @brief Read parameter values from non-volatile memory @@ -315,30 +314,21 @@ class Params * @param id The ID of the parameter * @return The value of the parameter */ - inline int get_param_int(uint16_t id) const - { - return params.values[id].ivalue; - } + inline int get_param_int(uint16_t id) const { return params.values[id].ivalue; } /** * @brief Get the value of a floating point parameter by id * @param id The ID of the parameter * @return The value of the parameter */ - inline float get_param_float(uint16_t id) const - { - return params.values[id].fvalue; - } + inline float get_param_float(uint16_t id) const { return params.values[id].fvalue; } /** * @brief Get the name of a parameter * @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 @@ -348,10 +338,7 @@ class Params * PARAM_TYPE_INT32, PARAM_TYPE_FLOAT, or PARAM_TYPE_INVALID * See line 165 */ - inline param_type_t get_param_type(uint16_t id) const - { - return params.types[id]; - } + inline param_type_t get_param_type(uint16_t id) const { return params.types[id]; } /** * @brief Sets the value of a parameter by ID and calls the parameter change callback @@ -384,7 +371,6 @@ class Params * @return True if a parameter value was changed, false otherwise */ bool set_param_by_name_float(const char name[PARAMS_NAME_LENGTH], float value); - }; } // namespace rosflight_firmware diff --git a/include/rc.h b/include/rc.h index b491bc8a..38e6318b 100644 --- a/include/rc.h +++ b/include/rc.h @@ -32,19 +32,17 @@ #ifndef ROSFLIGHT_FIRMWARE_RC_H #define ROSFLIGHT_FIRMWARE_RC_H -#include -#include - #include "interface/param_listener.h" +#include +#include + namespace rosflight_firmware { - class ROSflight; class RC : public ParamListenerInterface { - public: enum Stick { diff --git a/include/rosflight.h b/include/rosflight.h index 579f4860..bcb7bdaf 100644 --- a/include/rosflight.h +++ b/include/rosflight.h @@ -32,34 +32,32 @@ #ifndef ROSFLIGHT_FIRMWARE_ROSFLIGHT_H #define ROSFLIGHT_FIRMWARE_ROSFLIGHT_H -#include - #include "interface/comm_link.h" #include "interface/param_listener.h" #include "board.h" -#include "param.h" -#include "sensors.h" -#include "estimator.h" -#include "rc.h" -#include "controller.h" #include "comm_manager.h" -#include "mixer.h" -#include "state_manager.h" #include "command_manager.h" #include "config_manager.h" +#include "controller.h" +#include "estimator.h" #include "memory_manager.h" +#include "mixer.h" +#include "param.h" +#include "rc.h" +#include "sensors.h" +#include "state_manager.h" + +#include namespace rosflight_firmware { - class ROSflight { - public: ROSflight(Board& board, CommLinkInterface& comm_link); - Board &board_; + Board& board_; MemoryManager memory_manager_; CommManager comm_manager_; @@ -77,28 +75,21 @@ class ROSflight uint32_t loop_time_us; /** - * @brief Main initialization routine for the ROSflight autopilot flight stack - */ + * @brief Main initialization routine for the ROSflight autopilot flight stack + */ void init(); /** - * @brief Main loop for the ROSflight autopilot flight stack - */ + * @brief Main loop for the ROSflight autopilot flight stack + */ void run(); uint32_t get_loop_time_us(); 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 1fd82a43..87082638 100644 --- a/include/sensors.h +++ b/include/sensors.h @@ -33,12 +33,13 @@ #ifndef ROSFLIGHT_FIRMWARE_SENSORS_H #define ROSFLIGHT_FIRMWARE_SENSORS_H -#include -#include -#include +#include "interface/param_listener.h" + #include -#include "interface/param_listener.h" +#include +#include +#include namespace rosflight_firmware { @@ -55,26 +56,26 @@ struct GNSSData { struct ECEF { - int32_t x; // cm - int32_t y; // cm - int32_t z; // cm + int32_t x; // cm + int32_t y; // cm + int32_t z; // cm uint32_t p_acc; // cm - int32_t vx; // cm/s - int32_t vy; // cm/s - int32_t vz; // cm/s + int32_t vx; // cm/s + int32_t vy; // cm/s + int32_t vz; // cm/s uint32_t s_acc; // cm/s }; GNSSFixType fix_type; uint32_t time_of_week; - uint64_t time; // Unix time, in seconds + uint64_t time; // Unix time, in seconds uint64_t nanos; // Fractional time - int32_t lat; // deg*10^-7 - int32_t lon; // deg*10^-7 + int32_t lat; // deg*10^-7 + int32_t lon; // deg*10^-7 int32_t height; // mm - int32_t vel_n; // mm/s - int32_t vel_e; // mm/s - int32_t vel_d; // mm/s + int32_t vel_n; // mm/s + int32_t vel_e; // mm/s + int32_t vel_d; // mm/s uint32_t h_acc; // mm uint32_t v_acc; // mm @@ -82,10 +83,7 @@ struct GNSSData uint64_t rosflight_timestamp; // microseconds, time stamp of last byte in the message - GNSSData() - { - memset(this, 0, sizeof(GNSSData)); - } + GNSSData() { memset(this, 0, sizeof(GNSSData)); } }; struct GNSSRaw @@ -118,10 +116,7 @@ struct GNSSRaw uint16_t p_dop; uint64_t rosflight_timestamp; // microseconds, time stamp of last byte in the message - GNSSRaw() - { - memset(this, 0, sizeof(GNSSRaw)); - } + GNSSRaw() { memset(this, 0, sizeof(GNSSRaw)); } }; class ROSflight; diff --git a/include/state_manager.h b/include/state_manager.h index d6f56cce..c93ee2fd 100644 --- a/include/state_manager.h +++ b/include/state_manager.h @@ -39,12 +39,10 @@ namespace rosflight_firmware { - class ROSflight; class StateManager { - public: struct State { @@ -86,11 +84,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 + static constexpr uint32_t ARM_MAGIC = + 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 + 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 /** * @brief Low-level debugging information for case of hard fault @@ -99,13 +98,13 @@ class StateManager */ struct DebugInfo { - uint32_t r0; //!< register 0 - uint32_t r1; //!< register 1 - uint32_t r2; //!< register 2 - uint32_t r3; //!< register 3 + uint32_t r0; //!< register 0 + uint32_t r1; //!< register 1 + uint32_t r2; //!< register 2 + uint32_t r3; //!< register 3 uint32_t r12; //!< register 12 - uint32_t lr; //!< link register - uint32_t pc; //!< program counter + uint32_t lr; //!< link register + uint32_t pc; //!< program counter uint32_t psr; //!< program status register } debug; @@ -137,7 +136,7 @@ class StateManager 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); @@ -158,7 +157,7 @@ class StateManager 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 40eae4e7..deb9abf3 100644 --- a/include/util.h +++ b/include/util.h @@ -38,7 +38,6 @@ namespace rosflight_firmware { - /** * @brief Fletcher 16-bit checksum * @@ -50,7 +49,8 @@ namespace rosflight_firmware */ 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; diff --git a/src/comm_manager.cpp b/src/comm_manager.cpp index 3805f330..1c5fc47b 100644 --- a/src/comm_manager.cpp +++ b/src/comm_manager.cpp @@ -29,23 +29,22 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#include -#include +#include "comm_manager.h" -#include "rosflight.h" #include "param.h" -#include "comm_manager.h" +#include "rosflight.h" + +#include +#include namespace rosflight_firmware { - CommManager::LogMessageBuffer::LogMessageBuffer() { memset(buffer_, 0, sizeof(buffer_)); } - void CommManager::LogMessageBuffer::add_message(CommLinkInterface::LogSeverity severity, char msg[]) { LogMessage& newest_msg = buffer_[newest_]; @@ -72,11 +71,7 @@ void CommManager::LogMessageBuffer::pop() } } - -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() @@ -175,18 +170,12 @@ void CommManager::send_param_value(uint16_t param_id) 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)); + 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)); + 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; @@ -312,7 +301,7 @@ void CommManager::timesync_callback(int64_t tc1, int64_t ts1) uint64_t now_us = RF_.board_.clock_micros(); if (tc1 == 0) // check that this is a request, not a response - comm_link_.send_timesync(sysid_, static_cast(now_us)*1000, ts1); + comm_link_.send_timesync(sysid_, static_cast(now_us) * 1000, ts1); } void CommManager::offboard_control_callback(const CommLinkInterface::OffboardControl& control) @@ -358,7 +347,7 @@ 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; @@ -388,7 +377,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); } @@ -414,15 +403,16 @@ void CommManager::heartbeat_callback(void) void CommManager::config_set_callback(uint8_t device, uint8_t configuration) { uint8_t requested_device{device}; - if(device >=Configuration::DEVICE_COUNT) + if (device >= Configuration::DEVICE_COUNT) device = Configuration::DEVICE_COUNT; - ConfigManager::ConfigResponse resp = RF_.config_manager_.attempt_set_configuration(static_cast(device), configuration); + ConfigManager::ConfigResponse resp = + RF_.config_manager_.attempt_set_configuration(static_cast(device), configuration); comm_link_.send_config_status(sysid_, requested_device, resp.successful, resp.reboot_required, resp.message); } void CommManager::config_request_callback(uint8_t device) { - if(device < Configuration::DEVICE_COUNT) + if (device < Configuration::DEVICE_COUNT) send_config_value(static_cast(device)); } void CommManager::send_all_config_info() @@ -458,7 +448,7 @@ 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; @@ -495,23 +485,15 @@ void CommManager::send_status(void) 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(), + 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); } @@ -520,32 +502,25 @@ void CommManager::send_imu(void) turbomath::Vector acc, gyro; uint64_t stamp_us; RF_.sensors_.get_filtered_IMU(acc, gyro, stamp_us); - comm_link_.send_imu(sysid_, - stamp_us, - acc, - gyro, - RF_.sensors_.data().imu_temperature); - + comm_link_.send_imu(sysid_, stamp_us, acc, gyro, RF_.sensors_.data().imu_temperature); } void CommManager::send_output_raw(void) { - comm_link_.send_output_raw(sysid_, - RF_.board_.clock_millis(), - RF_.mixer_.get_outputs()); + comm_link_.send_output_raw(sysid_, RF_.board_.clock_millis(), RF_.mixer_.get_outputs()); } void CommManager::send_rc_raw(void) { // TODO better mechanism for retreiving RC (through RC module, not PWM-specific) - uint16_t channels[8] = { static_cast(RF_.board_.rc_read(0)*1000 + 1000), - static_cast(RF_.board_.rc_read(1)*1000 + 1000), - static_cast(RF_.board_.rc_read(2)*1000 + 1000), - static_cast(RF_.board_.rc_read(3)*1000 + 1000), - static_cast(RF_.board_.rc_read(4)*1000 + 1000), - static_cast(RF_.board_.rc_read(5)*1000 + 1000), - static_cast(RF_.board_.rc_read(6)*1000 + 1000), - static_cast(RF_.board_.rc_read(7)*1000 + 1000) }; + uint16_t channels[8] = {static_cast(RF_.board_.rc_read(0) * 1000 + 1000), + static_cast(RF_.board_.rc_read(1) * 1000 + 1000), + static_cast(RF_.board_.rc_read(2) * 1000 + 1000), + static_cast(RF_.board_.rc_read(3) * 1000 + 1000), + static_cast(RF_.board_.rc_read(4) * 1000 + 1000), + static_cast(RF_.board_.rc_read(5) * 1000 + 1000), + static_cast(RF_.board_.rc_read(6) * 1000 + 1000), + static_cast(RF_.board_.rc_read(7) * 1000 + 1000)}; comm_link_.send_rc_raw(sysid_, RF_.board_.clock_millis(), channels); } @@ -553,9 +528,7 @@ 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, + comm_link_.send_diff_pressure(sysid_, RF_.sensors_.data().diff_pressure_velocity, RF_.sensors_.data().diff_pressure, RF_.sensors_.data().diff_pressure_temp); } } @@ -564,9 +537,7 @@ 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, + comm_link_.send_baro(sysid_, RF_.sensors_.data().baro_altitude, RF_.sensors_.data().baro_pressure, RF_.sensors_.data().baro_temperature); } } @@ -577,9 +548,7 @@ void CommManager::send_sonar(void) { comm_link_.send_sonar(sysid_, 0, // TODO set sensor type (sonar/lidar), use enum - RF_.sensors_.data().sonar_range, - 8.0, - 0.25); + RF_.sensors_.data().sonar_range, 8.0, 0.25); } } @@ -590,9 +559,8 @@ void CommManager::send_mag(void) } 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); + if (RF_.sensors_.data().battery_monitor_present) + 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) @@ -606,7 +574,6 @@ void CommManager::send_backup_data(const StateManager::BackupData& backup_data) backup_data_buffer_ = backup_data; have_backup_data_ = true; } - } void CommManager::send_gnss(void) @@ -667,12 +634,12 @@ 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); } @@ -694,7 +661,7 @@ void CommManager::send_next_config_info(void) send_device_info(send_device_info_index_); send_config_info(send_device_info_index_, send_config_info_index_); send_config_info_index_++; - if(send_config_info_index_ > RF_.board_.get_board_config_manager().get_max_config(send_device_info_index_)) + if (send_config_info_index_ > RF_.board_.get_board_config_manager().get_max_config(send_device_info_index_)) { ++send_device_info_index_; send_config_info_index_ = 0; @@ -706,7 +673,8 @@ CommManager::Stream::Stream(uint32_t period_us, std::function send_f period_us_(period_us), next_time_us_(0), send_function_(send_function) -{} +{ +} void CommManager::Stream::stream(uint64_t now_us) { @@ -716,8 +684,7 @@ void CommManager::Stream::stream(uint64_t now_us) do { next_time_us_ += period_us_; - } - while(next_time_us_ < now_us); + } while (next_time_us_ < now_us); send_function_(); } @@ -725,10 +692,10 @@ void CommManager::Stream::stream(uint64_t now_us) void CommManager::Stream::set_rate(uint32_t rate_hz) { - period_us_ = (rate_hz == 0) ? 0 : 1000000/rate_hz; + period_us_ = (rate_hz == 0) ? 0 : 1000000 / rate_hz; } -//void Mavlink::mavlink_send_named_command_struct(const char *const name, control_t command_struct) +// void Mavlink::mavlink_send_named_command_struct(const char *const name, control_t command_struct) //{ // uint8_t control_mode; // if (command_struct.x.type == RATE && command_struct.y.type == RATE) diff --git a/src/command_manager.cpp b/src/command_manager.cpp index df0973ef..f26736a1 100644 --- a/src/command_manager.cpp +++ b/src/command_manager.cpp @@ -29,15 +29,15 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#include -#include - #include "command_manager.h" + #include "rosflight.h" +#include +#include + namespace rosflight_firmware { - typedef enum { ATT_MODE_RATE, @@ -50,12 +50,7 @@ typedef struct uint32_t last_override_time; } rc_stick_override_t; -rc_stick_override_t rc_stick_override[] = -{ - { RC::STICK_X, 0 }, - { RC::STICK_Y, 0 }, - { RC::STICK_Z, 0 } -}; +rc_stick_override_t rc_stick_override[] = {{RC::STICK_X, 0}, {RC::STICK_Y, 0}, {RC::STICK_Z, 0}}; typedef struct { @@ -64,10 +59,7 @@ typedef struct 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() { @@ -124,7 +116,7 @@ void CommandManager::interpret_rc(void) } else { - roll_pitch_type = (RF_.params_.get_param_int(PARAM_RC_ATTITUDE_MODE) == ATT_MODE_RATE) ? RATE: ANGLE; + roll_pitch_type = (RF_.params_.get_param_int(PARAM_RC_ATTITUDE_MODE) == ATT_MODE_RATE) ? RATE : ANGLE; } rc_command_.x.type = roll_pitch_type; @@ -158,7 +150,7 @@ 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; } @@ -177,7 +169,7 @@ bool CommandManager::stick_deviated(MuxChannel channel) bool CommandManager::do_roll_pitch_yaw_muxing(MuxChannel channel) { bool override_this_channel = false; - //Check if the override switch exists and is triggered, or if the sticks have deviated enough to trigger an override + // 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)) { @@ -265,7 +257,6 @@ void CommandManager::override_combined_command_with_rc() combined_command_ = rc_command_; } - bool CommandManager::run() { bool last_rc_override = rc_override_; @@ -291,7 +282,7 @@ bool CommandManager::run() } // Perform muxing - rc_override_ = do_roll_pitch_yaw_muxing(MUX_X); + rc_override_ = do_roll_pitch_yaw_muxing(MUX_X); rc_override_ |= do_roll_pitch_yaw_muxing(MUX_Y); rc_override_ |= do_roll_pitch_yaw_muxing(MUX_Z); rc_override_ |= do_throttle_muxing(); @@ -315,5 +306,4 @@ bool CommandManager::run() return true; } - -} +} // namespace rosflight_firmware diff --git a/src/config_manager.cpp b/src/config_manager.cpp index cfcfa153..2c0bde6d 100644 --- a/src/config_manager.cpp +++ b/src/config_manager.cpp @@ -1,16 +1,14 @@ #include "config_manager.h" + #include "rosflight.h" namespace rosflight_firmware { -ConfigManager::ConfigManager(ROSflight &RF, Config &config): - RF_{RF}, - config_{config} -{} +ConfigManager::ConfigManager(ROSflight &RF, Config &config) : RF_{RF}, config_{config} {} bool ConfigManager::init() { - if(!read()) + if (!read()) fill_defaults(); return true; } @@ -18,7 +16,7 @@ bool ConfigManager::init() bool ConfigManager::configure_devices() const { bool success = true; - for(device_t device{Configuration::FIRST_DEVICE}; device < Configuration::DEVICE_COUNT; ++device) + for (device_t device{Configuration::FIRST_DEVICE}; device < Configuration::DEVICE_COUNT; ++device) success = RF_.board_.enable_device(device, config_.config[device], RF_.params_) && success; return success; } @@ -26,7 +24,7 @@ bool ConfigManager::configure_devices() const ConfigManager::ConfigResponse ConfigManager::attempt_set_configuration(device_t device, uint8_t config) { ConfigResponse resp; - if(RF_.state_manager_.state().armed) + if (RF_.state_manager_.state().armed) { resp.successful = false; resp.reboot_required = false; @@ -34,7 +32,7 @@ ConfigManager::ConfigResponse ConfigManager::attempt_set_configuration(device_t return resp; } resp = RF_.board_.get_board_config_manager().check_config_change(device, config, *this); - if(resp.successful) + if (resp.successful) set_configuration(device, config); return resp; } @@ -59,11 +57,11 @@ void ConfigManager::prepare_write() bool ConfigManager::read() { - if(!RF_.memory_manager_.is_ready()) + if (!RF_.memory_manager_.is_ready()) return false; - if(generate_checksum() != config_.checksum) + if (generate_checksum() != config_.checksum) return false; - for (device_t device=Configuration::FIRST_DEVICE; device < Configuration::DEVICE_COUNT; ++device) + for (device_t device = Configuration::FIRST_DEVICE; device < Configuration::DEVICE_COUNT; ++device) if (config_.config[device] > RF_.board_.get_board_config_manager().get_max_config(device)) return false; return true; @@ -75,18 +73,18 @@ void ConfigManager::fill_defaults() } uint32_t ConfigManager::generate_checksum() const { - //8 bit fletcher algorithm, because we can't assume that the struct is a multiple of 16 bits - const uint8_t *config_data = reinterpret_cast(config_.config); + // 8 bit fletcher algorithm, because we can't assume that the struct is a multiple of 16 bits + const uint8_t *config_data = reinterpret_cast(config_.config); uint8_t check_a{0}; uint8_t check_b{0}; - for(size_t index{0}; index< sizeof(config_.config); index++ ) + for (size_t index{0}; index < sizeof(config_.config); index++) { check_a += config_data[index]; check_a %= (UINT8_MAX - 1); check_b += check_a; check_b %= (UINT8_MAX - 1); } - return check_a | (check_b<<8) | (~check_a<<16) | (~check_b<<24); + return check_a | (check_b << 8) | (~check_a << 16) | (~check_b << 24); } -} +} // namespace rosflight_firmware diff --git a/src/controller.cpp b/src/controller.cpp index 33c1b79d..e5803a8b 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -29,22 +29,19 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#include -#include +#include "controller.h" #include "command_manager.h" #include "estimator.h" + #include "rosflight.h" -#include "controller.h" +#include +#include namespace rosflight_firmware { - -Controller::Controller(ROSflight &rf) : - RF_(rf) -{ -} +Controller::Controller(ROSflight &rf) : RF_(rf) {} void Controller::init() { @@ -54,26 +51,19 @@ 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), - RF_.params_.get_param_float(PARAM_PID_ROLL_ANGLE_D), - max, min, tau); + 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), - RF_.params_.get_param_float(PARAM_PID_ROLL_RATE_D), - max, min, tau); + RF_.params_.get_param_float(PARAM_PID_ROLL_RATE_D), max, min, tau); pitch_.init(RF_.params_.get_param_float(PARAM_PID_PITCH_ANGLE_P), RF_.params_.get_param_float(PARAM_PID_PITCH_ANGLE_I), - RF_.params_.get_param_float(PARAM_PID_PITCH_ANGLE_D), - max, min, tau); + RF_.params_.get_param_float(PARAM_PID_PITCH_ANGLE_D), max, min, tau); 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), - RF_.params_.get_param_float(PARAM_PID_YAW_RATE_D), - max, min, tau); + 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), + RF_.params_.get_param_float(PARAM_PID_YAW_RATE_D), max, min, tau); } void Controller::run() @@ -95,10 +85,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); @@ -147,7 +139,8 @@ void Controller::calculate_equilbrium_torque_from_rc() } else { - RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_WARNING, "Cannot perform equilibrium offset calibration while armed"); + RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_WARNING, + "Cannot perform equilibrium offset calibration while armed"); } } @@ -180,12 +173,15 @@ void Controller::param_change_callback(uint16_t param_id) } } -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; - float dt = 1e-6*dt_us; + float dt = 1e-6 * dt_us; // ROLL if (command.x.type == RATE) @@ -222,7 +218,8 @@ Controller::PID::PID() : 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) { @@ -242,8 +239,8 @@ float Controller::PID::run(float dt, float x, float x_c, bool update_integrator) // 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 @@ -271,7 +268,7 @@ float Controller::PID::run(float dt, float x, float x_c, bool update_integrator, d_term = kd_ * xdot; } - //If there is an integrator term and we are updating integrators + // If there is an integrator term and we are updating integrators if ((ki_ > 0.0f) && update_integrator) { // integrate @@ -287,7 +284,7 @@ float Controller::PID::run(float dt, float x, float x_c, bool update_integrator, //// Include reference to Dr. Beard's notes here float u_sat = (u > max_) ? max_ : (u < min_) ? min_ : u; if (u != u_sat && fabs(i_term) > fabs(u - p_term + d_term) && ki_ > 0.0f) - integrator_ = (u_sat - p_term + d_term)/ki_; + integrator_ = (u_sat - p_term + d_term) / ki_; // Set output return u_sat; diff --git a/src/estimator.cpp b/src/estimator.cpp index 168ac20f..860d9fdd 100644 --- a/src/estimator.cpp +++ b/src/estimator.cpp @@ -30,14 +30,12 @@ */ #include "estimator.h" + #include "rosflight.h" namespace rosflight_firmware { - -Estimator::Estimator(ROSflight &_rf): - RF_(_rf) -{} +Estimator::Estimator(ROSflight& _rf) : RF_(_rf) {} void Estimator::reset_state() { @@ -99,26 +97,26 @@ void Estimator::init() void Estimator::param_change_callback(uint16_t param_id) { - (void) 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; - 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; + 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; - 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; + 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; @@ -126,7 +124,6 @@ void Estimator::set_external_attitude_update(const turbomath::Quaternion &q) void Estimator::run() { - // // Timing Setup // @@ -194,10 +191,10 @@ 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; + kp = RF_.params_.get_param_float(PARAM_FILTER_KP_ACC) * 10.0f; + ki = RF_.params_.get_param_float(PARAM_FILTER_KI) * 10.0f; } // @@ -206,7 +203,7 @@ void Estimator::run() // Integrate biases driven by measured angular error // eq 47b Mahony Paper, using correction term w_err found above - bias_ -= ki*w_err*dt; + bias_ -= ki * w_err * dt; // Build the composite omega vector for kinematic propagation // This the stuff inside the p function in eq. 47a - Mahony Paper @@ -245,7 +242,8 @@ 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(); @@ -256,8 +254,8 @@ bool Estimator::can_use_accel() const // Since there is noise, we give some margin to what a "non-accelerated state" means. // Establish allowed acceleration deviation from 1g (i.e., non-accelerated flight). const float margin = RF_.params_.get_param_float(PARAM_FILTER_ACCEL_MARGIN); - const float lowerbound = (1.0f - margin)*(1.0f - margin)*9.80665f*9.80665f; - const float upperbound = (1.0f + margin)*(1.0f + margin)*9.80665f*9.80665f; + const float lowerbound = (1.0f - margin) * (1.0f - margin) * 9.80665f * 9.80665f; + const float upperbound = (1.0f + margin) * (1.0f + margin) * 9.80665f * 9.80665f; // if the magnitude of the accel measurement is close to 1g, we can use the // accelerometer to correct roll and pitch and estimate gyro biases. @@ -285,8 +283,8 @@ turbomath::Vector Estimator::accel_correction() const // Correction Term of Eq. 47a and 47b Mahony Paper // w_acc = 2*s_tilde*v_tilde turbomath::Vector w_acc; - w_acc.x = -2.0f*q_tilde.w*q_tilde.x; - w_acc.y = -2.0f*q_tilde.w*q_tilde.y; + w_acc.x = -2.0f * q_tilde.w * q_tilde.x; + w_acc.y = -2.0f * q_tilde.w * q_tilde.y; w_acc.z = 0.0f; // Don't correct z, because it's unobservable from the accelerometer return w_acc; @@ -322,7 +320,7 @@ turbomath::Vector Estimator::smoothed_gyro_measurement() { // 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); + wbar = (w2_ / -12.0f) + w1_ * (8.0f / 12.0f) + gyro_LPF_ * (5.0f / 12.0f); w2_ = w1_; w1_ = gyro_LPF_; } @@ -335,12 +333,14 @@ turbomath::Vector Estimator::smoothed_gyro_measurement() } void Estimator::integrate_angular_rate(turbomath::Quaternion& quat, - const turbomath::Vector& omega, const float dt) const + 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; @@ -352,41 +352,48 @@ void Estimator::integrate_angular_rate(turbomath::Quaternion& quat, // (Eq. 12 Casey Paper) // This adds 90 us on STM32F10x chips float norm_w = sqrtf(sqrd_norm_w); - float t1 = cosf((norm_w*dt)/2.0f); - float t2 = 1.0f/norm_w * sinf((norm_w*dt)/2.0f); - quat.w = t1*quat.w + t2*(-p*quat.x - q*quat.y - r*quat.z); - quat.x = t1*quat.x + t2*( p*quat.w + r*quat.y - q*quat.z); - 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); + float t1 = cosf((norm_w * dt) / 2.0f); + float t2 = 1.0f / norm_w * sinf((norm_w * dt) / 2.0f); + quat.w = t1 * quat.w + t2 * (-p * quat.x - q * quat.y - r * quat.z); + quat.x = t1 * quat.x + t2 * (p * quat.w + r * quat.y - q * quat.z); + 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 { // 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)); - quat.w += qdot.w*dt; - quat.x += qdot.x*dt; - quat.y += qdot.y*dt; - quat.z += qdot.z*dt; + 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)); + quat.w += qdot.w * dt; + quat.x += qdot.x * dt; + quat.y += qdot.y * dt; + quat.z += qdot.z * dt; quat.normalize(); } } void Estimator::quaternion_to_dcm(const turbomath::Quaternion& q, - turbomath::Vector& X, turbomath::Vector& Y, turbomath::Vector& Z) const + turbomath::Vector& X, + turbomath::Vector& Y, + turbomath::Vector& Z) const { // R(q) = [X.x X.y X.z] // [Y.x Y.y Y.z] // [Z.x Z.y Z.z] - + const float &w = q.w, &x = q.x, &y = q.y, &z = q.z; - X.x = 1.0f - 2.0f*(y*y + z*z); X.y = 2.0f*(x*y - z*w); X.z = 2.0f*(x*z + y*w); - Y.x = 2.0f*(x*y + z*w); Y.y = 1.0f - 2.0f*(x*x + z*z); Y.z = 2.0f*(y*z - x*w); - Z.x = 2.0f*(x*z - y*w); Z.y = 2.0f*(y*z + x*w); Z.z = 1.0f - 2.0f*(x*x + y*y); + X.x = 1.0f - 2.0f * (y * y + z * z); + X.y = 2.0f * (x * y - z * w); + X.z = 2.0f * (x * z + y * w); + Y.x = 2.0f * (x * y + z * w); + Y.y = 1.0f - 2.0f * (x * x + z * z); + Y.z = 2.0f * (y * z - x * w); + Z.x = 2.0f * (x * z - y * w); + Z.y = 2.0f * (y * z + x * w); + Z.z = 1.0f - 2.0f * (x * x + y * y); } } // namespace rosflight_firmware diff --git a/src/memory_manager.cpp b/src/memory_manager.cpp index 2b6e4566..c257caa3 100644 --- a/src/memory_manager.cpp +++ b/src/memory_manager.cpp @@ -1,13 +1,10 @@ #include "memory_manager.h" + #include "rosflight.h" namespace rosflight_firmware { - -MemoryManager::MemoryManager(ROSflight &rf): - RF_{rf} -{ -} +MemoryManager::MemoryManager(ROSflight &rf) : RF_{rf} {} bool MemoryManager::read_memory() { RF_.board_.memory_init(); diff --git a/src/mixer.cpp b/src/mixer.cpp index c07995f8..e79aa835 100644 --- a/src/mixer.cpp +++ b/src/mixer.cpp @@ -29,17 +29,15 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ - -#include - #include "mixer.h" + #include "rosflight.h" +#include + namespace rosflight_firmware { - -Mixer::Mixer(ROSflight &_rf) : - RF_(_rf) +Mixer::Mixer(ROSflight &_rf) : RF_(_rf) { mixer_to_use_ = nullptr; } @@ -65,7 +63,6 @@ void Mixer::param_change_callback(uint16_t param_id) } } - void Mixer::init_mixing() { // clear the invalid mixer error @@ -86,7 +83,6 @@ void Mixer::init_mixing() mixer_to_use_ = array_of_mixers_[mixer_choice]; } - init_PWM(); for (int8_t i = 0; i < NUM_TOTAL_OUTPUTS; i++) @@ -111,7 +107,6 @@ void Mixer::init_PWM() RF_.board_.pwm_init(refresh_rate, off_pwm); } - void Mixer::write_motor(uint8_t index, float value) { if (RF_.state_manager_.state().armed) @@ -138,7 +133,6 @@ void Mixer::write_motor(uint8_t index, float value) RF_.board_.pwm_write(index, raw_outputs_[index]); } - void Mixer::write_servo(uint8_t index, float value) { if (value > 1.0) @@ -188,8 +182,8 @@ void Mixer::mix_output() 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]); + 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) @@ -203,14 +197,14 @@ void Mixer::mix_output() float scale_factor = 1.0; if (max_output > 1.0) { - scale_factor = 1.0/max_output; + scale_factor = 1.0 / max_output; } // Perform Motor Output Scaling 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; + // scale all motor outputs by scale factor (this is usually 1.0, unless we saturated) + outputs_[i] *= scale_factor; } // Insert AUX Commands, and assemble combined_output_types array (Does not override mixer values) @@ -250,4 +244,4 @@ void Mixer::mix_output() } } -} +} // namespace rosflight_firmware diff --git a/src/nanoprintf.cpp b/src/nanoprintf.cpp index c1fab8c0..6bd22529 100644 --- a/src/nanoprintf.cpp +++ b/src/nanoprintf.cpp @@ -35,117 +35,113 @@ namespace rosflight_firmware { namespace nanoprintf { - -typedef void (*putcf)(void *,char); +typedef void (*putcf)(void *, char); static putcf stdout_putf; static void *stdout_putp; - #ifdef PRINTF_LONG_SUPPORT -static void uli2a(unsigned long int num, unsigned int base, int uc,char *bf) +static void uli2a(unsigned long int num, unsigned int base, int uc, char *bf) { - int n=0; - unsigned int d=1; - while (num/d >= base) - d*=base; - while (d!=0) + int n = 0; + unsigned int d = 1; + while (num / d >= base) d *= base; + while (d != 0) { int dgt = num / d; - num%=d; - d/=base; - if (n || dgt>0|| d==0) + num %= d; + d /= base; + if (n || dgt > 0 || d == 0) { - *bf++ = dgt+(dgt<10 ? '0' : (uc ? 'A' : 'a')-10); + *bf++ = dgt + (dgt < 10 ? '0' : (uc ? 'A' : 'a') - 10); ++n; } } - *bf=0; + *bf = 0; } static void li2a(long num, char *bf) { - if (num<0) + if (num < 0) { - num=-num; + num = -num; *bf++ = '-'; } - uli2a(num,10,0,bf); + uli2a(num, 10, 0, bf); } #endif -static void ui2a(unsigned int num, unsigned int base, int uc,char *bf) +static void ui2a(unsigned int num, unsigned int base, int uc, char *bf) { - int n=0; - unsigned int d=1; + int n = 0; + unsigned int d = 1; #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wstrict-overflow" - while (num/d >= base) + while (num / d >= base) #pragma GCC diagnostic pop - d*=base; - while (d!=0) + d *= base; + while (d != 0) { int dgt = num / d; - num%= d; - d/=base; - if (n || dgt>0 || d==0) + num %= d; + d /= base; + if (n || dgt > 0 || d == 0) { - *bf++ = dgt+(dgt<10 ? '0' : (uc ? 'A' : 'a')-10); + *bf++ = dgt + (dgt < 10 ? '0' : (uc ? 'A' : 'a') - 10); ++n; } } - *bf=0; + *bf = 0; } static void i2a(int num, char *bf) { - if (num<0) + if (num < 0) { - num=-num; + num = -num; *bf++ = '-'; } - ui2a(num,10,0,bf); + ui2a(num, 10, 0, bf); } static int a2d(char ch) { - if (ch>='0' && ch<='9') - return ch-'0'; - else if (ch>='a' && ch<='f') - return ch-'a'+10; - else if (ch>='A' && ch<='F') - return ch-'A'+10; - else return -1; + if (ch >= '0' && ch <= '9') + return ch - '0'; + else if (ch >= 'a' && ch <= 'f') + return ch - 'a' + 10; + else if (ch >= 'A' && ch <= 'F') + return ch - 'A' + 10; + else + return -1; } -static char a2i(char ch, char **src,int base,int *nump) +static char a2i(char ch, char **src, int base, int *nump) { - char *p= *src; - int num=0; + char *p = *src; + int num = 0; int digit; - while ((digit=a2d(ch))>=0) + while ((digit = a2d(ch)) >= 0) { - if (digit>base) break; - num=num*base+digit; - ch=*p++; + if (digit > base) + break; + num = num * base + digit; + ch = *p++; } - *src=p; - *nump=num; + *src = p; + *nump = num; return ch; } static void putchw(void *putp, putcf putf, int n, char z, char *bf) { - char fc=z? '0' : ' '; + char fc = z ? '0' : ' '; char ch; - char *p=bf; - while (*p++ && n != 0) - n--; - while (n-- != 0) - putf(putp,fc); - while ((ch= *bf++)) - putf(putp,ch); + char *p = bf; + while (*p++ && n != 0) n--; + while (n-- != 0) putf(putp, fc); + while ((ch = *bf++)) putf(putp, ch); } void tfp_format(void *putp, putcf putf, const char *fmt, va_list va) @@ -154,114 +150,110 @@ void tfp_format(void *putp, putcf putf, const char *fmt, va_list va) char ch; - - while ((ch=*(fmt++))) + while ((ch = *(fmt++))) { - if (ch!='%') - putf(putp,ch); + if (ch != '%') + putf(putp, ch); else { - char lz=0; -#ifdef PRINTF_LONG_SUPPORT - char lng=0; + char lz = 0; +#ifdef PRINTF_LONG_SUPPORT + char lng = 0; #endif - int w=0; - ch=*(fmt++); - if (ch=='0') + int w = 0; + ch = *(fmt++); + if (ch == '0') { - ch=*(fmt++); - lz=1; + ch = *(fmt++); + lz = 1; } - if (ch>='0' && ch<='9') + if (ch >= '0' && ch <= '9') { - ch=a2i(ch, const_cast(&fmt), 10, &w); + ch = a2i(ch, const_cast(&fmt), 10, &w); } -#ifdef PRINTF_LONG_SUPPORT - if (ch=='l') +#ifdef PRINTF_LONG_SUPPORT + if (ch == 'l') { - ch=*(fmt++); - lng=1; + ch = *(fmt++); + lng = 1; } #endif switch (ch) { case 0: goto abort; - case 'u' : - { -#ifdef PRINTF_LONG_SUPPORT - if (lng) - uli2a(va_arg(va, unsigned long int),10,0,bf); - else + case 'u': + { +#ifdef PRINTF_LONG_SUPPORT + if (lng) + uli2a(va_arg(va, unsigned long int), 10, 0, bf); + else #endif - ui2a(va_arg(va, unsigned int),10,0,bf); - putchw(putp,putf,w,lz,bf); - break; - } - case 'd' : - { -#ifdef PRINTF_LONG_SUPPORT - if (lng) - li2a(va_arg(va, unsigned long int),bf); - else + ui2a(va_arg(va, unsigned int), 10, 0, bf); + putchw(putp, putf, w, lz, bf); + break; + } + case 'd': + { +#ifdef PRINTF_LONG_SUPPORT + if (lng) + li2a(va_arg(va, unsigned long int), bf); + else #endif - i2a(va_arg(va, int),bf); - putchw(putp,putf,w,lz,bf); - break; - } + i2a(va_arg(va, int), bf); + putchw(putp, putf, w, lz, bf); + break; + } case 'x': - case 'X' : -#ifdef PRINTF_LONG_SUPPORT + case 'X': +#ifdef PRINTF_LONG_SUPPORT if (lng) - uli2a(va_arg(va, unsigned long int),16,(ch=='X'),bf); + uli2a(va_arg(va, unsigned long int), 16, (ch == 'X'), bf); else #endif - ui2a(va_arg(va, unsigned int),16,(ch=='X'),bf); - putchw(putp,putf,w,lz,bf); + ui2a(va_arg(va, unsigned int), 16, (ch == 'X'), bf); + putchw(putp, putf, w, lz, bf); break; - case 'c' : - putf(putp,static_cast(va_arg(va, int))); + case 'c': + putf(putp, static_cast(va_arg(va, int))); break; - case 's' : - putchw(putp,putf,w,0,va_arg(va, char *)); + case 's': + putchw(putp, putf, w, 0, va_arg(va, char *)); break; - case '%' : - putf(putp,ch); + case '%': + putf(putp, ch); default: break; } } } -abort: - ; +abort:; } - void init_printf(void *putp, void (*putf)(void *, char)) { - stdout_putf=putf; - stdout_putp=putp; + stdout_putf = putf; + stdout_putp = putp; } void tfp_printf(const char *fmt, ...) { va_list va; - va_start(va,fmt); - tfp_format(stdout_putp,stdout_putf,fmt,va); + va_start(va, fmt); + tfp_format(stdout_putp, stdout_putf, fmt, va); va_end(va); } -static void putcp(void *p,char c) +static void putcp(void *p, char c) { *(*(static_cast(p)))++ = c; } void tfp_sprintf(char *s, const char *fmt, va_list va) { - tfp_format(&s,putcp,fmt,va); - putcp(&s,0); + tfp_format(&s, putcp, fmt, va); + putcp(&s, 0); } - } // namespace nanoprintf } // namespace rosflight_firmware diff --git a/src/param.cpp b/src/param.cpp index 554af9d7..e827fce1 100644 --- a/src/param.cpp +++ b/src/param.cpp @@ -29,16 +29,16 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#include -#include +#include "param.h" #include "board.h" #include "mixer.h" -#include "param.h" - #include "rosflight.h" +#include +#include + #ifndef GIT_VERSION_HASH #define GIT_VERSION_HASH 0x00 #pragma message "GIT_VERSION_HASH Undefined, setting to 0x00!" @@ -56,8 +56,7 @@ namespace rosflight_firmware { - -Params::Params(ROSflight& _rf, params_t ¶m_struct) : +Params::Params(ROSflight &_rf, params_t ¶m_struct) : RF_(_rf), params(param_struct), listeners_(nullptr), @@ -69,7 +68,7 @@ Params::Params(ROSflight& _rf, params_t ¶m_struct) : void Params::init_param_int(uint16_t id, const char name[PARAMS_NAME_LENGTH], int32_t value) { // copy cstr including '\0' or until maxlen - const uint8_t len = (strlen(name)>=PARAMS_NAME_LENGTH) ? PARAMS_NAME_LENGTH : strlen(name)+1; + const uint8_t len = (strlen(name) >= PARAMS_NAME_LENGTH) ? PARAMS_NAME_LENGTH : strlen(name) + 1; memcpy(params.names[id], name, len); params.values[id].ivalue = value; params.types[id] = PARAM_TYPE_INT32; @@ -78,7 +77,7 @@ void Params::init_param_int(uint16_t id, const char name[PARAMS_NAME_LENGTH], in void Params::init_param_float(uint16_t id, const char name[PARAMS_NAME_LENGTH], float value) { // copy cstr including '\0' or until maxlen - const uint8_t len = (strlen(name)>=PARAMS_NAME_LENGTH) ? PARAMS_NAME_LENGTH : strlen(name)+1; + const uint8_t len = (strlen(name) >= PARAMS_NAME_LENGTH) ? PARAMS_NAME_LENGTH : strlen(name) + 1; memcpy(params.names[id], name, len); params.values[id].fvalue = value; params.types[id] = PARAM_TYPE_FLOAT; @@ -89,23 +88,26 @@ uint8_t Params::compute_checksum(void) uint8_t chk = 0; const char *p; - for (p = reinterpret_cast(¶ms.values); p < reinterpret_cast(¶ms.values) + 4*PARAMS_COUNT; p++) + for (p = reinterpret_cast(¶ms.values); + p < reinterpret_cast(¶ms.values) + 4 * PARAMS_COUNT; p++) chk ^= *p; - for (p = reinterpret_cast(¶ms.names); p < reinterpret_cast(¶ms.names) + PARAMS_COUNT*PARAMS_NAME_LENGTH; p++) + for (p = reinterpret_cast(¶ms.names); + p < reinterpret_cast(¶ms.names) + PARAMS_COUNT * PARAMS_NAME_LENGTH; p++) chk ^= *p; - for (p = reinterpret_cast(¶ms.types); p < reinterpret_cast(¶ms.types) + PARAMS_COUNT; p++) + for (p = reinterpret_cast(¶ms.types); + p < reinterpret_cast(¶ms.types) + PARAMS_COUNT; p++) chk ^= *p; return chk; } - // function definitions void Params::init() { if (!read()) { - RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_WARNING, "Unable to load parameters; using default values"); + RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_WARNING, + "Unable to load parameters; using default values"); set_defaults(); RF_.memory_manager_.write_memory(); } @@ -116,27 +118,28 @@ void Params::set_defaults(void) /******************************/ /*** HARDWARE CONFIGURATION ***/ /******************************/ - init_param_int(PARAM_BAUD_RATE, "BAUD_RATE", 921600); // Baud rate of MAVlink communication with companion computer | 9600 | 921600 + init_param_int(PARAM_BAUD_RATE, "BAUD_RATE", + 921600); // Baud rate of MAVlink communication with companion computer | 9600 | 921600 /*****************************/ /*** MAVLINK CONFIGURATION ***/ /*****************************/ - init_param_int(PARAM_SYSTEM_ID, "SYS_ID", 1); // Mavlink System ID | 1 | 255 + init_param_int(PARAM_SYSTEM_ID, "SYS_ID", 1); // Mavlink System ID | 1 | 255 init_param_int(PARAM_STREAM_HEARTBEAT_RATE, "STRM_HRTBT", 1); // Rate of heartbeat stream (Hz) | 0 | 1000 - init_param_int(PARAM_STREAM_STATUS_RATE, "STRM_STATUS", 10); // Rate of status stream (Hz) | 0 | 1000 - - init_param_int(PARAM_STREAM_ATTITUDE_RATE, "STRM_ATTITUDE", 200); // Rate of attitude stream (Hz) | 0 | 1000 - init_param_int(PARAM_STREAM_IMU_RATE, "STRM_IMU", 250); // Rate of IMU stream (Hz) | 0 | 1000 - init_param_int(PARAM_STREAM_MAG_RATE, "STRM_MAG", 50); // Rate of magnetometer stream (Hz) | 0 | 75 - init_param_int(PARAM_STREAM_BARO_RATE, "STRM_BARO", 50); // Rate of barometer stream (Hz) | 0 | 100 - init_param_int(PARAM_STREAM_AIRSPEED_RATE, "STRM_AIRSPEED", 50); // Rate of airspeed stream (Hz) | 0 | 50 - init_param_int(PARAM_STREAM_SONAR_RATE, "STRM_SONAR", 40); // Rate of sonar stream (Hz) | 0 | 40 - init_param_int(PARAM_STREAM_GNSS_RATE, "STRM_GNSS", 1000); // Maximum rate of GNSS stream (Hz) | 0 | 10 - init_param_int(PARAM_STREAM_GNSS_RAW_RATE, "STRM_GNSS_RAW", 10); //Rate of GNSS raw stream (Hz) | 0 | 10 - init_param_int(PARAM_STREAM_BATTERY_STATUS_RATE, "STRM_BATTERY", 10); //Rate of GNSS raw stream (Hz) | 0 | 10 + init_param_int(PARAM_STREAM_STATUS_RATE, "STRM_STATUS", 10); // Rate of status stream (Hz) | 0 | 1000 + + init_param_int(PARAM_STREAM_ATTITUDE_RATE, "STRM_ATTITUDE", 200); // Rate of attitude stream (Hz) | 0 | 1000 + init_param_int(PARAM_STREAM_IMU_RATE, "STRM_IMU", 250); // Rate of IMU stream (Hz) | 0 | 1000 + init_param_int(PARAM_STREAM_MAG_RATE, "STRM_MAG", 50); // Rate of magnetometer stream (Hz) | 0 | 75 + init_param_int(PARAM_STREAM_BARO_RATE, "STRM_BARO", 50); // Rate of barometer stream (Hz) | 0 | 100 + init_param_int(PARAM_STREAM_AIRSPEED_RATE, "STRM_AIRSPEED", 50); // Rate of airspeed stream (Hz) | 0 | 50 + init_param_int(PARAM_STREAM_SONAR_RATE, "STRM_SONAR", 40); // Rate of sonar stream (Hz) | 0 | 40 + init_param_int(PARAM_STREAM_GNSS_RATE, "STRM_GNSS", 1000); // Maximum rate of GNSS stream (Hz) | 0 | 10 + init_param_int(PARAM_STREAM_GNSS_RAW_RATE, "STRM_GNSS_RAW", 10); // Rate of GNSS raw stream (Hz) | 0 | 10 + init_param_int(PARAM_STREAM_BATTERY_STATUS_RATE, "STRM_BATTERY", 10); // Rate of GNSS raw stream (Hz) | 0 | 10 init_param_int(PARAM_STREAM_OUTPUT_RAW_RATE, "STRM_SERVO", 50); // Rate of raw output stream | 0 | 490 - init_param_int(PARAM_STREAM_RC_RAW_RATE, "STRM_RC", 50); // Rate of raw RC input stream | 0 | 50 + init_param_int(PARAM_STREAM_RC_RAW_RATE, "STRM_RC", 50); // Rate of raw RC input stream | 0 | 50 /********************************/ /*** CONTROLLER CONFIGURATION ***/ @@ -151,76 +154,101 @@ void Params::set_defaults(void) init_param_float(PARAM_PID_PITCH_RATE_I, "PID_PITCH_RATE_I", 0.0000f); // Pitch Rate Integral Gain | 0.0 | 1000.0 init_param_float(PARAM_PID_PITCH_RATE_D, "PID_PITCH_RATE_D", 0.0000f); // Pitch Rate Derivative Gain | 0.0 | 1000.0 - init_param_float(PARAM_PID_YAW_RATE_P, "PID_YAW_RATE_P", 0.25f); // Yaw Rate Proportional Gain | 0.0 | 1000.0 + init_param_float(PARAM_PID_YAW_RATE_P, "PID_YAW_RATE_P", 0.25f); // Yaw Rate Proportional Gain | 0.0 | 1000.0 init_param_float(PARAM_PID_YAW_RATE_I, "PID_YAW_RATE_I", 0.0f); // Yaw Rate Integral Gain | 0.0 | 1000.0 init_param_float(PARAM_PID_YAW_RATE_D, "PID_YAW_RATE_D", 0.0f); // Yaw Rate Derivative Gain | 0.0 | 1000.0 - init_param_float(PARAM_PID_ROLL_ANGLE_P, "PID_ROLL_ANG_P", 0.15f); // Roll Angle Proportional Gain | 0.0 | 1000.0 - init_param_float(PARAM_PID_ROLL_ANGLE_I, "PID_ROLL_ANG_I", 0.0f); // Roll Angle Integral Gain | 0.0 | 1000.0 - init_param_float(PARAM_PID_ROLL_ANGLE_D, "PID_ROLL_ANG_D", 0.05f); // Roll Angle Derivative Gain | 0.0 | 1000.0 + init_param_float(PARAM_PID_ROLL_ANGLE_P, "PID_ROLL_ANG_P", 0.15f); // Roll Angle Proportional Gain | 0.0 | 1000.0 + init_param_float(PARAM_PID_ROLL_ANGLE_I, "PID_ROLL_ANG_I", 0.0f); // Roll Angle Integral Gain | 0.0 | 1000.0 + init_param_float(PARAM_PID_ROLL_ANGLE_D, "PID_ROLL_ANG_D", 0.05f); // Roll Angle Derivative Gain | 0.0 | 1000.0 - init_param_float(PARAM_PID_PITCH_ANGLE_P, "PID_PITCH_ANG_P", 0.15f); // Pitch Angle Proportional Gain | 0.0 | 1000.0 + init_param_float(PARAM_PID_PITCH_ANGLE_P, "PID_PITCH_ANG_P", 0.15f); // Pitch Angle Proportional Gain | 0.0 | 1000.0 init_param_float(PARAM_PID_PITCH_ANGLE_I, "PID_PITCH_ANG_I", 0.0f); // Pitch Angle Integral Gain | 0.0 | 1000.0 init_param_float(PARAM_PID_PITCH_ANGLE_D, "PID_PITCH_ANG_D", 0.05f); // Pitch Angle Derivative Gain | 0.0 | 1000.0 - init_param_float(PARAM_X_EQ_TORQUE, "X_EQ_TORQUE", 0.0f); // Equilibrium torque added to output of controller on x axis | -1.0 | 1.0 - init_param_float(PARAM_Y_EQ_TORQUE, "Y_EQ_TORQUE", 0.0f); // Equilibrium torque added to output of controller on y axis | -1.0 | 1.0 - init_param_float(PARAM_Z_EQ_TORQUE, "Z_EQ_TORQUE", 0.0f); // Equilibrium torque added to output of controller on z axis | -1.0 | 1.0 - - init_param_float(PARAM_PID_TAU, "PID_TAU", 0.05f); // Dirty Derivative time constant - See controller documentation | 0.0 | 1.0 + init_param_float(PARAM_X_EQ_TORQUE, "X_EQ_TORQUE", + 0.0f); // Equilibrium torque added to output of controller on x axis | -1.0 | 1.0 + init_param_float(PARAM_Y_EQ_TORQUE, "Y_EQ_TORQUE", + 0.0f); // Equilibrium torque added to output of controller on y axis | -1.0 | 1.0 + init_param_float(PARAM_Z_EQ_TORQUE, "Z_EQ_TORQUE", + 0.0f); // Equilibrium torque added to output of controller on z axis | -1.0 | 1.0 + init_param_float(PARAM_PID_TAU, "PID_TAU", + 0.05f); // Dirty Derivative time constant - See controller documentation | 0.0 | 1.0 /*************************/ /*** PWM CONFIGURATION ***/ /*************************/ - init_param_int(PARAM_MOTOR_PWM_SEND_RATE, "MOTOR_PWM_UPDATE", 0); // Overrides default PWM rate specified by mixer if non-zero - Requires reboot to take effect | 0 | 490 - init_param_float(PARAM_MOTOR_IDLE_THROTTLE, "MOTOR_IDLE_THR", 0.1); // min throttle command sent to motors when armed (Set above 0.1 to spin when armed) | 0.0 | 1.0 - init_param_float(PARAM_FAILSAFE_THROTTLE, "FAILSAFE_THR", 0.3); // Throttle sent to motors in failsafe condition (set just below hover throttle) | 0.0 | 1.0 + init_param_int( + PARAM_MOTOR_PWM_SEND_RATE, "MOTOR_PWM_UPDATE", + 0); // Overrides default PWM rate specified by mixer if non-zero - Requires reboot to take effect | 0 | 490 + init_param_float( + PARAM_MOTOR_IDLE_THROTTLE, "MOTOR_IDLE_THR", + 0.1); // min throttle command sent to motors when armed (Set above 0.1 to spin when armed) | 0.0 | 1.0 + init_param_float(PARAM_FAILSAFE_THROTTLE, "FAILSAFE_THR", + 0.3); // Throttle sent to motors in failsafe condition (set just below hover throttle) | 0.0 | 1.0 init_param_int(PARAM_SPIN_MOTORS_WHEN_ARMED, "ARM_SPIN_MOTORS", true); // Enforce MOTOR_IDLE_THR | 0 | 1 /*******************************/ /*** ESTIMATOR CONFIGURATION ***/ /*******************************/ init_param_int(PARAM_INIT_TIME, "FILTER_INIT_T", 3000); // Time in ms to initialize estimator | 0 | 100000 - init_param_float(PARAM_FILTER_KP_ACC, "FILTER_KP_ACC", 0.5f); // estimator proportional gain on accel-based error - See estimator documentation | 0 | 10.0 - init_param_float(PARAM_FILTER_KI, "FILTER_KI", 0.01f); // estimator integral gain - See estimator documentation | 0 | 1.0 - init_param_float(PARAM_FILTER_KP_EXT, "FILTER_KP_EXT", 1.5f); // estimator proportional gain on external attitude-based error - See estimator documentation | 0 | 10.0 - init_param_float(PARAM_FILTER_ACCEL_MARGIN, "FILTER_ACCMARGIN", 0.1f); // allowable accel norm margin around 1g to determine if accel is usable | 0 | 1.0 - - init_param_int(PARAM_FILTER_USE_QUAD_INT, "FILTER_QUAD_INT", 1); // Perform a quadratic averaging of LPF gyro data prior to integration (adds ~20 us to estimation loop on F1 processors) | 0 | 1 - init_param_int(PARAM_FILTER_USE_MAT_EXP, "FILTER_MAT_EXP", 1); // 1 - Use matrix exponential to improve gyro integration (adds ~90 us to estimation loop in F1 processors) 0 - use euler integration | 0 | 1 - init_param_int(PARAM_FILTER_USE_ACC, "FILTER_USE_ACC", 1); // Use accelerometer to correct gyro integration drift (adds ~70 us to estimation loop) | 0 | 1 - - init_param_int(PARAM_CALIBRATE_GYRO_ON_ARM, "CAL_GYRO_ARM", false); // True if desired to calibrate gyros on arm | 0 | 1 - - init_param_float(PARAM_GYRO_XY_ALPHA, "GYROXY_LPF_ALPHA", 0.3f); // Low-pass filter constant on gyro X and Y axes - See estimator documentation | 0 | 1.0 - init_param_float(PARAM_GYRO_Z_ALPHA, "GYROZ_LPF_ALPHA", 0.3f); // Low-pass filter constant on gyro Z axis - See estimator documentation | 0 | 1.0 - init_param_float(PARAM_ACC_ALPHA, "ACC_LPF_ALPHA", 0.5f); // Low-pass filter constant on all accel axes - See estimator documentation | 0 | 1.0 + init_param_float(PARAM_FILTER_KP_ACC, "FILTER_KP_ACC", + 0.5f); // estimator proportional gain on accel-based error - See estimator documentation | 0 | 10.0 + init_param_float(PARAM_FILTER_KI, "FILTER_KI", + 0.01f); // estimator integral gain - See estimator documentation | 0 | 1.0 + init_param_float( + PARAM_FILTER_KP_EXT, "FILTER_KP_EXT", + 1.5f); // estimator proportional gain on external attitude-based error - See estimator documentation | 0 | 10.0 + init_param_float(PARAM_FILTER_ACCEL_MARGIN, "FILTER_ACCMARGIN", + 0.1f); // allowable accel norm margin around 1g to determine if accel is usable | 0 | 1.0 + + init_param_int(PARAM_FILTER_USE_QUAD_INT, "FILTER_QUAD_INT", + 1); // Perform a quadratic averaging of LPF gyro data prior to integration (adds ~20 us to estimation + // loop on F1 processors) | 0 | 1 + init_param_int(PARAM_FILTER_USE_MAT_EXP, "FILTER_MAT_EXP", + 1); // 1 - Use matrix exponential to improve gyro integration (adds ~90 us to estimation loop in F1 + // processors) 0 - use euler integration | 0 | 1 + init_param_int(PARAM_FILTER_USE_ACC, "FILTER_USE_ACC", + 1); // Use accelerometer to correct gyro integration drift (adds ~70 us to estimation loop) | 0 | 1 + + init_param_int(PARAM_CALIBRATE_GYRO_ON_ARM, "CAL_GYRO_ARM", + false); // True if desired to calibrate gyros on arm | 0 | 1 + + init_param_float(PARAM_GYRO_XY_ALPHA, "GYROXY_LPF_ALPHA", + 0.3f); // Low-pass filter constant on gyro X and Y axes - See estimator documentation | 0 | 1.0 + init_param_float(PARAM_GYRO_Z_ALPHA, "GYROZ_LPF_ALPHA", + 0.3f); // Low-pass filter constant on gyro Z axis - See estimator documentation | 0 | 1.0 + init_param_float(PARAM_ACC_ALPHA, "ACC_LPF_ALPHA", + 0.5f); // Low-pass filter constant on all accel axes - See estimator documentation | 0 | 1.0 init_param_float(PARAM_GYRO_X_BIAS, "GYRO_X_BIAS", 0.0f); // Constant x-bias of gyroscope readings | -1.0 | 1.0 init_param_float(PARAM_GYRO_Y_BIAS, "GYRO_Y_BIAS", 0.0f); // Constant y-bias of gyroscope readings | -1.0 | 1.0 init_param_float(PARAM_GYRO_Z_BIAS, "GYRO_Z_BIAS", 0.0f); // Constant z-bias of gyroscope readings | -1.0 | 1.0 - init_param_float(PARAM_ACC_X_BIAS, "ACC_X_BIAS", 0.0f); // Constant x-bias of accelerometer readings | -2.0 | 2.0 - init_param_float(PARAM_ACC_Y_BIAS, "ACC_Y_BIAS", 0.0f); // Constant y-bias of accelerometer readings | -2.0 | 2.0 - init_param_float(PARAM_ACC_Z_BIAS, "ACC_Z_BIAS", 0.0f); // Constant z-bias of accelerometer readings | -2.0 | 2.0 - init_param_float(PARAM_ACC_X_TEMP_COMP, "ACC_X_TEMP_COMP", 0.0f); // Linear x-axis temperature compensation constant | -2.0 | 2.0 - init_param_float(PARAM_ACC_Y_TEMP_COMP, "ACC_Y_TEMP_COMP", 0.0f); // Linear y-axis temperature compensation constant | -2.0 | 2.0 - init_param_float(PARAM_ACC_Z_TEMP_COMP, "ACC_Z_TEMP_COMP", 0.0f); // Linear z-axis temperature compensation constant | -2.0 | 2.0 - - init_param_float(PARAM_MAG_A11_COMP, "MAG_A11_COMP", 1.0f); // Soft iron compensation constant | -999.0 | 999.0 - init_param_float(PARAM_MAG_A12_COMP, "MAG_A12_COMP", 0.0f); // Soft iron compensation constant | -999.0 | 999.0 - init_param_float(PARAM_MAG_A13_COMP, "MAG_A13_COMP", 0.0f); // Soft iron compensation constant | -999.0 | 999.0 - init_param_float(PARAM_MAG_A21_COMP, "MAG_A21_COMP", 0.0f); // Soft iron compensation constant | -999.0 | 999.0 - init_param_float(PARAM_MAG_A22_COMP, "MAG_A22_COMP", 1.0f); // Soft iron compensation constant | -999.0 | 999.0 - init_param_float(PARAM_MAG_A23_COMP, "MAG_A23_COMP", 0.0f); // Soft iron compensation constant | -999.0 | 999.0 - init_param_float(PARAM_MAG_A31_COMP, "MAG_A31_COMP", 0.0f); // Soft iron compensation constant | -999.0 | 999.0 - init_param_float(PARAM_MAG_A32_COMP, "MAG_A32_COMP", 0.0f); // Soft iron compensation constant | -999.0 | 999.0 - init_param_float(PARAM_MAG_A33_COMP, "MAG_A33_COMP", 1.0f); // Soft iron compensation constant | -999.0 | 999.0 - init_param_float(PARAM_MAG_X_BIAS, "MAG_X_BIAS", 0.0f); // Hard iron compensation constant | -999.0 | 999.0 - init_param_float(PARAM_MAG_Y_BIAS, "MAG_Y_BIAS", 0.0f); // Hard iron compensation constant | -999.0 | 999.0 - init_param_float(PARAM_MAG_Z_BIAS, "MAG_Z_BIAS", 0.0f); // Hard iron compensation constant | -999.0 | 999.0 - - init_param_float(PARAM_BARO_BIAS, "BARO_BIAS", 0.0f); // Barometer measurement bias (Pa) | 0 | inf + init_param_float(PARAM_ACC_X_BIAS, "ACC_X_BIAS", 0.0f); // Constant x-bias of accelerometer readings | -2.0 | 2.0 + init_param_float(PARAM_ACC_Y_BIAS, "ACC_Y_BIAS", 0.0f); // Constant y-bias of accelerometer readings | -2.0 | 2.0 + init_param_float(PARAM_ACC_Z_BIAS, "ACC_Z_BIAS", 0.0f); // Constant z-bias of accelerometer readings | -2.0 | 2.0 + init_param_float(PARAM_ACC_X_TEMP_COMP, "ACC_X_TEMP_COMP", + 0.0f); // Linear x-axis temperature compensation constant | -2.0 | 2.0 + init_param_float(PARAM_ACC_Y_TEMP_COMP, "ACC_Y_TEMP_COMP", + 0.0f); // Linear y-axis temperature compensation constant | -2.0 | 2.0 + init_param_float(PARAM_ACC_Z_TEMP_COMP, "ACC_Z_TEMP_COMP", + 0.0f); // Linear z-axis temperature compensation constant | -2.0 | 2.0 + + init_param_float(PARAM_MAG_A11_COMP, "MAG_A11_COMP", 1.0f); // Soft iron compensation constant | -999.0 | 999.0 + init_param_float(PARAM_MAG_A12_COMP, "MAG_A12_COMP", 0.0f); // Soft iron compensation constant | -999.0 | 999.0 + init_param_float(PARAM_MAG_A13_COMP, "MAG_A13_COMP", 0.0f); // Soft iron compensation constant | -999.0 | 999.0 + init_param_float(PARAM_MAG_A21_COMP, "MAG_A21_COMP", 0.0f); // Soft iron compensation constant | -999.0 | 999.0 + init_param_float(PARAM_MAG_A22_COMP, "MAG_A22_COMP", 1.0f); // Soft iron compensation constant | -999.0 | 999.0 + init_param_float(PARAM_MAG_A23_COMP, "MAG_A23_COMP", 0.0f); // Soft iron compensation constant | -999.0 | 999.0 + init_param_float(PARAM_MAG_A31_COMP, "MAG_A31_COMP", 0.0f); // Soft iron compensation constant | -999.0 | 999.0 + init_param_float(PARAM_MAG_A32_COMP, "MAG_A32_COMP", 0.0f); // Soft iron compensation constant | -999.0 | 999.0 + init_param_float(PARAM_MAG_A33_COMP, "MAG_A33_COMP", 1.0f); // Soft iron compensation constant | -999.0 | 999.0 + init_param_float(PARAM_MAG_X_BIAS, "MAG_X_BIAS", 0.0f); // Hard iron compensation constant | -999.0 | 999.0 + init_param_float(PARAM_MAG_Y_BIAS, "MAG_Y_BIAS", 0.0f); // Hard iron compensation constant | -999.0 | 999.0 + init_param_float(PARAM_MAG_Z_BIAS, "MAG_Z_BIAS", 0.0f); // Hard iron compensation constant | -999.0 | 999.0 + + init_param_float(PARAM_BARO_BIAS, "BARO_BIAS", 0.0f); // Barometer measurement bias (Pa) | 0 | inf init_param_float(PARAM_GROUND_LEVEL, "GROUND_LEVEL", 1387.0f); // Altitude of ground level (m) | -1000 | 10000 init_param_float(PARAM_DIFF_PRESS_BIAS, "DIFF_PRESS_BIAS", 0.0f); // Differential Pressure Bias (Pa) | -10 | 10 @@ -232,10 +260,15 @@ void Params::set_defaults(void) init_param_int(PARAM_RC_Y_CHANNEL, "RC_Y_CHN", 1); // RC input channel mapped to y-axis commands [0 - indexed] | 0 | 3 init_param_int(PARAM_RC_Z_CHANNEL, "RC_Z_CHN", 3); // RC input channel mapped to z-axis commands [0 - indexed] | 0 | 3 init_param_int(PARAM_RC_F_CHANNEL, "RC_F_CHN", 2); // RC input channel mapped to F-axis commands [0 - indexed] | 0 | 3 - init_param_int(PARAM_RC_ATTITUDE_OVERRIDE_CHANNEL, "RC_ATT_OVRD_CHN", 4); // RC switch mapped to attitude override [0 indexed, -1 to disable] | 4 | 7 - init_param_int(PARAM_RC_THROTTLE_OVERRIDE_CHANNEL, "RC_THR_OVRD_CHN", 4); // RC switch channel mapped to throttle override [0 indexed, -1 to disable] | 4 | 7 - init_param_int(PARAM_RC_ATT_CONTROL_TYPE_CHANNEL, "RC_ATT_CTRL_CHN",-1); // RC switch channel mapped to attitude control type [0 indexed, -1 to disable] | 4 | 7 - init_param_int(PARAM_RC_ARM_CHANNEL, "ARM_CHANNEL",-1); // RC switch channel mapped to arming (only if PARAM_ARM_STICKS is false) [0 indexed, -1 to disable] | 4 | 7 + init_param_int(PARAM_RC_ATTITUDE_OVERRIDE_CHANNEL, "RC_ATT_OVRD_CHN", + 4); // RC switch mapped to attitude override [0 indexed, -1 to disable] | 4 | 7 + init_param_int(PARAM_RC_THROTTLE_OVERRIDE_CHANNEL, "RC_THR_OVRD_CHN", + 4); // RC switch channel mapped to throttle override [0 indexed, -1 to disable] | 4 | 7 + init_param_int(PARAM_RC_ATT_CONTROL_TYPE_CHANNEL, "RC_ATT_CTRL_CHN", + -1); // RC switch channel mapped to attitude control type [0 indexed, -1 to disable] | 4 | 7 + init_param_int( + PARAM_RC_ARM_CHANNEL, "ARM_CHANNEL", + -1); // RC switch channel mapped to arming (only if PARAM_ARM_STICKS is false) [0 indexed, -1 to disable] | 4 | 7 init_param_int(PARAM_RC_NUM_CHANNELS, "RC_NUM_CHN", 6); // number of RC input channels | 1 | 8 init_param_int(PARAM_RC_SWITCH_5_DIRECTION, "SWITCH_5_DIR", 1); // RC switch 5 toggle direction | -1 | 1 @@ -243,36 +276,49 @@ void Params::set_defaults(void) init_param_int(PARAM_RC_SWITCH_7_DIRECTION, "SWITCH_7_DIR", 1); // RC switch 7 toggle direction | -1 | 1 init_param_int(PARAM_RC_SWITCH_8_DIRECTION, "SWITCH_8_DIR", 1); // RC switch 8 toggle direction | -1 | 1 - init_param_float(PARAM_RC_OVERRIDE_DEVIATION, "RC_OVRD_DEV", 0.1); // RC stick deviation from center for override | 0.0 | 1.0 - init_param_int(PARAM_OVERRIDE_LAG_TIME, "OVRD_LAG_TIME", 1000); // RC stick deviation lag time before returning control (ms) | 0 | 100000 - init_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE, "MIN_THROTTLE", true); // Take minimum throttle between RC and computer at all times | 0 | 1 - - init_param_int(PARAM_RC_ATTITUDE_MODE, "RC_ATT_MODE", 1); // Attitude mode for RC sticks (0: rate, 1: angle). Overridden if RC_ATT_CTRL_CHN is set. | 0 | 1 - init_param_float(PARAM_RC_MAX_ROLL, "RC_MAX_ROLL", 0.786f); // Maximum roll angle command sent by full deflection of RC sticks | 0.0 | 3.14159 - init_param_float(PARAM_RC_MAX_PITCH, "RC_MAX_PITCH", 0.786f); // Maximum pitch angle command sent by full stick deflection of RC sticks | 0.0 | 3.14159 - init_param_float(PARAM_RC_MAX_ROLLRATE, "RC_MAX_ROLLRATE", 3.14159f); // Maximum roll rate command sent by full stick deflection of RC sticks | 0.0 | 9.42477796077 - init_param_float(PARAM_RC_MAX_PITCHRATE, "RC_MAX_PITCHRATE", 3.14159f); // Maximum pitch command sent by full stick deflection of RC sticks | 0.0 | 3.14159 - init_param_float(PARAM_RC_MAX_YAWRATE, "RC_MAX_YAWRATE", 1.507f); // Maximum pitch command sent by full stick deflection of RC sticks | 0.0 | 3.14159 + init_param_float(PARAM_RC_OVERRIDE_DEVIATION, "RC_OVRD_DEV", + 0.1); // RC stick deviation from center for override | 0.0 | 1.0 + init_param_int(PARAM_OVERRIDE_LAG_TIME, "OVRD_LAG_TIME", + 1000); // RC stick deviation lag time before returning control (ms) | 0 | 100000 + init_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE, "MIN_THROTTLE", + true); // Take minimum throttle between RC and computer at all times | 0 | 1 + + init_param_int(PARAM_RC_ATTITUDE_MODE, "RC_ATT_MODE", + 1); // Attitude mode for RC sticks (0: rate, 1: angle). Overridden if RC_ATT_CTRL_CHN is set. | 0 | 1 + init_param_float(PARAM_RC_MAX_ROLL, "RC_MAX_ROLL", + 0.786f); // Maximum roll angle command sent by full deflection of RC sticks | 0.0 | 3.14159 + init_param_float(PARAM_RC_MAX_PITCH, "RC_MAX_PITCH", + 0.786f); // Maximum pitch angle command sent by full stick deflection of RC sticks | 0.0 | 3.14159 + init_param_float( + PARAM_RC_MAX_ROLLRATE, "RC_MAX_ROLLRATE", + 3.14159f); // Maximum roll rate command sent by full stick deflection of RC sticks | 0.0 | 9.42477796077 + init_param_float(PARAM_RC_MAX_PITCHRATE, "RC_MAX_PITCHRATE", + 3.14159f); // Maximum pitch command sent by full stick deflection of RC sticks | 0.0 | 3.14159 + init_param_float(PARAM_RC_MAX_YAWRATE, "RC_MAX_YAWRATE", + 1.507f); // Maximum pitch command sent by full stick deflection of RC sticks | 0.0 | 3.14159 /***************************/ /*** FRAME CONFIGURATION ***/ /***************************/ - init_param_int(PARAM_MIXER, "MIXER", Mixer::INVALID_MIXER); // Which mixer to choose - See Mixer documentation | 0 | 10 + init_param_int(PARAM_MIXER, "MIXER", + Mixer::INVALID_MIXER); // Which mixer to choose - See Mixer documentation | 0 | 10 - init_param_int(PARAM_FIXED_WING, "FIXED_WING", false); // switches on pass-through commands for fixed-wing operation | 0 | 1 + init_param_int(PARAM_FIXED_WING, "FIXED_WING", + false); // switches on pass-through commands for fixed-wing operation | 0 | 1 init_param_int(PARAM_ELEVATOR_REVERSE, "ELEVATOR_REV", 0); // reverses elevator servo output | 0 | 1 - init_param_int(PARAM_AILERON_REVERSE, "AIL_REV", 0); // reverses aileron servo output | 0 | 1 - init_param_int(PARAM_RUDDER_REVERSE, "RUDDER_REV", 0); // reverses rudder servo output | 0 | 1 + init_param_int(PARAM_AILERON_REVERSE, "AIL_REV", 0); // reverses aileron servo output | 0 | 1 + init_param_int(PARAM_RUDDER_REVERSE, "RUDDER_REV", 0); // reverses rudder servo output | 0 | 1 init_param_float(PARAM_FC_ROLL, "FC_ROLL", 0.0f); // roll angle (deg) of flight controller wrt aircraft body | 0 | 360 - init_param_float(PARAM_FC_PITCH, "FC_PITCH", 0.0f); // pitch angle (deg) of flight controller wrt aircraft body | 0 | 360 + init_param_float(PARAM_FC_PITCH, "FC_PITCH", + 0.0f); // pitch angle (deg) of flight controller wrt aircraft body | 0 | 360 init_param_float(PARAM_FC_YAW, "FC_YAW", 0.0f); // yaw angle (deg) of flight controller wrt aircraft body | 0 | 360 - /********************/ /*** ARMING SETUP ***/ /********************/ - init_param_float(PARAM_ARM_THRESHOLD, "ARM_THRESHOLD", 0.15); // RC deviation from max/min in yaw and throttle for arming and disarming check (us) | 0 | 500 + init_param_float(PARAM_ARM_THRESHOLD, "ARM_THRESHOLD", + 0.15); // RC deviation from max/min in yaw and throttle for arming and disarming check (us) | 0 | 500 /*****************************/ /*** BATTERY MONITOR SETUP ***/ @@ -285,10 +331,12 @@ void Params::set_defaults(void) /************************/ /*** OFFBOARD CONTROL ***/ /************************/ - init_param_int(PARAM_OFFBOARD_TIMEOUT, "OFFBOARD_TIMEOUT", 100); // Timeout in milliseconds for offboard commands, after which RC override is activated | 0 | 100000 + init_param_int( + PARAM_OFFBOARD_TIMEOUT, "OFFBOARD_TIMEOUT", + 100); // Timeout in milliseconds for offboard commands, after which RC override is activated | 0 | 100000 } -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; @@ -393,4 +441,4 @@ bool Params::set_param_by_name_float(const char name[PARAMS_NAME_LENGTH], float tmp.fvalue = value; return set_param_by_name_int(name, tmp.ivalue); } -} +} // namespace rosflight_firmware diff --git a/src/rc.cpp b/src/rc.cpp index 9e823310..91375f48 100644 --- a/src/rc.cpp +++ b/src/rc.cpp @@ -29,18 +29,15 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ - -#include - #include "rc.h" + #include "rosflight.h" +#include + namespace rosflight_firmware { - -RC::RC(ROSflight &_rf) : - RF_(_rf) -{} +RC::RC(ROSflight &_rf) : RF_(_rf) {} void RC::init() { @@ -95,7 +92,6 @@ bool RC::switch_mapped(Switch channel) return switches[channel].mapped; } - void RC::init_sticks(void) { sticks[STICK_X].channel = RF_.params_.get_param_int(PARAM_RC_X_CHANNEL); @@ -140,8 +136,8 @@ void RC::init_switches() break; } - switches[chan].mapped = switches[chan].channel > 3 - && switches[chan].channel < RF_.params_.get_param_int(PARAM_RC_NUM_CHANNELS); + switches[chan].mapped = + switches[chan].channel > 3 && switches[chan].channel < RF_.params_.get_param_int(PARAM_RC_NUM_CHANNELS); switch (switches[chan].channel) { @@ -163,7 +159,8 @@ void RC::init_switches() } if (switches[chan].mapped) - RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_INFO, "%s switch mapped to RC channel %d", channel_name, switches[chan].channel); + 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); } @@ -181,7 +178,7 @@ bool RC::check_rc_lost() else { // go into failsafe if we get an invalid RC command for any channel - for (int8_t i = 0; i 1.25) @@ -250,7 +247,8 @@ void RC::look_for_arm_disarm_signal() if (RF_.rc_.switch_on(SWITCH_ARM)) { if (!RF_.state_manager_.state().armed) - RF_.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM);; + RF_.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM); + ; } else { @@ -259,7 +257,6 @@ void RC::look_for_arm_disarm_signal() } } - bool RC::run() { uint32_t now = RF_.board_.clock_millis(); @@ -271,17 +268,15 @@ bool RC::run() } last_rc_receive_time = now; - // Check for rc lost if (check_rc_lost()) return false; - // read and normalize stick values 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 + if (sticks[channel].one_sided) // generally only F is one_sided { stick_values[channel] = pwm; } @@ -327,7 +322,8 @@ bool RC::new_command() return true; } else - return false;; + return false; + ; } -} +} // namespace rosflight_firmware diff --git a/src/rosflight.cpp b/src/rosflight.cpp index 088e59bc..8feddee7 100644 --- a/src/rosflight.cpp +++ b/src/rosflight.cpp @@ -29,12 +29,12 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#include "interface/param_listener.h" #include "rosflight.h" +#include "interface/param_listener.h" + namespace rosflight_firmware { - ROSflight::ROSflight(Board& board, CommLinkInterface& comm_link) : board_(board), memory_manager_(*this), @@ -66,7 +66,7 @@ void ROSflight::init() params_.init(); config_manager_.init(); - //Initialize devices + // Initialize devices config_manager_.configure_devices(); // Initialize Mixer @@ -105,7 +105,6 @@ void ROSflight::init() state_manager_.check_backup_memory(); } - // Main loop void ROSflight::run() { @@ -146,4 +145,4 @@ 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 22d56dac..28a874ee 100644 --- a/src/sensors.cpp +++ b/src/sensors.cpp @@ -30,35 +30,34 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#include -#include -#include - #include "sensors.h" + #include "param.h" + #include "rosflight.h" #include +#include +#include +#include + namespace rosflight_firmware { - -const float Sensors::BARO_MAX_CHANGE_RATE = 200.0f; // approx 200 m/s +const float Sensors::BARO_MAX_CHANGE_RATE = 200.0f; // approx 200 m/s const float Sensors::BARO_SAMPLE_RATE = 50.0f; -const float Sensors::DIFF_MAX_CHANGE_RATE = 225.0f; // approx 15 m/s^2 +const float Sensors::DIFF_MAX_CHANGE_RATE = 225.0f; // approx 15 m/s^2 const float Sensors::DIFF_SAMPLE_RATE = 50.0f; -const float Sensors::SONAR_MAX_CHANGE_RATE = 100.0f; // 100 m/s +const float Sensors::SONAR_MAX_CHANGE_RATE = 100.0f; // 100 m/s const float Sensors::SONAR_SAMPLE_RATE = 50.0f; const int Sensors::SENSOR_CAL_DELAY_CYCLES = 128; 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 +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() { @@ -73,7 +72,7 @@ void Sensors::init() next_sensor_to_update_ = BAROMETER; float alt = rf_.params_.get_param_float(PARAM_GROUND_LEVEL); - ground_pressure_ = 101325.0f*static_cast(pow((1-2.25694e-5 * alt), 5.2553)); + ground_pressure_ = 101325.0f * static_cast(pow((1 - 2.25694e-5 * alt), 5.2553)); baro_outlier_filt_.init(BARO_MAX_CHANGE_RATE, BARO_SAMPLE_RATE, ground_pressure_); diff_outlier_filt_.init(DIFF_MAX_CHANGE_RATE, DIFF_SAMPLE_RATE, 0.0f); @@ -92,9 +91,9 @@ 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); } @@ -125,7 +124,6 @@ void Sensors::param_change_callback(uint16_t param_id) } } - bool Sensors::run(void) { // First, check for new IMU data @@ -141,7 +139,6 @@ bool Sensors::run(void) return got_imu; } - void Sensors::update_other_sensors() { switch (next_sensor_to_update_) @@ -193,7 +190,7 @@ void Sensors::update_other_sensors() { // 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 + rf_.board_.diff_pressure_update(); // update assists in recovering sensor if it temporarily disappears if (rf_.board_.diff_pressure_present()) { @@ -223,11 +220,11 @@ void Sensors::update_other_sensors() } 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(); - } + 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; @@ -236,7 +233,6 @@ void Sensors::update_other_sensors() next_sensor_to_update_ = (next_sensor_to_update_ + 1) % NUM_LOW_PRIORITY_SENSORS; } - void Sensors::look_for_disabled_sensors() { // Look for disabled sensors while disarmed (poll every second) @@ -359,10 +355,9 @@ bool Sensors::update_imu(void) } } - 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; + float delta_t = (data_.imu_time - int_start_us_) * 1e-6; accel = accel_int_ / delta_t; gyro = gyro_int_ / delta_t; accel_int_ *= 0.0; @@ -376,14 +371,14 @@ void Sensors::update_battery_monitor() 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_); + data_.battery_voltage = data_.battery_voltage * 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_); + data_.battery_current = data_.battery_current * battery_current_alpha_ + + rf_.board_.battery_current_read() * (1 - battery_current_alpha_); } } //====================================================================== @@ -428,19 +423,14 @@ void Sensors::calibrate_gyro() turbomath::Vector vector_max(turbomath::Vector a, turbomath::Vector b) { - return turbomath::Vector(a.x > b.x ? a.x : b.x, - a.y > b.y ? a.y : b.y, - a.z > b.z ? a.z : b.z); + return turbomath::Vector(a.x > b.x ? a.x : b.x, a.y > b.y ? a.y : b.y, a.z > b.z ? a.z : b.z); } turbomath::Vector vector_min(turbomath::Vector a, turbomath::Vector b) { - return turbomath::Vector(a.x < b.x ? a.x : b.x, - a.y < b.y ? a.y : b.y, - a.z < b.z ? a.z : b.z); + return turbomath::Vector(a.x < b.x ? a.x : b.x, a.y < b.y ? a.y : b.y, a.z < b.z ? a.z : b.z); } - void Sensors::calibrate_accel(void) { acc_sum_ = acc_sum_ + data_.accel + gravity_; @@ -454,25 +444,22 @@ void Sensors::calibrate_accel(void) // 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. - turbomath::Vector accel_temp_bias = - { - rf_.params_.get_param_float(PARAM_ACC_X_TEMP_COMP), - rf_.params_.get_param_float(PARAM_ACC_Y_TEMP_COMP), - rf_.params_.get_param_float(PARAM_ACC_Z_TEMP_COMP) - }; + turbomath::Vector accel_temp_bias = {rf_.params_.get_param_float(PARAM_ACC_X_TEMP_COMP), + rf_.params_.get_param_float(PARAM_ACC_Y_TEMP_COMP), + rf_.params_.get_param_float(PARAM_ACC_Z_TEMP_COMP)}; // Figure out the proper accel bias. // We have to consider the contribution of temperature during the calibration, // Which is why this line is so confusing. What we are doing, is first removing // 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_); + turbomath::Vector accel_bias = + (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) + if ((max_ - min_).norm() > 1.0) { rf_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, "Too much movement for IMU cal"); calibrating_acc_flag_ = false; @@ -499,7 +486,7 @@ void Sensors::calibrate_accel(void) // 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", static_cast(accel_bias.norm()), - static_cast(accel_bias.norm()*1000)%1000); + static_cast(accel_bias.norm() * 1000) % 1000); } } @@ -591,17 +578,16 @@ void Sensors::calibrate_diff_pressure() } } - //====================================================== // Correction Functions (These apply calibration constants) 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 + 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); - data_.accel.y -= rf_.params_.get_param_float(PARAM_ACC_Y_TEMP_COMP)*data_.imu_temperature + 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); - data_.accel.z -= rf_.params_.get_param_float(PARAM_ACC_Z_TEMP_COMP)*data_.imu_temperature + 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); data_.gyro.x -= rf_.params_.get_param_float(PARAM_GYRO_X_BIAS); @@ -617,15 +603,15 @@ void Sensors::correct_mag(void) float mag_hard_z = data_.mag.z - rf_.params_.get_param_float(PARAM_MAG_Z_BIAS); // 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; - 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; - 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; + 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; + 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; + 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; } void Sensors::correct_baro(void) @@ -644,8 +630,9 @@ void Sensors::correct_diff_pressure() 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)); + 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) diff --git a/src/state_manager.cpp b/src/state_manager.cpp index 58b497c6..3ff63eb8 100644 --- a/src/state_manager.cpp +++ b/src/state_manager.cpp @@ -30,13 +30,12 @@ */ #include "state_manager.h" + #include "rosflight.h" 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; @@ -138,7 +137,8 @@ void StateManager::set_event(StateManager::Event event) } else { - RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, "RC throttle override must be active to arm"); + RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, + "RC throttle override must be active to arm"); } } else @@ -269,7 +269,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; @@ -278,7 +278,7 @@ void StateManager::write_backup_data(const BackupData::DebugInfo& debug) data.debug = debug; data.finalize(); - RF_.board_.backup_memory_write(reinterpret_cast(&data), sizeof(data)); + RF_.board_.backup_memory_write(reinterpret_cast(&data), sizeof(data)); } void StateManager::check_backup_memory() @@ -334,7 +334,7 @@ void StateManager::update_leds() if (next_led_blink_ms_ < RF_.board_.clock_millis()) { RF_.board_.led1_toggle(); - next_led_blink_ms_ = RF_.board_.clock_millis() + 100; + next_led_blink_ms_ = RF_.board_.clock_millis() + 100; } } // blink slowly if in error @@ -343,7 +343,7 @@ void StateManager::update_leds() if (next_led_blink_ms_ < RF_.board_.clock_millis()) { RF_.board_.led1_toggle(); - next_led_blink_ms_ = RF_.board_.clock_millis() + 500; + next_led_blink_ms_ = RF_.board_.clock_millis() + 500; } } // off if disarmed, on if armed @@ -353,4 +353,4 @@ void StateManager::update_leds() RF_.board_.led1_on(); } -} //namespace rosflight_firmware +} // namespace rosflight_firmware From 99bd20b87ef15d8e8722c82667ef718a35b07547 Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Fri, 3 Apr 2020 14:23:15 -0600 Subject: [PATCH 72/76] More clang-foramt --- boards/airbourne/airbourne_board.cpp | 66 +++---- .../airbourne_board_config_manager.cpp | 114 ++++++------ boards/airbourne/main.cpp | 107 ++++++----- boards/breezy/breezy_board.cpp | 32 ++-- boards/breezy/breezy_board_config_manager.cpp | 14 +- boards/breezy/main.cpp | 3 +- comms/mavlink/mavlink.cpp | 126 ++++++------- comms/mavlink/mavlink.h | 54 ++++-- include/interface/comm_link.h | 175 ++++++++++-------- include/interface/param_listener.h | 1 - test/command_manager_test.cpp | 114 ++++++------ test/common.cpp | 4 +- test/config_manager_test.cpp | 30 ++- test/estimator_test.cpp | 164 ++++++++-------- test/parameters_test.cpp | 6 +- test/state_machine_test.cpp | 90 ++++----- test/test_board.cpp | 100 +++++++--- test/test_board_config_manager.cpp | 27 ++- test/turbotrig_test.cpp | 155 +++++++--------- 19 files changed, 710 insertions(+), 672 deletions(-) diff --git a/boards/airbourne/airbourne_board.cpp b/boards/airbourne/airbourne_board.cpp index fe70dbf4..5d4f23d7 100644 --- a/boards/airbourne/airbourne_board.cpp +++ b/boards/airbourne/airbourne_board.cpp @@ -33,10 +33,7 @@ namespace rosflight_firmware { - -AirbourneBoard::AirbourneBoard() -{ -} +AirbourneBoard::AirbourneBoard() {} void AirbourneBoard::init_board() { @@ -76,7 +73,7 @@ void AirbourneBoard::clock_delay(uint32_t milliseconds) void AirbourneBoard::serial_init(uint32_t baud_rate, hardware_config_t configuration) { vcp_.init(); // VCP is always initialized, so that if UART is mistakenly enabled, it can still be used - switch(configuration) + switch (configuration) { default: case AirbourneConfiguration::SERIAL_VCP: @@ -99,7 +96,7 @@ void AirbourneBoard::serial_init(uint32_t baud_rate, hardware_config_t configura void AirbourneBoard::serial_write(const uint8_t *src, size_t len) { - if(vcp_.connected()) + if (vcp_.connected()) current_serial_ = &vcp_; current_serial_->write(src, len); } @@ -111,7 +108,6 @@ uint16_t AirbourneBoard::serial_bytes_available() uint8_t AirbourneBoard::serial_read() { - return current_serial_->read_byte(); } @@ -123,7 +119,7 @@ void AirbourneBoard::serial_flush() // Resources bool AirbourneBoard::enable_device(device_t device, hardware_config_t configuration, const Params ¶ms) { - switch(device) + switch (device) { case Configuration::SERIAL: { @@ -133,7 +129,7 @@ bool AirbourneBoard::enable_device(device_t device, hardware_config_t configurat break; } case Configuration::RC: - switch(configuration) + switch (configuration) { case AirbourneConfiguration::RC_PPM: rc_init(RC_TYPE_PPM); @@ -146,9 +142,9 @@ bool AirbourneBoard::enable_device(device_t device, hardware_config_t configurat } return true; case Configuration::AIRSPEED: - if(configuration==AirbourneConfiguration::AIRSPEED_I2C2) + if (configuration == AirbourneConfiguration::AIRSPEED_I2C2) { - if(!ext_i2c_.is_initialized()) + if (!ext_i2c_.is_initialized()) ext_i2c_.init(&i2c_config[EXTERNAL_I2C]); airspeed_.init(&ext_i2c_); } @@ -157,15 +153,15 @@ bool AirbourneBoard::enable_device(device_t device, hardware_config_t configurat // GNSS is currently disabled break; case Configuration::SONAR: - if(configuration == AirbourneConfiguration::SONAR_I2C2) + if (configuration == AirbourneConfiguration::SONAR_I2C2) { - if(!ext_i2c_.is_initialized()) + if (!ext_i2c_.is_initialized()) ext_i2c_.init(&i2c_config[EXTERNAL_I2C]); sonar_.init(&ext_i2c_); } break; case Configuration::BATTERY_MONITOR: - if(configuration == AirbourneConfiguration::BATTERY_MONITOR_ADC3) + if (configuration == AirbourneConfiguration::BATTERY_MONITOR_ADC3) { float voltage_multiplier = params.get_param_float(PARAM_BATTERY_VOLTAGE_MULTIPLIER); float current_multiplier = params.get_param_float(PARAM_BATTERY_CURRENT_MULTIPLIER); @@ -174,19 +170,23 @@ bool AirbourneBoard::enable_device(device_t device, hardware_config_t configurat } break; case Configuration::BAROMETER: - if(configuration == AirbourneConfiguration::BAROMETER_ONBOARD) + if (configuration == AirbourneConfiguration::BAROMETER_ONBOARD) { - while (millis() < 50) {} // wait for sensors to boot up - if(!int_i2c_.is_initialized()) + while (millis() < 50) + { + } // wait for sensors to boot up + if (!int_i2c_.is_initialized()) int_i2c_.init(&i2c_config[BARO_I2C]); baro_.init(&int_i2c_); } break; case Configuration::MAGNETOMETER: - if(configuration == AirbourneConfiguration::MAGNETOMETER_ONBOARD) + if (configuration == AirbourneConfiguration::MAGNETOMETER_ONBOARD) { - while (millis() < 50) {} // wait for sensors to boot up - if(!int_i2c_.is_initialized()) + while (millis() < 50) + { + } // wait for sensors to boot up + if (!int_i2c_.is_initialized()) int_i2c_.init(&i2c_config[BARO_I2C]); mag_.init(&int_i2c_); } @@ -205,7 +205,9 @@ AirbourneBoardConfigManager const &AirbourneBoard::get_board_config_manager() co // 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_); // Most sensors are set up through the configuration manager } @@ -249,7 +251,7 @@ bool AirbourneBoard::mag_present() void AirbourneBoard::mag_update() { - if(mag_.is_initialized()) + if (mag_.is_initialized()) mag_.update(); } @@ -266,7 +268,7 @@ bool AirbourneBoard::baro_present() void AirbourneBoard::baro_update() { - if(baro_.is_initialized()) + if (baro_.is_initialized()) baro_.update(); } @@ -278,7 +280,7 @@ void AirbourneBoard::baro_read(float *pressure, float *temperature) bool AirbourneBoard::diff_pressure_present() { - if(airspeed_.is_initialized()) + if (airspeed_.is_initialized()) return airspeed_.present(); return false; } @@ -288,11 +290,10 @@ void AirbourneBoard::diff_pressure_update() airspeed_.update(); } - 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); } @@ -304,7 +305,7 @@ bool AirbourneBoard::sonar_present() void AirbourneBoard::sonar_update() { - if(sonar_.is_initialized()) + if (sonar_.is_initialized()) sonar_.update(); } @@ -324,9 +325,9 @@ bool AirbourneBoard::gnss_has_new_data() // return this->gnss_.new_data(); return false; } -//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 +// 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 GNSSData AirbourneBoard::gnss_read() { // UBLOX::GNSSPVT gnss_pvt= gnss_.read(); @@ -367,7 +368,7 @@ GNSSData AirbourneBoard::gnss_read() } GNSSRaw AirbourneBoard::gnss_raw_read() { -// UBLOX::NAV_PVT_t pvt = gnss_.read_raw(); + // UBLOX::NAV_PVT_t pvt = gnss_.read_raw(); GNSSRaw raw = {}; // raw.time_of_week = pvt.iTOW; // raw.year = pvt.time.year; @@ -552,5 +553,4 @@ void AirbourneBoard::backup_memory_clear(size_t len) backup_sram_clear(len); } - } // namespace rosflight_firmware diff --git a/boards/airbourne/airbourne_board_config_manager.cpp b/boards/airbourne/airbourne_board_config_manager.cpp index d187e43e..84a9364a 100644 --- a/boards/airbourne/airbourne_board_config_manager.cpp +++ b/boards/airbourne/airbourne_board_config_manager.cpp @@ -1,42 +1,43 @@ #include "airbourne_board_config_manager.h" -#include - #include "rosflight.h" +#include + namespace rosflight_firmware { -constexpr hardware_config_t AirbourneBoardConfigManager::max_configs[]; // I can't wait for c++ 17 so that this is optional -AirbourneBoardConfigManager::AirbourneBoardConfigManager() -{ -} +constexpr hardware_config_t + AirbourneBoardConfigManager::max_configs[]; // I can't wait for c++ 17 so that this is optional +AirbourneBoardConfigManager::AirbourneBoardConfigManager() {} hardware_config_t AirbourneBoardConfigManager::get_max_config(device_t device) const { - if(device >= Configuration::DEVICE_COUNT) + if (device >= Configuration::DEVICE_COUNT) return 0; else return AirbourneBoardConfigManager::max_configs[device]; } -ConfigManager::ConfigResponse AirbourneBoardConfigManager::check_config_change(device_t device, hardware_config_t config, const ConfigManager &cm) const +ConfigManager::ConfigResponse AirbourneBoardConfigManager::check_config_change(device_t device, + hardware_config_t config, + const ConfigManager &cm) const { ConfigManager::ConfigResponse resp; resp.reboot_required = false; resp.successful = false; resp.message[0] = 0; // Just in case, because a string without a null terminator causes problems - if(device >= Configuration::DEVICE_COUNT) + if (device >= Configuration::DEVICE_COUNT) { strcpy(resp.message, "Device not found"); return resp; } - if(config > AirbourneBoardConfigManager::max_configs[device]) + if (config > AirbourneBoardConfigManager::max_configs[device]) { strcpy(resp.message, "Configuration not found"); return resp; } - if(config == cm[device]) + if (config == cm[device]) { strcpy(resp.message, "Configuration already set. No change required"); resp.successful = true; @@ -48,24 +49,26 @@ ConfigManager::ConfigResponse AirbourneBoardConfigManager::check_config_change(d device_t conflict_device = Configuration::DEVICE_COUNT; // While a config may conflict with multiple devices, this will only report one - switch(device) + switch (device) { case Configuration::RC: - if(config ==AirbourneConfiguration::RC_PPM) // PPM is not known to conflict with anything + if (config == AirbourneConfiguration::RC_PPM) // PPM is not known to conflict with anything break; [[gnu::fallthrough]]; // Fallthrough is intentional case Configuration::SERIAL: case Configuration::GNSS: - if(port != NO_PORT) + if (port != NO_PORT) { - for(device_t other_device{Configuration::FIRST_DEVICE}; other_device != Configuration::DEVICE_COUNT; ++other_device) + for (device_t other_device{Configuration::FIRST_DEVICE}; other_device != Configuration::DEVICE_COUNT; + ++other_device) { - if(other_device == device) + if (other_device == device) continue; - if(other_device == Configuration::RC) - if(cm[Configuration::RC] == AirbourneConfiguration::RC_PPM) // RC over PPM does not conflict with UART, even though both use the same port + if (other_device == Configuration::RC) + if (cm[Configuration::RC] == AirbourneConfiguration::RC_PPM) // RC over PPM does not conflict with UART, even + // though both use the same port continue; - if(port == get_port(other_device, cm[other_device])) + if (port == get_port(other_device, cm[other_device])) { conflict_device = other_device; break; @@ -75,17 +78,17 @@ ConfigManager::ConfigResponse AirbourneBoardConfigManager::check_config_change(d break; case Configuration::AIRSPEED: case Configuration::SONAR: - if(cm[Configuration::GNSS] == AirbourneConfiguration::GNSS_UART3) + if (cm[Configuration::GNSS] == AirbourneConfiguration::GNSS_UART3) conflict_device = Configuration::GNSS; - if(cm[Configuration::SERIAL] == AirbourneConfiguration::GNSS_UART3) + if (cm[Configuration::SERIAL] == AirbourneConfiguration::GNSS_UART3) conflict_device = Configuration::SERIAL; break; default: break; } - if(conflict_device != Configuration::DEVICE_COUNT) + if (conflict_device != Configuration::DEVICE_COUNT) { - switch(conflict_device) + switch (conflict_device) { case Configuration::SERIAL: strcpy(resp.message, "Port is used by serial."); @@ -120,12 +123,12 @@ ConfigManager::ConfigResponse AirbourneBoardConfigManager::check_config_change(d } resp.successful = true; resp.reboot_required = true; - resp.message[0]=0; // Ensuring that the message is treated as a zero-length string + resp.message[0] = 0; // Ensuring that the message is treated as a zero-length string return resp; } void AirbourneBoardConfigManager::get_device_name(device_t device, char (&name)[DEVICE_NAME_LENGTH]) const { - switch(device) + switch (device) { case Configuration::SERIAL: strcpy(name, "Serial"); @@ -156,18 +159,21 @@ void AirbourneBoardConfigManager::get_device_name(device_t device, char (&name)[ break; } } -void AirbourneBoardConfigManager::get_config_name(device_t device, hardware_config_t config, char (&name)[CONFIG_NAME_LENGTH]) const +void AirbourneBoardConfigManager::get_config_name(device_t device, + hardware_config_t config, + char (&name)[CONFIG_NAME_LENGTH]) const { name[0] = 0; // Prevents a string without a terminator being sent in case of programmer error - if(device >= Configuration::DEVICE_COUNT) // Although this is handled in the switch statement, this is so that it doesn't attempt to overflow max_configs + if (device >= Configuration::DEVICE_COUNT) // Although this is handled in the switch statement, this is so that it + // doesn't attempt to overflow max_configs strcpy(name, "Invalid device"); - if(config > AirbourneBoardConfigManager::max_configs[device]) + if (config > AirbourneBoardConfigManager::max_configs[device]) strcpy(name, "Invalid config"); else - switch(device) + switch (device) { case Configuration::SERIAL: - switch(config) + switch (config) { case AirbourneConfiguration::SERIAL_VCP: strcpy(name, "VCP over USB"); @@ -184,19 +190,19 @@ void AirbourneBoardConfigManager::get_config_name(device_t device, hardware_conf } break; case Configuration::RC: - if(config==AirbourneConfiguration::RC_PPM) + if (config == AirbourneConfiguration::RC_PPM) strcpy(name, "PPM on Flex-IO"); - else if(config==AirbourneConfiguration::RC_SBUS) + else if (config == AirbourneConfiguration::RC_SBUS) strcpy(name, "SBUS on Main"); break; case Configuration::AIRSPEED: - if(config==AirbourneConfiguration::AIRSPEED_DISABLED) + if (config == AirbourneConfiguration::AIRSPEED_DISABLED) strcpy(name, "Disabled"); - else if(config == AirbourneConfiguration::AIRSPEED_I2C2) + else if (config == AirbourneConfiguration::AIRSPEED_I2C2) strcpy(name, "I2C2 on Flexi"); break; case Configuration::GNSS: - switch(config) + switch (config) { case AirbourneConfiguration::GNSS_DISABLED: strcpy(name, "Disabled"); @@ -213,27 +219,27 @@ void AirbourneBoardConfigManager::get_config_name(device_t device, hardware_conf } break; case Configuration::SONAR: - if(config ==AirbourneConfiguration::SONAR_DISABLED) + if (config == AirbourneConfiguration::SONAR_DISABLED) strcpy(name, "Disabled"); - else if(config == AirbourneConfiguration::SONAR_I2C2) + else if (config == AirbourneConfiguration::SONAR_I2C2) strcpy(name, "I2C2 on Flexi"); break; case Configuration::BATTERY_MONITOR: - if(config==AirbourneConfiguration::BATTERY_MONITOR_DISABLED) + if (config == AirbourneConfiguration::BATTERY_MONITOR_DISABLED) strcpy(name, "Disabled"); - else if(config==AirbourneConfiguration::BATTERY_MONITOR_ADC3) + else if (config == AirbourneConfiguration::BATTERY_MONITOR_ADC3) strcpy(name, "ADC3 on Power"); break; case Configuration::BAROMETER: - if(config==AirbourneConfiguration::BAROMETER_DISABLED) + if (config == AirbourneConfiguration::BAROMETER_DISABLED) strcpy(name, "Disabled"); - else if(config == AirbourneConfiguration::BAROMETER_ONBOARD) + else if (config == AirbourneConfiguration::BAROMETER_ONBOARD) strcpy(name, "Onboard baro"); break; case Configuration::MAGNETOMETER: - if(config ==AirbourneConfiguration::MAGNETOMETER_DISABLED) + if (config == AirbourneConfiguration::MAGNETOMETER_DISABLED) strcpy(name, "Disabled"); - else if(config == AirbourneConfiguration::MAGNETOMETER_ONBOARD) + else if (config == AirbourneConfiguration::MAGNETOMETER_ONBOARD) strcpy(name, "Onboard mag"); break; default: @@ -242,10 +248,10 @@ void AirbourneBoardConfigManager::get_config_name(device_t device, hardware_conf } AirbourneBoardConfigManager::Port AirbourneBoardConfigManager::get_port(uint8_t device, uint8_t config) const { - switch(device) + switch (device) { case Configuration::SERIAL: - switch(config) + switch (config) { case AirbourneConfiguration::SERIAL_VCP: return USB_PORT; @@ -257,7 +263,7 @@ AirbourneBoardConfigManager::Port AirbourneBoardConfigManager::get_port(uint8_t return FLEXI_PORT; } case Configuration::GNSS: - switch(config) + switch (config) { case AirbourneConfiguration::GNSS_DISABLED: return NO_PORT; @@ -269,31 +275,31 @@ AirbourneBoardConfigManager::Port AirbourneBoardConfigManager::get_port(uint8_t return FLEXI_PORT; } case Configuration::RC: - if(config == AirbourneConfiguration::RC_PPM) + if (config == AirbourneConfiguration::RC_PPM) return FLEX_IO_PORT; - if(config == AirbourneConfiguration::RC_SBUS) + if (config == AirbourneConfiguration::RC_SBUS) return MAIN_PORT; break; case Configuration::AIRSPEED: - if(config==AirbourneConfiguration::AIRSPEED_I2C2) + if (config == AirbourneConfiguration::AIRSPEED_I2C2) return FLEXI_PORT; break; case Configuration::SONAR: - if(config==AirbourneConfiguration::SONAR_I2C2) + if (config == AirbourneConfiguration::SONAR_I2C2) return FLEXI_PORT; break; case Configuration::BATTERY_MONITOR: - if(config == AirbourneConfiguration::BATTERY_MONITOR_ADC3) + if (config == AirbourneConfiguration::BATTERY_MONITOR_ADC3) return POWER_PORT; break; case Configuration::MAGNETOMETER: - if(config == AirbourneConfiguration::MAGNETOMETER_ONBOARD) + if (config == AirbourneConfiguration::MAGNETOMETER_ONBOARD) return INTERNAL_I2C; break; case Configuration::BAROMETER: - if(config == AirbourneConfiguration::BAROMETER_ONBOARD) + if (config == AirbourneConfiguration::BAROMETER_ONBOARD) return INTERNAL_I2C; } return NO_PORT; } -} //rosflight_firmware +} // namespace rosflight_firmware diff --git a/boards/airbourne/main.cpp b/boards/airbourne/main.cpp index a56df3c6..74b3ef5c 100644 --- a/boards/airbourne/main.cpp +++ b/boards/airbourne/main.cpp @@ -30,22 +30,24 @@ */ #include "airbourne_board.h" -#include "rosflight.h" -#include "mavlink.h" #include "backup_sram.h" #include "board.h" +#include "mavlink.h" #include "state_manager.h" +#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) { - 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" { +extern "C" +{ /* The prototype shows it is a naked function - in effect this is just an assembly function. */ void HardFault_Handler(void) __attribute__((naked)); @@ -54,20 +56,18 @@ extern "C" { 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" - ); + __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) + 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 @@ -78,24 +78,24 @@ extern "C" { 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. */ + 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 ]; + r0 = pulFaultStackAddress[0]; + r1 = pulFaultStackAddress[1]; + r2 = pulFaultStackAddress[2]; + r3 = pulFaultStackAddress[3]; - r12 = pulFaultStackAddress[ 4 ]; - lr = pulFaultStackAddress[ 5 ]; - pc = pulFaultStackAddress[ 6 ]; - psr = pulFaultStackAddress[ 7 ]; + 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 }; + rosflight_firmware::StateManager::BackupData::DebugInfo debug = {r0, r1, r2, r3, r12, lr, pc, psr}; write_backup_data(debug); // reboot @@ -104,48 +104,55 @@ extern "C" { void NMI_Handler() { - while (1) {} + while (1) + { + } } - void MemManage_Handler() { - while (1) {} + while (1) + { + } } void BusFault_Handler() { - while (1) {} + while (1) + { + } } void UsageFault_Handler() { - while (1) {} + while (1) + { + } } void WWDG_IRQHandler() { - while (1) {} + while (1) + { + } } } // extern "C" - - int main(void) { - rosflight_firmware::AirbourneBoard board; - rosflight_firmware::Mavlink mavlink(board); - rosflight_firmware::ROSflight firmware(board, mavlink); + rosflight_firmware::AirbourneBoard board; + rosflight_firmware::Mavlink mavlink(board); + rosflight_firmware::ROSflight firmware(board, mavlink); - rosflight_ptr = &firmware; // this allows crashes to grab some info + rosflight_ptr = &firmware; // this allows crashes to grab some info - board.init_board(); - firmware.init(); + board.init_board(); + firmware.init(); - while (true) - { - firmware.run(); - } + while (true) + { + firmware.run(); + } - return 0; + return 0; } diff --git a/boards/breezy/breezy_board.cpp b/boards/breezy/breezy_board.cpp index 59148d5a..8ee3ce21 100644 --- a/boards/breezy/breezy_board.cpp +++ b/boards/breezy/breezy_board.cpp @@ -34,19 +34,16 @@ extern "C" { +#include "flash.h" #include -#include "flash.h" extern void SetSysClock(bool overclock); - } - #include "breezy_board.h" namespace rosflight_firmware { - BreezyBoard::BreezyBoard() {} void BreezyBoard::init_board() @@ -112,7 +109,7 @@ void BreezyBoard::serial_flush() bool BreezyBoard::enable_device(device_t device, hardware_config_t configuration, const Params ¶ms) { (void)configuration; - switch(device) + switch (device) { case Configuration::RC: rc_init(); @@ -139,9 +136,10 @@ void BreezyBoard::sensors_init() // Initialize I2c i2cInit(I2CDEV_2); - while (millis() < 50); + while (millis() < 50) + ; - i2cWrite(0,0,0); + i2cWrite(0, 0, 0); if (bmp280_init()) baro_type = BARO_BMP280; else if (ms5611_init()) @@ -151,11 +149,10 @@ void BreezyBoard::sensors_init() mb1242_init(); ms4525_init(); - // IMU uint16_t acc1G; mpu6050_init(true, &acc1G, &_gyro_scale, _board_revision); - _accel_scale = 9.80665f/acc1G; + _accel_scale = 9.80665f / acc1G; } uint16_t BreezyBoard::num_sensor_errors() @@ -182,13 +179,14 @@ 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) { return false; } - else return true; + else + return true; } void BreezyBoard::imu_not_responding_error() @@ -227,8 +225,6 @@ void BreezyBoard::baro_update() } } - - void BreezyBoard::baro_read(float *pressure, float *temperature) { if (baro_type == BARO_BMP280) @@ -377,7 +373,7 @@ void BreezyBoard::pwm_disable() float BreezyBoard::rc_read(uint8_t channel) { - return (float)(pwmRead(channel) - 1000)/1000.0; + return (float)(pwmRead(channel) - 1000) / 1000.0; } void BreezyBoard::pwm_write(uint8_t channel, float value) @@ -407,19 +403,19 @@ bool BreezyBoard::memory_write(const void *src, size_t len) return writeEEPROM(src, len); } -//GNSS is not supported on breezy boards +// GNSS is not supported on breezy boards GNSSData BreezyBoard::gnss_read() { return {}; } -//GNSS is not supported on breezy boards +// GNSS is not supported on breezy boards GNSSRaw BreezyBoard::gnss_raw_read() { return {}; } -//GNSS is not supported on breezy boards +// GNSS is not supported on breezy boards bool BreezyBoard::gnss_has_new_data() { return false; @@ -457,6 +453,6 @@ void BreezyBoard::led1_toggle() LED1_TOGGLE; } -} +} // namespace rosflight_firmware #pragma GCC diagnostic pop diff --git a/boards/breezy/breezy_board_config_manager.cpp b/boards/breezy/breezy_board_config_manager.cpp index 48b77c86..e47279e2 100644 --- a/boards/breezy/breezy_board_config_manager.cpp +++ b/boards/breezy/breezy_board_config_manager.cpp @@ -1,4 +1,5 @@ #include "breezy_board_config_manager.h" + #include namespace rosflight_firmware @@ -9,7 +10,9 @@ hardware_config_t BreezyBoardConfigManager::get_max_config(device_t device) cons return 0; } -ConfigManager::ConfigResponse BreezyBoardConfigManager::check_config_change(device_t device, hardware_config_t config, const ConfigManager &cm) const +ConfigManager::ConfigResponse BreezyBoardConfigManager::check_config_change(device_t device, + hardware_config_t config, + const ConfigManager &cm) const { (void)device; (void)config; @@ -17,17 +20,20 @@ ConfigManager::ConfigResponse BreezyBoardConfigManager::check_config_change(devi ConfigManager::ConfigResponse response; response.successful = false; response.reboot_required = false; - strcpy(reinterpret_cast(response.message), "Feature unsupported on naze"); + strcpy(reinterpret_cast(response.message), "Feature unsupported on naze"); return response; } -void BreezyBoardConfigManager::get_device_name(device_t device, char (&name)[BoardConfigManager::DEVICE_NAME_LENGTH]) const +void BreezyBoardConfigManager::get_device_name(device_t device, + char (&name)[BoardConfigManager::DEVICE_NAME_LENGTH]) const { (void)device; strcpy(name, "Unsupported"); } -void BreezyBoardConfigManager::get_config_name(device_t device, hardware_config_t config, char (&name)[BoardConfigManager::CONFIG_NAME_LENGTH]) const +void BreezyBoardConfigManager::get_config_name(device_t device, + hardware_config_t config, + char (&name)[BoardConfigManager::CONFIG_NAME_LENGTH]) const { (void)device; (void)config; diff --git a/boards/breezy/main.cpp b/boards/breezy/main.cpp index f9b1fd65..0f6835c7 100644 --- a/boards/breezy/main.cpp +++ b/boards/breezy/main.cpp @@ -30,9 +30,10 @@ */ #include "breezy_board.h" -#include "rosflight.h" #include "mavlink.h" +#include "rosflight.h" + int main() { rosflight_firmware::BreezyBoard board; diff --git a/comms/mavlink/mavlink.cpp b/comms/mavlink/mavlink.cpp index 8b9f0499..39de0b1c 100644 --- a/comms/mavlink/mavlink.cpp +++ b/comms/mavlink/mavlink.cpp @@ -28,18 +28,15 @@ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#include +#include "mavlink.h" #include "board.h" -#include "mavlink.h" +#include namespace rosflight_firmware { - -Mavlink::Mavlink(Board &board) : - board_(board) -{} +Mavlink::Mavlink(Board &board) : board_(board) {} void Mavlink::init() { @@ -61,14 +58,8 @@ void Mavlink::send_attitude_quaternion(uint8_t system_id, 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, + 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); } @@ -124,8 +115,8 @@ void Mavlink::send_command_ack(uint8_t system_id, Command command, bool success) } mavlink_message_t msg; - mavlink_msg_rosflight_cmd_ack_pack(system_id, compid_, &msg, - rosflight_cmd, (success) ? ROSFLIGHT_CMD_SUCCESS : ROSFLIGHT_CMD_FAILED); + mavlink_msg_rosflight_cmd_ack_pack(system_id, compid_, &msg, rosflight_cmd, + (success) ? ROSFLIGHT_CMD_SUCCESS : ROSFLIGHT_CMD_FAILED); send_message(msg); } @@ -139,47 +130,36 @@ 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, +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, + 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_raw(uint8_t system_id, const GNSSRaw& raw) +void Mavlink::send_gnss_raw(uint8_t system_id, const GNSSRaw &raw) { mavlink_message_t msg; - mavlink_rosflight_gnss_raw_t data= {}; + mavlink_rosflight_gnss_raw_t data = {}; data.time_of_week = raw.time_of_week; data.year = raw.year; data.month = raw.month; @@ -295,50 +275,57 @@ void Mavlink::send_config_value(uint8_t system_id, uint8_t device, uint8_t confi send_message(msg); } -void Mavlink::send_device_info(uint8_t system_id, uint8_t device, uint8_t max_config, char (&name)[BoardConfigManager::DEVICE_NAME_LENGTH], uint8_t num_devices) +void Mavlink::send_device_info(uint8_t system_id, + uint8_t device, + uint8_t max_config, + char (&name)[BoardConfigManager::DEVICE_NAME_LENGTH], + uint8_t num_devices) { mavlink_message_t msg; - mavlink_msg_rosflight_device_info_pack(system_id, 0, &msg, device, max_config, reinterpret_cast(name), num_devices); + mavlink_msg_rosflight_device_info_pack(system_id, 0, &msg, device, max_config, reinterpret_cast(name), + num_devices); send_message(msg); } -void Mavlink::send_config_info(uint8_t system_id, uint8_t device, uint8_t config, char (&name)[BoardConfigManager::CONFIG_NAME_LENGTH]) +void Mavlink::send_config_info(uint8_t system_id, + uint8_t device, + uint8_t config, + char (&name)[BoardConfigManager::CONFIG_NAME_LENGTH]) { mavlink_message_t msg; - mavlink_msg_rosflight_config_info_pack(system_id, 0, &msg, device, config, reinterpret_cast(name)); + mavlink_msg_rosflight_config_info_pack(system_id, 0, &msg, device, config, reinterpret_cast(name)); send_message(msg); } -void Mavlink::send_config_status(uint8_t system_id, uint8_t device, bool success, bool reboot_required, char (&error_message)[ConfigManager::CONFIG_RESPONSE_MESSAGE_LENGTH]) +void Mavlink::send_config_status(uint8_t system_id, + uint8_t device, + bool success, + bool reboot_required, + char (&error_message)[ConfigManager::CONFIG_RESPONSE_MESSAGE_LENGTH]) { mavlink_message_t msg; - mavlink_msg_rosflight_config_status_pack(system_id, 0, &msg, device, success, reboot_required, reinterpret_cast(error_message)); + mavlink_msg_rosflight_config_status_pack(system_id, 0, &msg, device, success, reboot_required, + reinterpret_cast(error_message)); 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, +void Mavlink::send_sonar(uint8_t system_id, + /* 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); } @@ -353,15 +340,8 @@ void Mavlink::send_status(uint8_t system_id, 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); } @@ -382,7 +362,7 @@ void Mavlink::send_error_data(uint8_t system_id, const StateManager::BackupData { 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, + 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); } @@ -598,7 +578,7 @@ void Mavlink::handle_msg_external_attitude(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 + // none of the information from the heartbeat is used (void)msg; if (listener_ != nullptr) @@ -611,7 +591,7 @@ void Mavlink::handle_msg_config(const mavlink_message_t *const msg) mavlink_msg_rosflight_config_decode(msg, &config_msg); uint8_t device = config_msg.device; uint8_t config = config_msg.config; - if(listener_ != nullptr) + if (listener_ != nullptr) listener_->config_set_callback(device, config); } @@ -620,7 +600,7 @@ void Mavlink::handle_msg_config_request(const mavlink_message_t *const msg) mavlink_rosflight_config_request_t request_msg; mavlink_msg_rosflight_config_request_decode(msg, &request_msg); uint8_t device = request_msg.device; - if(listener_ != nullptr) + if (listener_ != nullptr) listener_->config_request_callback(device); } diff --git a/comms/mavlink/mavlink.h b/comms/mavlink/mavlink.h index 434c0401..32a8dbed 100644 --- a/comms/mavlink/mavlink.h +++ b/comms/mavlink/mavlink.h @@ -40,20 +40,20 @@ #pragma GCC diagnostic ignored "-Wpragmas" #pragma GCC diagnostic ignored "-Waddress-of-packed-member" #include "v1.0/rosflight/mavlink.h" -# pragma GCC diagnostic pop +#pragma GCC diagnostic pop #include "interface/comm_link.h" + #include "board.h" namespace rosflight_firmware { - class Board; class Mavlink : public CommLinkInterface { public: - Mavlink(Board& board); + Mavlink(Board &board); void init() override; void receive() override; @@ -65,14 +65,15 @@ class Mavlink : public CommLinkInterface 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_heartbeat(uint8_t system_id, bool fixed_wing) override; - void send_imu(uint8_t system_id, uint64_t timestamp_us, + 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_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_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, @@ -85,11 +86,26 @@ class Mavlink : public CommLinkInterface float value, uint16_t param_count) override; void send_config_value(uint8_t system_id, uint8_t device, uint8_t config) override; - void send_device_info(uint8_t system_id, uint8_t device, uint8_t max_config, char (&name)[BoardConfigManager::DEVICE_NAME_LENGTH], uint8_t num_devices) override; - void send_config_info(uint8_t system_id, uint8_t device, uint8_t config, char (&name)[BoardConfigManager::CONFIG_NAME_LENGTH]) override; - void send_config_status(uint8_t system_id, uint8_t device, bool success, bool reboot_required, char (&error_message)[ConfigManager::CONFIG_RESPONSE_MESSAGE_LENGTH]) override; + void send_device_info(uint8_t system_id, + uint8_t device, + uint8_t max_config, + char (&name)[BoardConfigManager::DEVICE_NAME_LENGTH], + uint8_t num_devices) override; + void send_config_info(uint8_t system_id, + uint8_t device, + uint8_t config, + char (&name)[BoardConfigManager::CONFIG_NAME_LENGTH]) override; + void send_config_status(uint8_t system_id, + uint8_t device, + bool success, + bool reboot_required, + char (&error_message)[ConfigManager::CONFIG_RESPONSE_MESSAGE_LENGTH]) 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, float min_range) override; + void send_sonar(uint8_t system_id, + /* 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, @@ -100,13 +116,13 @@ class Mavlink : public CommLinkInterface 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_raw(uint8_t system_id, const GNSSRaw& data) 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_raw(uint8_t system_id, const GNSSRaw &data) 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); @@ -119,19 +135,19 @@ class Mavlink : public CommLinkInterface 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_heartbeat(const mavlink_message_t *const msg); void handle_msg_config(const mavlink_message_t *const msg); void handle_msg_config_request(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/interface/comm_link.h b/include/interface/comm_link.h index fa4103a3..8d2a0069 100644 --- a/include/interface/comm_link.h +++ b/include/interface/comm_link.h @@ -32,22 +32,20 @@ #ifndef ROSFLIGHT_FIRMWARE_COMM_LINK_H #define ROSFLIGHT_FIRMWARE_COMM_LINK_H -#include - -#include - -#include "param.h" #include "board.h" +#include "param.h" #include "sensors.h" #include "state_manager.h" +#include + +#include + namespace rosflight_firmware { - class CommLinkInterface { public: - enum class LogSeverity { LOG_INFO, @@ -112,80 +110,103 @@ class CommLinkInterface AuxChannel cmd_array[14]; }; - class ListenerInterface - { - 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, 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 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 heartbeat_callback() = 0; - virtual void config_set_callback(uint8_t device, uint8_t configuration) = 0; - virtual void config_request_callback(uint8_t device) = 0; - }; + class ListenerInterface + { + 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, + 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 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 heartbeat_callback() = 0; + virtual void config_set_callback(uint8_t device, uint8_t configuration) = 0; + virtual void config_request_callback(uint8_t device) = 0; + }; - virtual void init() = 0; - virtual void receive() = 0; - - // 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_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_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, + virtual void init() = 0; + virtual void receive() = 0; + + // 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_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_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, - int32_t value, + float 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_config_value(uint8_t system_id, uint8_t device, uint8_t config) = 0; - virtual void send_device_info(uint8_t system_id, uint8_t device, uint8_t max_config, char (&name)[BoardConfigManager::DEVICE_NAME_LENGTH], uint8_t num_devices) = 0; - virtual void send_config_info(uint8_t system_id, uint8_t device, uint8_t config, char (&name)[BoardConfigManager::CONFIG_NAME_LENGTH]) = 0; - virtual void send_config_status(uint8_t system_id, uint8_t device, bool success, bool reboot_required, char (&error_message)[ConfigManager::CONFIG_RESPONSE_MESSAGE_LENGTH]) = 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, 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_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_raw(uint8_t system_id, const GNSSRaw &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 send_config_value(uint8_t system_id, uint8_t device, uint8_t config) = 0; + virtual void send_device_info(uint8_t system_id, + uint8_t device, + uint8_t max_config, + char (&name)[BoardConfigManager::DEVICE_NAME_LENGTH], + uint8_t num_devices) = 0; + virtual void send_config_info(uint8_t system_id, + uint8_t device, + uint8_t config, + char (&name)[BoardConfigManager::CONFIG_NAME_LENGTH]) = 0; + virtual void send_config_status(uint8_t system_id, + uint8_t device, + bool success, + bool reboot_required, + char (&error_message)[ConfigManager::CONFIG_RESPONSE_MESSAGE_LENGTH]) = 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, + 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_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_raw(uint8_t system_id, const GNSSRaw &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; }; } // namespace rosflight_firmware diff --git a/include/interface/param_listener.h b/include/interface/param_listener.h index 7b8460a5..ffa5cb40 100644 --- a/include/interface/param_listener.h +++ b/include/interface/param_listener.h @@ -36,7 +36,6 @@ namespace rosflight_firmware { - class ParamListenerInterface { public: diff --git a/test/command_manager_test.cpp b/test/command_manager_test.cpp index ff68a0ab..14214c15 100644 --- a/test/command_manager_test.cpp +++ b/test/command_manager_test.cpp @@ -1,8 +1,9 @@ +#include "cmath" #include "common.h" -#include "rosflight.h" -#include "test_board.h" #include "mavlink.h" -#include "cmath" +#include "test_board.h" + +#include "rosflight.h" #define CHN_LOW 1100 #define CHN_HIGH 1900 @@ -13,7 +14,7 @@ #define OFFBOARD_F 0.9 #define RC_X_PWM 1800 -#define RC_X ((RC_X_PWM - 1500)/500.0 * rf.params_.get_param_float(PARAM_RC_MAX_ROLL)) +#define RC_X ((RC_X_PWM - 1500) / 500.0 * rf.params_.get_param_float(PARAM_RC_MAX_ROLL)) using namespace rosflight_firmware; @@ -27,19 +28,13 @@ class CommandManagerTest : public ::testing::Test uint16_t rc_values[8]; float max_roll, max_pitch, max_yawrate; - control_t offboard_command = - { - 20000, - {true, ANGLE, OFFBOARD_X}, - {true, ANGLE, OFFBOARD_Y}, - {true, RATE, OFFBOARD_Z}, - {true, THROTTLE, OFFBOARD_F} - }; - - CommandManagerTest() : - mavlink(board), - rf(board,mavlink) - {} + control_t offboard_command = {20000, + {true, ANGLE, OFFBOARD_X}, + {true, ANGLE, OFFBOARD_Y}, + {true, RATE, OFFBOARD_Z}, + {true, THROTTLE, OFFBOARD_F}}; + + CommandManagerTest() : mavlink(board), rf(board, mavlink) {} void SetUp() override { @@ -78,7 +73,7 @@ class CommandManagerTest : public ::testing::Test } }; -TEST_F (CommandManagerTest, Default) +TEST_F(CommandManagerTest, Default) { board.set_rc(rc_values); stepFirmware(20000); @@ -94,7 +89,7 @@ TEST_F (CommandManagerTest, Default) EXPECT_CLOSE(output.F.value, 0.0); } -TEST_F (CommandManagerTest, RCCommands) +TEST_F(CommandManagerTest, RCCommands) { rc_values[0] = 2000; rc_values[1] = 1000; @@ -105,16 +100,16 @@ TEST_F (CommandManagerTest, RCCommands) control_t output = rf.command_manager_.combined_control(); EXPECT_EQ(output.x.type, ANGLE); - EXPECT_CLOSE(output.x.value, 1.0*max_roll); + EXPECT_CLOSE(output.x.value, 1.0 * max_roll); EXPECT_EQ(output.y.type, ANGLE); - EXPECT_CLOSE(output.y.value, -1.0*max_pitch); + EXPECT_CLOSE(output.y.value, -1.0 * max_pitch); EXPECT_EQ(output.z.type, RATE); - EXPECT_CLOSE(output.z.value, -0.5*max_yawrate); + EXPECT_CLOSE(output.z.value, -0.5 * max_yawrate); EXPECT_EQ(output.F.type, THROTTLE); EXPECT_CLOSE(output.F.value, 0.5); } -TEST_F (CommandManagerTest, ArmWithSticksByDefault) +TEST_F(CommandManagerTest, ArmWithSticksByDefault) { EXPECT_EQ(rf.state_manager_.state().armed, false); rc_values[2] = 1000; @@ -126,7 +121,7 @@ TEST_F (CommandManagerTest, ArmWithSticksByDefault) EXPECT_EQ(rf.state_manager_.state().armed, true); } -TEST_F (CommandManagerTest, DontArmWithSticksWhenUsingSwitch) +TEST_F(CommandManagerTest, DontArmWithSticksWhenUsingSwitch) { rf.params_.set_param_int(PARAM_RC_ARM_CHANNEL, 4); rc_values[2] = 1000; // throttle low @@ -136,7 +131,7 @@ TEST_F (CommandManagerTest, DontArmWithSticksWhenUsingSwitch) EXPECT_EQ(rf.state_manager_.state().armed, false); } -TEST_F (CommandManagerTest, DisarmWithSticksByDefault) +TEST_F(CommandManagerTest, DisarmWithSticksByDefault) { rf.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM); EXPECT_EQ(rf.state_manager_.state().armed, true); @@ -147,17 +142,17 @@ TEST_F (CommandManagerTest, DisarmWithSticksByDefault) EXPECT_EQ(rf.state_manager_.state().armed, false); } -TEST_F (CommandManagerTest, ArmWithSwitch) +TEST_F(CommandManagerTest, ArmWithSwitch) { rf.params_.set_param_int(PARAM_RC_ARM_CHANNEL, 4); - rc_values[2] = 1000; // throttle low + rc_values[2] = 1000; // throttle low rc_values[4] = CHN_HIGH; // switch on board.set_rc(rc_values); stepFirmware(50000); // Immediate EXPECT_EQ(rf.state_manager_.state().armed, true); } -TEST_F (CommandManagerTest, DisarmWithStick) +TEST_F(CommandManagerTest, DisarmWithStick) { rf.params_.set_param_int(PARAM_RC_ARM_CHANNEL, 4); rf.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM); @@ -167,11 +162,11 @@ TEST_F (CommandManagerTest, DisarmWithStick) EXPECT_EQ(rf.state_manager_.state().armed, false); } -TEST_F (CommandManagerTest, DontDisarmWithSticksWhenUsingSwitch) +TEST_F(CommandManagerTest, DontDisarmWithSticksWhenUsingSwitch) { rf.params_.set_param_int(PARAM_RC_ARM_CHANNEL, 4); rc_values[4] = CHN_HIGH; // switch on - rc_values[2] = 1000; // throttle low + rc_values[2] = 1000; // throttle low rf.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM); board.set_rc(rc_values); stepFirmware(50000); @@ -183,32 +178,32 @@ TEST_F (CommandManagerTest, DontDisarmWithSticksWhenUsingSwitch) EXPECT_EQ(rf.state_manager_.state().armed, true); // don't disarm } -TEST_F (CommandManagerTest, ArmStickReversed) +TEST_F(CommandManagerTest, ArmStickReversed) { rf.params_.set_param_int(PARAM_RC_SWITCH_5_DIRECTION, -1); rf.params_.set_param_int(PARAM_RC_ARM_CHANNEL, 4); - rc_values[2] = 1000; // throttle low + rc_values[2] = 1000; // throttle low rc_values[4] = CHN_LOW; // switch on board.set_rc(rc_values); stepFirmware(50000); // Immediate EXPECT_EQ(rf.state_manager_.state().armed, true); } -TEST_F (CommandManagerTest, DisarmStickReversed) +TEST_F(CommandManagerTest, DisarmStickReversed) { rf.params_.set_param_int(PARAM_RC_SWITCH_5_DIRECTION, -1); rf.params_.set_param_int(PARAM_RC_ARM_CHANNEL, 4); rf.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM); EXPECT_EQ(rf.state_manager_.state().armed, true); - rc_values[2] = 1000; // throttle low + rc_values[2] = 1000; // throttle low rc_values[4] = CHN_HIGH; // switch on board.set_rc(rc_values); stepFirmware(50000); // Immediate EXPECT_EQ(rf.state_manager_.state().armed, false); } -TEST_F (CommandManagerTest, DefaultRCOutputd) +TEST_F(CommandManagerTest, DefaultRCOutputd) { board.set_rc(rc_values); stepFirmware(600000); @@ -225,8 +220,7 @@ TEST_F (CommandManagerTest, DefaultRCOutputd) EXPECT_CLOSE(output.F.value, 0.0); } - -TEST_F (CommandManagerTest, RCOutput) +TEST_F(CommandManagerTest, RCOutput) { rc_values[0] = 1250; rc_values[1] = 1750; @@ -248,18 +242,18 @@ TEST_F (CommandManagerTest, RCOutput) EXPECT_CLOSE(output.F.value, 0.5); } -TEST_F (CommandManagerTest, LoseRCDisarmed) +TEST_F(CommandManagerTest, LoseRCDisarmed) { board.set_pwm_lost(true); stepFirmware(20000); control_t output = rf.command_manager_.combined_control(); EXPECT_EQ(output.x.type, ANGLE); - EXPECT_CLOSE(output.x.value, 0.0*max_roll); + EXPECT_CLOSE(output.x.value, 0.0 * max_roll); EXPECT_EQ(output.y.type, ANGLE); - EXPECT_CLOSE(output.y.value, 0.0*max_pitch); + EXPECT_CLOSE(output.y.value, 0.0 * max_pitch); EXPECT_EQ(output.z.type, RATE); - EXPECT_CLOSE(output.z.value, 0.0*max_yawrate); + EXPECT_CLOSE(output.z.value, 0.0 * max_yawrate); EXPECT_EQ(output.F.type, THROTTLE); EXPECT_CLOSE(output.F.value, 0.0); @@ -270,7 +264,7 @@ TEST_F (CommandManagerTest, LoseRCDisarmed) EXPECT_EQ(rf.state_manager_.state().error_codes, StateManager::ERROR_RC_LOST); } -TEST_F (CommandManagerTest, RegainRCDisarmed) +TEST_F(CommandManagerTest, RegainRCDisarmed) { board.set_pwm_lost(true); stepFirmware(40000); @@ -281,7 +275,7 @@ TEST_F (CommandManagerTest, RegainRCDisarmed) EXPECT_EQ(rf.state_manager_.state().error_codes, StateManager::ERROR_NONE); } -TEST_F (CommandManagerTest, LoseRCArmed) +TEST_F(CommandManagerTest, LoseRCArmed) { board.set_rc(rc_values); stepFirmware(50000); @@ -293,11 +287,11 @@ TEST_F (CommandManagerTest, LoseRCArmed) control_t output = rf.command_manager_.combined_control(); EXPECT_EQ(output.x.type, ANGLE); - EXPECT_CLOSE(output.x.value, 0.0*max_roll); + EXPECT_CLOSE(output.x.value, 0.0 * max_roll); EXPECT_EQ(output.y.type, ANGLE); - EXPECT_CLOSE(output.y.value, 0.0*max_pitch); + EXPECT_CLOSE(output.y.value, 0.0 * max_pitch); EXPECT_EQ(output.z.type, RATE); - EXPECT_CLOSE(output.z.value, 0.0*max_yawrate); + EXPECT_CLOSE(output.z.value, 0.0 * max_yawrate); EXPECT_EQ(output.F.type, THROTTLE); EXPECT_CLOSE(output.F.value, rf.params_.get_param_float(PARAM_FAILSAFE_THROTTLE)); @@ -308,7 +302,7 @@ TEST_F (CommandManagerTest, LoseRCArmed) EXPECT_EQ(rf.state_manager_.state().error_codes, StateManager::ERROR_RC_LOST); } -TEST_F (CommandManagerTest, RegainRCArmed) +TEST_F(CommandManagerTest, RegainRCArmed) { board.set_rc(rc_values); stepFirmware(50000); @@ -324,7 +318,7 @@ TEST_F (CommandManagerTest, RegainRCArmed) EXPECT_EQ(rf.state_manager_.state().error_codes, StateManager::ERROR_NONE); } -TEST_F (CommandManagerTest, OffboardCommandMuxNoMinThrottle) +TEST_F(CommandManagerTest, OffboardCommandMuxNoMinThrottle) { stepFirmware(1100000); // Get past LAG_TIME rf.params_.set_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE, false); @@ -341,7 +335,7 @@ TEST_F (CommandManagerTest, OffboardCommandMuxNoMinThrottle) EXPECT_CLOSE(output.F.value, OFFBOARD_F); } -TEST_F (CommandManagerTest, OffboardCommandMuxMinThrottle) +TEST_F(CommandManagerTest, OffboardCommandMuxMinThrottle) { stepFirmware(1100000); // Get past LAG_TIME rf.params_.set_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE, true); @@ -358,7 +352,7 @@ TEST_F (CommandManagerTest, OffboardCommandMuxMinThrottle) EXPECT_CLOSE(output.F.value, 0.0); } -TEST_F (CommandManagerTest, OffboardCommandMuxRollDeviation) +TEST_F(CommandManagerTest, OffboardCommandMuxRollDeviation) { stepFirmware(1100000); // Get past LAG_TIME rf.params_.set_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE, true); @@ -374,7 +368,7 @@ TEST_F (CommandManagerTest, OffboardCommandMuxRollDeviation) EXPECT_CLOSE(output.F.value, 0.0); } -TEST_F (CommandManagerTest, OffboardCommandMuxPitchDeviation) +TEST_F(CommandManagerTest, OffboardCommandMuxPitchDeviation) { stepFirmware(1100000); // Get past LAG_TIME rf.params_.set_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE, true); @@ -390,7 +384,7 @@ TEST_F (CommandManagerTest, OffboardCommandMuxPitchDeviation) EXPECT_CLOSE(output.F.value, 0.0); } -TEST_F (CommandManagerTest, OffboardCommandMuxYawrateDeviation) +TEST_F(CommandManagerTest, OffboardCommandMuxYawrateDeviation) { stepFirmware(1100000); // Get past LAG_TIME rf.params_.set_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE, true); @@ -406,7 +400,7 @@ TEST_F (CommandManagerTest, OffboardCommandMuxYawrateDeviation) EXPECT_CLOSE(output.F.value, 0.0); } -TEST_F (CommandManagerTest, OffboardCommandMuxLag) +TEST_F(CommandManagerTest, OffboardCommandMuxLag) { stepFirmware(1100000); // Get past LAG_TIME rf.params_.set_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE, true); @@ -423,34 +417,34 @@ TEST_F (CommandManagerTest, OffboardCommandMuxLag) stepFirmware(500000); setOffboard(offboard_command); - output=rf.command_manager_.combined_control(); + output = rf.command_manager_.combined_control(); EXPECT_CLOSE(output.x.value, 0.0); // lag stepFirmware(600000); setOffboard(offboard_command); - output=rf.command_manager_.combined_control(); + output = rf.command_manager_.combined_control(); EXPECT_CLOSE(output.x.value, 0.0); // lag setOffboard(offboard_command); stepFirmware(20000); - output=rf.command_manager_.combined_control(); + output = rf.command_manager_.combined_control(); EXPECT_CLOSE(output.x.value, OFFBOARD_X); } -TEST_F (CommandManagerTest, StaleOffboardCommand) +TEST_F(CommandManagerTest, StaleOffboardCommand) { stepFirmware(1100000); // Get past LAG_TIME rf.params_.set_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE, true); setOffboard(offboard_command); - int timeout_us = rf.params_.get_param_int(PARAM_OFFBOARD_TIMEOUT)*1000; + int timeout_us = rf.params_.get_param_int(PARAM_OFFBOARD_TIMEOUT) * 1000; stepFirmware(timeout_us + 40000); control_t output = rf.command_manager_.combined_control(); EXPECT_CLOSE(output.x.value, 0.0); } -TEST_F (CommandManagerTest, PartialMux) +TEST_F(CommandManagerTest, PartialMux) { offboard_command.x.active = false; stepFirmware(1000000); @@ -464,7 +458,7 @@ TEST_F (CommandManagerTest, PartialMux) EXPECT_CLOSE(output.F.value, 0.0); } -TEST_F (CommandManagerTest, MixedTypes) +TEST_F(CommandManagerTest, MixedTypes) { offboard_command.x.type = RATE; stepFirmware(1000000); diff --git a/test/common.cpp b/test/common.cpp index 2c02283c..5f5de157 100644 --- a/test/common.cpp +++ b/test/common.cpp @@ -8,7 +8,7 @@ double quaternion_error(Eigen::Quaternionf q_eig, turbomath::Quaternion q) return 0; else { - Eigen::Vector3f v_tilde = atan2(q_tilde.vec().norm(), q_tilde.w())*q_tilde.vec()/q_tilde.vec().norm(); + Eigen::Vector3f v_tilde = atan2(q_tilde.vec().norm(), q_tilde.w()) * q_tilde.vec() / q_tilde.vec().norm(); return v_tilde.norm(); } } @@ -22,7 +22,7 @@ double quaternion_error(turbomath::Quaternion q0, turbomath::Quaternion q) return 0; else { - Eigen::Vector3f v_tilde = atan2(q_tilde.vec().norm(), q_tilde.w())*q_tilde.vec()/q_tilde.vec().norm(); + Eigen::Vector3f v_tilde = atan2(q_tilde.vec().norm(), q_tilde.w()) * q_tilde.vec() / q_tilde.vec().norm(); return v_tilde.norm(); } } diff --git a/test/config_manager_test.cpp b/test/config_manager_test.cpp index bd19137f..5e81386f 100644 --- a/test/config_manager_test.cpp +++ b/test/config_manager_test.cpp @@ -1,11 +1,14 @@ -#include -#include +#include "config_manager.h" -#include "rosflight.h" +#include "configuration_enum.h" #include "mavlink.h" #include "test_board.h" -#include "configuration_enum.h" -#include "config_manager.h" + +#include "rosflight.h" + +#include + +#include using namespace rosflight_firmware; @@ -16,18 +19,13 @@ class ConfigManagerTest : public ::testing::Test Mavlink mavlink; ROSflight rf; - ConfigManagerTest() : - mavlink(board), - rf(board, mavlink) {} - void SetUp() override - { - rf.init(); - } + ConfigManagerTest() : mavlink(board), rf(board, mavlink) {} + void SetUp() override { rf.init(); } }; TEST_F(ConfigManagerTest, DefaultValues) { - for(device_t device{Configuration::FIRST_DEVICE}; device < Configuration::DEVICE_COUNT; ++device) + for (device_t device{Configuration::FIRST_DEVICE}; device < Configuration::DEVICE_COUNT; ++device) EXPECT_EQ(rf.config_manager_[device], 0); } @@ -39,8 +37,8 @@ TEST_F(ConfigManagerTest, SetValid) EXPECT_TRUE(response.successful); EXPECT_TRUE(response.reboot_required); EXPECT_EQ(std::string(reinterpret_cast(response.message)), "Succeed for testing"); - for(device_t device{Configuration::FIRST_DEVICE}; device < Configuration::DEVICE_COUNT; ++device) - if(device == changed_device) + for (device_t device{Configuration::FIRST_DEVICE}; device < Configuration::DEVICE_COUNT; ++device) + if (device == changed_device) EXPECT_EQ(rf.config_manager_[device], config); else EXPECT_EQ(rf.config_manager_[device], 0); @@ -54,6 +52,6 @@ TEST_F(ConfigManagerTest, SetInvalid) EXPECT_FALSE(response.successful); EXPECT_FALSE(response.reboot_required); EXPECT_EQ(std::string(reinterpret_cast(response.message)), "Fail for testing"); - for(device_t device{Configuration::FIRST_DEVICE}; device < Configuration::DEVICE_COUNT; ++device) + for (device_t device{Configuration::FIRST_DEVICE}; device < Configuration::DEVICE_COUNT; ++device) EXPECT_EQ(rf.config_manager_[device], 0); } diff --git a/test/estimator_test.cpp b/test/estimator_test.cpp index 0a510c2f..a91e78b7 100644 --- a/test/estimator_test.cpp +++ b/test/estimator_test.cpp @@ -1,9 +1,11 @@ #include "common.h" +#include "eigen3/unsupported/Eigen/MatrixFunctions" #include "math.h" -#include "rosflight.h" #include "mavlink.h" #include "test_board.h" -#include "eigen3/unsupported/Eigen/MatrixFunctions" + +#include "rosflight.h" + #include #include @@ -35,10 +37,7 @@ 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 { @@ -70,10 +69,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() { @@ -86,24 +82,24 @@ class EstimatorTest : public ::testing::Test 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_); - w[2] = z_amp_*sin(z_freq_/(2.0*M_PI)*t_); - integrate(q_, w, dt_/double(oversampling_factor_)); - t_ += dt_/double(oversampling_factor_); + w[0] = x_amp_ * sin(x_freq_ / (2.0 * M_PI) * t_); + w[1] = y_amp_ * sin(y_freq_ / (2.0 * M_PI) * t_); + w[2] = z_amp_ * sin(z_freq_ / (2.0 * M_PI) * t_); + integrate(q_, w, dt_ / double(oversampling_factor_)); + t_ += dt_ / double(oversampling_factor_); } simulateIMU(acc, gyro); extAttUpdate(); - board.set_imu(acc, gyro, t_*1e6); - board.set_time(t_*1e6); + board.set_imu(acc, gyro, t_ * 1e6); + board.set_time(t_ * 1e6); rf.run(); 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); + err_norm = std::abs(err_norm - 2.0 * M_PI); } max_error = (err_norm > max_error) ? err_norm : max_error; } @@ -112,14 +108,14 @@ class EstimatorTest : public ::testing::Test void integrate(Quaterniond& q, const Vector3d& _w, double dt) { - Vector3d w = _w*dt; + Vector3d w = _w * dt; Quaterniond w_exp; double w_norm = w.norm(); 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.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); @@ -128,9 +124,9 @@ class EstimatorTest : public ::testing::Test else { w_exp.w() = 1.0; - w_exp.x() = w(0)/2.0; - w_exp.y() = w(1)/2.0; - w_exp.z() = w(2)/2.0; + w_exp.x() = w(0) / 2.0; + w_exp.y() = w(1) / 2.0; + w_exp.z() = w(2) / 2.0; w_exp.normalize(); } q = q * w_exp; @@ -139,15 +135,15 @@ class EstimatorTest : public ::testing::Test void simulateIMU(float* acc, float* gyro) { - Vector3d y_acc = q_.inverse() * gravity; + Vector3d y_acc = q_.inverse() * gravity; acc[0] = y_acc.x(); acc[1] = y_acc.y(); acc[2] = y_acc.z(); // Create gyro measurement - gyro[0] = x_amp_*sin(x_freq_/(2.0*M_PI)*t_) + x_gyro_bias_; - gyro[1] = y_amp_*sin(y_freq_/(2.0*M_PI)*t_) + y_gyro_bias_; - gyro[2] = z_amp_*sin(z_freq_/(2.0*M_PI)*t_) + z_gyro_bias_; + gyro[0] = x_amp_ * sin(x_freq_ / (2.0 * M_PI) * t_) + x_gyro_bias_; + gyro[1] = y_amp_ * sin(y_freq_ / (2.0 * M_PI) * t_) + y_gyro_bias_; + gyro[2] = z_amp_ * sin(z_freq_ / (2.0 * M_PI) * t_) + z_gyro_bias_; } void extAttUpdate() @@ -165,17 +161,12 @@ class EstimatorTest : public ::testing::Test } } - double sign(double x) - { - return x < 0 ? -1 : 1; - } + double sign(double x) { return x < 0 ? -1 : 1; } Vector3d computeError() { - Quaterniond rf_quat(rf.estimator_.state().attitude.w, - rf.estimator_.state().attitude.x, - rf.estimator_.state().attitude.y, - rf.estimator_.state().attitude.z); + Quaterniond rf_quat(rf.estimator_.state().attitude.w, rf.estimator_.state().attitude.x, + rf.estimator_.state().attitude.y, rf.estimator_.state().attitude.z); Quaterniond err = q_ * rf_quat.inverse(); Vector3d v(err.x(), err.y(), err.z()); @@ -191,7 +182,7 @@ class EstimatorTest : public ::testing::Test Vector3d log_err; if (norm_v > 1e-4) { - log_err = 2.0 * std::atan(norm_v/w) * v / norm_v; + log_err = 2.0 * std::atan(norm_v / w) * v / norm_v; } else { @@ -201,11 +192,11 @@ class EstimatorTest : public ::testing::Test 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); + 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; @@ -228,20 +219,17 @@ class EstimatorTest : public ::testing::Test double xerr = x_gyro_bias_ - rf.estimator_.bias().x; double yerr = y_gyro_bias_ - rf.estimator_.bias().y; double zerr = z_gyro_bias_ - rf.estimator_.bias().z; - return std::sqrt(xerr*xerr + yerr*yerr + zerr*zerr); + return std::sqrt(xerr * xerr + yerr * yerr + zerr * zerr); } 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(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(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())); return rpy; } - }; TEST_F(EstimatorTest, LinearGyro) @@ -435,7 +423,6 @@ TEST_F(EstimatorTest, EstimateBiasAccel) #endif } - TEST_F(EstimatorTest, StaticExtAtt) { rf.params_.set_param_int(PARAM_FILTER_USE_ACC, false); @@ -486,48 +473,47 @@ TEST_F(EstimatorTest, StaticExtAtt) // This test is fixed by #357 TEST_F(EstimatorTest, MovingExtAtt) { - rf.params_.set_param_int(PARAM_FILTER_USE_ACC, false); - rf.params_.set_param_int(PARAM_FILTER_USE_QUAD_INT, true); - rf.params_.set_param_int(PARAM_FILTER_USE_MAT_EXP, true); - rf.params_.set_param_int(PARAM_ACC_ALPHA, 0); - rf.params_.set_param_int(PARAM_GYRO_XY_ALPHA, 0); - rf.params_.set_param_int(PARAM_GYRO_Z_ALPHA, 0); - rf.params_.set_param_int(PARAM_INIT_TIME, 0.0f); - - turbomath::Quaternion q_tweaked; - q_tweaked.from_RPY(0.2, 0.1, 0.0); - q_.w() = q_tweaked.w; - q_.x() = q_tweaked.x; - q_.y() = q_tweaked.y; - q_.z() = q_tweaked.z; - - x_freq_ = 2.0; - y_freq_ = 3.0; - z_freq_ = 0.5; - x_amp_ = 0.1; - y_amp_ = 0.2; - z_amp_ = -0.1; - - - tmax_ = 150.0; - x_gyro_bias_ = 0.01; - y_gyro_bias_ = -0.03; - z_gyro_bias_ = 0.01; - - oversampling_factor_ = 1; - - ext_att_update_rate_ = 3; + rf.params_.set_param_int(PARAM_FILTER_USE_ACC, false); + rf.params_.set_param_int(PARAM_FILTER_USE_QUAD_INT, true); + rf.params_.set_param_int(PARAM_FILTER_USE_MAT_EXP, true); + rf.params_.set_param_int(PARAM_ACC_ALPHA, 0); + rf.params_.set_param_int(PARAM_GYRO_XY_ALPHA, 0); + rf.params_.set_param_int(PARAM_GYRO_Z_ALPHA, 0); + rf.params_.set_param_int(PARAM_INIT_TIME, 0.0f); + + turbomath::Quaternion q_tweaked; + q_tweaked.from_RPY(0.2, 0.1, 0.0); + q_.w() = q_tweaked.w; + q_.x() = q_tweaked.x; + q_.y() = q_tweaked.y; + q_.z() = q_tweaked.z; + + x_freq_ = 2.0; + y_freq_ = 3.0; + z_freq_ = 0.5; + x_amp_ = 0.1; + y_amp_ = 0.2; + z_amp_ = -0.1; + + tmax_ = 150.0; + x_gyro_bias_ = 0.01; + y_gyro_bias_ = -0.03; + z_gyro_bias_ = 0.01; + + oversampling_factor_ = 1; + + ext_att_update_rate_ = 3; #ifdef DEBUG - initFile("movingExtAtt.bin"); + initFile("movingExtAtt.bin"); #endif - run(); + run(); - double error = computeError().norm(); - EXPECT_LE(error, 4e-3); - EXPECT_LE(biasError(), 2e-2); + double error = computeError().norm(); + EXPECT_LE(error, 4e-3); + EXPECT_LE(biasError(), 2e-2); #ifdef DEBUG - std::cout << "stateError = " << error << std::endl; - std::cout << "biasError = " << biasError() << std::endl; + std::cout << "stateError = " << error << std::endl; + std::cout << "biasError = " << biasError() << std::endl; #endif } diff --git a/test/parameters_test.cpp b/test/parameters_test.cpp index 7a9ac617..c68eb437 100644 --- a/test/parameters_test.cpp +++ b/test/parameters_test.cpp @@ -1,8 +1,10 @@ -#include -#include "test_board.h" #include "mavlink.h" +#include "test_board.h" + #include "rosflight.h" +#include + using namespace rosflight_firmware; #define EXPECT_PARAM_EQ_INT(id, value) EXPECT_EQ(value, rf.params_.get_param_int(id)) diff --git a/test/state_machine_test.cpp b/test/state_machine_test.cpp index f5b2b450..c0aab452 100644 --- a/test/state_machine_test.cpp +++ b/test/state_machine_test.cpp @@ -1,9 +1,9 @@ #include "common.h" - -#include "rosflight.h" #include "mavlink.h" -#include "test_board.h" #include "state_manager.h" +#include "test_board.h" + +#include "rosflight.h" using namespace rosflight_firmware; @@ -14,10 +14,7 @@ class StateMachineTest : public ::testing::Test Mavlink mavlink; ROSflight rf; - StateMachineTest() : - mavlink(board), - rf(board,mavlink) - {} + StateMachineTest() : mavlink(board), rf(board, mavlink) {} void SetUp() override { @@ -40,7 +37,6 @@ class StateMachineTest : public ::testing::Test rf.run(); } } - }; TEST_F(StateMachineTest, Init) @@ -75,9 +71,8 @@ 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; + uint32_t error = StateManager::ERROR_IMU_NOT_RESPONDING | StateManager::ERROR_TIME_GOING_BACKWARDS + | 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); @@ -87,9 +82,8 @@ 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; + uint32_t error = StateManager::ERROR_IMU_NOT_RESPONDING | StateManager::ERROR_TIME_GOING_BACKWARDS + | 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); @@ -100,26 +94,23 @@ 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; + uint32_t error = StateManager::ERROR_IMU_NOT_RESPONDING | StateManager::ERROR_TIME_GOING_BACKWARDS + | 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); EXPECT_EQ(rf.state_manager_.state().failsafe, false); - EXPECT_EQ(rf.state_manager_.state().error_codes, StateManager::ERROR_IMU_NOT_RESPONDING | - StateManager::ERROR_TIME_GOING_BACKWARDS); + EXPECT_EQ(rf.state_manager_.state().error_codes, + StateManager::ERROR_IMU_NOT_RESPONDING | StateManager::ERROR_TIME_GOING_BACKWARDS); EXPECT_EQ(rf.state_manager_.state().error, true); } TEST_F(StateMachineTest, ClearMultipleErrorsAtOnce) { - uint32_t error = StateManager::ERROR_IMU_NOT_RESPONDING | - StateManager::ERROR_TIME_GOING_BACKWARDS | - StateManager::ERROR_UNCALIBRATED_IMU; + uint32_t error = StateManager::ERROR_IMU_NOT_RESPONDING | StateManager::ERROR_TIME_GOING_BACKWARDS + | 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); @@ -128,9 +119,8 @@ 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; + uint32_t error = StateManager::ERROR_IMU_NOT_RESPONDING | StateManager::ERROR_TIME_GOING_BACKWARDS + | StateManager::ERROR_UNCALIBRATED_IMU; rf.state_manager_.set_error(error); rf.state_manager_.clear_error(error); EXPECT_EQ(rf.state_manager_.state().armed, false); @@ -139,7 +129,7 @@ TEST_F(StateMachineTest, ClearAllErrors) EXPECT_EQ(rf.state_manager_.state().error, false); } -TEST_F (StateMachineTest, DoNotArmIfError) +TEST_F(StateMachineTest, DoNotArmIfError) { // Now add, an error, and then try to arm rf.state_manager_.set_error(StateManager::ERROR_INVALID_MIXER); @@ -150,7 +140,7 @@ TEST_F (StateMachineTest, DoNotArmIfError) EXPECT_EQ(rf.state_manager_.state().error, true); } -TEST_F (StateMachineTest, ArmIfNoError) +TEST_F(StateMachineTest, ArmIfNoError) { rf.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM); EXPECT_EQ(rf.state_manager_.state().armed, true); @@ -159,7 +149,7 @@ TEST_F (StateMachineTest, ArmIfNoError) EXPECT_EQ(rf.state_manager_.state().error, false); } -TEST_F (StateMachineTest, ArmAndDisarm) +TEST_F(StateMachineTest, ArmAndDisarm) { rf.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM); EXPECT_EQ(rf.state_manager_.state().armed, true); @@ -171,7 +161,7 @@ TEST_F (StateMachineTest, ArmAndDisarm) EXPECT_EQ(rf.state_manager_.state().error, false); } -TEST_F (StateMachineTest, WaitForCalibrationToArm) +TEST_F(StateMachineTest, WaitForCalibrationToArm) { rf.params_.set_param_int(PARAM_CALIBRATE_GYRO_ON_ARM, true); // try to arm @@ -190,7 +180,7 @@ TEST_F (StateMachineTest, WaitForCalibrationToArm) EXPECT_EQ(rf.state_manager_.state().error, false); } -TEST_F (StateMachineTest, CalibrationFailedDontArm) +TEST_F(StateMachineTest, CalibrationFailedDontArm) { rf.params_.set_param_int(PARAM_CALIBRATE_GYRO_ON_ARM, true); rf.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM); @@ -202,7 +192,7 @@ TEST_F (StateMachineTest, CalibrationFailedDontArm) EXPECT_EQ(rf.state_manager_.state().error, false); } -TEST_F (StateMachineTest, ErrorDuringCalibrationDontArm) +TEST_F(StateMachineTest, ErrorDuringCalibrationDontArm) { rf.params_.set_param_int(PARAM_CALIBRATE_GYRO_ON_ARM, true); rf.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM); @@ -214,7 +204,7 @@ TEST_F (StateMachineTest, ErrorDuringCalibrationDontArm) EXPECT_EQ(rf.state_manager_.state().error, true); } -TEST_F (StateMachineTest, RCLostDuringCalibrationDontArm) +TEST_F(StateMachineTest, RCLostDuringCalibrationDontArm) { rf.params_.set_param_int(PARAM_CALIBRATE_GYRO_ON_ARM, true); rf.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM); @@ -226,7 +216,7 @@ TEST_F (StateMachineTest, RCLostDuringCalibrationDontArm) EXPECT_EQ(rf.state_manager_.state().error, true); } -TEST_F (StateMachineTest, ClearErrorStayDisarmed) +TEST_F(StateMachineTest, ClearErrorStayDisarmed) { rf.params_.set_param_int(PARAM_CALIBRATE_GYRO_ON_ARM, true); rf.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM); @@ -238,7 +228,7 @@ TEST_F (StateMachineTest, ClearErrorStayDisarmed) EXPECT_EQ(rf.state_manager_.state().error_codes, StateManager::ERROR_NONE); } -TEST_F (StateMachineTest, RecoverRCStayDisarmed) +TEST_F(StateMachineTest, RecoverRCStayDisarmed) { rf.params_.set_param_int(PARAM_CALIBRATE_GYRO_ON_ARM, true); rf.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM); @@ -250,7 +240,7 @@ TEST_F (StateMachineTest, RecoverRCStayDisarmed) EXPECT_EQ(rf.state_manager_.state().error_codes, StateManager::ERROR_NONE); } -TEST_F (StateMachineTest, SetErrorsWhileArmed) +TEST_F(StateMachineTest, SetErrorsWhileArmed) { rf.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM); EXPECT_EQ(rf.state_manager_.state().armed, true); @@ -262,7 +252,7 @@ TEST_F (StateMachineTest, SetErrorsWhileArmed) EXPECT_EQ(rf.state_manager_.state().error, true); } -TEST_F (StateMachineTest, ErrorsPersistWhenDisarmed) +TEST_F(StateMachineTest, ErrorsPersistWhenDisarmed) { rf.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM); rf.state_manager_.set_error(StateManager::ERROR_TIME_GOING_BACKWARDS); @@ -274,7 +264,7 @@ TEST_F (StateMachineTest, ErrorsPersistWhenDisarmed) EXPECT_EQ(rf.state_manager_.state().error, true); } -TEST_F (StateMachineTest, UnableToArmWithPersistentErrors) +TEST_F(StateMachineTest, UnableToArmWithPersistentErrors) { rf.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM); rf.state_manager_.set_error(StateManager::ERROR_TIME_GOING_BACKWARDS); @@ -286,7 +276,7 @@ TEST_F (StateMachineTest, UnableToArmWithPersistentErrors) EXPECT_EQ(rf.state_manager_.state().error, true); } -TEST_F (StateMachineTest, ArmIfThrottleLow) +TEST_F(StateMachineTest, ArmIfThrottleLow) { uint16_t rc_values[8]; for (int i = 0; i < 8; i++) @@ -300,7 +290,7 @@ TEST_F (StateMachineTest, ArmIfThrottleLow) EXPECT_EQ(true, rf.state_manager_.state().armed); } -TEST_F (StateMachineTest, ArmIfThrottleHighWithMinThrottle) +TEST_F(StateMachineTest, ArmIfThrottleHighWithMinThrottle) { rf.params_.set_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE, true); uint16_t rc_values[8]; @@ -317,7 +307,7 @@ TEST_F (StateMachineTest, ArmIfThrottleHighWithMinThrottle) EXPECT_EQ(rf.state_manager_.state().armed, false); } -TEST_F (StateMachineTest, DontArmIfThrottleHighWithoutMinThrottle) +TEST_F(StateMachineTest, DontArmIfThrottleHighWithoutMinThrottle) { rf.params_.set_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE, false); uint16_t rc_values[8]; @@ -334,7 +324,7 @@ TEST_F (StateMachineTest, DontArmIfThrottleHighWithoutMinThrottle) EXPECT_EQ(rf.state_manager_.state().armed, false); } -TEST_F (StateMachineTest, LostRCWhenDisarmNoFailsafe) +TEST_F(StateMachineTest, LostRCWhenDisarmNoFailsafe) { rf.state_manager_.set_event(StateManager::EVENT_RC_LOST); EXPECT_EQ(rf.state_manager_.state().armed, false); @@ -343,14 +333,14 @@ TEST_F (StateMachineTest, LostRCWhenDisarmNoFailsafe) EXPECT_EQ(rf.state_manager_.state().error, true); } -TEST_F (StateMachineTest, UnableToArmWithoutRC) +TEST_F(StateMachineTest, UnableToArmWithoutRC) { rf.state_manager_.set_event(StateManager::EVENT_RC_LOST); rf.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM); EXPECT_EQ(rf.state_manager_.state().armed, false); } -TEST_F (StateMachineTest, AbleToArmAfterRCRecovery) +TEST_F(StateMachineTest, AbleToArmAfterRCRecovery) { rf.state_manager_.set_event(StateManager::EVENT_RC_LOST); rf.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM); @@ -360,7 +350,7 @@ TEST_F (StateMachineTest, AbleToArmAfterRCRecovery) EXPECT_EQ(rf.state_manager_.state().armed, true); } -TEST_F (StateMachineTest, RCLostWhileArmedEnterFailsafe) +TEST_F(StateMachineTest, RCLostWhileArmedEnterFailsafe) { rf.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM); rf.state_manager_.set_event(StateManager::EVENT_RC_LOST); @@ -370,7 +360,7 @@ TEST_F (StateMachineTest, RCLostWhileArmedEnterFailsafe) EXPECT_EQ(rf.state_manager_.state().failsafe, true); } -TEST_F (StateMachineTest, DisarmWhileInFailsafeGoToError) +TEST_F(StateMachineTest, DisarmWhileInFailsafeGoToError) { rf.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM); rf.state_manager_.set_event(StateManager::EVENT_RC_LOST); @@ -381,7 +371,7 @@ TEST_F (StateMachineTest, DisarmWhileInFailsafeGoToError) EXPECT_EQ(rf.state_manager_.state().failsafe, true); } -TEST_F (StateMachineTest, RegainRCAfterFailsafe) +TEST_F(StateMachineTest, RegainRCAfterFailsafe) { rf.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM); rf.state_manager_.set_event(StateManager::EVENT_RC_LOST); @@ -392,7 +382,7 @@ TEST_F (StateMachineTest, RegainRCAfterFailsafe) EXPECT_EQ(rf.state_manager_.state().failsafe, false); } constexpr uint32_t StateManager::BackupData::ARM_MAGIC; // C++ is weird -TEST_F (StateMachineTest, NormalBoot) +TEST_F(StateMachineTest, NormalBoot) { board.backup_memory_clear(); rf.state_manager_.check_backup_memory(); @@ -451,7 +441,7 @@ TEST_F(StateMachineTest, CrashRecoveryInvalidArmMagic) { board.backup_memory_clear(); StateManager::BackupData data; - data.arm_flag = StateManager::BackupData::ARM_MAGIC-101; + data.arm_flag = StateManager::BackupData::ARM_MAGIC - 101; data.error_code = 1; data.reset_count = 1; data.finalize(); diff --git a/test/test_board.cpp b/test/test_board.cpp index 2509858e..713bef84 100644 --- a/test/test_board.cpp +++ b/test/test_board.cpp @@ -36,7 +36,6 @@ namespace rosflight_firmware { - void testBoard::set_rc(uint16_t *values) { for (int i = 0; i < 8; i++) @@ -66,24 +65,35 @@ void testBoard::set_imu(float *acc, float *gyro, uint64_t time_us) new_imu_ = true; } - // setup -void testBoard::init_board() +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, hardware_config_t configuration) {} 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; } +uint16_t testBoard::serial_bytes_available() +{ + return 0; +} +uint8_t testBoard::serial_read() +{ + return 0; +} void testBoard::serial_flush() {} // Hardware config @@ -91,10 +101,10 @@ bool testBoard::enable_device(device_t device, hardware_config_t configuration, { (void)configuration; (void)params; - switch(configuration) + switch (configuration) { case Configuration::SERIAL: - serial_init(0,0); + serial_init(0, 0); break; case Configuration::RC: rc_init(); @@ -110,7 +120,10 @@ const TestBoardConfigManager &testBoard::get_board_config_manager() const } // 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() { @@ -122,7 +135,6 @@ bool testBoard::new_imu_data() return false; } - bool testBoard::imu_read(float accel[3], float *temperature, float gyro[3], uint64_t *time) { for (int i = 0; i < 3; i++) @@ -138,7 +150,7 @@ bool testBoard::imu_read(float accel[3], float *temperature, float gyro[3], uint 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; @@ -149,7 +161,7 @@ bool testBoard::backup_memory_read(void *dest, size_t len) void testBoard::backup_memory_write(const void *src, size_t len) { - if(len > BACKUP_MEMORY_SIZE) + if (len > BACKUP_MEMORY_SIZE) len = BACKUP_MEMORY_SIZE; memcpy(backup_memory_, src, len); } @@ -164,21 +176,36 @@ void testBoard::backup_memory_clear() 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) {} -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) {} -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 { @@ -206,23 +233,34 @@ void testBoard::battery_current_set_multiplier(double multiplier) (void)multiplier; } -//GNSS is not supported on the test board -GNSSData testBoard::gnss_read() { return {}; } - -//GNSS is not supported on the test board -GNSSRaw testBoard::gnss_raw_read() { return {}; } +// GNSS is not supported on the test board +GNSSData testBoard::gnss_read() +{ + return {}; +} -//GNSS is not supported on the test board -bool testBoard::gnss_has_new_data() { return false; } +// GNSS is not supported on the test board +GNSSRaw testBoard::gnss_raw_read() +{ + return {}; +} +// GNSS is not supported on the test board +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() {} -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 ; + return static_cast(rc_values[channel] - 1000) / 1000.0; } void testBoard::pwm_write(uint8_t channel, float value) {} void testBoard::pwm_init(uint32_t refresh_rate, uint16_t idle_pwm) {} @@ -230,11 +268,15 @@ void testBoard::pwm_disable() {} // non-volatile memory void testBoard::memory_init() {} -bool testBoard::memory_read(void *dest, size_t len) { +bool testBoard::memory_read(void *dest, size_t len) +{ memset(dest, 0, len); return true; } -bool testBoard::memory_write(const void *src, 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_config_manager.cpp b/test/test_board_config_manager.cpp index 2b8045dc..bb57acd3 100644 --- a/test/test_board_config_manager.cpp +++ b/test/test_board_config_manager.cpp @@ -1,4 +1,5 @@ #include "test_board_config_manager.h" + #include #include @@ -9,31 +10,37 @@ hardware_config_t TestBoardConfigManager::get_max_config(device_t device) const (void)device; return 0; // This is not needed to test other software } -ConfigManager::ConfigResponse TestBoardConfigManager::check_config_change(device_t device, hardware_config_t config, const ConfigManager &cm) const +ConfigManager::ConfigResponse TestBoardConfigManager::check_config_change(device_t device, + hardware_config_t config, + const ConfigManager &cm) const { (void)cm; // A couple variations are given for testing ConfigManager::ConfigResponse response; response.successful = true; response.reboot_required = true; - if(device == Configuration::SERIAL && config == 1) + if (device == Configuration::SERIAL && config == 1) { response.successful = false; response.reboot_required = false; - strcpy(reinterpret_cast(response.message), "Fail for testing"); + strcpy(reinterpret_cast(response.message), "Fail for testing"); return response; } - strcpy(reinterpret_cast(response.message), "Succeed for testing"); + strcpy(reinterpret_cast(response.message), "Succeed for testing"); return response; } -void TestBoardConfigManager::get_device_name(device_t device, char (&name)[BoardConfigManager::DEVICE_NAME_LENGTH]) const +void TestBoardConfigManager::get_device_name(device_t device, + char (&name)[BoardConfigManager::DEVICE_NAME_LENGTH]) const { std::string device_name = "device #" + std::to_string(static_cast(device)); - strcpy(reinterpret_cast(name), device_name.c_str()); + strcpy(reinterpret_cast(name), device_name.c_str()); } -void TestBoardConfigManager::get_config_name(device_t device, hardware_config_t config, char (&name)[BoardConfigManager::CONFIG_NAME_LENGTH]) const +void TestBoardConfigManager::get_config_name(device_t device, + hardware_config_t config, + char (&name)[BoardConfigManager::CONFIG_NAME_LENGTH]) const { - std::string config_name = "config " + std::to_string(static_cast(device)) + ","+std::to_string(static_cast(config)); - strcpy(reinterpret_cast(name), config_name.c_str()); -} + std::string config_name = + "config " + std::to_string(static_cast(device)) + "," + std::to_string(static_cast(config)); + strcpy(reinterpret_cast(name), config_name.c_str()); } +} // namespace rosflight_firmware diff --git a/test/turbotrig_test.cpp b/test/turbotrig_test.cpp index 08c15659..9111b73d 100644 --- a/test/turbotrig_test.cpp +++ b/test/turbotrig_test.cpp @@ -31,69 +31,63 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#include "math.h" #include "common.h" -#include +#include "math.h" -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) -}; +#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::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)}; TEST(TurboMath, atan) { @@ -112,7 +106,6 @@ TEST(TurboMath, sin_cos) } } - TEST(TurboMath, atan2) { for (float i = -100.0; i <= 100.0; i += 0.1) @@ -140,23 +133,22 @@ TEST(TurboMath, asin) TEST(TurboMath, fastAlt) { - - //out of bounds + // out of bounds EXPECT_EQ(turbomath::alt(69681), 0.0); EXPECT_EQ(turbomath::alt(10700), 0.0); - //all valid int values + // 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); + 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 + // 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 } } - TEST(TurboMath, Vector) { for (int i = 0; i < 24; i++) @@ -187,17 +179,17 @@ TEST(TurboMath, Vector) EXPECT_VEC3_SUPERCLOSE((vec1 + vec2), (eig1 + eig2)); EXPECT_VEC3_SUPERCLOSE((vec1 - vec2), (eig1 - eig2)); - EXPECT_VEC3_SUPERCLOSE((vec1*5.0f), (eig1*5.0f)); - EXPECT_VEC3_SUPERCLOSE((vec1*-10.0f), (eig1*-10.0f)); + EXPECT_VEC3_SUPERCLOSE((vec1 * 5.0f), (eig1 * 5.0f)); + EXPECT_VEC3_SUPERCLOSE((vec1 * -10.0f), (eig1 * -10.0f)); vec1 *= -3.0f; - EXPECT_VEC3_SUPERCLOSE(vec1, (eig1*-3.0f)); + EXPECT_VEC3_SUPERCLOSE(vec1, (eig1 * -3.0f)); eig1 *= -3.0f; EXPECT_VEC3_SUPERCLOSE(vec1, eig1); - EXPECT_VEC3_SUPERCLOSE((vec1/-10.0f), (eig1/-10.0f)); + EXPECT_VEC3_SUPERCLOSE((vec1 / -10.0f), (eig1 / -10.0f)); vec1 /= -3.0f; - EXPECT_VEC3_SUPERCLOSE(vec1, (eig1/-3.0f)); + EXPECT_VEC3_SUPERCLOSE(vec1, (eig1 / -3.0f)); eig1 /= -3.0f; // Test Vector Dot Product @@ -208,14 +200,12 @@ TEST(TurboMath, Vector) } } - TEST(TurboMath, Quaternion) { - for (int i = 0; i < 24; i++) { turbomath::Quaternion quat1 = random_quaternions[i].normalize(); - turbomath::Quaternion quat2 = random_quaternions[i+1].normalize(); + turbomath::Quaternion quat2 = random_quaternions[i + 1].normalize(); Eigen::Quaternionf eig1(quat1.w, quat1.x, quat1.y, quat1.z); Eigen::Quaternionf eig2(quat2.w, quat2.x, quat2.y, quat2.z); @@ -224,23 +214,21 @@ 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)); + ASSERT_QUAT_SUPERCLOSE((quat1 * quat2), (eig2 * eig1)); ASSERT_QUAT_SUPERCLOSE(quat1.inverse(), eig1.inverse()); - // Test Quaternion Rotate turbomath::Vector vec1 = random_vectors[i]; Eigen::Vector3f veig1; veig1 << vec1.x, vec1.y, vec1.z; - // Test rotate_vector by rotating vector to new frame turbomath::Vector vec2 = quat1.rotate(vec1); - Eigen::Vector3f veig2 = veig1.transpose()*eig1.toRotationMatrix(); + Eigen::Vector3f veig2 = veig1.transpose() * eig1.toRotationMatrix(); EXPECT_VEC3_SUPERCLOSE(vec2, veig2); // compare with eigen // And rotate back @@ -270,14 +258,13 @@ TEST(TurboMath, QuatFromTwoVectors) { // Test the "quat_from_two_vectors" turbomath::Vector vec1(1.0f, 0.0f, 0.0f); - turbomath::Quaternion quat1(1.0f/sqrt(2.0f), 0.0f, 0.0f, 1.0f/sqrt(2.0f)); + turbomath::Quaternion quat1(1.0f / sqrt(2.0f), 0.0f, 0.0f, 1.0f / sqrt(2.0f)); turbomath::Vector vec2 = quat1.rotate(vec1); turbomath::Quaternion quat_test; quat_test.from_two_unit_vectors(vec2, vec1); ASSERT_TURBOQUAT_SUPERCLOSE(quat1, quat_test); - // A bunch of random (off-axes tests) for (int i = 0; i < 25; i++) { From 7e35fcacab4435663e2395ab60be993b85ee567b Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Fri, 3 Apr 2020 14:53:21 -0600 Subject: [PATCH 73/76] Fix merge error --- src/param.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/param.cpp b/src/param.cpp index d2d7d479..de12a9c8 100644 --- a/src/param.cpp +++ b/src/param.cpp @@ -56,7 +56,7 @@ namespace rosflight_firmware { -Params::Params(ROSflight &_rf) : RF_(_rf), params(param_struct), listeners_(nullptr), num_listeners_(0) {} +Params::Params(ROSflight &_rf, params_t ¶m_struct) : RF_(_rf), params(param_struct), 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) From 6229327bab635c6b17490a54835a04153d5041a7 Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Fri, 3 Apr 2020 16:38:45 -0600 Subject: [PATCH 74/76] Fix merge breaking F1 build --- boards/breezy/breezy_board.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/boards/breezy/breezy_board.cpp b/boards/breezy/breezy_board.cpp index 392cd4a4..8ee3ce21 100644 --- a/boards/breezy/breezy_board.cpp +++ b/boards/breezy/breezy_board.cpp @@ -354,7 +354,6 @@ void BreezyBoard::battery_current_set_multiplier(double multiplier) void BreezyBoard::rc_init() { - (void) rc_type; // TODO SBUS is not supported on F1 pwmInit(true, false, false, pwm_refresh_rate_, pwm_idle_pwm_); } From 0f31dda319436dbc0bf7a776f07050a89c8bed54 Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Fri, 3 Apr 2020 16:41:57 -0600 Subject: [PATCH 75/76] Fix formatting --- boards/airbourne/airbourne_board.cpp | 2 +- boards/airbourne/airbourne_board.h | 2 +- .../airbourne/airbourne_board_config_manager.h | 10 +++++++--- .../airbourne/airbourne_configuration_enum.h | 18 +++++++++--------- boards/breezy/breezy_board.h | 6 +++--- boards/breezy/breezy_board_config_manager.h | 8 ++++++-- src/param.cpp | 8 +++++++- test/test_board.h | 4 ++-- test/test_board_config_manager.h | 10 +++++++--- 9 files changed, 43 insertions(+), 25 deletions(-) diff --git a/boards/airbourne/airbourne_board.cpp b/boards/airbourne/airbourne_board.cpp index 77c3e364..935205c7 100644 --- a/boards/airbourne/airbourne_board.cpp +++ b/boards/airbourne/airbourne_board.cpp @@ -50,7 +50,7 @@ void AirbourneBoard::init_board() backup_sram_init(); - current_serial_ = &vcp_; //uncomment this to switch to VCP as the main output + current_serial_ = &vcp_; // uncomment this to switch to VCP as the main output } void AirbourneBoard::board_reset(bool bootloader) diff --git a/boards/airbourne/airbourne_board.h b/boards/airbourne/airbourne_board.h index 181ea62b..484f7fcc 100644 --- a/boards/airbourne/airbourne_board.h +++ b/boards/airbourne/airbourne_board.h @@ -61,8 +61,8 @@ #include // #include "ublox.h" -#include "board.h" #include "airbourne_board_config_manager.h" +#include "board.h" namespace rosflight_firmware { diff --git a/boards/airbourne/airbourne_board_config_manager.h b/boards/airbourne/airbourne_board_config_manager.h index 2bbc3aa6..23e60b4a 100644 --- a/boards/airbourne/airbourne_board_config_manager.h +++ b/boards/airbourne/airbourne_board_config_manager.h @@ -1,10 +1,11 @@ #ifndef AIRBOURNE_BOARD_CONFIG_MANAGER_H #define AIRBOURNE_BOARD_CONFIG_MANAGER_H -#include "board_config_manager.h" #include "airbourne_configuration_enum.h" +#include "board_config_manager.h" -namespace rosflight_firmware{ +namespace rosflight_firmware +{ class ROSflight; class AirbourneBoard; class AirbourneBoardConfigManager : public BoardConfigManager @@ -12,9 +13,12 @@ class AirbourneBoardConfigManager : public BoardConfigManager public: AirbourneBoardConfigManager(); hardware_config_t get_max_config(device_t device) const override; - ConfigManager::ConfigResponse check_config_change(device_t device, hardware_config_t config, const ConfigManager &cm) const override; + ConfigManager::ConfigResponse check_config_change(device_t device, + hardware_config_t config, + const ConfigManager &cm) const override; void get_device_name(device_t device, char (&name)[DEVICE_NAME_LENGTH]) const override; void get_config_name(device_t device, hardware_config_t config, char (&name)[CONFIG_NAME_LENGTH]) const override; + private: static constexpr hardware_config_t max_configs[Configuration::DEVICE_COUNT]{3, 1, 1, 3, 1, 1, 1, 1}; diff --git a/boards/airbourne/airbourne_configuration_enum.h b/boards/airbourne/airbourne_configuration_enum.h index 4cecfac7..117a8e72 100644 --- a/boards/airbourne/airbourne_configuration_enum.h +++ b/boards/airbourne/airbourne_configuration_enum.h @@ -5,7 +5,7 @@ namespace rosflight_firmware { namespace AirbourneConfiguration { -enum serial_config_t: uint8_t +enum serial_config_t : uint8_t { SERIAL_VCP, SERIAL_UART1, @@ -13,19 +13,19 @@ enum serial_config_t: uint8_t SERIAL_UART3 }; -enum rc_config_t: uint8_t +enum rc_config_t : uint8_t { RC_PPM, RC_SBUS }; -enum airspeed_config_t: uint8_t +enum airspeed_config_t : uint8_t { AIRSPEED_DISABLED, AIRSPEED_I2C2 }; -enum gnss_config_t: uint8_t +enum gnss_config_t : uint8_t { GNSS_DISABLED, GNSS_UART1, @@ -33,30 +33,30 @@ enum gnss_config_t: uint8_t GNSS_UART3 }; -enum sonar_config_t: uint8_t +enum sonar_config_t : uint8_t { SONAR_DISABLED, SONAR_I2C2 }; -enum battery_monitor_config_t: uint8_t +enum battery_monitor_config_t : uint8_t { BATTERY_MONITOR_DISABLED, BATTERY_MONITOR_ADC3 }; -enum barometer_config_t: uint8_t +enum barometer_config_t : uint8_t { BAROMETER_DISABLED, BAROMETER_ONBOARD }; -enum magnetometer_config_t: uint8_t +enum magnetometer_config_t : uint8_t { MAGNETOMETER_DISABLED, MAGNETOMETER_ONBOARD }; -} +} // namespace AirbourneConfiguration } // namespace rosflight_firmware #endif // AIRBOURNE_CONFIGURATION_ENUM_H diff --git a/boards/breezy/breezy_board.h b/boards/breezy/breezy_board.h index 945cbf4d..cef767bf 100644 --- a/boards/breezy/breezy_board.h +++ b/boards/breezy/breezy_board.h @@ -42,9 +42,9 @@ extern "C" } #include "board.h" -#include "sensors.h" -#include "configuration_enum.h" #include "breezy_board_config_manager.h" +#include "configuration_enum.h" +#include "sensors.h" namespace rosflight_firmware { @@ -102,7 +102,7 @@ class BreezyBoard : public Board // hardware config bool enable_device(device_t device, hardware_config_t configuration, const Params ¶ms) override; - const BreezyBoardConfigManager & get_board_config_manager() const override; + const BreezyBoardConfigManager &get_board_config_manager() const override; // sensors void sensors_init() override; diff --git a/boards/breezy/breezy_board_config_manager.h b/boards/breezy/breezy_board_config_manager.h index f375041a..88616f73 100644 --- a/boards/breezy/breezy_board_config_manager.h +++ b/boards/breezy/breezy_board_config_manager.h @@ -9,9 +9,13 @@ class BreezyBoardConfigManager : public BoardConfigManager { public: hardware_config_t get_max_config(device_t device) const override; - ConfigManager::ConfigResponse check_config_change(device_t device, hardware_config_t config, const ConfigManager &cm) const override; + ConfigManager::ConfigResponse check_config_change(device_t device, + hardware_config_t config, + const ConfigManager &cm) const override; void get_device_name(device_t device, char (&name)[BoardConfigManager::DEVICE_NAME_LENGTH]) const override; - void get_config_name(device_t device, hardware_config_t config, char (&name)[BoardConfigManager::CONFIG_NAME_LENGTH]) const override; + void get_config_name(device_t device, + hardware_config_t config, + char (&name)[BoardConfigManager::CONFIG_NAME_LENGTH]) const override; }; } // namespace rosflight_firmware diff --git a/src/param.cpp b/src/param.cpp index de12a9c8..94bef8bc 100644 --- a/src/param.cpp +++ b/src/param.cpp @@ -56,7 +56,13 @@ namespace rosflight_firmware { -Params::Params(ROSflight &_rf, params_t ¶m_struct) : RF_(_rf), params(param_struct), listeners_(nullptr), num_listeners_(0) {} +Params::Params(ROSflight &_rf, params_t ¶m_struct) : + RF_(_rf), + params(param_struct), + 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) diff --git a/test/test_board.h b/test/test_board.h index 1058858e..0c7006b9 100644 --- a/test/test_board.h +++ b/test/test_board.h @@ -68,9 +68,9 @@ class testBoard : public Board uint8_t serial_read() override; void serial_flush() override; -// Hardware config + // Hardware config bool enable_device(device_t device, hardware_config_t configuration, const Params ¶ms) override; - const TestBoardConfigManager & get_board_config_manager() const override; + const TestBoardConfigManager &get_board_config_manager() const override; // sensors void sensors_init() override; diff --git a/test/test_board_config_manager.h b/test/test_board_config_manager.h index edf57799..32a23628 100644 --- a/test/test_board_config_manager.h +++ b/test/test_board_config_manager.h @@ -9,10 +9,14 @@ class TestBoardConfigManager : public BoardConfigManager { public: hardware_config_t get_max_config(device_t device) const override; - ConfigManager::ConfigResponse check_config_change(device_t device, hardware_config_t config, const ConfigManager &cm) const override; + ConfigManager::ConfigResponse check_config_change(device_t device, + hardware_config_t config, + const ConfigManager &cm) const override; void get_device_name(device_t device, char (&name)[BoardConfigManager::DEVICE_NAME_LENGTH]) const override; - void get_config_name(device_t device, hardware_config_t config, char (&name)[BoardConfigManager::CONFIG_NAME_LENGTH]) const override; + void get_config_name(device_t device, + hardware_config_t config, + char (&name)[BoardConfigManager::CONFIG_NAME_LENGTH]) const override; }; -} // rosflight_firmware +} // namespace rosflight_firmware #endif // TESTBOARDCONFIGMANAGER_H From 3639b7c1281b50386c930a6f420f79a79db8b6f6 Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Fri, 3 Apr 2020 16:57:41 -0600 Subject: [PATCH 76/76] Fix style with fancy new clang-format --- boards/airbourne/airbourne_board.cpp | 66 ++-- boards/airbourne/airbourne_board.h | 46 ++- .../airbourne_board_config_manager.cpp | 114 +++--- .../airbourne_board_config_manager.h | 10 +- .../airbourne/airbourne_configuration_enum.h | 18 +- boards/airbourne/main.cpp | 107 +++--- boards/breezy/breezy_board.cpp | 32 +- boards/breezy/breezy_board.h | 34 +- boards/breezy/breezy_board_config_manager.cpp | 14 +- boards/breezy/breezy_board_config_manager.h | 8 +- boards/breezy/flash.h | 16 +- boards/breezy/main.cpp | 3 +- comms/mavlink/mavlink.cpp | 126 +++---- comms/mavlink/mavlink.h | 54 ++- include/board.h | 43 ++- include/board_config_manager.h | 15 +- include/comm_manager.h | 45 +-- include/command_manager.h | 116 +++---- include/config_manager.h | 11 +- include/configuration_enum.h | 9 +- include/controller.h | 11 +- include/estimator.h | 42 +-- include/interface/comm_link.h | 175 +++++----- include/interface/param_listener.h | 1 - include/memory_manager.h | 9 +- include/mixer.h | 219 +++++------- include/nanoprintf.h | 13 +- include/param.h | 50 +-- include/rc.h | 8 +- include/rosflight.h | 41 +-- include/sensors.h | 43 +-- include/state_manager.h | 25 +- include/util.h | 4 +- lib/turbomath/turbomath.cpp | 326 ++++++++---------- lib/turbomath/turbomath.h | 42 ++- scripts/fix-code-style.sh | 14 +- src/comm_manager.cpp | 123 +++---- src/command_manager.cpp | 46 +-- src/config_manager.cpp | 30 +- src/controller.cpp | 61 ++-- src/estimator.cpp | 99 +++--- src/memory_manager.cpp | 7 +- src/mixer.cpp | 26 +- src/nanoprintf.cpp | 222 ++++++------ src/param.cpp | 246 +++++++------ src/rc.cpp | 34 +- src/rosflight.cpp | 9 +- src/sensors.cpp | 117 +++---- src/state_manager.cpp | 18 +- test/command_manager_test.cpp | 126 ++++--- test/common.cpp | 4 +- test/common.h | 72 ++-- test/config_manager_test.cpp | 30 +- test/estimator_test.cpp | 164 ++++----- test/parameters_test.cpp | 6 +- test/state_machine_test.cpp | 90 +++-- test/test_board.cpp | 100 ++++-- test/test_board.h | 27 +- test/test_board_config_manager.cpp | 27 +- test/test_board_config_manager.h | 10 +- test/turbotrig_test.cpp | 155 ++++----- 61 files changed, 1797 insertions(+), 1962 deletions(-) diff --git a/boards/airbourne/airbourne_board.cpp b/boards/airbourne/airbourne_board.cpp index fe70dbf4..5d4f23d7 100644 --- a/boards/airbourne/airbourne_board.cpp +++ b/boards/airbourne/airbourne_board.cpp @@ -33,10 +33,7 @@ namespace rosflight_firmware { - -AirbourneBoard::AirbourneBoard() -{ -} +AirbourneBoard::AirbourneBoard() {} void AirbourneBoard::init_board() { @@ -76,7 +73,7 @@ void AirbourneBoard::clock_delay(uint32_t milliseconds) void AirbourneBoard::serial_init(uint32_t baud_rate, hardware_config_t configuration) { vcp_.init(); // VCP is always initialized, so that if UART is mistakenly enabled, it can still be used - switch(configuration) + switch (configuration) { default: case AirbourneConfiguration::SERIAL_VCP: @@ -99,7 +96,7 @@ void AirbourneBoard::serial_init(uint32_t baud_rate, hardware_config_t configura void AirbourneBoard::serial_write(const uint8_t *src, size_t len) { - if(vcp_.connected()) + if (vcp_.connected()) current_serial_ = &vcp_; current_serial_->write(src, len); } @@ -111,7 +108,6 @@ uint16_t AirbourneBoard::serial_bytes_available() uint8_t AirbourneBoard::serial_read() { - return current_serial_->read_byte(); } @@ -123,7 +119,7 @@ void AirbourneBoard::serial_flush() // Resources bool AirbourneBoard::enable_device(device_t device, hardware_config_t configuration, const Params ¶ms) { - switch(device) + switch (device) { case Configuration::SERIAL: { @@ -133,7 +129,7 @@ bool AirbourneBoard::enable_device(device_t device, hardware_config_t configurat break; } case Configuration::RC: - switch(configuration) + switch (configuration) { case AirbourneConfiguration::RC_PPM: rc_init(RC_TYPE_PPM); @@ -146,9 +142,9 @@ bool AirbourneBoard::enable_device(device_t device, hardware_config_t configurat } return true; case Configuration::AIRSPEED: - if(configuration==AirbourneConfiguration::AIRSPEED_I2C2) + if (configuration == AirbourneConfiguration::AIRSPEED_I2C2) { - if(!ext_i2c_.is_initialized()) + if (!ext_i2c_.is_initialized()) ext_i2c_.init(&i2c_config[EXTERNAL_I2C]); airspeed_.init(&ext_i2c_); } @@ -157,15 +153,15 @@ bool AirbourneBoard::enable_device(device_t device, hardware_config_t configurat // GNSS is currently disabled break; case Configuration::SONAR: - if(configuration == AirbourneConfiguration::SONAR_I2C2) + if (configuration == AirbourneConfiguration::SONAR_I2C2) { - if(!ext_i2c_.is_initialized()) + if (!ext_i2c_.is_initialized()) ext_i2c_.init(&i2c_config[EXTERNAL_I2C]); sonar_.init(&ext_i2c_); } break; case Configuration::BATTERY_MONITOR: - if(configuration == AirbourneConfiguration::BATTERY_MONITOR_ADC3) + if (configuration == AirbourneConfiguration::BATTERY_MONITOR_ADC3) { float voltage_multiplier = params.get_param_float(PARAM_BATTERY_VOLTAGE_MULTIPLIER); float current_multiplier = params.get_param_float(PARAM_BATTERY_CURRENT_MULTIPLIER); @@ -174,19 +170,23 @@ bool AirbourneBoard::enable_device(device_t device, hardware_config_t configurat } break; case Configuration::BAROMETER: - if(configuration == AirbourneConfiguration::BAROMETER_ONBOARD) + if (configuration == AirbourneConfiguration::BAROMETER_ONBOARD) { - while (millis() < 50) {} // wait for sensors to boot up - if(!int_i2c_.is_initialized()) + while (millis() < 50) + { + } // wait for sensors to boot up + if (!int_i2c_.is_initialized()) int_i2c_.init(&i2c_config[BARO_I2C]); baro_.init(&int_i2c_); } break; case Configuration::MAGNETOMETER: - if(configuration == AirbourneConfiguration::MAGNETOMETER_ONBOARD) + if (configuration == AirbourneConfiguration::MAGNETOMETER_ONBOARD) { - while (millis() < 50) {} // wait for sensors to boot up - if(!int_i2c_.is_initialized()) + while (millis() < 50) + { + } // wait for sensors to boot up + if (!int_i2c_.is_initialized()) int_i2c_.init(&i2c_config[BARO_I2C]); mag_.init(&int_i2c_); } @@ -205,7 +205,9 @@ AirbourneBoardConfigManager const &AirbourneBoard::get_board_config_manager() co // 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_); // Most sensors are set up through the configuration manager } @@ -249,7 +251,7 @@ bool AirbourneBoard::mag_present() void AirbourneBoard::mag_update() { - if(mag_.is_initialized()) + if (mag_.is_initialized()) mag_.update(); } @@ -266,7 +268,7 @@ bool AirbourneBoard::baro_present() void AirbourneBoard::baro_update() { - if(baro_.is_initialized()) + if (baro_.is_initialized()) baro_.update(); } @@ -278,7 +280,7 @@ void AirbourneBoard::baro_read(float *pressure, float *temperature) bool AirbourneBoard::diff_pressure_present() { - if(airspeed_.is_initialized()) + if (airspeed_.is_initialized()) return airspeed_.present(); return false; } @@ -288,11 +290,10 @@ void AirbourneBoard::diff_pressure_update() airspeed_.update(); } - 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); } @@ -304,7 +305,7 @@ bool AirbourneBoard::sonar_present() void AirbourneBoard::sonar_update() { - if(sonar_.is_initialized()) + if (sonar_.is_initialized()) sonar_.update(); } @@ -324,9 +325,9 @@ bool AirbourneBoard::gnss_has_new_data() // return this->gnss_.new_data(); return false; } -//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 +// 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 GNSSData AirbourneBoard::gnss_read() { // UBLOX::GNSSPVT gnss_pvt= gnss_.read(); @@ -367,7 +368,7 @@ GNSSData AirbourneBoard::gnss_read() } GNSSRaw AirbourneBoard::gnss_raw_read() { -// UBLOX::NAV_PVT_t pvt = gnss_.read_raw(); + // UBLOX::NAV_PVT_t pvt = gnss_.read_raw(); GNSSRaw raw = {}; // raw.time_of_week = pvt.iTOW; // raw.year = pvt.time.year; @@ -552,5 +553,4 @@ 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 15e3f248..f5899a26 100644 --- a/boards/airbourne/airbourne_board.h +++ b/boards/airbourne/airbourne_board.h @@ -32,45 +32,41 @@ #ifndef ROSFLIGHT_FIRMWARE_AIRBOURNE_BOARD_H #define ROSFLIGHT_FIRMWARE_AIRBOURNE_BOARD_H -#include -#include -#include - -#include - -#include "vcp.h" -#include "uart.h" -#include "i2c.h" -#include "spi.h" -#include "mpu6000.h" -#include "ms5611.h" #include "M25P16.h" +#include "analog_digital_converter.h" +#include "analog_pin.h" +#include "backup_sram.h" +#include "battery_monitor.h" #include "hmc5883l.h" +#include "i2c.h" +#include "led.h" +#include "mb1242.h" +#include "mpu6000.h" #include "ms4525.h" +#include "ms5611.h" +#include "pwm.h" #include "rc_base.h" #include "rc_ppm.h" #include "rc_sbus.h" -#include "pwm.h" -#include "led.h" #include "serial.h" +#include "spi.h" #include "system.h" #include "uart.h" -#include "mb1242.h" -#include "backup_sram.h" -#include "analog_digital_converter.h" -#include "analog_pin.h" -#include "battery_monitor.h" +#include "vcp.h" + +#include +#include +#include +#include // #include "ublox.h" -#include "board.h" #include "airbourne_board_config_manager.h" +#include "board.h" namespace rosflight_firmware { - class AirbourneBoard : public Board { - private: AirbourneBoardConfigManager board_config_manager_; @@ -78,7 +74,7 @@ class AirbourneBoard : public Board UART uart1_; UART uart2_; 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_; @@ -184,7 +180,7 @@ class AirbourneBoard : public Board float battery_current_read() const override; void battery_current_set_multiplier(double multiplier) override; - //GNSS + // GNSS GNSSData gnss_read() override; bool gnss_has_new_data() override; GNSSRaw gnss_raw_read() override; @@ -194,7 +190,7 @@ class AirbourneBoard : public Board float rc_read(uint8_t channel) override; // PWM - void pwm_init(uint32_t refresh_rate, uint16_t idle_pwm) override; + void pwm_init(uint32_t refresh_rate, uint16_t idle_pwm) override; void pwm_disable() override; void pwm_write(uint8_t channel, float value) override; diff --git a/boards/airbourne/airbourne_board_config_manager.cpp b/boards/airbourne/airbourne_board_config_manager.cpp index d187e43e..84a9364a 100644 --- a/boards/airbourne/airbourne_board_config_manager.cpp +++ b/boards/airbourne/airbourne_board_config_manager.cpp @@ -1,42 +1,43 @@ #include "airbourne_board_config_manager.h" -#include - #include "rosflight.h" +#include + namespace rosflight_firmware { -constexpr hardware_config_t AirbourneBoardConfigManager::max_configs[]; // I can't wait for c++ 17 so that this is optional -AirbourneBoardConfigManager::AirbourneBoardConfigManager() -{ -} +constexpr hardware_config_t + AirbourneBoardConfigManager::max_configs[]; // I can't wait for c++ 17 so that this is optional +AirbourneBoardConfigManager::AirbourneBoardConfigManager() {} hardware_config_t AirbourneBoardConfigManager::get_max_config(device_t device) const { - if(device >= Configuration::DEVICE_COUNT) + if (device >= Configuration::DEVICE_COUNT) return 0; else return AirbourneBoardConfigManager::max_configs[device]; } -ConfigManager::ConfigResponse AirbourneBoardConfigManager::check_config_change(device_t device, hardware_config_t config, const ConfigManager &cm) const +ConfigManager::ConfigResponse AirbourneBoardConfigManager::check_config_change(device_t device, + hardware_config_t config, + const ConfigManager &cm) const { ConfigManager::ConfigResponse resp; resp.reboot_required = false; resp.successful = false; resp.message[0] = 0; // Just in case, because a string without a null terminator causes problems - if(device >= Configuration::DEVICE_COUNT) + if (device >= Configuration::DEVICE_COUNT) { strcpy(resp.message, "Device not found"); return resp; } - if(config > AirbourneBoardConfigManager::max_configs[device]) + if (config > AirbourneBoardConfigManager::max_configs[device]) { strcpy(resp.message, "Configuration not found"); return resp; } - if(config == cm[device]) + if (config == cm[device]) { strcpy(resp.message, "Configuration already set. No change required"); resp.successful = true; @@ -48,24 +49,26 @@ ConfigManager::ConfigResponse AirbourneBoardConfigManager::check_config_change(d device_t conflict_device = Configuration::DEVICE_COUNT; // While a config may conflict with multiple devices, this will only report one - switch(device) + switch (device) { case Configuration::RC: - if(config ==AirbourneConfiguration::RC_PPM) // PPM is not known to conflict with anything + if (config == AirbourneConfiguration::RC_PPM) // PPM is not known to conflict with anything break; [[gnu::fallthrough]]; // Fallthrough is intentional case Configuration::SERIAL: case Configuration::GNSS: - if(port != NO_PORT) + if (port != NO_PORT) { - for(device_t other_device{Configuration::FIRST_DEVICE}; other_device != Configuration::DEVICE_COUNT; ++other_device) + for (device_t other_device{Configuration::FIRST_DEVICE}; other_device != Configuration::DEVICE_COUNT; + ++other_device) { - if(other_device == device) + if (other_device == device) continue; - if(other_device == Configuration::RC) - if(cm[Configuration::RC] == AirbourneConfiguration::RC_PPM) // RC over PPM does not conflict with UART, even though both use the same port + if (other_device == Configuration::RC) + if (cm[Configuration::RC] == AirbourneConfiguration::RC_PPM) // RC over PPM does not conflict with UART, even + // though both use the same port continue; - if(port == get_port(other_device, cm[other_device])) + if (port == get_port(other_device, cm[other_device])) { conflict_device = other_device; break; @@ -75,17 +78,17 @@ ConfigManager::ConfigResponse AirbourneBoardConfigManager::check_config_change(d break; case Configuration::AIRSPEED: case Configuration::SONAR: - if(cm[Configuration::GNSS] == AirbourneConfiguration::GNSS_UART3) + if (cm[Configuration::GNSS] == AirbourneConfiguration::GNSS_UART3) conflict_device = Configuration::GNSS; - if(cm[Configuration::SERIAL] == AirbourneConfiguration::GNSS_UART3) + if (cm[Configuration::SERIAL] == AirbourneConfiguration::GNSS_UART3) conflict_device = Configuration::SERIAL; break; default: break; } - if(conflict_device != Configuration::DEVICE_COUNT) + if (conflict_device != Configuration::DEVICE_COUNT) { - switch(conflict_device) + switch (conflict_device) { case Configuration::SERIAL: strcpy(resp.message, "Port is used by serial."); @@ -120,12 +123,12 @@ ConfigManager::ConfigResponse AirbourneBoardConfigManager::check_config_change(d } resp.successful = true; resp.reboot_required = true; - resp.message[0]=0; // Ensuring that the message is treated as a zero-length string + resp.message[0] = 0; // Ensuring that the message is treated as a zero-length string return resp; } void AirbourneBoardConfigManager::get_device_name(device_t device, char (&name)[DEVICE_NAME_LENGTH]) const { - switch(device) + switch (device) { case Configuration::SERIAL: strcpy(name, "Serial"); @@ -156,18 +159,21 @@ void AirbourneBoardConfigManager::get_device_name(device_t device, char (&name)[ break; } } -void AirbourneBoardConfigManager::get_config_name(device_t device, hardware_config_t config, char (&name)[CONFIG_NAME_LENGTH]) const +void AirbourneBoardConfigManager::get_config_name(device_t device, + hardware_config_t config, + char (&name)[CONFIG_NAME_LENGTH]) const { name[0] = 0; // Prevents a string without a terminator being sent in case of programmer error - if(device >= Configuration::DEVICE_COUNT) // Although this is handled in the switch statement, this is so that it doesn't attempt to overflow max_configs + if (device >= Configuration::DEVICE_COUNT) // Although this is handled in the switch statement, this is so that it + // doesn't attempt to overflow max_configs strcpy(name, "Invalid device"); - if(config > AirbourneBoardConfigManager::max_configs[device]) + if (config > AirbourneBoardConfigManager::max_configs[device]) strcpy(name, "Invalid config"); else - switch(device) + switch (device) { case Configuration::SERIAL: - switch(config) + switch (config) { case AirbourneConfiguration::SERIAL_VCP: strcpy(name, "VCP over USB"); @@ -184,19 +190,19 @@ void AirbourneBoardConfigManager::get_config_name(device_t device, hardware_conf } break; case Configuration::RC: - if(config==AirbourneConfiguration::RC_PPM) + if (config == AirbourneConfiguration::RC_PPM) strcpy(name, "PPM on Flex-IO"); - else if(config==AirbourneConfiguration::RC_SBUS) + else if (config == AirbourneConfiguration::RC_SBUS) strcpy(name, "SBUS on Main"); break; case Configuration::AIRSPEED: - if(config==AirbourneConfiguration::AIRSPEED_DISABLED) + if (config == AirbourneConfiguration::AIRSPEED_DISABLED) strcpy(name, "Disabled"); - else if(config == AirbourneConfiguration::AIRSPEED_I2C2) + else if (config == AirbourneConfiguration::AIRSPEED_I2C2) strcpy(name, "I2C2 on Flexi"); break; case Configuration::GNSS: - switch(config) + switch (config) { case AirbourneConfiguration::GNSS_DISABLED: strcpy(name, "Disabled"); @@ -213,27 +219,27 @@ void AirbourneBoardConfigManager::get_config_name(device_t device, hardware_conf } break; case Configuration::SONAR: - if(config ==AirbourneConfiguration::SONAR_DISABLED) + if (config == AirbourneConfiguration::SONAR_DISABLED) strcpy(name, "Disabled"); - else if(config == AirbourneConfiguration::SONAR_I2C2) + else if (config == AirbourneConfiguration::SONAR_I2C2) strcpy(name, "I2C2 on Flexi"); break; case Configuration::BATTERY_MONITOR: - if(config==AirbourneConfiguration::BATTERY_MONITOR_DISABLED) + if (config == AirbourneConfiguration::BATTERY_MONITOR_DISABLED) strcpy(name, "Disabled"); - else if(config==AirbourneConfiguration::BATTERY_MONITOR_ADC3) + else if (config == AirbourneConfiguration::BATTERY_MONITOR_ADC3) strcpy(name, "ADC3 on Power"); break; case Configuration::BAROMETER: - if(config==AirbourneConfiguration::BAROMETER_DISABLED) + if (config == AirbourneConfiguration::BAROMETER_DISABLED) strcpy(name, "Disabled"); - else if(config == AirbourneConfiguration::BAROMETER_ONBOARD) + else if (config == AirbourneConfiguration::BAROMETER_ONBOARD) strcpy(name, "Onboard baro"); break; case Configuration::MAGNETOMETER: - if(config ==AirbourneConfiguration::MAGNETOMETER_DISABLED) + if (config == AirbourneConfiguration::MAGNETOMETER_DISABLED) strcpy(name, "Disabled"); - else if(config == AirbourneConfiguration::MAGNETOMETER_ONBOARD) + else if (config == AirbourneConfiguration::MAGNETOMETER_ONBOARD) strcpy(name, "Onboard mag"); break; default: @@ -242,10 +248,10 @@ void AirbourneBoardConfigManager::get_config_name(device_t device, hardware_conf } AirbourneBoardConfigManager::Port AirbourneBoardConfigManager::get_port(uint8_t device, uint8_t config) const { - switch(device) + switch (device) { case Configuration::SERIAL: - switch(config) + switch (config) { case AirbourneConfiguration::SERIAL_VCP: return USB_PORT; @@ -257,7 +263,7 @@ AirbourneBoardConfigManager::Port AirbourneBoardConfigManager::get_port(uint8_t return FLEXI_PORT; } case Configuration::GNSS: - switch(config) + switch (config) { case AirbourneConfiguration::GNSS_DISABLED: return NO_PORT; @@ -269,31 +275,31 @@ AirbourneBoardConfigManager::Port AirbourneBoardConfigManager::get_port(uint8_t return FLEXI_PORT; } case Configuration::RC: - if(config == AirbourneConfiguration::RC_PPM) + if (config == AirbourneConfiguration::RC_PPM) return FLEX_IO_PORT; - if(config == AirbourneConfiguration::RC_SBUS) + if (config == AirbourneConfiguration::RC_SBUS) return MAIN_PORT; break; case Configuration::AIRSPEED: - if(config==AirbourneConfiguration::AIRSPEED_I2C2) + if (config == AirbourneConfiguration::AIRSPEED_I2C2) return FLEXI_PORT; break; case Configuration::SONAR: - if(config==AirbourneConfiguration::SONAR_I2C2) + if (config == AirbourneConfiguration::SONAR_I2C2) return FLEXI_PORT; break; case Configuration::BATTERY_MONITOR: - if(config == AirbourneConfiguration::BATTERY_MONITOR_ADC3) + if (config == AirbourneConfiguration::BATTERY_MONITOR_ADC3) return POWER_PORT; break; case Configuration::MAGNETOMETER: - if(config == AirbourneConfiguration::MAGNETOMETER_ONBOARD) + if (config == AirbourneConfiguration::MAGNETOMETER_ONBOARD) return INTERNAL_I2C; break; case Configuration::BAROMETER: - if(config == AirbourneConfiguration::BAROMETER_ONBOARD) + if (config == AirbourneConfiguration::BAROMETER_ONBOARD) return INTERNAL_I2C; } return NO_PORT; } -} //rosflight_firmware +} // namespace rosflight_firmware diff --git a/boards/airbourne/airbourne_board_config_manager.h b/boards/airbourne/airbourne_board_config_manager.h index 2bbc3aa6..23e60b4a 100644 --- a/boards/airbourne/airbourne_board_config_manager.h +++ b/boards/airbourne/airbourne_board_config_manager.h @@ -1,10 +1,11 @@ #ifndef AIRBOURNE_BOARD_CONFIG_MANAGER_H #define AIRBOURNE_BOARD_CONFIG_MANAGER_H -#include "board_config_manager.h" #include "airbourne_configuration_enum.h" +#include "board_config_manager.h" -namespace rosflight_firmware{ +namespace rosflight_firmware +{ class ROSflight; class AirbourneBoard; class AirbourneBoardConfigManager : public BoardConfigManager @@ -12,9 +13,12 @@ class AirbourneBoardConfigManager : public BoardConfigManager public: AirbourneBoardConfigManager(); hardware_config_t get_max_config(device_t device) const override; - ConfigManager::ConfigResponse check_config_change(device_t device, hardware_config_t config, const ConfigManager &cm) const override; + ConfigManager::ConfigResponse check_config_change(device_t device, + hardware_config_t config, + const ConfigManager &cm) const override; void get_device_name(device_t device, char (&name)[DEVICE_NAME_LENGTH]) const override; void get_config_name(device_t device, hardware_config_t config, char (&name)[CONFIG_NAME_LENGTH]) const override; + private: static constexpr hardware_config_t max_configs[Configuration::DEVICE_COUNT]{3, 1, 1, 3, 1, 1, 1, 1}; diff --git a/boards/airbourne/airbourne_configuration_enum.h b/boards/airbourne/airbourne_configuration_enum.h index 4cecfac7..117a8e72 100644 --- a/boards/airbourne/airbourne_configuration_enum.h +++ b/boards/airbourne/airbourne_configuration_enum.h @@ -5,7 +5,7 @@ namespace rosflight_firmware { namespace AirbourneConfiguration { -enum serial_config_t: uint8_t +enum serial_config_t : uint8_t { SERIAL_VCP, SERIAL_UART1, @@ -13,19 +13,19 @@ enum serial_config_t: uint8_t SERIAL_UART3 }; -enum rc_config_t: uint8_t +enum rc_config_t : uint8_t { RC_PPM, RC_SBUS }; -enum airspeed_config_t: uint8_t +enum airspeed_config_t : uint8_t { AIRSPEED_DISABLED, AIRSPEED_I2C2 }; -enum gnss_config_t: uint8_t +enum gnss_config_t : uint8_t { GNSS_DISABLED, GNSS_UART1, @@ -33,30 +33,30 @@ enum gnss_config_t: uint8_t GNSS_UART3 }; -enum sonar_config_t: uint8_t +enum sonar_config_t : uint8_t { SONAR_DISABLED, SONAR_I2C2 }; -enum battery_monitor_config_t: uint8_t +enum battery_monitor_config_t : uint8_t { BATTERY_MONITOR_DISABLED, BATTERY_MONITOR_ADC3 }; -enum barometer_config_t: uint8_t +enum barometer_config_t : uint8_t { BAROMETER_DISABLED, BAROMETER_ONBOARD }; -enum magnetometer_config_t: uint8_t +enum magnetometer_config_t : uint8_t { MAGNETOMETER_DISABLED, MAGNETOMETER_ONBOARD }; -} +} // namespace AirbourneConfiguration } // namespace rosflight_firmware #endif // AIRBOURNE_CONFIGURATION_ENUM_H diff --git a/boards/airbourne/main.cpp b/boards/airbourne/main.cpp index a56df3c6..74b3ef5c 100644 --- a/boards/airbourne/main.cpp +++ b/boards/airbourne/main.cpp @@ -30,22 +30,24 @@ */ #include "airbourne_board.h" -#include "rosflight.h" -#include "mavlink.h" #include "backup_sram.h" #include "board.h" +#include "mavlink.h" #include "state_manager.h" +#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) { - 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" { +extern "C" +{ /* The prototype shows it is a naked function - in effect this is just an assembly function. */ void HardFault_Handler(void) __attribute__((naked)); @@ -54,20 +56,18 @@ extern "C" { 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" - ); + __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) + 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 @@ -78,24 +78,24 @@ extern "C" { 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. */ + 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 ]; + r0 = pulFaultStackAddress[0]; + r1 = pulFaultStackAddress[1]; + r2 = pulFaultStackAddress[2]; + r3 = pulFaultStackAddress[3]; - r12 = pulFaultStackAddress[ 4 ]; - lr = pulFaultStackAddress[ 5 ]; - pc = pulFaultStackAddress[ 6 ]; - psr = pulFaultStackAddress[ 7 ]; + 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 }; + rosflight_firmware::StateManager::BackupData::DebugInfo debug = {r0, r1, r2, r3, r12, lr, pc, psr}; write_backup_data(debug); // reboot @@ -104,48 +104,55 @@ extern "C" { void NMI_Handler() { - while (1) {} + while (1) + { + } } - void MemManage_Handler() { - while (1) {} + while (1) + { + } } void BusFault_Handler() { - while (1) {} + while (1) + { + } } void UsageFault_Handler() { - while (1) {} + while (1) + { + } } void WWDG_IRQHandler() { - while (1) {} + while (1) + { + } } } // extern "C" - - int main(void) { - rosflight_firmware::AirbourneBoard board; - rosflight_firmware::Mavlink mavlink(board); - rosflight_firmware::ROSflight firmware(board, mavlink); + rosflight_firmware::AirbourneBoard board; + rosflight_firmware::Mavlink mavlink(board); + rosflight_firmware::ROSflight firmware(board, mavlink); - rosflight_ptr = &firmware; // this allows crashes to grab some info + rosflight_ptr = &firmware; // this allows crashes to grab some info - board.init_board(); - firmware.init(); + board.init_board(); + firmware.init(); - while (true) - { - firmware.run(); - } + while (true) + { + firmware.run(); + } - return 0; + return 0; } diff --git a/boards/breezy/breezy_board.cpp b/boards/breezy/breezy_board.cpp index 59148d5a..8ee3ce21 100644 --- a/boards/breezy/breezy_board.cpp +++ b/boards/breezy/breezy_board.cpp @@ -34,19 +34,16 @@ extern "C" { +#include "flash.h" #include -#include "flash.h" extern void SetSysClock(bool overclock); - } - #include "breezy_board.h" namespace rosflight_firmware { - BreezyBoard::BreezyBoard() {} void BreezyBoard::init_board() @@ -112,7 +109,7 @@ void BreezyBoard::serial_flush() bool BreezyBoard::enable_device(device_t device, hardware_config_t configuration, const Params ¶ms) { (void)configuration; - switch(device) + switch (device) { case Configuration::RC: rc_init(); @@ -139,9 +136,10 @@ void BreezyBoard::sensors_init() // Initialize I2c i2cInit(I2CDEV_2); - while (millis() < 50); + while (millis() < 50) + ; - i2cWrite(0,0,0); + i2cWrite(0, 0, 0); if (bmp280_init()) baro_type = BARO_BMP280; else if (ms5611_init()) @@ -151,11 +149,10 @@ void BreezyBoard::sensors_init() mb1242_init(); ms4525_init(); - // IMU uint16_t acc1G; mpu6050_init(true, &acc1G, &_gyro_scale, _board_revision); - _accel_scale = 9.80665f/acc1G; + _accel_scale = 9.80665f / acc1G; } uint16_t BreezyBoard::num_sensor_errors() @@ -182,13 +179,14 @@ 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) { return false; } - else return true; + else + return true; } void BreezyBoard::imu_not_responding_error() @@ -227,8 +225,6 @@ void BreezyBoard::baro_update() } } - - void BreezyBoard::baro_read(float *pressure, float *temperature) { if (baro_type == BARO_BMP280) @@ -377,7 +373,7 @@ void BreezyBoard::pwm_disable() float BreezyBoard::rc_read(uint8_t channel) { - return (float)(pwmRead(channel) - 1000)/1000.0; + return (float)(pwmRead(channel) - 1000) / 1000.0; } void BreezyBoard::pwm_write(uint8_t channel, float value) @@ -407,19 +403,19 @@ bool BreezyBoard::memory_write(const void *src, size_t len) return writeEEPROM(src, len); } -//GNSS is not supported on breezy boards +// GNSS is not supported on breezy boards GNSSData BreezyBoard::gnss_read() { return {}; } -//GNSS is not supported on breezy boards +// GNSS is not supported on breezy boards GNSSRaw BreezyBoard::gnss_raw_read() { return {}; } -//GNSS is not supported on breezy boards +// GNSS is not supported on breezy boards bool BreezyBoard::gnss_has_new_data() { return false; @@ -457,6 +453,6 @@ void BreezyBoard::led1_toggle() LED1_TOGGLE; } -} +} // namespace rosflight_firmware #pragma GCC diagnostic pop diff --git a/boards/breezy/breezy_board.h b/boards/breezy/breezy_board.h index bc289396..55beafa7 100644 --- a/boards/breezy/breezy_board.h +++ b/boards/breezy/breezy_board.h @@ -32,8 +32,8 @@ #ifndef ROSFLIGHT_FIRMWARE_BREEZY_BOARD_H #define ROSFLIGHT_FIRMWARE_BREEZY_BOARD_H -#include #include +#include #include extern "C" @@ -42,16 +42,14 @@ extern "C" } #include "board.h" -#include "sensors.h" -#include "configuration_enum.h" #include "breezy_board_config_manager.h" +#include "configuration_enum.h" +#include "sensors.h" namespace rosflight_firmware { - class BreezyBoard : public Board { - private: serialPort_t *Serial1; @@ -104,7 +102,7 @@ class BreezyBoard : public Board // hardware config bool enable_device(device_t device, hardware_config_t configuration, const Params ¶ms) override; - const BreezyBoardConfigManager & get_board_config_manager() const override; + const BreezyBoardConfigManager &get_board_config_manager() const override; // sensors void sensors_init() override; @@ -130,15 +128,9 @@ class BreezyBoard : public Board void sonar_update() override; float sonar_read() override; - bool gnss_present() override - { - return false; - } + bool gnss_present() override { return false; } - void gnss_update() override - { - return; - } + void gnss_update() override { return; } bool battery_voltage_present() const override; float battery_voltage_read() const override; void battery_voltage_set_multiplier(double multiplier) override; @@ -151,7 +143,6 @@ class BreezyBoard : public Board bool gnss_has_new_data() override; GNSSRaw gnss_raw_read() override; - // PWM // TODO make these deal in normalized (-1 to 1 or 0 to 1) values (not pwm-specific) void rc_init(); @@ -178,8 +169,17 @@ class BreezyBoard : public Board // Backup Data void backup_memory_init() override {} - bool backup_memory_read(void *dest, size_t len) override { (void)dest; (void)len; return false; } - void backup_memory_write(const void *src, size_t len) override { (void)src; (void)len; } + bool backup_memory_read(void *dest, size_t len) override + { + (void)dest; + (void)len; + return false; + } + void backup_memory_write(const void *src, size_t len) override + { + (void)src; + (void)len; + } void backup_memory_clear(size_t len) override { (void)len; } }; diff --git a/boards/breezy/breezy_board_config_manager.cpp b/boards/breezy/breezy_board_config_manager.cpp index 48b77c86..e47279e2 100644 --- a/boards/breezy/breezy_board_config_manager.cpp +++ b/boards/breezy/breezy_board_config_manager.cpp @@ -1,4 +1,5 @@ #include "breezy_board_config_manager.h" + #include namespace rosflight_firmware @@ -9,7 +10,9 @@ hardware_config_t BreezyBoardConfigManager::get_max_config(device_t device) cons return 0; } -ConfigManager::ConfigResponse BreezyBoardConfigManager::check_config_change(device_t device, hardware_config_t config, const ConfigManager &cm) const +ConfigManager::ConfigResponse BreezyBoardConfigManager::check_config_change(device_t device, + hardware_config_t config, + const ConfigManager &cm) const { (void)device; (void)config; @@ -17,17 +20,20 @@ ConfigManager::ConfigResponse BreezyBoardConfigManager::check_config_change(devi ConfigManager::ConfigResponse response; response.successful = false; response.reboot_required = false; - strcpy(reinterpret_cast(response.message), "Feature unsupported on naze"); + strcpy(reinterpret_cast(response.message), "Feature unsupported on naze"); return response; } -void BreezyBoardConfigManager::get_device_name(device_t device, char (&name)[BoardConfigManager::DEVICE_NAME_LENGTH]) const +void BreezyBoardConfigManager::get_device_name(device_t device, + char (&name)[BoardConfigManager::DEVICE_NAME_LENGTH]) const { (void)device; strcpy(name, "Unsupported"); } -void BreezyBoardConfigManager::get_config_name(device_t device, hardware_config_t config, char (&name)[BoardConfigManager::CONFIG_NAME_LENGTH]) const +void BreezyBoardConfigManager::get_config_name(device_t device, + hardware_config_t config, + char (&name)[BoardConfigManager::CONFIG_NAME_LENGTH]) const { (void)device; (void)config; diff --git a/boards/breezy/breezy_board_config_manager.h b/boards/breezy/breezy_board_config_manager.h index f375041a..88616f73 100644 --- a/boards/breezy/breezy_board_config_manager.h +++ b/boards/breezy/breezy_board_config_manager.h @@ -9,9 +9,13 @@ class BreezyBoardConfigManager : public BoardConfigManager { public: hardware_config_t get_max_config(device_t device) const override; - ConfigManager::ConfigResponse check_config_change(device_t device, hardware_config_t config, const ConfigManager &cm) const override; + ConfigManager::ConfigResponse check_config_change(device_t device, + hardware_config_t config, + const ConfigManager &cm) const override; void get_device_name(device_t device, char (&name)[BoardConfigManager::DEVICE_NAME_LENGTH]) const override; - void get_config_name(device_t device, hardware_config_t config, char (&name)[BoardConfigManager::CONFIG_NAME_LENGTH]) const override; + void get_config_name(device_t device, + hardware_config_t config, + char (&name)[BoardConfigManager::CONFIG_NAME_LENGTH]) const override; }; } // namespace rosflight_firmware diff --git a/boards/breezy/flash.h b/boards/breezy/flash.h index 0654be88..4163d963 100644 --- a/boards/breezy/flash.h +++ b/boards/breezy/flash.h @@ -30,12 +30,11 @@ */ #pragma once -#include +#include #include +#include #include -#include - // #define ASSERT_CONCAT_(a, b) a##b // #define ASSERT_CONCAT(a, b) ASSERT_CONCAT_(a, b) // #define ct_assert(e) enum { ASSERT_CONCAT(assert_line_, __LINE__) = 1/(!!(e)) } @@ -45,18 +44,17 @@ #define FLASH_PAGE_COUNT 128 #endif -#define FLASH_PAGE_SIZE ((uint16_t)0x400) -#define NUM_PAGES 3 +#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. // TODO compile time check is currently disabled -#define CONFIG_SIZE (FLASH_PAGE_SIZE * NUM_PAGES) +#define CONFIG_SIZE (FLASH_PAGE_SIZE * NUM_PAGES) // static const uint8_t EEPROM_CONF_VERSION = 76; -//static uint32_t enabledSensors = 0; -//static void resetConf(void); +// 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))); - /** * @brief Initialize Flash */ diff --git a/boards/breezy/main.cpp b/boards/breezy/main.cpp index f9b1fd65..0f6835c7 100644 --- a/boards/breezy/main.cpp +++ b/boards/breezy/main.cpp @@ -30,9 +30,10 @@ */ #include "breezy_board.h" -#include "rosflight.h" #include "mavlink.h" +#include "rosflight.h" + int main() { rosflight_firmware::BreezyBoard board; diff --git a/comms/mavlink/mavlink.cpp b/comms/mavlink/mavlink.cpp index b1cf1298..d7155814 100644 --- a/comms/mavlink/mavlink.cpp +++ b/comms/mavlink/mavlink.cpp @@ -28,18 +28,15 @@ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#include +#include "mavlink.h" #include "board.h" -#include "mavlink.h" +#include namespace rosflight_firmware { - -Mavlink::Mavlink(Board &board) : - board_(board) -{} +Mavlink::Mavlink(Board &board) : board_(board) {} void Mavlink::init() { @@ -61,14 +58,8 @@ void Mavlink::send_attitude_quaternion(uint8_t system_id, 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, + 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); } @@ -124,8 +115,8 @@ void Mavlink::send_command_ack(uint8_t system_id, Command command, bool success) } mavlink_message_t msg; - mavlink_msg_rosflight_cmd_ack_pack(system_id, compid_, &msg, - rosflight_cmd, (success) ? ROSFLIGHT_CMD_SUCCESS : ROSFLIGHT_CMD_FAILED); + mavlink_msg_rosflight_cmd_ack_pack(system_id, compid_, &msg, rosflight_cmd, + (success) ? ROSFLIGHT_CMD_SUCCESS : ROSFLIGHT_CMD_FAILED); send_message(msg); } @@ -139,47 +130,36 @@ 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, +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, + 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_raw(uint8_t system_id, const GNSSRaw& raw) +void Mavlink::send_gnss_raw(uint8_t system_id, const GNSSRaw &raw) { mavlink_message_t msg; - mavlink_rosflight_gnss_raw_t data= {}; + mavlink_rosflight_gnss_raw_t data = {}; data.time_of_week = raw.time_of_week; data.year = raw.year; data.month = raw.month; @@ -295,50 +275,57 @@ void Mavlink::send_config_value(uint8_t system_id, uint8_t device, uint8_t confi send_message(msg); } -void Mavlink::send_device_info(uint8_t system_id, uint8_t device, uint8_t max_config, char (&name)[BoardConfigManager::DEVICE_NAME_LENGTH], uint8_t num_devices) +void Mavlink::send_device_info(uint8_t system_id, + uint8_t device, + uint8_t max_config, + char (&name)[BoardConfigManager::DEVICE_NAME_LENGTH], + uint8_t num_devices) { mavlink_message_t msg; - mavlink_msg_rosflight_device_info_pack(system_id, 0, &msg, device, max_config, reinterpret_cast(name), num_devices); + mavlink_msg_rosflight_device_info_pack(system_id, 0, &msg, device, max_config, reinterpret_cast(name), + num_devices); send_message(msg); } -void Mavlink::send_config_info(uint8_t system_id, uint8_t device, uint8_t config, char (&name)[BoardConfigManager::CONFIG_NAME_LENGTH]) +void Mavlink::send_config_info(uint8_t system_id, + uint8_t device, + uint8_t config, + char (&name)[BoardConfigManager::CONFIG_NAME_LENGTH]) { mavlink_message_t msg; - mavlink_msg_rosflight_config_info_pack(system_id, 0, &msg, device, config, reinterpret_cast(name)); + mavlink_msg_rosflight_config_info_pack(system_id, 0, &msg, device, config, reinterpret_cast(name)); send_message(msg); } -void Mavlink::send_config_status(uint8_t system_id, uint8_t device, bool success, bool reboot_required, char (&error_message)[ConfigManager::CONFIG_RESPONSE_MESSAGE_LENGTH]) +void Mavlink::send_config_status(uint8_t system_id, + uint8_t device, + bool success, + bool reboot_required, + char (&error_message)[ConfigManager::CONFIG_RESPONSE_MESSAGE_LENGTH]) { mavlink_message_t msg; - mavlink_msg_rosflight_config_status_pack(system_id, 0, &msg, device, success, reboot_required, reinterpret_cast(error_message)); + mavlink_msg_rosflight_config_status_pack(system_id, 0, &msg, device, success, reboot_required, + reinterpret_cast(error_message)); 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, +void Mavlink::send_sonar(uint8_t system_id, + /* 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); } @@ -353,15 +340,8 @@ void Mavlink::send_status(uint8_t system_id, 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); } @@ -382,7 +362,7 @@ void Mavlink::send_error_data(uint8_t system_id, const StateManager::BackupData { 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, + 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); } @@ -598,7 +578,7 @@ void Mavlink::handle_msg_external_attitude(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 + // none of the information from the heartbeat is used (void)msg; if (listener_ != nullptr) @@ -611,7 +591,7 @@ void Mavlink::handle_msg_config(const mavlink_message_t *const msg) mavlink_msg_rosflight_config_decode(msg, &config_msg); uint8_t device = config_msg.device; uint8_t config = config_msg.config; - if(listener_ != nullptr) + if (listener_ != nullptr) listener_->config_set_callback(device, config); } @@ -620,7 +600,7 @@ void Mavlink::handle_msg_config_request(const mavlink_message_t *const msg) mavlink_rosflight_config_request_t request_msg; mavlink_msg_rosflight_config_request_decode(msg, &request_msg); uint8_t device = request_msg.device; - if(listener_ != nullptr) + if (listener_ != nullptr) listener_->config_request_callback(device); } diff --git a/comms/mavlink/mavlink.h b/comms/mavlink/mavlink.h index de54e8ea..7bc1ba5d 100644 --- a/comms/mavlink/mavlink.h +++ b/comms/mavlink/mavlink.h @@ -40,20 +40,20 @@ #pragma GCC diagnostic ignored "-Wpragmas" #pragma GCC diagnostic ignored "-Waddress-of-packed-member" #include "v1.0/rosflight/mavlink.h" -# pragma GCC diagnostic pop +#pragma GCC diagnostic pop #include "interface/comm_link.h" + #include "board.h" namespace rosflight_firmware { - class Board; class Mavlink : public CommLinkInterface { public: - Mavlink(Board& board); + Mavlink(Board &board); void init() override; void receive() override; @@ -65,14 +65,15 @@ class Mavlink : public CommLinkInterface 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_heartbeat(uint8_t system_id, bool fixed_wing) override; - void send_imu(uint8_t system_id, uint64_t timestamp_us, + 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_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_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, @@ -85,11 +86,26 @@ class Mavlink : public CommLinkInterface float value, uint16_t param_count) override; void send_config_value(uint8_t system_id, uint8_t device, uint8_t config) override; - void send_device_info(uint8_t system_id, uint8_t device, uint8_t max_config, char (&name)[BoardConfigManager::DEVICE_NAME_LENGTH], uint8_t num_devices) override; - void send_config_info(uint8_t system_id, uint8_t device, uint8_t config, char (&name)[BoardConfigManager::CONFIG_NAME_LENGTH]) override; - void send_config_status(uint8_t system_id, uint8_t device, bool success, bool reboot_required, char (&error_message)[ConfigManager::CONFIG_RESPONSE_MESSAGE_LENGTH]) override; + void send_device_info(uint8_t system_id, + uint8_t device, + uint8_t max_config, + char (&name)[BoardConfigManager::DEVICE_NAME_LENGTH], + uint8_t num_devices) override; + void send_config_info(uint8_t system_id, + uint8_t device, + uint8_t config, + char (&name)[BoardConfigManager::CONFIG_NAME_LENGTH]) override; + void send_config_status(uint8_t system_id, + uint8_t device, + bool success, + bool reboot_required, + char (&error_message)[ConfigManager::CONFIG_RESPONSE_MESSAGE_LENGTH]) 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, float min_range) override; + void send_sonar(uint8_t system_id, + /* 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, @@ -100,13 +116,13 @@ class Mavlink : public CommLinkInterface 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_raw(uint8_t system_id, const GNSSRaw& data) 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_raw(uint8_t system_id, const GNSSRaw &data) 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); @@ -119,19 +135,19 @@ class Mavlink : public CommLinkInterface 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_heartbeat(const mavlink_message_t *const msg); void handle_msg_config(const mavlink_message_t *const msg); void handle_msg_config_request(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 a2484c87..16b1b3d2 100644 --- a/include/board.h +++ b/include/board.h @@ -32,22 +32,20 @@ #ifndef ROSFLIGHT_FIRMWARE_BOARD_H #define ROSFLIGHT_FIRMWARE_BOARD_H -#include -#include -#include - -#include "sensors.h" -#include "state_manager.h" -#include "configuration_enum.h" #include "board_config_manager.h" +#include "configuration_enum.h" #include "param.h" +#include "sensors.h" +#include "state_manager.h" + +#include +#include +#include namespace rosflight_firmware { - class Board { - public: typedef enum { @@ -55,28 +53,28 @@ class Board RC_TYPE_SBUS = 1 } rc_type_t; -// setup + // setup virtual void init_board() = 0; virtual void board_reset(bool bootloader) = 0; -// clock + // clock virtual uint32_t clock_millis() = 0; virtual uint64_t clock_micros() = 0; virtual void clock_delay(uint32_t milliseconds) = 0; -// serial + // serial 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; -// hardware config - virtual bool enable_device(device_t device, hardware_config_t configuration, const Params ¶ms)=0; + // hardware config + virtual bool enable_device(device_t device, hardware_config_t configuration, const Params ¶ms) = 0; virtual const BoardConfigManager &get_board_config_manager() const = 0; -// sensors + // sensors virtual void sensors_init() = 0; - virtual uint16_t num_sensor_errors() = 0; + 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; @@ -113,21 +111,21 @@ class Board virtual float battery_current_read() const = 0; virtual void battery_current_set_multiplier(double multiplier) = 0; -// RC + // RC virtual bool rc_lost() = 0; virtual float rc_read(uint8_t channel) = 0; -// PWM - virtual void pwm_init(uint32_t refresh_rate, uint16_t idle_pwm) = 0; + // PWM + virtual void pwm_init(uint32_t refresh_rate, uint16_t idle_pwm) = 0; virtual void pwm_disable() = 0; virtual void pwm_write(uint8_t channel, float value) = 0; -// non-volatile memory + // 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; -// LEDs + // LEDs virtual void led0_on() = 0; virtual void led0_off() = 0; virtual void led0_toggle() = 0; @@ -136,12 +134,11 @@ class Board virtual void led1_off() = 0; virtual void led1_toggle() = 0; -// Backup memory + // 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 void backup_memory_clear(size_t len) = 0; - }; } // namespace rosflight_firmware diff --git a/include/board_config_manager.h b/include/board_config_manager.h index 3753442c..db8ce331 100644 --- a/include/board_config_manager.h +++ b/include/board_config_manager.h @@ -2,8 +2,8 @@ #define BOARD_CONFIG_MANAGER_H -#include "configuration_enum.h" #include "config_manager.h" +#include "configuration_enum.h" namespace rosflight_firmware { @@ -15,7 +15,6 @@ namespace rosflight_firmware class BoardConfigManager { public: - /** * @brief Get the largest number that is valid for the configuration of a given device * @details This number is inclusive, i.e. a value of 2 means 0, 1, or 2 are valid @@ -32,9 +31,13 @@ class BoardConfigManager * @param config The new configuration for the device * @param cm The ConfigManager with the current configurations */ - virtual ConfigManager::ConfigResponse check_config_change(device_t device, hardware_config_t config, const ConfigManager &cm) const = 0; - static constexpr int DEVICE_NAME_LENGTH = 20; /**< This includes the null terminator, so 19 is the practical maximum.*/ - static constexpr int CONFIG_NAME_LENGTH = 20; /**< This includes the null terminator, so 19 is the practical maximum.*/ + virtual ConfigManager::ConfigResponse check_config_change(device_t device, + hardware_config_t config, + const ConfigManager &cm) const = 0; + static constexpr int DEVICE_NAME_LENGTH = + 20; /**< This includes the null terminator, so 19 is the practical maximum.*/ + static constexpr int CONFIG_NAME_LENGTH = + 20; /**< This includes the null terminator, so 19 is the practical maximum.*/ /** * @brief Returns the name of the device * @details Do not assume that the device number is valid. @@ -42,7 +45,7 @@ class BoardConfigManager * Note the requirement for a null terminator at the end of the string. * @param device Any device */ - virtual void get_device_name(device_t device, char (&name)[DEVICE_NAME_LENGTH]) const=0; + virtual void get_device_name(device_t device, char (&name)[DEVICE_NAME_LENGTH]) const = 0; /** * @brief Returns the name of a configuration * @details Do not assume that the device number or configuration are valid. diff --git a/include/comm_manager.h b/include/comm_manager.h index 2269f6b2..1dbef25f 100644 --- a/include/comm_manager.h +++ b/include/comm_manager.h @@ -32,17 +32,16 @@ #ifndef ROSFLIGHT_FIRMWARE_COMM_MANAGER_H #define ROSFLIGHT_FIRMWARE_COMM_MANAGER_H -#include -#include - #include "interface/comm_link.h" #include "interface/param_listener.h" #include "nanoprintf.h" +#include +#include + namespace rosflight_firmware { - class ROSflight; class CommManager : public CommLinkInterface::ListenerInterface, public ParamListenerInterface @@ -88,7 +87,6 @@ class CommManager : public CommLinkInterface::ListenerInterface, public ParamLis bool initialized_ = false; bool connected_ = false; - static constexpr int LOG_MSG_SIZE = 50; class LogMessageBuffer { @@ -141,8 +139,8 @@ class CommManager : public CommLinkInterface::ListenerInterface, public ParamLis 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 aux_command_callback(const CommLinkInterface::AuxCommand& command) override; + void external_attitude_callback(const turbomath::Quaternion& q) override; void heartbeat_callback() override; void config_set_callback(uint8_t device, uint8_t configuration) override; void config_request_callback(uint8_t device) override; @@ -163,35 +161,26 @@ 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_command_struct(const char *const name, control_t command_struct); + 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); void send_next_config_info(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_raw();}), - Stream(0, [this]{this->send_rc_raw();}), - Stream(20000, [this]{this->send_low_priority();}) - }; + 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_raw(); }), + 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_raw_tow_ = 0; public: - CommManager(ROSflight& rf, CommLinkInterface& comm_link); void init(); @@ -205,12 +194,12 @@ class CommManager : public CommLinkInterface::ListenerInterface, public ParamLis void send_config_info(device_t device, hardware_config_t config); 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 5e98b428..a5db41da 100644 --- a/include/command_manager.h +++ b/include/command_manager.h @@ -32,31 +32,30 @@ #ifndef ROSFLIGHT_FIRMWARE_COMMAND_MANAGER_H #define ROSFLIGHT_FIRMWARE_COMMAND_MANAGER_H -#include -#include - #include "interface/param_listener.h" #include "rc.h" +#include +#include + namespace rosflight_firmware { - class ROSflight; typedef enum { - RATE, // Channel is is in rate mode (mrad/s) - ANGLE, // Channel command is in angle mode (mrad) - THROTTLE, // Channel is direcly controlling throttle max/1000 - PASSTHROUGH, // Channel directly passes PWM input to the mixer + RATE, // Channel is is in rate mode (mrad/s) + ANGLE, // Channel command is in angle mode (mrad) + THROTTLE, // Channel is direcly controlling throttle max/1000 + PASSTHROUGH, // Channel directly passes PWM input to the mixer } control_type_t; typedef struct { - bool active; // Whether or not the channel is active - control_type_t type; // What type the channel is - float value; // The value of the channel + bool active; // Whether or not the channel is active + control_type_t type; // What type the channel is + float value; // The value of the channel } control_channel_t; typedef struct @@ -70,9 +69,7 @@ typedef struct class CommandManager : public ParamListenerInterface { - private: - typedef struct { control_channel_t *rc; @@ -80,55 +77,33 @@ class CommandManager : public ParamListenerInterface control_channel_t *combined; } mux_t; - mux_t muxes[4] = - { - {&rc_command_.x, &offboard_command_.x, &combined_command_.x}, - {&rc_command_.y, &offboard_command_.y, &combined_command_.y}, - {&rc_command_.z, &offboard_command_.z, &combined_command_.z}, - {&rc_command_.F, &offboard_command_.F, &combined_command_.F} - }; - - control_t rc_command_ = - { - 0, - {false, ANGLE, 0.0}, - {false, ANGLE, 0.0}, - {false, RATE, 0.0}, - {false, THROTTLE, 0.0} - }; - control_t offboard_command_ = - { - 0, - {false, ANGLE, 0.0}, - {false, ANGLE, 0.0}, - {false, RATE, 0.0}, - {false, THROTTLE, 0.0} - }; - control_t combined_command_ = - { - 0, - {false, ANGLE, 0.0}, - {false, ANGLE, 0.0}, - {false, RATE, 0.0}, - {false, THROTTLE, 0.0} - }; - - control_t multirotor_failsafe_command_ = - { - 0, - {true, ANGLE, 0.0}, - {true, ANGLE, 0.0}, - {true, RATE, 0.0}, - {true, THROTTLE, 0.0} - }; - control_t fixedwing_failsafe_command_ = - { - 0, - {true, PASSTHROUGH, 0.0}, - {true, PASSTHROUGH, 0.0}, - {true, PASSTHROUGH, 0.0}, - {true, THROTTLE, 0.0} - }; + mux_t muxes[4] = {{&rc_command_.x, &offboard_command_.x, &combined_command_.x}, + {&rc_command_.y, &offboard_command_.y, &combined_command_.y}, + {&rc_command_.z, &offboard_command_.z, &combined_command_.z}, + {&rc_command_.F, &offboard_command_.F, &combined_command_.F}}; + + control_t rc_command_ = {0, {false, ANGLE, 0.0}, {false, ANGLE, 0.0}, {false, RATE, 0.0}, {false, THROTTLE, 0.0}}; + control_t offboard_command_ = {0, + {false, ANGLE, 0.0}, + {false, ANGLE, 0.0}, + {false, RATE, 0.0}, + {false, THROTTLE, 0.0}}; + control_t combined_command_ = {0, + {false, ANGLE, 0.0}, + {false, ANGLE, 0.0}, + {false, RATE, 0.0}, + {false, THROTTLE, 0.0}}; + + control_t multirotor_failsafe_command_ = {0, + {true, ANGLE, 0.0}, + {true, ANGLE, 0.0}, + {true, RATE, 0.0}, + {true, THROTTLE, 0.0}}; + control_t fixedwing_failsafe_command_ = {0, + {true, PASSTHROUGH, 0.0}, + {true, PASSTHROUGH, 0.0}, + {true, PASSTHROUGH, 0.0}, + {true, THROTTLE, 0.0}}; typedef enum { @@ -143,7 +118,7 @@ class CommandManager : public ParamListenerInterface MUX_Z, MUX_F, }; - enum RCOverrideReason: uint16_t + enum RCOverrideReason : uint16_t { OVERRIDE_NO_OVERRIDE = 0x0, OVERRIDE_ATT_SWITCH = 0x1, @@ -171,12 +146,12 @@ class CommandManager : public ParamListenerInterface uint16_t override_mask; } channel_override_t; - channel_override_t channel_override_[4] = - { - { RC::STICK_X, 0, OVERRIDE_X, OVERRIDE_OFFBOARD_X_INACTIVE, X_OVERRIDDEN}, - { RC::STICK_Y, 0, OVERRIDE_Y, OVERRIDE_OFFBOARD_Y_INACTIVE, Y_OVERRIDDEN}, - { RC::STICK_Z, 0, OVERRIDE_Z, OVERRIDE_OFFBOARD_Z_INACTIVE, Z_OVERRIDDEN}, - { RC::STICK_F, 0, OVERRIDE_T, OVERRIDE_OFFBOARD_T_INACTIVE, T_OVERRIDDEN} // Note that throttle overriding works a bit differently + channel_override_t channel_override_[4] = { + {RC::STICK_X, 0, OVERRIDE_X, OVERRIDE_OFFBOARD_X_INACTIVE, X_OVERRIDDEN}, + {RC::STICK_Y, 0, OVERRIDE_Y, OVERRIDE_OFFBOARD_Y_INACTIVE, Y_OVERRIDDEN}, + {RC::STICK_Z, 0, OVERRIDE_Z, OVERRIDE_OFFBOARD_Z_INACTIVE, Z_OVERRIDDEN}, + {RC::STICK_F, 0, OVERRIDE_T, OVERRIDE_OFFBOARD_T_INACTIVE, + T_OVERRIDDEN} // Note that throttle overriding works a bit differently }; ROSflight &RF_; @@ -208,11 +183,10 @@ class CommandManager : public ParamListenerInterface bool stick_deviated(MuxChannel channel); public: - CommandManager(ROSflight &_rf); void init(); bool run(); - /** + /** * @brief Checks which channels are overridden, and why * @details There are many reasons that a channel could be overriden. These reasons include: * - A stick is deflected diff --git a/include/config_manager.h b/include/config_manager.h index d58a07aa..1058804d 100644 --- a/include/config_manager.h +++ b/include/config_manager.h @@ -15,7 +15,7 @@ class ROSflight; class ConfigManager { public: - struct __attribute__ ((packed)) Config + struct __attribute__((packed)) Config { uint32_t checksum; hardware_config_t config[Configuration::DEVICE_COUNT]; @@ -27,8 +27,8 @@ class ConfigManager */ struct ConfigResponse { - bool successful; /**< If the change was successfully made **/ - bool reboot_required; /**< If a reboot is required for the change to take effect */ + bool successful; /**< If the change was successfully made **/ + bool reboot_required; /**< If a reboot is required for the change to take effect */ char message[CONFIG_RESPONSE_MESSAGE_LENGTH]; /**< An optional message, often an error message */ }; @@ -81,10 +81,9 @@ class ConfigManager // Sets a config without checks. This may cause an invalid configuration combo, // so attempt_set_configuration is recommended void set_configuration(device_t device, uint8_t config); - bool read(); // currently just checks that the memory manager is ready and the checksum is correct + bool read(); // currently just checks that the memory manager is ready and the checksum is correct void fill_defaults(); // Default values are 0, by convention uint32_t generate_checksum() const; // Based off of fletcher algorithm - }; -} +} // namespace rosflight_firmware #endif // HARDWARE_CONFIG_H diff --git a/include/configuration_enum.h b/include/configuration_enum.h index ff72d58b..44420af1 100644 --- a/include/configuration_enum.h +++ b/include/configuration_enum.h @@ -13,11 +13,10 @@ namespace rosflight_firmware */ namespace Configuration { - /** * @brief An enumeration of configurable devices */ -enum device_t: uint8_t +enum device_t : uint8_t { SERIAL, RC, @@ -34,7 +33,7 @@ constexpr device_t FIRST_DEVICE{static_cast(0)}; /** * @brief Allows incrementing device_t's for use in for loops. Stops incrementing past DEVICE_COUNT */ -inline device_t& operator++(device_t &dev) +inline device_t& operator++(device_t& dev) { uint8_t return_value = dev; return_value++; @@ -44,9 +43,9 @@ inline device_t& operator++(device_t &dev) return dev; } -} +} // namespace Configuration typedef uint8_t hardware_config_t; typedef Configuration::device_t device_t; /**< typedef'd for your convenience */ -} +} // namespace rosflight_firmware #endif // CONFIGURATION_ENUM_H diff --git a/include/controller.h b/include/controller.h index a7f77f8c..2d139661 100644 --- a/include/controller.h +++ b/include/controller.h @@ -32,19 +32,18 @@ #ifndef ROSFLIGHT_FIRMWARE_CONTROLLER_H #define ROSFLIGHT_FIRMWARE_CONTROLLER_H -#include -#include - -#include - #include "interface/param_listener.h" #include "command_manager.h" #include "estimator.h" +#include + +#include +#include + namespace rosflight_firmware { - class ROSflight; class Controller : public ParamListenerInterface diff --git a/include/estimator.h b/include/estimator.h index a5341e99..68d2bf79 100644 --- a/include/estimator.h +++ b/include/estimator.h @@ -32,22 +32,20 @@ #ifndef ROSFLIGHT_FIRMWARE_ESTIMATOR_H #define ROSFLIGHT_FIRMWARE_ESTIMATOR_H -#include -#include -#include +#include "interface/param_listener.h" #include -#include "interface/param_listener.h" +#include +#include +#include namespace rosflight_firmware { - class ROSflight; class Estimator : public ParamListenerInterface { - public: struct State { @@ -59,36 +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_; @@ -115,10 +104,11 @@ 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 d477b5d1..861d80d5 100644 --- a/include/interface/comm_link.h +++ b/include/interface/comm_link.h @@ -32,22 +32,20 @@ #ifndef ROSFLIGHT_FIRMWARE_COMM_LINK_H #define ROSFLIGHT_FIRMWARE_COMM_LINK_H -#include - -#include - -#include "param.h" #include "board.h" +#include "param.h" #include "sensors.h" #include "state_manager.h" +#include + +#include + namespace rosflight_firmware { - class CommLinkInterface { public: - enum class LogSeverity { LOG_INFO, @@ -112,80 +110,103 @@ class CommLinkInterface AuxChannel cmd_array[14]; }; - class ListenerInterface - { - 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, 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 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 heartbeat_callback() = 0; - virtual void config_set_callback(uint8_t device, uint8_t configuration) = 0; - virtual void config_request_callback(uint8_t device) = 0; - }; + class ListenerInterface + { + 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, + 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 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 heartbeat_callback() = 0; + virtual void config_set_callback(uint8_t device, uint8_t configuration) = 0; + virtual void config_request_callback(uint8_t device) = 0; + }; - virtual void init() = 0; - virtual void receive() = 0; - - // 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_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_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, + virtual void init() = 0; + virtual void receive() = 0; + + // 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_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_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, - int32_t value, + float 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_config_value(uint8_t system_id, uint8_t device, uint8_t config) = 0; - virtual void send_device_info(uint8_t system_id, uint8_t device, uint8_t max_config, char (&name)[BoardConfigManager::DEVICE_NAME_LENGTH], uint8_t num_devices) = 0; - virtual void send_config_info(uint8_t system_id, uint8_t device, uint8_t config, char (&name)[BoardConfigManager::CONFIG_NAME_LENGTH]) = 0; - virtual void send_config_status(uint8_t system_id, uint8_t device, bool success, bool reboot_required, char (&error_message)[ConfigManager::CONFIG_RESPONSE_MESSAGE_LENGTH]) = 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, float min_range) = 0; - virtual void send_status(uint8_t system_id, - bool armed, - bool failsafe, - uint16_t 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_raw(uint8_t system_id, const GNSSRaw &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 send_config_value(uint8_t system_id, uint8_t device, uint8_t config) = 0; + virtual void send_device_info(uint8_t system_id, + uint8_t device, + uint8_t max_config, + char (&name)[BoardConfigManager::DEVICE_NAME_LENGTH], + uint8_t num_devices) = 0; + virtual void send_config_info(uint8_t system_id, + uint8_t device, + uint8_t config, + char (&name)[BoardConfigManager::CONFIG_NAME_LENGTH]) = 0; + virtual void send_config_status(uint8_t system_id, + uint8_t device, + bool success, + bool reboot_required, + char (&error_message)[ConfigManager::CONFIG_RESPONSE_MESSAGE_LENGTH]) = 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, + float min_range) = 0; + virtual void send_status(uint8_t system_id, + bool armed, + bool failsafe, + uint16_t 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_raw(uint8_t system_id, const GNSSRaw &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; }; } // namespace rosflight_firmware diff --git a/include/interface/param_listener.h b/include/interface/param_listener.h index 7b8460a5..ffa5cb40 100644 --- a/include/interface/param_listener.h +++ b/include/interface/param_listener.h @@ -36,7 +36,6 @@ namespace rosflight_firmware { - class ParamListenerInterface { public: diff --git a/include/memory_manager.h b/include/memory_manager.h index a63550e4..ad55a970 100644 --- a/include/memory_manager.h +++ b/include/memory_manager.h @@ -1,8 +1,8 @@ #ifndef MEMORY_MANAGER_H #define MEMORY_MANAGER_H -#include "param.h" #include "config_manager.h" +#include "param.h" namespace rosflight_firmware { @@ -32,24 +32,23 @@ class MemoryManager * @brief Checks if memory has been read successfully at some point in the past * @return if memory has been successfully loaded */ - inline bool is_ready() {return ready_;} + inline bool is_ready() { return ready_; } /** * @brief Get a pointer to the parameter structure, which was stored in persistent memory * @return A pointer to the parameter struct */ - inline Params::params_t &get_params(){return memory_.params;} + inline Params::params_t &get_params() { return memory_.params; } /** * @brief Get a pointer to the config structure, which was stored in persistent memory * @return A pointer to the config struct */ - inline ConfigManager::Config &get_config(){return memory_.config;} + inline ConfigManager::Config &get_config() { return memory_.config; } private: ROSflight &RF_; PersistentMemory memory_; bool ready_{false}; - }; } // namespace rosflight_firmware diff --git a/include/mixer.h b/include/mixer.h index 348c385d..8db86886 100644 --- a/include/mixer.h +++ b/include/mixer.h @@ -29,25 +29,21 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ - #ifndef ROSFLIGHT_FIRMWARE_MIXER_H #define ROSFLIGHT_FIRMWARE_MIXER_H -#include -#include - #include "interface/param_listener.h" +#include +#include + namespace rosflight_firmware { - class ROSflight; class Mixer : public ParamListenerInterface { - public: - static constexpr uint8_t NUM_TOTAL_OUTPUTS = 14; static constexpr uint8_t NUM_MIXER_OUTPUTS = 8; @@ -72,9 +68,9 @@ class Mixer : public ParamListenerInterface typedef enum { NONE, // None - S, // Servo - M, // Motor - G // GPIO + S, // Servo + M, // Motor + G // GPIO } output_type_t; typedef struct @@ -109,154 +105,107 @@ class Mixer : public ParamListenerInterface void write_motor(uint8_t index, float value); void write_servo(uint8_t index, float value); - const mixer_t esc_calibration_mixing = - { - {M, M, M, M, M, M, NONE, NONE}, - { 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f}, // F Mix - { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix - { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix - { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix - 490 - }; + const mixer_t esc_calibration_mixing = {{M, M, M, M, M, M, NONE, NONE}, + {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f}, // F Mix + {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix + {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix + {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 + const mixer_t quadcopter_x_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 - {-1.0f, -1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix - { 1.0f, -1.0f,-1.0f, 1.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 + {-1.0f, -1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix + {1.0f, -1.0f, -1.0f, 1.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 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 + const mixer_t X8_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 - {-1.0f, -1.0f, -1.0f, -1.0f, 1.0f, 1.0f, 1.0f, 1.0f}, // X Mix - { 1.0f, 1.0f, -1.0f, -1.0f, -1.0f, -1.0f, 1.0f, 1.0f}, // 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 + {-1.0f, -1.0f, -1.0f, -1.0f, 1.0f, 1.0f, 1.0f, 1.0f}, // X Mix + {1.0f, 1.0f, -1.0f, -1.0f, -1.0f, -1.0f, 1.0f, 1.0f}, // Y Mix + {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}, + const mixer_t fixedwing_mixing = {{S, S, M, S, S, M, NONE, NONE}, - { 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix - { 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix - { 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix - { 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Z Mix - 50 - }; + {0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix + {1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix + {0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix + {0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Z Mix + 50}; - const mixer_t passthrough_mixing = - { - {NONE, NONE, NONE, NONE, NONE, NONE, NONE, NONE}, + const mixer_t passthrough_mixing = {{NONE, NONE, NONE, NONE, NONE, NONE, NONE, NONE}, - { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix - { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix - { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix - { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Z Mix - 50 - }; + {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix + {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix + {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix + {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_; - const mixer_t *array_of_mixers_[NUM_MIXERS] = - { - &esc_calibration_mixing, - &quadcopter_plus_mixing, - &quadcopter_x_mixing, - &hex_plus_mixing, - &hex_x_mixing, - &octocopter_plus_mixing, - &octocopter_x_mixing, - &Y6_mixing, - &X8_mixing, - &tricopter_mixing, - &fixedwing_mixing, - &passthrough_mixing - }; + const mixer_t* array_of_mixers_[NUM_MIXERS] = { + &esc_calibration_mixing, &quadcopter_plus_mixing, &quadcopter_x_mixing, &hex_plus_mixing, &hex_x_mixing, + &octocopter_plus_mixing, &octocopter_x_mixing, &Y6_mixing, &X8_mixing, &tricopter_mixing, + &fixedwing_mixing, &passthrough_mixing}; public: Mixer(ROSflight& _rf); @@ -266,7 +215,7 @@ class Mixer : public ParamListenerInterface 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/nanoprintf.h b/include/nanoprintf.h index 49ce3393..a4967847 100644 --- a/include/nanoprintf.h +++ b/include/nanoprintf.h @@ -66,10 +66,10 @@ To use the printf you need to supply your own character output function, something like : void putc ( void* p, char c) - { - while (!SERIAL_PORT_EMPTY) ; - SERIAL_PORT_TX_REGISTER = c; - } + { + while (!SERIAL_PORT_EMPTY) ; + SERIAL_PORT_TX_REGISTER = c; + } Before you can call printf you need to initialize it to use your character output function with something like: @@ -111,13 +111,12 @@ namespace rosflight_firmware { namespace nanoprintf { - -void init_printf(void *putp,void (*putf)(void *,char)); +void init_printf(void *putp, void (*putf)(void *, char)); void tfp_printf(const char *fmt, ...); void tfp_sprintf(char *s, const char *fmt, va_list va); -void tfp_format(void *putp, void (*putf)(void *,char), const char *fmt, va_list va); +void tfp_format(void *putp, void (*putf)(void *, char), const char *fmt, va_list va); } // namespace nanoprintf } // namespace rosflight_firmware diff --git a/include/param.h b/include/param.h index 6a871b1c..64dabacb 100644 --- a/include/param.h +++ b/include/param.h @@ -32,14 +32,13 @@ #ifndef ROSFLIGHT_FIRMWARE_PARAM_H #define ROSFLIGHT_FIRMWARE_PARAM_H +#include "interface/param_listener.h" + #include #include -#include "interface/param_listener.h" - namespace rosflight_firmware { - enum : uint16_t { /******************************/ @@ -67,7 +66,6 @@ enum : uint16_t PARAM_STREAM_OUTPUT_RAW_RATE, PARAM_STREAM_RC_RAW_RATE, - /********************************/ /*** CONTROLLER CONFIGURATION ***/ /********************************/ @@ -229,29 +227,31 @@ typedef enum class ROSflight; class Params { - public: static constexpr uint8_t PARAMS_NAME_LENGTH = 16; + private: union param_value_t { float fvalue; int32_t ivalue; }; + public: typedef struct { uint32_t version; uint16_t size; - uint8_t magic_be; // magic number, should be 0xBE + uint8_t magic_be; // magic number, should be 0xBE param_value_t values[PARAMS_COUNT]; char names[PARAMS_COUNT][PARAMS_NAME_LENGTH]; param_type_t types[PARAMS_COUNT]; - uint8_t magic_ef; // magic number, should be 0xEF - uint8_t chk; // XOR checksum + uint8_t magic_ef; // magic number, should be 0xEF + uint8_t chk; // XOR checksum } params_t; + private: ROSflight &RF_; params_t ¶ms; @@ -260,10 +260,9 @@ class Params 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_t ¶m_struct); @@ -280,11 +279,11 @@ class Params void set_defaults(void); /** - * @brief Specify listeners for parameter changes - * @param listeners An array of pointers to objects that implement the ParamListenerInterface interface - * @param num_listeners The length of the array passed as the listeners parameter - */ - void set_listeners(ParamListenerInterface * const listeners[], size_t num_listeners); + * @brief Specify listeners for parameter changes + * @param listeners An array of pointers to objects that implement the ParamListenerInterface interface + * @param num_listeners The length of the array passed as the listeners parameter + */ + void set_listeners(ParamListenerInterface *const listeners[], size_t num_listeners); /** * @brief Read parameter values from non-volatile memory @@ -315,30 +314,21 @@ class Params * @param id The ID of the parameter * @return The value of the parameter */ - inline int get_param_int(uint16_t id) const - { - return params.values[id].ivalue; - } + inline int get_param_int(uint16_t id) const { return params.values[id].ivalue; } /** * @brief Get the value of a floating point parameter by id * @param id The ID of the parameter * @return The value of the parameter */ - inline float get_param_float(uint16_t id) const - { - return params.values[id].fvalue; - } + inline float get_param_float(uint16_t id) const { return params.values[id].fvalue; } /** * @brief Get the name of a parameter * @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 @@ -348,10 +338,7 @@ class Params * PARAM_TYPE_INT32, PARAM_TYPE_FLOAT, or PARAM_TYPE_INVALID * See line 165 */ - inline param_type_t get_param_type(uint16_t id) const - { - return params.types[id]; - } + inline param_type_t get_param_type(uint16_t id) const { return params.types[id]; } /** * @brief Sets the value of a parameter by ID and calls the parameter change callback @@ -384,7 +371,6 @@ class Params * @return True if a parameter value was changed, false otherwise */ bool set_param_by_name_float(const char name[PARAMS_NAME_LENGTH], float value); - }; } // namespace rosflight_firmware diff --git a/include/rc.h b/include/rc.h index b491bc8a..38e6318b 100644 --- a/include/rc.h +++ b/include/rc.h @@ -32,19 +32,17 @@ #ifndef ROSFLIGHT_FIRMWARE_RC_H #define ROSFLIGHT_FIRMWARE_RC_H -#include -#include - #include "interface/param_listener.h" +#include +#include + namespace rosflight_firmware { - class ROSflight; class RC : public ParamListenerInterface { - public: enum Stick { diff --git a/include/rosflight.h b/include/rosflight.h index 579f4860..bcb7bdaf 100644 --- a/include/rosflight.h +++ b/include/rosflight.h @@ -32,34 +32,32 @@ #ifndef ROSFLIGHT_FIRMWARE_ROSFLIGHT_H #define ROSFLIGHT_FIRMWARE_ROSFLIGHT_H -#include - #include "interface/comm_link.h" #include "interface/param_listener.h" #include "board.h" -#include "param.h" -#include "sensors.h" -#include "estimator.h" -#include "rc.h" -#include "controller.h" #include "comm_manager.h" -#include "mixer.h" -#include "state_manager.h" #include "command_manager.h" #include "config_manager.h" +#include "controller.h" +#include "estimator.h" #include "memory_manager.h" +#include "mixer.h" +#include "param.h" +#include "rc.h" +#include "sensors.h" +#include "state_manager.h" + +#include namespace rosflight_firmware { - class ROSflight { - public: ROSflight(Board& board, CommLinkInterface& comm_link); - Board &board_; + Board& board_; MemoryManager memory_manager_; CommManager comm_manager_; @@ -77,28 +75,21 @@ class ROSflight uint32_t loop_time_us; /** - * @brief Main initialization routine for the ROSflight autopilot flight stack - */ + * @brief Main initialization routine for the ROSflight autopilot flight stack + */ void init(); /** - * @brief Main loop for the ROSflight autopilot flight stack - */ + * @brief Main loop for the ROSflight autopilot flight stack + */ void run(); uint32_t get_loop_time_us(); 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 1fd82a43..87082638 100644 --- a/include/sensors.h +++ b/include/sensors.h @@ -33,12 +33,13 @@ #ifndef ROSFLIGHT_FIRMWARE_SENSORS_H #define ROSFLIGHT_FIRMWARE_SENSORS_H -#include -#include -#include +#include "interface/param_listener.h" + #include -#include "interface/param_listener.h" +#include +#include +#include namespace rosflight_firmware { @@ -55,26 +56,26 @@ struct GNSSData { struct ECEF { - int32_t x; // cm - int32_t y; // cm - int32_t z; // cm + int32_t x; // cm + int32_t y; // cm + int32_t z; // cm uint32_t p_acc; // cm - int32_t vx; // cm/s - int32_t vy; // cm/s - int32_t vz; // cm/s + int32_t vx; // cm/s + int32_t vy; // cm/s + int32_t vz; // cm/s uint32_t s_acc; // cm/s }; GNSSFixType fix_type; uint32_t time_of_week; - uint64_t time; // Unix time, in seconds + uint64_t time; // Unix time, in seconds uint64_t nanos; // Fractional time - int32_t lat; // deg*10^-7 - int32_t lon; // deg*10^-7 + int32_t lat; // deg*10^-7 + int32_t lon; // deg*10^-7 int32_t height; // mm - int32_t vel_n; // mm/s - int32_t vel_e; // mm/s - int32_t vel_d; // mm/s + int32_t vel_n; // mm/s + int32_t vel_e; // mm/s + int32_t vel_d; // mm/s uint32_t h_acc; // mm uint32_t v_acc; // mm @@ -82,10 +83,7 @@ struct GNSSData uint64_t rosflight_timestamp; // microseconds, time stamp of last byte in the message - GNSSData() - { - memset(this, 0, sizeof(GNSSData)); - } + GNSSData() { memset(this, 0, sizeof(GNSSData)); } }; struct GNSSRaw @@ -118,10 +116,7 @@ struct GNSSRaw uint16_t p_dop; uint64_t rosflight_timestamp; // microseconds, time stamp of last byte in the message - GNSSRaw() - { - memset(this, 0, sizeof(GNSSRaw)); - } + GNSSRaw() { memset(this, 0, sizeof(GNSSRaw)); } }; class ROSflight; diff --git a/include/state_manager.h b/include/state_manager.h index d6f56cce..c93ee2fd 100644 --- a/include/state_manager.h +++ b/include/state_manager.h @@ -39,12 +39,10 @@ namespace rosflight_firmware { - class ROSflight; class StateManager { - public: struct State { @@ -86,11 +84,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 + static constexpr uint32_t ARM_MAGIC = + 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 + 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 /** * @brief Low-level debugging information for case of hard fault @@ -99,13 +98,13 @@ class StateManager */ struct DebugInfo { - uint32_t r0; //!< register 0 - uint32_t r1; //!< register 1 - uint32_t r2; //!< register 2 - uint32_t r3; //!< register 3 + uint32_t r0; //!< register 0 + uint32_t r1; //!< register 1 + uint32_t r2; //!< register 2 + uint32_t r3; //!< register 3 uint32_t r12; //!< register 12 - uint32_t lr; //!< link register - uint32_t pc; //!< program counter + uint32_t lr; //!< link register + uint32_t pc; //!< program counter uint32_t psr; //!< program status register } debug; @@ -137,7 +136,7 @@ class StateManager 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); @@ -158,7 +157,7 @@ class StateManager 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 40eae4e7..deb9abf3 100644 --- a/include/util.h +++ b/include/util.h @@ -38,7 +38,6 @@ namespace rosflight_firmware { - /** * @brief Fletcher 16-bit checksum * @@ -50,7 +49,8 @@ namespace rosflight_firmware */ 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; diff --git a/lib/turbomath/turbomath.cpp b/lib/turbomath/turbomath.cpp index fc8dd281..b9ca4ff8 100644 --- a/lib/turbomath/turbomath.cpp +++ b/lib/turbomath/turbomath.cpp @@ -35,57 +35,47 @@ 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); + return 1.0f / inv_sqrt(x * x + y * y + z * z); } - float Vector::sqrd_norm() const { - return x*x + y*y + z*z; + return x * x + y * y + z * z; } - Vector& Vector::normalize() { - float recip_norm = inv_sqrt(x*x + y*y + z*z); + float recip_norm = inv_sqrt(x * x + y * y + z * z); x *= recip_norm; y *= recip_norm; z *= recip_norm; return *this; } - Vector Vector::normalized() const { - float recip_norm = inv_sqrt(x*x + y*y + z*z); - Vector out(x*recip_norm, y*recip_norm, z*recip_norm); + float recip_norm = inv_sqrt(x * x + y * y + z * z); + Vector out(x * recip_norm, y * recip_norm, z * recip_norm); 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) +Vector& Vector::operator+=(const Vector& v) { x += v.x; y += v.y; @@ -93,8 +83,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; @@ -102,20 +91,17 @@ Vector& Vector::operator -=(const Vector& v) return *this; } - -Vector Vector::operator *(float s) const +Vector Vector::operator*(float s) const { - return Vector(x*s, y*s, z*s); + return Vector(x * s, y * s, z * s); } - -Vector Vector::operator /(float s) const +Vector Vector::operator/(float s) const { - return Vector(x/s, y/s, z/s); + return Vector(x / s, y / s, z / s); } - -Vector& Vector::operator *=(float s) +Vector& Vector::operator*=(float s) { x *= s; y *= s; @@ -123,8 +109,7 @@ Vector& Vector::operator *=(float s) return *this; } - -Vector& Vector::operator /=(float s) +Vector& Vector::operator/=(float s) { x /= s; y /= s; @@ -132,25 +117,19 @@ Vector& Vector::operator /=(float s) return *this; } - float Vector::dot(const Vector& v) const { - return x*v.x + y*v.y + z*v.z; + return x * v.x + y * v.y + z * v.z; } - 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); + 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) { @@ -164,7 +143,7 @@ Quaternion::Quaternion(float roll, float pitch, float yaw) Quaternion& Quaternion::normalize() { - float recip_norm = inv_sqrt(w*w + x*x + y*y + z*z); + float recip_norm = inv_sqrt(w * w + x * x + y * y + z * z); w *= recip_norm; x *= recip_norm; y *= recip_norm; @@ -182,31 +161,30 @@ 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); + 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; - y = w*q.y + x*q.z + y*q.w - z*q.x; - z = w*q.z - x*q.y + y*q.x + z*q.w; + 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; + y = w * q.y + x * q.z + y * q.w - z * q.x; + z = w * q.z - x * q.y + y * q.x + z * q.w; return *this; } 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 +Vector Quaternion::operator*(const Vector& v) const { return rotate(v); } @@ -239,9 +217,9 @@ Quaternion& Quaternion::from_two_unit_vectors(const Vector& u, const Vector& v) } else { - float invs = inv_sqrt(2.0f*(1.0f+d)); - Vector xyz = u.cross(v)*invs; - w = 0.5f/invs; + float invs = inv_sqrt(2.0f * (1.0f + d)); + Vector xyz = u.cross(v) * invs; + w = 0.5f / invs; x = xyz.x; y = xyz.y; z = xyz.z; @@ -253,23 +231,23 @@ Quaternion& Quaternion::from_two_unit_vectors(const Vector& u, const Vector& v) 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); - float sp = turbomath::sin(roll/2.0); - float ct = turbomath::cos(pitch/2.0); - float st = turbomath::sin(pitch/2.0); - float cs = turbomath::cos(yaw/2.0); - float ss = turbomath::sin(yaw/2.0); - - w = cs*ct*cp + ss*st*sp; - x = cs*ct*sp - ss*st*cp; - y = cs*st*cp + ss*ct*sp; - z = ss*ct*cp - cs*st*sp; + float cp = turbomath::cos(roll / 2.0); + float sp = turbomath::sin(roll / 2.0); + float ct = turbomath::cos(pitch / 2.0); + float st = turbomath::sin(pitch / 2.0); + float cs = turbomath::cos(yaw / 2.0); + float ss = turbomath::sin(yaw / 2.0); + + w = cs * ct * cp + ss * st * sp; + x = cs * ct * sp - ss * st * cp; + y = cs * st * cp + ss * ct * sp; + z = ss * ct * cp - cs * st * sp; normalize(); 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) @@ -282,15 +260,13 @@ 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)); - *yaw = turbomath::atan2(2.0f * (w*z + x*y), 1.0f - 2.0f * (y*y + z*z)); + *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)); + *yaw = turbomath::atan2(2.0f * (w * z + x * y), 1.0f - 2.0f * (y * y + z * z)); } - - #ifndef M_PI #define M_PI 3.14159265359 #endif @@ -300,73 +276,54 @@ 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; 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; 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; @@ -374,20 +331,14 @@ 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 -}; + 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) { @@ -396,66 +347,66 @@ float fsign(float y) float cos(float x) { - return sin(M_PI/2.0 - x); + return sin(M_PI / 2.0 - x); } float sin(float x) { // wrap down to +/x PI - while (x > M_PI) - x -= 2.0*M_PI; + while (x > M_PI) x -= 2.0 * M_PI; - while (x <= -M_PI) - x += 2.0*M_PI; + while (x <= -M_PI) x += 2.0 * M_PI; // sin is symmetric if (x < 0) - return -1.0*sin(-x); + return -1.0 * sin(-x); // wrap onto (0, PI) if (x > M_PI) - return -1.0*sin(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); + 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; + 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; + return 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; + return sin_lookup_table[index] / 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); + 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); + 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); + 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; + 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; + return 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; + return atan_lookup_table[index] / 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 @@ -463,11 +414,11 @@ float atan2(float y, float x) { if (y < 0.0) { - return - M_PI/2.0; + return -M_PI / 2.0; } - else if ( y > 0.0) + else if (y > 0.0) { - return M_PI/2.0; + return M_PI / 2.0; } else { @@ -475,11 +426,11 @@ float atan2(float y, float x) } } - float arctan = atan(y/x); + float arctan = atan(y / x); if (x < 0.0) { - if ( y < 0) + if (y < 0) { return arctan - M_PI; } @@ -491,45 +442,47 @@ float atan2(float y, float x) else { - return arctan; + return arctan; } } - float asin(float x) { if (x < 0.0) { - return -1.0*asin(-1.0*x); + return -1.0 * asin(-1.0 * x); } - float t = (x - asin_min_x)/(asin_max_x - asin_min_x) * static_cast(asin_num_entries); + 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; + 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; + return 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; + return asin_lookup_table[index] / 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) + if (press < max_pressure && press > min_pressure) { - float t = (press - min_pressure)/(max_pressure - min_pressure) * static_cast(pressure_num_entries); + 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; if (index >= pressure_num_entries) - return asin_lookup_table[pressure_num_entries - 1]/pressure_scale_factor; + 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; + return 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; + return pressure_lookup_table[index] / pressure_scale_factor + + delta_x * (pressure_lookup_table[index] - pressure_lookup_table[index - 1]) / pressure_scale_factor; } else return 0.0; @@ -541,7 +494,6 @@ float fabs(float x) return -x; else return x; - } float inv_sqrt(float x) @@ -551,12 +503,12 @@ float inv_sqrt(float x) const float threehalfs = 1.5F; x2 = x * 0.5F; - y.fvalue = x; - i.ivalue = y.ivalue; // evil floating point bit level hacking - i.ivalue = 0x5f3759df - (i.ivalue >> 1); + y.fvalue = x; + i.ivalue = y.ivalue; // evil floating point bit level hacking + 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)); // 1st iteration + 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 5808b766..d22326aa 100644 --- a/lib/turbomath/turbomath.h +++ b/lib/turbomath/turbomath.h @@ -38,7 +38,6 @@ namespace turbomath { - // float-based wrappers float cos(float x); float sin(float x); @@ -78,19 +77,24 @@ class Vector 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) 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); }; -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 { @@ -111,13 +115,13 @@ class Quaternion 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; + 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 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) + static Vector log(const Quaternion& q) { Vector v{q.x, q.y, q.z}; float norm_v = v.norm(); @@ -129,12 +133,12 @@ class Quaternion } else { - out = 2.0*atan2(norm_v, q.w)*v/norm_v; + 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/scripts/fix-code-style.sh b/scripts/fix-code-style.sh index 4191af8e..85d77f50 100755 --- a/scripts/fix-code-style.sh +++ b/scripts/fix-code-style.sh @@ -1,9 +1,11 @@ #!/bin/bash -CMD="astyle --options=.astylerc" +SCRIPT=$(readlink -f $0) +echo $SCRIPT +SCRIPTPATH=`dirname $SCRIPT` +echo #SCRIPTPATH +cd $SCRIPTPATH/.. -$CMD ../include/* -$CMD ../src/* -$CMD ../lib/turbotrig/* -$CMD ../boards/F1/* -$CMD ../boards/F4/* +find . -iname "*.h" -o -iname "*.cpp" | \ +egrep -v "^(./comms/mavlink/v1.0/|./boards/airbourne/airbourne/|./boards/breezy/breezystm32|./.git|./test/build)" | \ +xargs clang-format -i diff --git a/src/comm_manager.cpp b/src/comm_manager.cpp index 989fd2c2..6fb35e4a 100644 --- a/src/comm_manager.cpp +++ b/src/comm_manager.cpp @@ -29,23 +29,22 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#include -#include +#include "comm_manager.h" -#include "rosflight.h" #include "param.h" -#include "comm_manager.h" +#include "rosflight.h" + +#include +#include namespace rosflight_firmware { - CommManager::LogMessageBuffer::LogMessageBuffer() { memset(buffer_, 0, sizeof(buffer_)); } - void CommManager::LogMessageBuffer::add_message(CommLinkInterface::LogSeverity severity, char msg[]) { LogMessage& newest_msg = buffer_[newest_]; @@ -72,11 +71,7 @@ void CommManager::LogMessageBuffer::pop() } } - -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() @@ -175,18 +170,12 @@ void CommManager::send_param_value(uint16_t param_id) 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)); + 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)); + 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; @@ -312,7 +301,7 @@ void CommManager::timesync_callback(int64_t tc1, int64_t ts1) uint64_t now_us = RF_.board_.clock_micros(); if (tc1 == 0) // check that this is a request, not a response - comm_link_.send_timesync(sysid_, static_cast(now_us)*1000, ts1); + comm_link_.send_timesync(sysid_, static_cast(now_us) * 1000, ts1); } void CommManager::offboard_control_callback(const CommLinkInterface::OffboardControl& control) @@ -358,7 +347,7 @@ 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; @@ -388,7 +377,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); } @@ -414,15 +403,16 @@ void CommManager::heartbeat_callback(void) void CommManager::config_set_callback(uint8_t device, uint8_t configuration) { uint8_t requested_device{device}; - if(device >=Configuration::DEVICE_COUNT) + if (device >= Configuration::DEVICE_COUNT) device = Configuration::DEVICE_COUNT; - ConfigManager::ConfigResponse resp = RF_.config_manager_.attempt_set_configuration(static_cast(device), configuration); + ConfigManager::ConfigResponse resp = + RF_.config_manager_.attempt_set_configuration(static_cast(device), configuration); comm_link_.send_config_status(sysid_, requested_device, resp.successful, resp.reboot_required, resp.message); } void CommManager::config_request_callback(uint8_t device) { - if(device < Configuration::DEVICE_COUNT) + if (device < Configuration::DEVICE_COUNT) send_config_value(static_cast(device)); } void CommManager::send_all_config_info() @@ -458,7 +448,7 @@ 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; @@ -495,23 +485,15 @@ void CommManager::send_status(void) 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_.get_rc_override(), - RF_.command_manager_.offboard_control_active(), - RF_.state_manager_.state().error_codes, - control_mode, - RF_.board_.num_sensor_errors(), + comm_link_.send_status(sysid_, RF_.state_manager_.state().armed, RF_.state_manager_.state().failsafe, + RF_.command_manager_.get_rc_override(), 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); } @@ -520,32 +502,25 @@ void CommManager::send_imu(void) turbomath::Vector acc, gyro; uint64_t stamp_us; RF_.sensors_.get_filtered_IMU(acc, gyro, stamp_us); - comm_link_.send_imu(sysid_, - stamp_us, - acc, - gyro, - RF_.sensors_.data().imu_temperature); - + comm_link_.send_imu(sysid_, stamp_us, acc, gyro, RF_.sensors_.data().imu_temperature); } void CommManager::send_output_raw(void) { - comm_link_.send_output_raw(sysid_, - RF_.board_.clock_millis(), - RF_.mixer_.get_outputs()); + comm_link_.send_output_raw(sysid_, RF_.board_.clock_millis(), RF_.mixer_.get_outputs()); } void CommManager::send_rc_raw(void) { // TODO better mechanism for retreiving RC (through RC module, not PWM-specific) - uint16_t channels[8] = { static_cast(RF_.board_.rc_read(0)*1000 + 1000), - static_cast(RF_.board_.rc_read(1)*1000 + 1000), - static_cast(RF_.board_.rc_read(2)*1000 + 1000), - static_cast(RF_.board_.rc_read(3)*1000 + 1000), - static_cast(RF_.board_.rc_read(4)*1000 + 1000), - static_cast(RF_.board_.rc_read(5)*1000 + 1000), - static_cast(RF_.board_.rc_read(6)*1000 + 1000), - static_cast(RF_.board_.rc_read(7)*1000 + 1000) }; + uint16_t channels[8] = {static_cast(RF_.board_.rc_read(0) * 1000 + 1000), + static_cast(RF_.board_.rc_read(1) * 1000 + 1000), + static_cast(RF_.board_.rc_read(2) * 1000 + 1000), + static_cast(RF_.board_.rc_read(3) * 1000 + 1000), + static_cast(RF_.board_.rc_read(4) * 1000 + 1000), + static_cast(RF_.board_.rc_read(5) * 1000 + 1000), + static_cast(RF_.board_.rc_read(6) * 1000 + 1000), + static_cast(RF_.board_.rc_read(7) * 1000 + 1000)}; comm_link_.send_rc_raw(sysid_, RF_.board_.clock_millis(), channels); } @@ -553,9 +528,7 @@ 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, + comm_link_.send_diff_pressure(sysid_, RF_.sensors_.data().diff_pressure_velocity, RF_.sensors_.data().diff_pressure, RF_.sensors_.data().diff_pressure_temp); } } @@ -564,9 +537,7 @@ 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, + comm_link_.send_baro(sysid_, RF_.sensors_.data().baro_altitude, RF_.sensors_.data().baro_pressure, RF_.sensors_.data().baro_temperature); } } @@ -577,9 +548,7 @@ void CommManager::send_sonar(void) { comm_link_.send_sonar(sysid_, 0, // TODO set sensor type (sonar/lidar), use enum - RF_.sensors_.data().sonar_range, - 8.0, - 0.25); + RF_.sensors_.data().sonar_range, 8.0, 0.25); } } @@ -590,9 +559,8 @@ void CommManager::send_mag(void) } 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); + if (RF_.sensors_.data().battery_monitor_present) + 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) @@ -606,7 +574,6 @@ void CommManager::send_backup_data(const StateManager::BackupData& backup_data) backup_data_buffer_ = backup_data; have_backup_data_ = true; } - } void CommManager::send_gnss(void) @@ -667,12 +634,12 @@ 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); } @@ -694,7 +661,7 @@ void CommManager::send_next_config_info(void) send_device_info(send_device_info_index_); send_config_info(send_device_info_index_, send_config_info_index_); send_config_info_index_++; - if(send_config_info_index_ > RF_.board_.get_board_config_manager().get_max_config(send_device_info_index_)) + if (send_config_info_index_ > RF_.board_.get_board_config_manager().get_max_config(send_device_info_index_)) { ++send_device_info_index_; send_config_info_index_ = 0; @@ -706,7 +673,8 @@ CommManager::Stream::Stream(uint32_t period_us, std::function send_f period_us_(period_us), next_time_us_(0), send_function_(send_function) -{} +{ +} void CommManager::Stream::stream(uint64_t now_us) { @@ -716,8 +684,7 @@ void CommManager::Stream::stream(uint64_t now_us) do { next_time_us_ += period_us_; - } - while(next_time_us_ < now_us); + } while (next_time_us_ < now_us); send_function_(); } @@ -725,10 +692,10 @@ void CommManager::Stream::stream(uint64_t now_us) void CommManager::Stream::set_rate(uint32_t rate_hz) { - period_us_ = (rate_hz == 0) ? 0 : 1000000/rate_hz; + period_us_ = (rate_hz == 0) ? 0 : 1000000 / rate_hz; } -//void Mavlink::mavlink_send_named_command_struct(const char *const name, control_t command_struct) +// void Mavlink::mavlink_send_named_command_struct(const char *const name, control_t command_struct) //{ // uint8_t control_mode; // if (command_struct.x.type == RATE && command_struct.y.type == RATE) diff --git a/src/command_manager.cpp b/src/command_manager.cpp index 5cf64f20..93ffcfab 100644 --- a/src/command_manager.cpp +++ b/src/command_manager.cpp @@ -29,15 +29,15 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#include -#include - #include "command_manager.h" + #include "rosflight.h" +#include +#include + namespace rosflight_firmware { - typedef enum { ATT_MODE_RATE, @@ -50,12 +50,7 @@ typedef struct uint32_t last_override_time; } rc_stick_override_t; -rc_stick_override_t rc_stick_override[] = -{ - { RC::STICK_X, 0 }, - { RC::STICK_Y, 0 }, - { RC::STICK_Z, 0 } -}; +rc_stick_override_t rc_stick_override[] = {{RC::STICK_X, 0}, {RC::STICK_Y, 0}, {RC::STICK_Z, 0}}; typedef struct { @@ -64,10 +59,7 @@ typedef struct 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() { @@ -125,7 +117,7 @@ void CommandManager::interpret_rc(void) } else { - roll_pitch_type = (RF_.params_.get_param_int(PARAM_RC_ATTITUDE_MODE) == ATT_MODE_RATE) ? RATE: ANGLE; + roll_pitch_type = (RF_.params_.get_param_int(PARAM_RC_ATTITUDE_MODE) == ATT_MODE_RATE) ? RATE : ANGLE; } rc_command_.x.type = roll_pitch_type; @@ -159,7 +151,7 @@ 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 < channel_override_[channel].last_override_time + RF_.params_.get_param_int(PARAM_OVERRIDE_LAG_TIME)) + if (now < channel_override_[channel].last_override_time + RF_.params_.get_param_int(PARAM_OVERRIDE_LAG_TIME)) { return true; } @@ -177,28 +169,28 @@ bool CommandManager::stick_deviated(MuxChannel channel) uint16_t CommandManager::determine_override_status() { uint16_t rc_override{OVERRIDE_NO_OVERRIDE}; - if(RF_.rc_.switch_mapped(RC::SWITCH_ATT_OVERRIDE) && RF_.rc_.switch_on(RC::SWITCH_ATT_OVERRIDE)) + if (RF_.rc_.switch_mapped(RC::SWITCH_ATT_OVERRIDE) && RF_.rc_.switch_on(RC::SWITCH_ATT_OVERRIDE)) rc_override |= OVERRIDE_ATT_SWITCH; - 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)) rc_override |= OVERRIDE_THR_SWITCH; - for(uint8_t channel{0}; channel < MUX_F; channel++) + for (uint8_t channel{0}; channel < MUX_F; channel++) { - if(stick_deviated(static_cast(channel))) + if (stick_deviated(static_cast(channel))) rc_override |= channel_override_[channel].stick_override_reason; - if(!(muxes[channel].onboard->active)) + if (!(muxes[channel].onboard->active)) rc_override |= channel_override_[channel].offboard_inactive_override_reason; } - if(!(muxes[MUX_F].onboard->active)) // The throttle has unique override behavior + if (!(muxes[MUX_F].onboard->active)) // The throttle has unique override behavior rc_override |= OVERRIDE_OFFBOARD_T_INACTIVE; - if(RF_.params_.get_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE)) - if(muxes[MUX_F].rc->value < muxes[MUX_F].onboard->value) + if (RF_.params_.get_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE)) + if (muxes[MUX_F].rc->value < muxes[MUX_F].onboard->value) rc_override |= OVERRIDE_T; return rc_override; } void CommandManager::do_muxing(uint16_t rc_override) { - for(uint8_t channel{0}; channel <=MUX_F; channel++) + for (uint8_t channel{0}; channel <= MUX_F; channel++) do_channel_muxing(static_cast(channel), rc_override); } void CommandManager::do_channel_muxing(MuxChannel channel, uint16_t rc_override) @@ -241,7 +233,6 @@ void CommandManager::override_combined_command_with_rc() combined_command_ = rc_command_; } - bool CommandManager::run() { uint16_t last_rc_override = rc_override_; @@ -289,5 +280,4 @@ bool CommandManager::run() return true; } - -} +} // namespace rosflight_firmware diff --git a/src/config_manager.cpp b/src/config_manager.cpp index cfcfa153..2c0bde6d 100644 --- a/src/config_manager.cpp +++ b/src/config_manager.cpp @@ -1,16 +1,14 @@ #include "config_manager.h" + #include "rosflight.h" namespace rosflight_firmware { -ConfigManager::ConfigManager(ROSflight &RF, Config &config): - RF_{RF}, - config_{config} -{} +ConfigManager::ConfigManager(ROSflight &RF, Config &config) : RF_{RF}, config_{config} {} bool ConfigManager::init() { - if(!read()) + if (!read()) fill_defaults(); return true; } @@ -18,7 +16,7 @@ bool ConfigManager::init() bool ConfigManager::configure_devices() const { bool success = true; - for(device_t device{Configuration::FIRST_DEVICE}; device < Configuration::DEVICE_COUNT; ++device) + for (device_t device{Configuration::FIRST_DEVICE}; device < Configuration::DEVICE_COUNT; ++device) success = RF_.board_.enable_device(device, config_.config[device], RF_.params_) && success; return success; } @@ -26,7 +24,7 @@ bool ConfigManager::configure_devices() const ConfigManager::ConfigResponse ConfigManager::attempt_set_configuration(device_t device, uint8_t config) { ConfigResponse resp; - if(RF_.state_manager_.state().armed) + if (RF_.state_manager_.state().armed) { resp.successful = false; resp.reboot_required = false; @@ -34,7 +32,7 @@ ConfigManager::ConfigResponse ConfigManager::attempt_set_configuration(device_t return resp; } resp = RF_.board_.get_board_config_manager().check_config_change(device, config, *this); - if(resp.successful) + if (resp.successful) set_configuration(device, config); return resp; } @@ -59,11 +57,11 @@ void ConfigManager::prepare_write() bool ConfigManager::read() { - if(!RF_.memory_manager_.is_ready()) + if (!RF_.memory_manager_.is_ready()) return false; - if(generate_checksum() != config_.checksum) + if (generate_checksum() != config_.checksum) return false; - for (device_t device=Configuration::FIRST_DEVICE; device < Configuration::DEVICE_COUNT; ++device) + for (device_t device = Configuration::FIRST_DEVICE; device < Configuration::DEVICE_COUNT; ++device) if (config_.config[device] > RF_.board_.get_board_config_manager().get_max_config(device)) return false; return true; @@ -75,18 +73,18 @@ void ConfigManager::fill_defaults() } uint32_t ConfigManager::generate_checksum() const { - //8 bit fletcher algorithm, because we can't assume that the struct is a multiple of 16 bits - const uint8_t *config_data = reinterpret_cast(config_.config); + // 8 bit fletcher algorithm, because we can't assume that the struct is a multiple of 16 bits + const uint8_t *config_data = reinterpret_cast(config_.config); uint8_t check_a{0}; uint8_t check_b{0}; - for(size_t index{0}; index< sizeof(config_.config); index++ ) + for (size_t index{0}; index < sizeof(config_.config); index++) { check_a += config_data[index]; check_a %= (UINT8_MAX - 1); check_b += check_a; check_b %= (UINT8_MAX - 1); } - return check_a | (check_b<<8) | (~check_a<<16) | (~check_b<<24); + return check_a | (check_b << 8) | (~check_a << 16) | (~check_b << 24); } -} +} // namespace rosflight_firmware diff --git a/src/controller.cpp b/src/controller.cpp index 33c1b79d..e5803a8b 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -29,22 +29,19 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#include -#include +#include "controller.h" #include "command_manager.h" #include "estimator.h" + #include "rosflight.h" -#include "controller.h" +#include +#include namespace rosflight_firmware { - -Controller::Controller(ROSflight &rf) : - RF_(rf) -{ -} +Controller::Controller(ROSflight &rf) : RF_(rf) {} void Controller::init() { @@ -54,26 +51,19 @@ 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), - RF_.params_.get_param_float(PARAM_PID_ROLL_ANGLE_D), - max, min, tau); + 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), - RF_.params_.get_param_float(PARAM_PID_ROLL_RATE_D), - max, min, tau); + RF_.params_.get_param_float(PARAM_PID_ROLL_RATE_D), max, min, tau); pitch_.init(RF_.params_.get_param_float(PARAM_PID_PITCH_ANGLE_P), RF_.params_.get_param_float(PARAM_PID_PITCH_ANGLE_I), - RF_.params_.get_param_float(PARAM_PID_PITCH_ANGLE_D), - max, min, tau); + RF_.params_.get_param_float(PARAM_PID_PITCH_ANGLE_D), max, min, tau); 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), - RF_.params_.get_param_float(PARAM_PID_YAW_RATE_D), - max, min, tau); + 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), + RF_.params_.get_param_float(PARAM_PID_YAW_RATE_D), max, min, tau); } void Controller::run() @@ -95,10 +85,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); @@ -147,7 +139,8 @@ void Controller::calculate_equilbrium_torque_from_rc() } else { - RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_WARNING, "Cannot perform equilibrium offset calibration while armed"); + RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_WARNING, + "Cannot perform equilibrium offset calibration while armed"); } } @@ -180,12 +173,15 @@ void Controller::param_change_callback(uint16_t param_id) } } -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; - float dt = 1e-6*dt_us; + float dt = 1e-6 * dt_us; // ROLL if (command.x.type == RATE) @@ -222,7 +218,8 @@ Controller::PID::PID() : 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) { @@ -242,8 +239,8 @@ float Controller::PID::run(float dt, float x, float x_c, bool update_integrator) // 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 @@ -271,7 +268,7 @@ float Controller::PID::run(float dt, float x, float x_c, bool update_integrator, d_term = kd_ * xdot; } - //If there is an integrator term and we are updating integrators + // If there is an integrator term and we are updating integrators if ((ki_ > 0.0f) && update_integrator) { // integrate @@ -287,7 +284,7 @@ float Controller::PID::run(float dt, float x, float x_c, bool update_integrator, //// Include reference to Dr. Beard's notes here float u_sat = (u > max_) ? max_ : (u < min_) ? min_ : u; if (u != u_sat && fabs(i_term) > fabs(u - p_term + d_term) && ki_ > 0.0f) - integrator_ = (u_sat - p_term + d_term)/ki_; + integrator_ = (u_sat - p_term + d_term) / ki_; // Set output return u_sat; diff --git a/src/estimator.cpp b/src/estimator.cpp index 168ac20f..860d9fdd 100644 --- a/src/estimator.cpp +++ b/src/estimator.cpp @@ -30,14 +30,12 @@ */ #include "estimator.h" + #include "rosflight.h" namespace rosflight_firmware { - -Estimator::Estimator(ROSflight &_rf): - RF_(_rf) -{} +Estimator::Estimator(ROSflight& _rf) : RF_(_rf) {} void Estimator::reset_state() { @@ -99,26 +97,26 @@ void Estimator::init() void Estimator::param_change_callback(uint16_t param_id) { - (void) 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; - 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; + 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; - 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; + 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; @@ -126,7 +124,6 @@ void Estimator::set_external_attitude_update(const turbomath::Quaternion &q) void Estimator::run() { - // // Timing Setup // @@ -194,10 +191,10 @@ 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; + kp = RF_.params_.get_param_float(PARAM_FILTER_KP_ACC) * 10.0f; + ki = RF_.params_.get_param_float(PARAM_FILTER_KI) * 10.0f; } // @@ -206,7 +203,7 @@ void Estimator::run() // Integrate biases driven by measured angular error // eq 47b Mahony Paper, using correction term w_err found above - bias_ -= ki*w_err*dt; + bias_ -= ki * w_err * dt; // Build the composite omega vector for kinematic propagation // This the stuff inside the p function in eq. 47a - Mahony Paper @@ -245,7 +242,8 @@ 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(); @@ -256,8 +254,8 @@ bool Estimator::can_use_accel() const // Since there is noise, we give some margin to what a "non-accelerated state" means. // Establish allowed acceleration deviation from 1g (i.e., non-accelerated flight). const float margin = RF_.params_.get_param_float(PARAM_FILTER_ACCEL_MARGIN); - const float lowerbound = (1.0f - margin)*(1.0f - margin)*9.80665f*9.80665f; - const float upperbound = (1.0f + margin)*(1.0f + margin)*9.80665f*9.80665f; + const float lowerbound = (1.0f - margin) * (1.0f - margin) * 9.80665f * 9.80665f; + const float upperbound = (1.0f + margin) * (1.0f + margin) * 9.80665f * 9.80665f; // if the magnitude of the accel measurement is close to 1g, we can use the // accelerometer to correct roll and pitch and estimate gyro biases. @@ -285,8 +283,8 @@ turbomath::Vector Estimator::accel_correction() const // Correction Term of Eq. 47a and 47b Mahony Paper // w_acc = 2*s_tilde*v_tilde turbomath::Vector w_acc; - w_acc.x = -2.0f*q_tilde.w*q_tilde.x; - w_acc.y = -2.0f*q_tilde.w*q_tilde.y; + w_acc.x = -2.0f * q_tilde.w * q_tilde.x; + w_acc.y = -2.0f * q_tilde.w * q_tilde.y; w_acc.z = 0.0f; // Don't correct z, because it's unobservable from the accelerometer return w_acc; @@ -322,7 +320,7 @@ turbomath::Vector Estimator::smoothed_gyro_measurement() { // 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); + wbar = (w2_ / -12.0f) + w1_ * (8.0f / 12.0f) + gyro_LPF_ * (5.0f / 12.0f); w2_ = w1_; w1_ = gyro_LPF_; } @@ -335,12 +333,14 @@ turbomath::Vector Estimator::smoothed_gyro_measurement() } void Estimator::integrate_angular_rate(turbomath::Quaternion& quat, - const turbomath::Vector& omega, const float dt) const + 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; @@ -352,41 +352,48 @@ void Estimator::integrate_angular_rate(turbomath::Quaternion& quat, // (Eq. 12 Casey Paper) // This adds 90 us on STM32F10x chips float norm_w = sqrtf(sqrd_norm_w); - float t1 = cosf((norm_w*dt)/2.0f); - float t2 = 1.0f/norm_w * sinf((norm_w*dt)/2.0f); - quat.w = t1*quat.w + t2*(-p*quat.x - q*quat.y - r*quat.z); - quat.x = t1*quat.x + t2*( p*quat.w + r*quat.y - q*quat.z); - 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); + float t1 = cosf((norm_w * dt) / 2.0f); + float t2 = 1.0f / norm_w * sinf((norm_w * dt) / 2.0f); + quat.w = t1 * quat.w + t2 * (-p * quat.x - q * quat.y - r * quat.z); + quat.x = t1 * quat.x + t2 * (p * quat.w + r * quat.y - q * quat.z); + 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 { // 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)); - quat.w += qdot.w*dt; - quat.x += qdot.x*dt; - quat.y += qdot.y*dt; - quat.z += qdot.z*dt; + 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)); + quat.w += qdot.w * dt; + quat.x += qdot.x * dt; + quat.y += qdot.y * dt; + quat.z += qdot.z * dt; quat.normalize(); } } void Estimator::quaternion_to_dcm(const turbomath::Quaternion& q, - turbomath::Vector& X, turbomath::Vector& Y, turbomath::Vector& Z) const + turbomath::Vector& X, + turbomath::Vector& Y, + turbomath::Vector& Z) const { // R(q) = [X.x X.y X.z] // [Y.x Y.y Y.z] // [Z.x Z.y Z.z] - + const float &w = q.w, &x = q.x, &y = q.y, &z = q.z; - X.x = 1.0f - 2.0f*(y*y + z*z); X.y = 2.0f*(x*y - z*w); X.z = 2.0f*(x*z + y*w); - Y.x = 2.0f*(x*y + z*w); Y.y = 1.0f - 2.0f*(x*x + z*z); Y.z = 2.0f*(y*z - x*w); - Z.x = 2.0f*(x*z - y*w); Z.y = 2.0f*(y*z + x*w); Z.z = 1.0f - 2.0f*(x*x + y*y); + X.x = 1.0f - 2.0f * (y * y + z * z); + X.y = 2.0f * (x * y - z * w); + X.z = 2.0f * (x * z + y * w); + Y.x = 2.0f * (x * y + z * w); + Y.y = 1.0f - 2.0f * (x * x + z * z); + Y.z = 2.0f * (y * z - x * w); + Z.x = 2.0f * (x * z - y * w); + Z.y = 2.0f * (y * z + x * w); + Z.z = 1.0f - 2.0f * (x * x + y * y); } } // namespace rosflight_firmware diff --git a/src/memory_manager.cpp b/src/memory_manager.cpp index 2b6e4566..c257caa3 100644 --- a/src/memory_manager.cpp +++ b/src/memory_manager.cpp @@ -1,13 +1,10 @@ #include "memory_manager.h" + #include "rosflight.h" namespace rosflight_firmware { - -MemoryManager::MemoryManager(ROSflight &rf): - RF_{rf} -{ -} +MemoryManager::MemoryManager(ROSflight &rf) : RF_{rf} {} bool MemoryManager::read_memory() { RF_.board_.memory_init(); diff --git a/src/mixer.cpp b/src/mixer.cpp index c07995f8..e79aa835 100644 --- a/src/mixer.cpp +++ b/src/mixer.cpp @@ -29,17 +29,15 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ - -#include - #include "mixer.h" + #include "rosflight.h" +#include + namespace rosflight_firmware { - -Mixer::Mixer(ROSflight &_rf) : - RF_(_rf) +Mixer::Mixer(ROSflight &_rf) : RF_(_rf) { mixer_to_use_ = nullptr; } @@ -65,7 +63,6 @@ void Mixer::param_change_callback(uint16_t param_id) } } - void Mixer::init_mixing() { // clear the invalid mixer error @@ -86,7 +83,6 @@ void Mixer::init_mixing() mixer_to_use_ = array_of_mixers_[mixer_choice]; } - init_PWM(); for (int8_t i = 0; i < NUM_TOTAL_OUTPUTS; i++) @@ -111,7 +107,6 @@ void Mixer::init_PWM() RF_.board_.pwm_init(refresh_rate, off_pwm); } - void Mixer::write_motor(uint8_t index, float value) { if (RF_.state_manager_.state().armed) @@ -138,7 +133,6 @@ void Mixer::write_motor(uint8_t index, float value) RF_.board_.pwm_write(index, raw_outputs_[index]); } - void Mixer::write_servo(uint8_t index, float value) { if (value > 1.0) @@ -188,8 +182,8 @@ void Mixer::mix_output() 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]); + 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) @@ -203,14 +197,14 @@ void Mixer::mix_output() float scale_factor = 1.0; if (max_output > 1.0) { - scale_factor = 1.0/max_output; + scale_factor = 1.0 / max_output; } // Perform Motor Output Scaling 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; + // scale all motor outputs by scale factor (this is usually 1.0, unless we saturated) + outputs_[i] *= scale_factor; } // Insert AUX Commands, and assemble combined_output_types array (Does not override mixer values) @@ -250,4 +244,4 @@ void Mixer::mix_output() } } -} +} // namespace rosflight_firmware diff --git a/src/nanoprintf.cpp b/src/nanoprintf.cpp index c1fab8c0..6bd22529 100644 --- a/src/nanoprintf.cpp +++ b/src/nanoprintf.cpp @@ -35,117 +35,113 @@ namespace rosflight_firmware { namespace nanoprintf { - -typedef void (*putcf)(void *,char); +typedef void (*putcf)(void *, char); static putcf stdout_putf; static void *stdout_putp; - #ifdef PRINTF_LONG_SUPPORT -static void uli2a(unsigned long int num, unsigned int base, int uc,char *bf) +static void uli2a(unsigned long int num, unsigned int base, int uc, char *bf) { - int n=0; - unsigned int d=1; - while (num/d >= base) - d*=base; - while (d!=0) + int n = 0; + unsigned int d = 1; + while (num / d >= base) d *= base; + while (d != 0) { int dgt = num / d; - num%=d; - d/=base; - if (n || dgt>0|| d==0) + num %= d; + d /= base; + if (n || dgt > 0 || d == 0) { - *bf++ = dgt+(dgt<10 ? '0' : (uc ? 'A' : 'a')-10); + *bf++ = dgt + (dgt < 10 ? '0' : (uc ? 'A' : 'a') - 10); ++n; } } - *bf=0; + *bf = 0; } static void li2a(long num, char *bf) { - if (num<0) + if (num < 0) { - num=-num; + num = -num; *bf++ = '-'; } - uli2a(num,10,0,bf); + uli2a(num, 10, 0, bf); } #endif -static void ui2a(unsigned int num, unsigned int base, int uc,char *bf) +static void ui2a(unsigned int num, unsigned int base, int uc, char *bf) { - int n=0; - unsigned int d=1; + int n = 0; + unsigned int d = 1; #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wstrict-overflow" - while (num/d >= base) + while (num / d >= base) #pragma GCC diagnostic pop - d*=base; - while (d!=0) + d *= base; + while (d != 0) { int dgt = num / d; - num%= d; - d/=base; - if (n || dgt>0 || d==0) + num %= d; + d /= base; + if (n || dgt > 0 || d == 0) { - *bf++ = dgt+(dgt<10 ? '0' : (uc ? 'A' : 'a')-10); + *bf++ = dgt + (dgt < 10 ? '0' : (uc ? 'A' : 'a') - 10); ++n; } } - *bf=0; + *bf = 0; } static void i2a(int num, char *bf) { - if (num<0) + if (num < 0) { - num=-num; + num = -num; *bf++ = '-'; } - ui2a(num,10,0,bf); + ui2a(num, 10, 0, bf); } static int a2d(char ch) { - if (ch>='0' && ch<='9') - return ch-'0'; - else if (ch>='a' && ch<='f') - return ch-'a'+10; - else if (ch>='A' && ch<='F') - return ch-'A'+10; - else return -1; + if (ch >= '0' && ch <= '9') + return ch - '0'; + else if (ch >= 'a' && ch <= 'f') + return ch - 'a' + 10; + else if (ch >= 'A' && ch <= 'F') + return ch - 'A' + 10; + else + return -1; } -static char a2i(char ch, char **src,int base,int *nump) +static char a2i(char ch, char **src, int base, int *nump) { - char *p= *src; - int num=0; + char *p = *src; + int num = 0; int digit; - while ((digit=a2d(ch))>=0) + while ((digit = a2d(ch)) >= 0) { - if (digit>base) break; - num=num*base+digit; - ch=*p++; + if (digit > base) + break; + num = num * base + digit; + ch = *p++; } - *src=p; - *nump=num; + *src = p; + *nump = num; return ch; } static void putchw(void *putp, putcf putf, int n, char z, char *bf) { - char fc=z? '0' : ' '; + char fc = z ? '0' : ' '; char ch; - char *p=bf; - while (*p++ && n != 0) - n--; - while (n-- != 0) - putf(putp,fc); - while ((ch= *bf++)) - putf(putp,ch); + char *p = bf; + while (*p++ && n != 0) n--; + while (n-- != 0) putf(putp, fc); + while ((ch = *bf++)) putf(putp, ch); } void tfp_format(void *putp, putcf putf, const char *fmt, va_list va) @@ -154,114 +150,110 @@ void tfp_format(void *putp, putcf putf, const char *fmt, va_list va) char ch; - - while ((ch=*(fmt++))) + while ((ch = *(fmt++))) { - if (ch!='%') - putf(putp,ch); + if (ch != '%') + putf(putp, ch); else { - char lz=0; -#ifdef PRINTF_LONG_SUPPORT - char lng=0; + char lz = 0; +#ifdef PRINTF_LONG_SUPPORT + char lng = 0; #endif - int w=0; - ch=*(fmt++); - if (ch=='0') + int w = 0; + ch = *(fmt++); + if (ch == '0') { - ch=*(fmt++); - lz=1; + ch = *(fmt++); + lz = 1; } - if (ch>='0' && ch<='9') + if (ch >= '0' && ch <= '9') { - ch=a2i(ch, const_cast(&fmt), 10, &w); + ch = a2i(ch, const_cast(&fmt), 10, &w); } -#ifdef PRINTF_LONG_SUPPORT - if (ch=='l') +#ifdef PRINTF_LONG_SUPPORT + if (ch == 'l') { - ch=*(fmt++); - lng=1; + ch = *(fmt++); + lng = 1; } #endif switch (ch) { case 0: goto abort; - case 'u' : - { -#ifdef PRINTF_LONG_SUPPORT - if (lng) - uli2a(va_arg(va, unsigned long int),10,0,bf); - else + case 'u': + { +#ifdef PRINTF_LONG_SUPPORT + if (lng) + uli2a(va_arg(va, unsigned long int), 10, 0, bf); + else #endif - ui2a(va_arg(va, unsigned int),10,0,bf); - putchw(putp,putf,w,lz,bf); - break; - } - case 'd' : - { -#ifdef PRINTF_LONG_SUPPORT - if (lng) - li2a(va_arg(va, unsigned long int),bf); - else + ui2a(va_arg(va, unsigned int), 10, 0, bf); + putchw(putp, putf, w, lz, bf); + break; + } + case 'd': + { +#ifdef PRINTF_LONG_SUPPORT + if (lng) + li2a(va_arg(va, unsigned long int), bf); + else #endif - i2a(va_arg(va, int),bf); - putchw(putp,putf,w,lz,bf); - break; - } + i2a(va_arg(va, int), bf); + putchw(putp, putf, w, lz, bf); + break; + } case 'x': - case 'X' : -#ifdef PRINTF_LONG_SUPPORT + case 'X': +#ifdef PRINTF_LONG_SUPPORT if (lng) - uli2a(va_arg(va, unsigned long int),16,(ch=='X'),bf); + uli2a(va_arg(va, unsigned long int), 16, (ch == 'X'), bf); else #endif - ui2a(va_arg(va, unsigned int),16,(ch=='X'),bf); - putchw(putp,putf,w,lz,bf); + ui2a(va_arg(va, unsigned int), 16, (ch == 'X'), bf); + putchw(putp, putf, w, lz, bf); break; - case 'c' : - putf(putp,static_cast(va_arg(va, int))); + case 'c': + putf(putp, static_cast(va_arg(va, int))); break; - case 's' : - putchw(putp,putf,w,0,va_arg(va, char *)); + case 's': + putchw(putp, putf, w, 0, va_arg(va, char *)); break; - case '%' : - putf(putp,ch); + case '%': + putf(putp, ch); default: break; } } } -abort: - ; +abort:; } - void init_printf(void *putp, void (*putf)(void *, char)) { - stdout_putf=putf; - stdout_putp=putp; + stdout_putf = putf; + stdout_putp = putp; } void tfp_printf(const char *fmt, ...) { va_list va; - va_start(va,fmt); - tfp_format(stdout_putp,stdout_putf,fmt,va); + va_start(va, fmt); + tfp_format(stdout_putp, stdout_putf, fmt, va); va_end(va); } -static void putcp(void *p,char c) +static void putcp(void *p, char c) { *(*(static_cast(p)))++ = c; } void tfp_sprintf(char *s, const char *fmt, va_list va) { - tfp_format(&s,putcp,fmt,va); - putcp(&s,0); + tfp_format(&s, putcp, fmt, va); + putcp(&s, 0); } - } // namespace nanoprintf } // namespace rosflight_firmware diff --git a/src/param.cpp b/src/param.cpp index 554af9d7..e827fce1 100644 --- a/src/param.cpp +++ b/src/param.cpp @@ -29,16 +29,16 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#include -#include +#include "param.h" #include "board.h" #include "mixer.h" -#include "param.h" - #include "rosflight.h" +#include +#include + #ifndef GIT_VERSION_HASH #define GIT_VERSION_HASH 0x00 #pragma message "GIT_VERSION_HASH Undefined, setting to 0x00!" @@ -56,8 +56,7 @@ namespace rosflight_firmware { - -Params::Params(ROSflight& _rf, params_t ¶m_struct) : +Params::Params(ROSflight &_rf, params_t ¶m_struct) : RF_(_rf), params(param_struct), listeners_(nullptr), @@ -69,7 +68,7 @@ Params::Params(ROSflight& _rf, params_t ¶m_struct) : void Params::init_param_int(uint16_t id, const char name[PARAMS_NAME_LENGTH], int32_t value) { // copy cstr including '\0' or until maxlen - const uint8_t len = (strlen(name)>=PARAMS_NAME_LENGTH) ? PARAMS_NAME_LENGTH : strlen(name)+1; + const uint8_t len = (strlen(name) >= PARAMS_NAME_LENGTH) ? PARAMS_NAME_LENGTH : strlen(name) + 1; memcpy(params.names[id], name, len); params.values[id].ivalue = value; params.types[id] = PARAM_TYPE_INT32; @@ -78,7 +77,7 @@ void Params::init_param_int(uint16_t id, const char name[PARAMS_NAME_LENGTH], in void Params::init_param_float(uint16_t id, const char name[PARAMS_NAME_LENGTH], float value) { // copy cstr including '\0' or until maxlen - const uint8_t len = (strlen(name)>=PARAMS_NAME_LENGTH) ? PARAMS_NAME_LENGTH : strlen(name)+1; + const uint8_t len = (strlen(name) >= PARAMS_NAME_LENGTH) ? PARAMS_NAME_LENGTH : strlen(name) + 1; memcpy(params.names[id], name, len); params.values[id].fvalue = value; params.types[id] = PARAM_TYPE_FLOAT; @@ -89,23 +88,26 @@ uint8_t Params::compute_checksum(void) uint8_t chk = 0; const char *p; - for (p = reinterpret_cast(¶ms.values); p < reinterpret_cast(¶ms.values) + 4*PARAMS_COUNT; p++) + for (p = reinterpret_cast(¶ms.values); + p < reinterpret_cast(¶ms.values) + 4 * PARAMS_COUNT; p++) chk ^= *p; - for (p = reinterpret_cast(¶ms.names); p < reinterpret_cast(¶ms.names) + PARAMS_COUNT*PARAMS_NAME_LENGTH; p++) + for (p = reinterpret_cast(¶ms.names); + p < reinterpret_cast(¶ms.names) + PARAMS_COUNT * PARAMS_NAME_LENGTH; p++) chk ^= *p; - for (p = reinterpret_cast(¶ms.types); p < reinterpret_cast(¶ms.types) + PARAMS_COUNT; p++) + for (p = reinterpret_cast(¶ms.types); + p < reinterpret_cast(¶ms.types) + PARAMS_COUNT; p++) chk ^= *p; return chk; } - // function definitions void Params::init() { if (!read()) { - RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_WARNING, "Unable to load parameters; using default values"); + RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_WARNING, + "Unable to load parameters; using default values"); set_defaults(); RF_.memory_manager_.write_memory(); } @@ -116,27 +118,28 @@ void Params::set_defaults(void) /******************************/ /*** HARDWARE CONFIGURATION ***/ /******************************/ - init_param_int(PARAM_BAUD_RATE, "BAUD_RATE", 921600); // Baud rate of MAVlink communication with companion computer | 9600 | 921600 + init_param_int(PARAM_BAUD_RATE, "BAUD_RATE", + 921600); // Baud rate of MAVlink communication with companion computer | 9600 | 921600 /*****************************/ /*** MAVLINK CONFIGURATION ***/ /*****************************/ - init_param_int(PARAM_SYSTEM_ID, "SYS_ID", 1); // Mavlink System ID | 1 | 255 + init_param_int(PARAM_SYSTEM_ID, "SYS_ID", 1); // Mavlink System ID | 1 | 255 init_param_int(PARAM_STREAM_HEARTBEAT_RATE, "STRM_HRTBT", 1); // Rate of heartbeat stream (Hz) | 0 | 1000 - init_param_int(PARAM_STREAM_STATUS_RATE, "STRM_STATUS", 10); // Rate of status stream (Hz) | 0 | 1000 - - init_param_int(PARAM_STREAM_ATTITUDE_RATE, "STRM_ATTITUDE", 200); // Rate of attitude stream (Hz) | 0 | 1000 - init_param_int(PARAM_STREAM_IMU_RATE, "STRM_IMU", 250); // Rate of IMU stream (Hz) | 0 | 1000 - init_param_int(PARAM_STREAM_MAG_RATE, "STRM_MAG", 50); // Rate of magnetometer stream (Hz) | 0 | 75 - init_param_int(PARAM_STREAM_BARO_RATE, "STRM_BARO", 50); // Rate of barometer stream (Hz) | 0 | 100 - init_param_int(PARAM_STREAM_AIRSPEED_RATE, "STRM_AIRSPEED", 50); // Rate of airspeed stream (Hz) | 0 | 50 - init_param_int(PARAM_STREAM_SONAR_RATE, "STRM_SONAR", 40); // Rate of sonar stream (Hz) | 0 | 40 - init_param_int(PARAM_STREAM_GNSS_RATE, "STRM_GNSS", 1000); // Maximum rate of GNSS stream (Hz) | 0 | 10 - init_param_int(PARAM_STREAM_GNSS_RAW_RATE, "STRM_GNSS_RAW", 10); //Rate of GNSS raw stream (Hz) | 0 | 10 - init_param_int(PARAM_STREAM_BATTERY_STATUS_RATE, "STRM_BATTERY", 10); //Rate of GNSS raw stream (Hz) | 0 | 10 + init_param_int(PARAM_STREAM_STATUS_RATE, "STRM_STATUS", 10); // Rate of status stream (Hz) | 0 | 1000 + + init_param_int(PARAM_STREAM_ATTITUDE_RATE, "STRM_ATTITUDE", 200); // Rate of attitude stream (Hz) | 0 | 1000 + init_param_int(PARAM_STREAM_IMU_RATE, "STRM_IMU", 250); // Rate of IMU stream (Hz) | 0 | 1000 + init_param_int(PARAM_STREAM_MAG_RATE, "STRM_MAG", 50); // Rate of magnetometer stream (Hz) | 0 | 75 + init_param_int(PARAM_STREAM_BARO_RATE, "STRM_BARO", 50); // Rate of barometer stream (Hz) | 0 | 100 + init_param_int(PARAM_STREAM_AIRSPEED_RATE, "STRM_AIRSPEED", 50); // Rate of airspeed stream (Hz) | 0 | 50 + init_param_int(PARAM_STREAM_SONAR_RATE, "STRM_SONAR", 40); // Rate of sonar stream (Hz) | 0 | 40 + init_param_int(PARAM_STREAM_GNSS_RATE, "STRM_GNSS", 1000); // Maximum rate of GNSS stream (Hz) | 0 | 10 + init_param_int(PARAM_STREAM_GNSS_RAW_RATE, "STRM_GNSS_RAW", 10); // Rate of GNSS raw stream (Hz) | 0 | 10 + init_param_int(PARAM_STREAM_BATTERY_STATUS_RATE, "STRM_BATTERY", 10); // Rate of GNSS raw stream (Hz) | 0 | 10 init_param_int(PARAM_STREAM_OUTPUT_RAW_RATE, "STRM_SERVO", 50); // Rate of raw output stream | 0 | 490 - init_param_int(PARAM_STREAM_RC_RAW_RATE, "STRM_RC", 50); // Rate of raw RC input stream | 0 | 50 + init_param_int(PARAM_STREAM_RC_RAW_RATE, "STRM_RC", 50); // Rate of raw RC input stream | 0 | 50 /********************************/ /*** CONTROLLER CONFIGURATION ***/ @@ -151,76 +154,101 @@ void Params::set_defaults(void) init_param_float(PARAM_PID_PITCH_RATE_I, "PID_PITCH_RATE_I", 0.0000f); // Pitch Rate Integral Gain | 0.0 | 1000.0 init_param_float(PARAM_PID_PITCH_RATE_D, "PID_PITCH_RATE_D", 0.0000f); // Pitch Rate Derivative Gain | 0.0 | 1000.0 - init_param_float(PARAM_PID_YAW_RATE_P, "PID_YAW_RATE_P", 0.25f); // Yaw Rate Proportional Gain | 0.0 | 1000.0 + init_param_float(PARAM_PID_YAW_RATE_P, "PID_YAW_RATE_P", 0.25f); // Yaw Rate Proportional Gain | 0.0 | 1000.0 init_param_float(PARAM_PID_YAW_RATE_I, "PID_YAW_RATE_I", 0.0f); // Yaw Rate Integral Gain | 0.0 | 1000.0 init_param_float(PARAM_PID_YAW_RATE_D, "PID_YAW_RATE_D", 0.0f); // Yaw Rate Derivative Gain | 0.0 | 1000.0 - init_param_float(PARAM_PID_ROLL_ANGLE_P, "PID_ROLL_ANG_P", 0.15f); // Roll Angle Proportional Gain | 0.0 | 1000.0 - init_param_float(PARAM_PID_ROLL_ANGLE_I, "PID_ROLL_ANG_I", 0.0f); // Roll Angle Integral Gain | 0.0 | 1000.0 - init_param_float(PARAM_PID_ROLL_ANGLE_D, "PID_ROLL_ANG_D", 0.05f); // Roll Angle Derivative Gain | 0.0 | 1000.0 + init_param_float(PARAM_PID_ROLL_ANGLE_P, "PID_ROLL_ANG_P", 0.15f); // Roll Angle Proportional Gain | 0.0 | 1000.0 + init_param_float(PARAM_PID_ROLL_ANGLE_I, "PID_ROLL_ANG_I", 0.0f); // Roll Angle Integral Gain | 0.0 | 1000.0 + init_param_float(PARAM_PID_ROLL_ANGLE_D, "PID_ROLL_ANG_D", 0.05f); // Roll Angle Derivative Gain | 0.0 | 1000.0 - init_param_float(PARAM_PID_PITCH_ANGLE_P, "PID_PITCH_ANG_P", 0.15f); // Pitch Angle Proportional Gain | 0.0 | 1000.0 + init_param_float(PARAM_PID_PITCH_ANGLE_P, "PID_PITCH_ANG_P", 0.15f); // Pitch Angle Proportional Gain | 0.0 | 1000.0 init_param_float(PARAM_PID_PITCH_ANGLE_I, "PID_PITCH_ANG_I", 0.0f); // Pitch Angle Integral Gain | 0.0 | 1000.0 init_param_float(PARAM_PID_PITCH_ANGLE_D, "PID_PITCH_ANG_D", 0.05f); // Pitch Angle Derivative Gain | 0.0 | 1000.0 - init_param_float(PARAM_X_EQ_TORQUE, "X_EQ_TORQUE", 0.0f); // Equilibrium torque added to output of controller on x axis | -1.0 | 1.0 - init_param_float(PARAM_Y_EQ_TORQUE, "Y_EQ_TORQUE", 0.0f); // Equilibrium torque added to output of controller on y axis | -1.0 | 1.0 - init_param_float(PARAM_Z_EQ_TORQUE, "Z_EQ_TORQUE", 0.0f); // Equilibrium torque added to output of controller on z axis | -1.0 | 1.0 - - init_param_float(PARAM_PID_TAU, "PID_TAU", 0.05f); // Dirty Derivative time constant - See controller documentation | 0.0 | 1.0 + init_param_float(PARAM_X_EQ_TORQUE, "X_EQ_TORQUE", + 0.0f); // Equilibrium torque added to output of controller on x axis | -1.0 | 1.0 + init_param_float(PARAM_Y_EQ_TORQUE, "Y_EQ_TORQUE", + 0.0f); // Equilibrium torque added to output of controller on y axis | -1.0 | 1.0 + init_param_float(PARAM_Z_EQ_TORQUE, "Z_EQ_TORQUE", + 0.0f); // Equilibrium torque added to output of controller on z axis | -1.0 | 1.0 + init_param_float(PARAM_PID_TAU, "PID_TAU", + 0.05f); // Dirty Derivative time constant - See controller documentation | 0.0 | 1.0 /*************************/ /*** PWM CONFIGURATION ***/ /*************************/ - init_param_int(PARAM_MOTOR_PWM_SEND_RATE, "MOTOR_PWM_UPDATE", 0); // Overrides default PWM rate specified by mixer if non-zero - Requires reboot to take effect | 0 | 490 - init_param_float(PARAM_MOTOR_IDLE_THROTTLE, "MOTOR_IDLE_THR", 0.1); // min throttle command sent to motors when armed (Set above 0.1 to spin when armed) | 0.0 | 1.0 - init_param_float(PARAM_FAILSAFE_THROTTLE, "FAILSAFE_THR", 0.3); // Throttle sent to motors in failsafe condition (set just below hover throttle) | 0.0 | 1.0 + init_param_int( + PARAM_MOTOR_PWM_SEND_RATE, "MOTOR_PWM_UPDATE", + 0); // Overrides default PWM rate specified by mixer if non-zero - Requires reboot to take effect | 0 | 490 + init_param_float( + PARAM_MOTOR_IDLE_THROTTLE, "MOTOR_IDLE_THR", + 0.1); // min throttle command sent to motors when armed (Set above 0.1 to spin when armed) | 0.0 | 1.0 + init_param_float(PARAM_FAILSAFE_THROTTLE, "FAILSAFE_THR", + 0.3); // Throttle sent to motors in failsafe condition (set just below hover throttle) | 0.0 | 1.0 init_param_int(PARAM_SPIN_MOTORS_WHEN_ARMED, "ARM_SPIN_MOTORS", true); // Enforce MOTOR_IDLE_THR | 0 | 1 /*******************************/ /*** ESTIMATOR CONFIGURATION ***/ /*******************************/ init_param_int(PARAM_INIT_TIME, "FILTER_INIT_T", 3000); // Time in ms to initialize estimator | 0 | 100000 - init_param_float(PARAM_FILTER_KP_ACC, "FILTER_KP_ACC", 0.5f); // estimator proportional gain on accel-based error - See estimator documentation | 0 | 10.0 - init_param_float(PARAM_FILTER_KI, "FILTER_KI", 0.01f); // estimator integral gain - See estimator documentation | 0 | 1.0 - init_param_float(PARAM_FILTER_KP_EXT, "FILTER_KP_EXT", 1.5f); // estimator proportional gain on external attitude-based error - See estimator documentation | 0 | 10.0 - init_param_float(PARAM_FILTER_ACCEL_MARGIN, "FILTER_ACCMARGIN", 0.1f); // allowable accel norm margin around 1g to determine if accel is usable | 0 | 1.0 - - init_param_int(PARAM_FILTER_USE_QUAD_INT, "FILTER_QUAD_INT", 1); // Perform a quadratic averaging of LPF gyro data prior to integration (adds ~20 us to estimation loop on F1 processors) | 0 | 1 - init_param_int(PARAM_FILTER_USE_MAT_EXP, "FILTER_MAT_EXP", 1); // 1 - Use matrix exponential to improve gyro integration (adds ~90 us to estimation loop in F1 processors) 0 - use euler integration | 0 | 1 - init_param_int(PARAM_FILTER_USE_ACC, "FILTER_USE_ACC", 1); // Use accelerometer to correct gyro integration drift (adds ~70 us to estimation loop) | 0 | 1 - - init_param_int(PARAM_CALIBRATE_GYRO_ON_ARM, "CAL_GYRO_ARM", false); // True if desired to calibrate gyros on arm | 0 | 1 - - init_param_float(PARAM_GYRO_XY_ALPHA, "GYROXY_LPF_ALPHA", 0.3f); // Low-pass filter constant on gyro X and Y axes - See estimator documentation | 0 | 1.0 - init_param_float(PARAM_GYRO_Z_ALPHA, "GYROZ_LPF_ALPHA", 0.3f); // Low-pass filter constant on gyro Z axis - See estimator documentation | 0 | 1.0 - init_param_float(PARAM_ACC_ALPHA, "ACC_LPF_ALPHA", 0.5f); // Low-pass filter constant on all accel axes - See estimator documentation | 0 | 1.0 + init_param_float(PARAM_FILTER_KP_ACC, "FILTER_KP_ACC", + 0.5f); // estimator proportional gain on accel-based error - See estimator documentation | 0 | 10.0 + init_param_float(PARAM_FILTER_KI, "FILTER_KI", + 0.01f); // estimator integral gain - See estimator documentation | 0 | 1.0 + init_param_float( + PARAM_FILTER_KP_EXT, "FILTER_KP_EXT", + 1.5f); // estimator proportional gain on external attitude-based error - See estimator documentation | 0 | 10.0 + init_param_float(PARAM_FILTER_ACCEL_MARGIN, "FILTER_ACCMARGIN", + 0.1f); // allowable accel norm margin around 1g to determine if accel is usable | 0 | 1.0 + + init_param_int(PARAM_FILTER_USE_QUAD_INT, "FILTER_QUAD_INT", + 1); // Perform a quadratic averaging of LPF gyro data prior to integration (adds ~20 us to estimation + // loop on F1 processors) | 0 | 1 + init_param_int(PARAM_FILTER_USE_MAT_EXP, "FILTER_MAT_EXP", + 1); // 1 - Use matrix exponential to improve gyro integration (adds ~90 us to estimation loop in F1 + // processors) 0 - use euler integration | 0 | 1 + init_param_int(PARAM_FILTER_USE_ACC, "FILTER_USE_ACC", + 1); // Use accelerometer to correct gyro integration drift (adds ~70 us to estimation loop) | 0 | 1 + + init_param_int(PARAM_CALIBRATE_GYRO_ON_ARM, "CAL_GYRO_ARM", + false); // True if desired to calibrate gyros on arm | 0 | 1 + + init_param_float(PARAM_GYRO_XY_ALPHA, "GYROXY_LPF_ALPHA", + 0.3f); // Low-pass filter constant on gyro X and Y axes - See estimator documentation | 0 | 1.0 + init_param_float(PARAM_GYRO_Z_ALPHA, "GYROZ_LPF_ALPHA", + 0.3f); // Low-pass filter constant on gyro Z axis - See estimator documentation | 0 | 1.0 + init_param_float(PARAM_ACC_ALPHA, "ACC_LPF_ALPHA", + 0.5f); // Low-pass filter constant on all accel axes - See estimator documentation | 0 | 1.0 init_param_float(PARAM_GYRO_X_BIAS, "GYRO_X_BIAS", 0.0f); // Constant x-bias of gyroscope readings | -1.0 | 1.0 init_param_float(PARAM_GYRO_Y_BIAS, "GYRO_Y_BIAS", 0.0f); // Constant y-bias of gyroscope readings | -1.0 | 1.0 init_param_float(PARAM_GYRO_Z_BIAS, "GYRO_Z_BIAS", 0.0f); // Constant z-bias of gyroscope readings | -1.0 | 1.0 - init_param_float(PARAM_ACC_X_BIAS, "ACC_X_BIAS", 0.0f); // Constant x-bias of accelerometer readings | -2.0 | 2.0 - init_param_float(PARAM_ACC_Y_BIAS, "ACC_Y_BIAS", 0.0f); // Constant y-bias of accelerometer readings | -2.0 | 2.0 - init_param_float(PARAM_ACC_Z_BIAS, "ACC_Z_BIAS", 0.0f); // Constant z-bias of accelerometer readings | -2.0 | 2.0 - init_param_float(PARAM_ACC_X_TEMP_COMP, "ACC_X_TEMP_COMP", 0.0f); // Linear x-axis temperature compensation constant | -2.0 | 2.0 - init_param_float(PARAM_ACC_Y_TEMP_COMP, "ACC_Y_TEMP_COMP", 0.0f); // Linear y-axis temperature compensation constant | -2.0 | 2.0 - init_param_float(PARAM_ACC_Z_TEMP_COMP, "ACC_Z_TEMP_COMP", 0.0f); // Linear z-axis temperature compensation constant | -2.0 | 2.0 - - init_param_float(PARAM_MAG_A11_COMP, "MAG_A11_COMP", 1.0f); // Soft iron compensation constant | -999.0 | 999.0 - init_param_float(PARAM_MAG_A12_COMP, "MAG_A12_COMP", 0.0f); // Soft iron compensation constant | -999.0 | 999.0 - init_param_float(PARAM_MAG_A13_COMP, "MAG_A13_COMP", 0.0f); // Soft iron compensation constant | -999.0 | 999.0 - init_param_float(PARAM_MAG_A21_COMP, "MAG_A21_COMP", 0.0f); // Soft iron compensation constant | -999.0 | 999.0 - init_param_float(PARAM_MAG_A22_COMP, "MAG_A22_COMP", 1.0f); // Soft iron compensation constant | -999.0 | 999.0 - init_param_float(PARAM_MAG_A23_COMP, "MAG_A23_COMP", 0.0f); // Soft iron compensation constant | -999.0 | 999.0 - init_param_float(PARAM_MAG_A31_COMP, "MAG_A31_COMP", 0.0f); // Soft iron compensation constant | -999.0 | 999.0 - init_param_float(PARAM_MAG_A32_COMP, "MAG_A32_COMP", 0.0f); // Soft iron compensation constant | -999.0 | 999.0 - init_param_float(PARAM_MAG_A33_COMP, "MAG_A33_COMP", 1.0f); // Soft iron compensation constant | -999.0 | 999.0 - init_param_float(PARAM_MAG_X_BIAS, "MAG_X_BIAS", 0.0f); // Hard iron compensation constant | -999.0 | 999.0 - init_param_float(PARAM_MAG_Y_BIAS, "MAG_Y_BIAS", 0.0f); // Hard iron compensation constant | -999.0 | 999.0 - init_param_float(PARAM_MAG_Z_BIAS, "MAG_Z_BIAS", 0.0f); // Hard iron compensation constant | -999.0 | 999.0 - - init_param_float(PARAM_BARO_BIAS, "BARO_BIAS", 0.0f); // Barometer measurement bias (Pa) | 0 | inf + init_param_float(PARAM_ACC_X_BIAS, "ACC_X_BIAS", 0.0f); // Constant x-bias of accelerometer readings | -2.0 | 2.0 + init_param_float(PARAM_ACC_Y_BIAS, "ACC_Y_BIAS", 0.0f); // Constant y-bias of accelerometer readings | -2.0 | 2.0 + init_param_float(PARAM_ACC_Z_BIAS, "ACC_Z_BIAS", 0.0f); // Constant z-bias of accelerometer readings | -2.0 | 2.0 + init_param_float(PARAM_ACC_X_TEMP_COMP, "ACC_X_TEMP_COMP", + 0.0f); // Linear x-axis temperature compensation constant | -2.0 | 2.0 + init_param_float(PARAM_ACC_Y_TEMP_COMP, "ACC_Y_TEMP_COMP", + 0.0f); // Linear y-axis temperature compensation constant | -2.0 | 2.0 + init_param_float(PARAM_ACC_Z_TEMP_COMP, "ACC_Z_TEMP_COMP", + 0.0f); // Linear z-axis temperature compensation constant | -2.0 | 2.0 + + init_param_float(PARAM_MAG_A11_COMP, "MAG_A11_COMP", 1.0f); // Soft iron compensation constant | -999.0 | 999.0 + init_param_float(PARAM_MAG_A12_COMP, "MAG_A12_COMP", 0.0f); // Soft iron compensation constant | -999.0 | 999.0 + init_param_float(PARAM_MAG_A13_COMP, "MAG_A13_COMP", 0.0f); // Soft iron compensation constant | -999.0 | 999.0 + init_param_float(PARAM_MAG_A21_COMP, "MAG_A21_COMP", 0.0f); // Soft iron compensation constant | -999.0 | 999.0 + init_param_float(PARAM_MAG_A22_COMP, "MAG_A22_COMP", 1.0f); // Soft iron compensation constant | -999.0 | 999.0 + init_param_float(PARAM_MAG_A23_COMP, "MAG_A23_COMP", 0.0f); // Soft iron compensation constant | -999.0 | 999.0 + init_param_float(PARAM_MAG_A31_COMP, "MAG_A31_COMP", 0.0f); // Soft iron compensation constant | -999.0 | 999.0 + init_param_float(PARAM_MAG_A32_COMP, "MAG_A32_COMP", 0.0f); // Soft iron compensation constant | -999.0 | 999.0 + init_param_float(PARAM_MAG_A33_COMP, "MAG_A33_COMP", 1.0f); // Soft iron compensation constant | -999.0 | 999.0 + init_param_float(PARAM_MAG_X_BIAS, "MAG_X_BIAS", 0.0f); // Hard iron compensation constant | -999.0 | 999.0 + init_param_float(PARAM_MAG_Y_BIAS, "MAG_Y_BIAS", 0.0f); // Hard iron compensation constant | -999.0 | 999.0 + init_param_float(PARAM_MAG_Z_BIAS, "MAG_Z_BIAS", 0.0f); // Hard iron compensation constant | -999.0 | 999.0 + + init_param_float(PARAM_BARO_BIAS, "BARO_BIAS", 0.0f); // Barometer measurement bias (Pa) | 0 | inf init_param_float(PARAM_GROUND_LEVEL, "GROUND_LEVEL", 1387.0f); // Altitude of ground level (m) | -1000 | 10000 init_param_float(PARAM_DIFF_PRESS_BIAS, "DIFF_PRESS_BIAS", 0.0f); // Differential Pressure Bias (Pa) | -10 | 10 @@ -232,10 +260,15 @@ void Params::set_defaults(void) init_param_int(PARAM_RC_Y_CHANNEL, "RC_Y_CHN", 1); // RC input channel mapped to y-axis commands [0 - indexed] | 0 | 3 init_param_int(PARAM_RC_Z_CHANNEL, "RC_Z_CHN", 3); // RC input channel mapped to z-axis commands [0 - indexed] | 0 | 3 init_param_int(PARAM_RC_F_CHANNEL, "RC_F_CHN", 2); // RC input channel mapped to F-axis commands [0 - indexed] | 0 | 3 - init_param_int(PARAM_RC_ATTITUDE_OVERRIDE_CHANNEL, "RC_ATT_OVRD_CHN", 4); // RC switch mapped to attitude override [0 indexed, -1 to disable] | 4 | 7 - init_param_int(PARAM_RC_THROTTLE_OVERRIDE_CHANNEL, "RC_THR_OVRD_CHN", 4); // RC switch channel mapped to throttle override [0 indexed, -1 to disable] | 4 | 7 - init_param_int(PARAM_RC_ATT_CONTROL_TYPE_CHANNEL, "RC_ATT_CTRL_CHN",-1); // RC switch channel mapped to attitude control type [0 indexed, -1 to disable] | 4 | 7 - init_param_int(PARAM_RC_ARM_CHANNEL, "ARM_CHANNEL",-1); // RC switch channel mapped to arming (only if PARAM_ARM_STICKS is false) [0 indexed, -1 to disable] | 4 | 7 + init_param_int(PARAM_RC_ATTITUDE_OVERRIDE_CHANNEL, "RC_ATT_OVRD_CHN", + 4); // RC switch mapped to attitude override [0 indexed, -1 to disable] | 4 | 7 + init_param_int(PARAM_RC_THROTTLE_OVERRIDE_CHANNEL, "RC_THR_OVRD_CHN", + 4); // RC switch channel mapped to throttle override [0 indexed, -1 to disable] | 4 | 7 + init_param_int(PARAM_RC_ATT_CONTROL_TYPE_CHANNEL, "RC_ATT_CTRL_CHN", + -1); // RC switch channel mapped to attitude control type [0 indexed, -1 to disable] | 4 | 7 + init_param_int( + PARAM_RC_ARM_CHANNEL, "ARM_CHANNEL", + -1); // RC switch channel mapped to arming (only if PARAM_ARM_STICKS is false) [0 indexed, -1 to disable] | 4 | 7 init_param_int(PARAM_RC_NUM_CHANNELS, "RC_NUM_CHN", 6); // number of RC input channels | 1 | 8 init_param_int(PARAM_RC_SWITCH_5_DIRECTION, "SWITCH_5_DIR", 1); // RC switch 5 toggle direction | -1 | 1 @@ -243,36 +276,49 @@ void Params::set_defaults(void) init_param_int(PARAM_RC_SWITCH_7_DIRECTION, "SWITCH_7_DIR", 1); // RC switch 7 toggle direction | -1 | 1 init_param_int(PARAM_RC_SWITCH_8_DIRECTION, "SWITCH_8_DIR", 1); // RC switch 8 toggle direction | -1 | 1 - init_param_float(PARAM_RC_OVERRIDE_DEVIATION, "RC_OVRD_DEV", 0.1); // RC stick deviation from center for override | 0.0 | 1.0 - init_param_int(PARAM_OVERRIDE_LAG_TIME, "OVRD_LAG_TIME", 1000); // RC stick deviation lag time before returning control (ms) | 0 | 100000 - init_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE, "MIN_THROTTLE", true); // Take minimum throttle between RC and computer at all times | 0 | 1 - - init_param_int(PARAM_RC_ATTITUDE_MODE, "RC_ATT_MODE", 1); // Attitude mode for RC sticks (0: rate, 1: angle). Overridden if RC_ATT_CTRL_CHN is set. | 0 | 1 - init_param_float(PARAM_RC_MAX_ROLL, "RC_MAX_ROLL", 0.786f); // Maximum roll angle command sent by full deflection of RC sticks | 0.0 | 3.14159 - init_param_float(PARAM_RC_MAX_PITCH, "RC_MAX_PITCH", 0.786f); // Maximum pitch angle command sent by full stick deflection of RC sticks | 0.0 | 3.14159 - init_param_float(PARAM_RC_MAX_ROLLRATE, "RC_MAX_ROLLRATE", 3.14159f); // Maximum roll rate command sent by full stick deflection of RC sticks | 0.0 | 9.42477796077 - init_param_float(PARAM_RC_MAX_PITCHRATE, "RC_MAX_PITCHRATE", 3.14159f); // Maximum pitch command sent by full stick deflection of RC sticks | 0.0 | 3.14159 - init_param_float(PARAM_RC_MAX_YAWRATE, "RC_MAX_YAWRATE", 1.507f); // Maximum pitch command sent by full stick deflection of RC sticks | 0.0 | 3.14159 + init_param_float(PARAM_RC_OVERRIDE_DEVIATION, "RC_OVRD_DEV", + 0.1); // RC stick deviation from center for override | 0.0 | 1.0 + init_param_int(PARAM_OVERRIDE_LAG_TIME, "OVRD_LAG_TIME", + 1000); // RC stick deviation lag time before returning control (ms) | 0 | 100000 + init_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE, "MIN_THROTTLE", + true); // Take minimum throttle between RC and computer at all times | 0 | 1 + + init_param_int(PARAM_RC_ATTITUDE_MODE, "RC_ATT_MODE", + 1); // Attitude mode for RC sticks (0: rate, 1: angle). Overridden if RC_ATT_CTRL_CHN is set. | 0 | 1 + init_param_float(PARAM_RC_MAX_ROLL, "RC_MAX_ROLL", + 0.786f); // Maximum roll angle command sent by full deflection of RC sticks | 0.0 | 3.14159 + init_param_float(PARAM_RC_MAX_PITCH, "RC_MAX_PITCH", + 0.786f); // Maximum pitch angle command sent by full stick deflection of RC sticks | 0.0 | 3.14159 + init_param_float( + PARAM_RC_MAX_ROLLRATE, "RC_MAX_ROLLRATE", + 3.14159f); // Maximum roll rate command sent by full stick deflection of RC sticks | 0.0 | 9.42477796077 + init_param_float(PARAM_RC_MAX_PITCHRATE, "RC_MAX_PITCHRATE", + 3.14159f); // Maximum pitch command sent by full stick deflection of RC sticks | 0.0 | 3.14159 + init_param_float(PARAM_RC_MAX_YAWRATE, "RC_MAX_YAWRATE", + 1.507f); // Maximum pitch command sent by full stick deflection of RC sticks | 0.0 | 3.14159 /***************************/ /*** FRAME CONFIGURATION ***/ /***************************/ - init_param_int(PARAM_MIXER, "MIXER", Mixer::INVALID_MIXER); // Which mixer to choose - See Mixer documentation | 0 | 10 + init_param_int(PARAM_MIXER, "MIXER", + Mixer::INVALID_MIXER); // Which mixer to choose - See Mixer documentation | 0 | 10 - init_param_int(PARAM_FIXED_WING, "FIXED_WING", false); // switches on pass-through commands for fixed-wing operation | 0 | 1 + init_param_int(PARAM_FIXED_WING, "FIXED_WING", + false); // switches on pass-through commands for fixed-wing operation | 0 | 1 init_param_int(PARAM_ELEVATOR_REVERSE, "ELEVATOR_REV", 0); // reverses elevator servo output | 0 | 1 - init_param_int(PARAM_AILERON_REVERSE, "AIL_REV", 0); // reverses aileron servo output | 0 | 1 - init_param_int(PARAM_RUDDER_REVERSE, "RUDDER_REV", 0); // reverses rudder servo output | 0 | 1 + init_param_int(PARAM_AILERON_REVERSE, "AIL_REV", 0); // reverses aileron servo output | 0 | 1 + init_param_int(PARAM_RUDDER_REVERSE, "RUDDER_REV", 0); // reverses rudder servo output | 0 | 1 init_param_float(PARAM_FC_ROLL, "FC_ROLL", 0.0f); // roll angle (deg) of flight controller wrt aircraft body | 0 | 360 - init_param_float(PARAM_FC_PITCH, "FC_PITCH", 0.0f); // pitch angle (deg) of flight controller wrt aircraft body | 0 | 360 + init_param_float(PARAM_FC_PITCH, "FC_PITCH", + 0.0f); // pitch angle (deg) of flight controller wrt aircraft body | 0 | 360 init_param_float(PARAM_FC_YAW, "FC_YAW", 0.0f); // yaw angle (deg) of flight controller wrt aircraft body | 0 | 360 - /********************/ /*** ARMING SETUP ***/ /********************/ - init_param_float(PARAM_ARM_THRESHOLD, "ARM_THRESHOLD", 0.15); // RC deviation from max/min in yaw and throttle for arming and disarming check (us) | 0 | 500 + init_param_float(PARAM_ARM_THRESHOLD, "ARM_THRESHOLD", + 0.15); // RC deviation from max/min in yaw and throttle for arming and disarming check (us) | 0 | 500 /*****************************/ /*** BATTERY MONITOR SETUP ***/ @@ -285,10 +331,12 @@ void Params::set_defaults(void) /************************/ /*** OFFBOARD CONTROL ***/ /************************/ - init_param_int(PARAM_OFFBOARD_TIMEOUT, "OFFBOARD_TIMEOUT", 100); // Timeout in milliseconds for offboard commands, after which RC override is activated | 0 | 100000 + init_param_int( + PARAM_OFFBOARD_TIMEOUT, "OFFBOARD_TIMEOUT", + 100); // Timeout in milliseconds for offboard commands, after which RC override is activated | 0 | 100000 } -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; @@ -393,4 +441,4 @@ bool Params::set_param_by_name_float(const char name[PARAMS_NAME_LENGTH], float tmp.fvalue = value; return set_param_by_name_int(name, tmp.ivalue); } -} +} // namespace rosflight_firmware diff --git a/src/rc.cpp b/src/rc.cpp index 9e823310..91375f48 100644 --- a/src/rc.cpp +++ b/src/rc.cpp @@ -29,18 +29,15 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ - -#include - #include "rc.h" + #include "rosflight.h" +#include + namespace rosflight_firmware { - -RC::RC(ROSflight &_rf) : - RF_(_rf) -{} +RC::RC(ROSflight &_rf) : RF_(_rf) {} void RC::init() { @@ -95,7 +92,6 @@ bool RC::switch_mapped(Switch channel) return switches[channel].mapped; } - void RC::init_sticks(void) { sticks[STICK_X].channel = RF_.params_.get_param_int(PARAM_RC_X_CHANNEL); @@ -140,8 +136,8 @@ void RC::init_switches() break; } - switches[chan].mapped = switches[chan].channel > 3 - && switches[chan].channel < RF_.params_.get_param_int(PARAM_RC_NUM_CHANNELS); + switches[chan].mapped = + switches[chan].channel > 3 && switches[chan].channel < RF_.params_.get_param_int(PARAM_RC_NUM_CHANNELS); switch (switches[chan].channel) { @@ -163,7 +159,8 @@ void RC::init_switches() } if (switches[chan].mapped) - RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_INFO, "%s switch mapped to RC channel %d", channel_name, switches[chan].channel); + 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); } @@ -181,7 +178,7 @@ bool RC::check_rc_lost() else { // go into failsafe if we get an invalid RC command for any channel - for (int8_t i = 0; i 1.25) @@ -250,7 +247,8 @@ void RC::look_for_arm_disarm_signal() if (RF_.rc_.switch_on(SWITCH_ARM)) { if (!RF_.state_manager_.state().armed) - RF_.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM);; + RF_.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM); + ; } else { @@ -259,7 +257,6 @@ void RC::look_for_arm_disarm_signal() } } - bool RC::run() { uint32_t now = RF_.board_.clock_millis(); @@ -271,17 +268,15 @@ bool RC::run() } last_rc_receive_time = now; - // Check for rc lost if (check_rc_lost()) return false; - // read and normalize stick values 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 + if (sticks[channel].one_sided) // generally only F is one_sided { stick_values[channel] = pwm; } @@ -327,7 +322,8 @@ bool RC::new_command() return true; } else - return false;; + return false; + ; } -} +} // namespace rosflight_firmware diff --git a/src/rosflight.cpp b/src/rosflight.cpp index 088e59bc..8feddee7 100644 --- a/src/rosflight.cpp +++ b/src/rosflight.cpp @@ -29,12 +29,12 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#include "interface/param_listener.h" #include "rosflight.h" +#include "interface/param_listener.h" + namespace rosflight_firmware { - ROSflight::ROSflight(Board& board, CommLinkInterface& comm_link) : board_(board), memory_manager_(*this), @@ -66,7 +66,7 @@ void ROSflight::init() params_.init(); config_manager_.init(); - //Initialize devices + // Initialize devices config_manager_.configure_devices(); // Initialize Mixer @@ -105,7 +105,6 @@ void ROSflight::init() state_manager_.check_backup_memory(); } - // Main loop void ROSflight::run() { @@ -146,4 +145,4 @@ 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 22d56dac..28a874ee 100644 --- a/src/sensors.cpp +++ b/src/sensors.cpp @@ -30,35 +30,34 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#include -#include -#include - #include "sensors.h" + #include "param.h" + #include "rosflight.h" #include +#include +#include +#include + namespace rosflight_firmware { - -const float Sensors::BARO_MAX_CHANGE_RATE = 200.0f; // approx 200 m/s +const float Sensors::BARO_MAX_CHANGE_RATE = 200.0f; // approx 200 m/s const float Sensors::BARO_SAMPLE_RATE = 50.0f; -const float Sensors::DIFF_MAX_CHANGE_RATE = 225.0f; // approx 15 m/s^2 +const float Sensors::DIFF_MAX_CHANGE_RATE = 225.0f; // approx 15 m/s^2 const float Sensors::DIFF_SAMPLE_RATE = 50.0f; -const float Sensors::SONAR_MAX_CHANGE_RATE = 100.0f; // 100 m/s +const float Sensors::SONAR_MAX_CHANGE_RATE = 100.0f; // 100 m/s const float Sensors::SONAR_SAMPLE_RATE = 50.0f; const int Sensors::SENSOR_CAL_DELAY_CYCLES = 128; 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 +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() { @@ -73,7 +72,7 @@ void Sensors::init() next_sensor_to_update_ = BAROMETER; float alt = rf_.params_.get_param_float(PARAM_GROUND_LEVEL); - ground_pressure_ = 101325.0f*static_cast(pow((1-2.25694e-5 * alt), 5.2553)); + ground_pressure_ = 101325.0f * static_cast(pow((1 - 2.25694e-5 * alt), 5.2553)); baro_outlier_filt_.init(BARO_MAX_CHANGE_RATE, BARO_SAMPLE_RATE, ground_pressure_); diff_outlier_filt_.init(DIFF_MAX_CHANGE_RATE, DIFF_SAMPLE_RATE, 0.0f); @@ -92,9 +91,9 @@ 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); } @@ -125,7 +124,6 @@ void Sensors::param_change_callback(uint16_t param_id) } } - bool Sensors::run(void) { // First, check for new IMU data @@ -141,7 +139,6 @@ bool Sensors::run(void) return got_imu; } - void Sensors::update_other_sensors() { switch (next_sensor_to_update_) @@ -193,7 +190,7 @@ void Sensors::update_other_sensors() { // 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 + rf_.board_.diff_pressure_update(); // update assists in recovering sensor if it temporarily disappears if (rf_.board_.diff_pressure_present()) { @@ -223,11 +220,11 @@ void Sensors::update_other_sensors() } 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(); - } + 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; @@ -236,7 +233,6 @@ void Sensors::update_other_sensors() next_sensor_to_update_ = (next_sensor_to_update_ + 1) % NUM_LOW_PRIORITY_SENSORS; } - void Sensors::look_for_disabled_sensors() { // Look for disabled sensors while disarmed (poll every second) @@ -359,10 +355,9 @@ bool Sensors::update_imu(void) } } - 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; + float delta_t = (data_.imu_time - int_start_us_) * 1e-6; accel = accel_int_ / delta_t; gyro = gyro_int_ / delta_t; accel_int_ *= 0.0; @@ -376,14 +371,14 @@ void Sensors::update_battery_monitor() 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_); + data_.battery_voltage = data_.battery_voltage * 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_); + data_.battery_current = data_.battery_current * battery_current_alpha_ + + rf_.board_.battery_current_read() * (1 - battery_current_alpha_); } } //====================================================================== @@ -428,19 +423,14 @@ void Sensors::calibrate_gyro() turbomath::Vector vector_max(turbomath::Vector a, turbomath::Vector b) { - return turbomath::Vector(a.x > b.x ? a.x : b.x, - a.y > b.y ? a.y : b.y, - a.z > b.z ? a.z : b.z); + return turbomath::Vector(a.x > b.x ? a.x : b.x, a.y > b.y ? a.y : b.y, a.z > b.z ? a.z : b.z); } turbomath::Vector vector_min(turbomath::Vector a, turbomath::Vector b) { - return turbomath::Vector(a.x < b.x ? a.x : b.x, - a.y < b.y ? a.y : b.y, - a.z < b.z ? a.z : b.z); + return turbomath::Vector(a.x < b.x ? a.x : b.x, a.y < b.y ? a.y : b.y, a.z < b.z ? a.z : b.z); } - void Sensors::calibrate_accel(void) { acc_sum_ = acc_sum_ + data_.accel + gravity_; @@ -454,25 +444,22 @@ void Sensors::calibrate_accel(void) // 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. - turbomath::Vector accel_temp_bias = - { - rf_.params_.get_param_float(PARAM_ACC_X_TEMP_COMP), - rf_.params_.get_param_float(PARAM_ACC_Y_TEMP_COMP), - rf_.params_.get_param_float(PARAM_ACC_Z_TEMP_COMP) - }; + turbomath::Vector accel_temp_bias = {rf_.params_.get_param_float(PARAM_ACC_X_TEMP_COMP), + rf_.params_.get_param_float(PARAM_ACC_Y_TEMP_COMP), + rf_.params_.get_param_float(PARAM_ACC_Z_TEMP_COMP)}; // Figure out the proper accel bias. // We have to consider the contribution of temperature during the calibration, // Which is why this line is so confusing. What we are doing, is first removing // 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_); + turbomath::Vector accel_bias = + (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) + if ((max_ - min_).norm() > 1.0) { rf_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, "Too much movement for IMU cal"); calibrating_acc_flag_ = false; @@ -499,7 +486,7 @@ void Sensors::calibrate_accel(void) // 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", static_cast(accel_bias.norm()), - static_cast(accel_bias.norm()*1000)%1000); + static_cast(accel_bias.norm() * 1000) % 1000); } } @@ -591,17 +578,16 @@ void Sensors::calibrate_diff_pressure() } } - //====================================================== // Correction Functions (These apply calibration constants) 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 + 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); - data_.accel.y -= rf_.params_.get_param_float(PARAM_ACC_Y_TEMP_COMP)*data_.imu_temperature + 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); - data_.accel.z -= rf_.params_.get_param_float(PARAM_ACC_Z_TEMP_COMP)*data_.imu_temperature + 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); data_.gyro.x -= rf_.params_.get_param_float(PARAM_GYRO_X_BIAS); @@ -617,15 +603,15 @@ void Sensors::correct_mag(void) float mag_hard_z = data_.mag.z - rf_.params_.get_param_float(PARAM_MAG_Z_BIAS); // 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; - 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; - 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; + 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; + 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; + 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; } void Sensors::correct_baro(void) @@ -644,8 +630,9 @@ void Sensors::correct_diff_pressure() 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)); + 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) diff --git a/src/state_manager.cpp b/src/state_manager.cpp index 58b497c6..3ff63eb8 100644 --- a/src/state_manager.cpp +++ b/src/state_manager.cpp @@ -30,13 +30,12 @@ */ #include "state_manager.h" + #include "rosflight.h" 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; @@ -138,7 +137,8 @@ void StateManager::set_event(StateManager::Event event) } else { - RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, "RC throttle override must be active to arm"); + RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, + "RC throttle override must be active to arm"); } } else @@ -269,7 +269,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; @@ -278,7 +278,7 @@ void StateManager::write_backup_data(const BackupData::DebugInfo& debug) data.debug = debug; data.finalize(); - RF_.board_.backup_memory_write(reinterpret_cast(&data), sizeof(data)); + RF_.board_.backup_memory_write(reinterpret_cast(&data), sizeof(data)); } void StateManager::check_backup_memory() @@ -334,7 +334,7 @@ void StateManager::update_leds() if (next_led_blink_ms_ < RF_.board_.clock_millis()) { RF_.board_.led1_toggle(); - next_led_blink_ms_ = RF_.board_.clock_millis() + 100; + next_led_blink_ms_ = RF_.board_.clock_millis() + 100; } } // blink slowly if in error @@ -343,7 +343,7 @@ void StateManager::update_leds() if (next_led_blink_ms_ < RF_.board_.clock_millis()) { RF_.board_.led1_toggle(); - next_led_blink_ms_ = RF_.board_.clock_millis() + 500; + next_led_blink_ms_ = RF_.board_.clock_millis() + 500; } } // off if disarmed, on if armed @@ -353,4 +353,4 @@ void StateManager::update_leds() RF_.board_.led1_on(); } -} //namespace rosflight_firmware +} // namespace rosflight_firmware diff --git a/test/command_manager_test.cpp b/test/command_manager_test.cpp index 031188de..c2c4d768 100644 --- a/test/command_manager_test.cpp +++ b/test/command_manager_test.cpp @@ -1,8 +1,9 @@ +#include "cmath" #include "common.h" -#include "rosflight.h" -#include "test_board.h" #include "mavlink.h" -#include "cmath" +#include "test_board.h" + +#include "rosflight.h" #define CHN_LOW 1100 #define CHN_HIGH 1900 @@ -13,7 +14,7 @@ #define OFFBOARD_F 0.9 #define RC_X_PWM 1800 -#define RC_X ((RC_X_PWM - 1500)/500.0 * rf.params_.get_param_float(PARAM_RC_MAX_ROLL)) +#define RC_X ((RC_X_PWM - 1500) / 500.0 * rf.params_.get_param_float(PARAM_RC_MAX_ROLL)) using namespace rosflight_firmware; @@ -27,19 +28,13 @@ class CommandManagerTest : public ::testing::Test uint16_t rc_values[8]; float max_roll, max_pitch, max_yawrate; - control_t offboard_command = - { - 20000, - {true, ANGLE, OFFBOARD_X}, - {true, ANGLE, OFFBOARD_Y}, - {true, RATE, OFFBOARD_Z}, - {true, THROTTLE, OFFBOARD_F} - }; - - CommandManagerTest() : - mavlink(board), - rf(board,mavlink) - {} + control_t offboard_command = {20000, + {true, ANGLE, OFFBOARD_X}, + {true, ANGLE, OFFBOARD_Y}, + {true, RATE, OFFBOARD_Z}, + {true, THROTTLE, OFFBOARD_F}}; + + CommandManagerTest() : mavlink(board), rf(board, mavlink) {} void SetUp() override { @@ -47,10 +42,8 @@ class CommandManagerTest : public ::testing::Test 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 < 4; i++) - rc_values[i] = 1500; - for (int i=4;i<8;i++) - rc_values[i]=1000; + for (int i = 0; i < 4; i++) rc_values[i] = 1500; + for (int i = 4; i < 8; i++) rc_values[i] = 1000; rc_values[2] = 1000; rf.params_.set_param_int(PARAM_MIXER, Mixer::PASSTHROUGH); @@ -78,7 +71,7 @@ class CommandManagerTest : public ::testing::Test } }; -TEST_F (CommandManagerTest, Default) +TEST_F(CommandManagerTest, Default) { board.set_rc(rc_values); stepFirmware(20000); // 20 ms @@ -94,7 +87,7 @@ TEST_F (CommandManagerTest, Default) EXPECT_CLOSE(output.F.value, 0.0); } -TEST_F (CommandManagerTest, RCCommands) +TEST_F(CommandManagerTest, RCCommands) { rc_values[0] = 2000; rc_values[1] = 1000; @@ -105,16 +98,16 @@ TEST_F (CommandManagerTest, RCCommands) control_t output = rf.command_manager_.combined_control(); EXPECT_EQ(output.x.type, ANGLE); - EXPECT_CLOSE(output.x.value, 1.0*max_roll); + EXPECT_CLOSE(output.x.value, 1.0 * max_roll); EXPECT_EQ(output.y.type, ANGLE); - EXPECT_CLOSE(output.y.value, -1.0*max_pitch); + EXPECT_CLOSE(output.y.value, -1.0 * max_pitch); EXPECT_EQ(output.z.type, RATE); - EXPECT_CLOSE(output.z.value, -0.5*max_yawrate); + EXPECT_CLOSE(output.z.value, -0.5 * max_yawrate); EXPECT_EQ(output.F.type, THROTTLE); EXPECT_CLOSE(output.F.value, 0.5); } -TEST_F (CommandManagerTest, ArmWithSticksByDefault) +TEST_F(CommandManagerTest, ArmWithSticksByDefault) { EXPECT_EQ(rf.state_manager_.state().armed, false); rc_values[2] = 1000; @@ -126,7 +119,7 @@ TEST_F (CommandManagerTest, ArmWithSticksByDefault) EXPECT_EQ(rf.state_manager_.state().armed, true); } -TEST_F (CommandManagerTest, DontArmWithSticksWhenUsingSwitch) +TEST_F(CommandManagerTest, DontArmWithSticksWhenUsingSwitch) { rf.params_.set_param_int(PARAM_RC_ARM_CHANNEL, 4); rc_values[2] = 1000; // throttle low @@ -136,7 +129,7 @@ TEST_F (CommandManagerTest, DontArmWithSticksWhenUsingSwitch) EXPECT_EQ(rf.state_manager_.state().armed, false); } -TEST_F (CommandManagerTest, DisarmWithSticksByDefault) +TEST_F(CommandManagerTest, DisarmWithSticksByDefault) { rf.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM); EXPECT_EQ(rf.state_manager_.state().armed, true); @@ -147,17 +140,17 @@ TEST_F (CommandManagerTest, DisarmWithSticksByDefault) EXPECT_EQ(rf.state_manager_.state().armed, false); } -TEST_F (CommandManagerTest, ArmWithSwitch) +TEST_F(CommandManagerTest, ArmWithSwitch) { rf.params_.set_param_int(PARAM_RC_ARM_CHANNEL, 4); - rc_values[2] = 1000; // throttle low + rc_values[2] = 1000; // throttle low rc_values[4] = CHN_HIGH; // switch on board.set_rc(rc_values); stepFirmware(50000); // Immediate EXPECT_EQ(rf.state_manager_.state().armed, true); } -TEST_F (CommandManagerTest, DisarmWithStick) +TEST_F(CommandManagerTest, DisarmWithStick) { rf.params_.set_param_int(PARAM_RC_ARM_CHANNEL, 4); rf.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM); @@ -167,11 +160,11 @@ TEST_F (CommandManagerTest, DisarmWithStick) EXPECT_EQ(rf.state_manager_.state().armed, false); } -TEST_F (CommandManagerTest, DontDisarmWithSticksWhenUsingSwitch) +TEST_F(CommandManagerTest, DontDisarmWithSticksWhenUsingSwitch) { rf.params_.set_param_int(PARAM_RC_ARM_CHANNEL, 4); rc_values[4] = CHN_HIGH; // switch on - rc_values[2] = 1000; // throttle low + rc_values[2] = 1000; // throttle low rf.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM); board.set_rc(rc_values); stepFirmware(50000); @@ -183,32 +176,32 @@ TEST_F (CommandManagerTest, DontDisarmWithSticksWhenUsingSwitch) EXPECT_EQ(rf.state_manager_.state().armed, true); // don't disarm } -TEST_F (CommandManagerTest, ArmStickReversed) +TEST_F(CommandManagerTest, ArmStickReversed) { rf.params_.set_param_int(PARAM_RC_SWITCH_5_DIRECTION, -1); rf.params_.set_param_int(PARAM_RC_ARM_CHANNEL, 4); - rc_values[2] = 1000; // throttle low + rc_values[2] = 1000; // throttle low rc_values[4] = CHN_LOW; // switch on board.set_rc(rc_values); stepFirmware(50000); // Immediate EXPECT_EQ(rf.state_manager_.state().armed, true); } -TEST_F (CommandManagerTest, DisarmStickReversed) +TEST_F(CommandManagerTest, DisarmStickReversed) { rf.params_.set_param_int(PARAM_RC_SWITCH_5_DIRECTION, -1); rf.params_.set_param_int(PARAM_RC_ARM_CHANNEL, 4); rf.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM); EXPECT_EQ(rf.state_manager_.state().armed, true); - rc_values[2] = 1000; // throttle low + rc_values[2] = 1000; // throttle low rc_values[4] = CHN_HIGH; // switch on board.set_rc(rc_values); stepFirmware(50000); // Immediate EXPECT_EQ(rf.state_manager_.state().armed, false); } -TEST_F (CommandManagerTest, DefaultRCOutputd) +TEST_F(CommandManagerTest, DefaultRCOutputd) { board.set_rc(rc_values); stepFirmware(600000); @@ -225,8 +218,7 @@ TEST_F (CommandManagerTest, DefaultRCOutputd) EXPECT_CLOSE(output.F.value, 0.0); } - -TEST_F (CommandManagerTest, RCOutput) +TEST_F(CommandManagerTest, RCOutput) { rc_values[0] = 1250; rc_values[1] = 1750; @@ -248,18 +240,18 @@ TEST_F (CommandManagerTest, RCOutput) EXPECT_CLOSE(output.F.value, 0.5); } -TEST_F (CommandManagerTest, LoseRCDisarmed) +TEST_F(CommandManagerTest, LoseRCDisarmed) { board.set_pwm_lost(true); stepFirmware(20000); control_t output = rf.command_manager_.combined_control(); EXPECT_EQ(output.x.type, ANGLE); - EXPECT_CLOSE(output.x.value, 0.0*max_roll); + EXPECT_CLOSE(output.x.value, 0.0 * max_roll); EXPECT_EQ(output.y.type, ANGLE); - EXPECT_CLOSE(output.y.value, 0.0*max_pitch); + EXPECT_CLOSE(output.y.value, 0.0 * max_pitch); EXPECT_EQ(output.z.type, RATE); - EXPECT_CLOSE(output.z.value, 0.0*max_yawrate); + EXPECT_CLOSE(output.z.value, 0.0 * max_yawrate); EXPECT_EQ(output.F.type, THROTTLE); EXPECT_CLOSE(output.F.value, 0.0); @@ -270,7 +262,7 @@ TEST_F (CommandManagerTest, LoseRCDisarmed) EXPECT_EQ(rf.state_manager_.state().error_codes, StateManager::ERROR_RC_LOST); } -TEST_F (CommandManagerTest, RegainRCDisarmed) +TEST_F(CommandManagerTest, RegainRCDisarmed) { board.set_pwm_lost(true); stepFirmware(40000); @@ -281,7 +273,7 @@ TEST_F (CommandManagerTest, RegainRCDisarmed) EXPECT_EQ(rf.state_manager_.state().error_codes, StateManager::ERROR_NONE); } -TEST_F (CommandManagerTest, LoseRCArmed) +TEST_F(CommandManagerTest, LoseRCArmed) { board.set_rc(rc_values); stepFirmware(50000); @@ -293,11 +285,11 @@ TEST_F (CommandManagerTest, LoseRCArmed) control_t output = rf.command_manager_.combined_control(); EXPECT_EQ(output.x.type, ANGLE); - EXPECT_CLOSE(output.x.value, 0.0*max_roll); + EXPECT_CLOSE(output.x.value, 0.0 * max_roll); EXPECT_EQ(output.y.type, ANGLE); - EXPECT_CLOSE(output.y.value, 0.0*max_pitch); + EXPECT_CLOSE(output.y.value, 0.0 * max_pitch); EXPECT_EQ(output.z.type, RATE); - EXPECT_CLOSE(output.z.value, 0.0*max_yawrate); + EXPECT_CLOSE(output.z.value, 0.0 * max_yawrate); EXPECT_EQ(output.F.type, THROTTLE); EXPECT_CLOSE(output.F.value, rf.params_.get_param_float(PARAM_FAILSAFE_THROTTLE)); @@ -308,7 +300,7 @@ TEST_F (CommandManagerTest, LoseRCArmed) EXPECT_EQ(rf.state_manager_.state().error_codes, StateManager::ERROR_RC_LOST); } -TEST_F (CommandManagerTest, RegainRCArmed) +TEST_F(CommandManagerTest, RegainRCArmed) { board.set_rc(rc_values); stepFirmware(50000); @@ -324,7 +316,7 @@ TEST_F (CommandManagerTest, RegainRCArmed) EXPECT_EQ(rf.state_manager_.state().error_codes, StateManager::ERROR_NONE); } -TEST_F (CommandManagerTest, OffboardCommandMuxNoMinThrottle) +TEST_F(CommandManagerTest, OffboardCommandMuxNoMinThrottle) { stepFirmware(1100000); // Get past LAG_TIME rf.params_.set_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE, false); @@ -345,7 +337,7 @@ TEST_F (CommandManagerTest, OffboardCommandMuxNoMinThrottle) EXPECT_CLOSE(output.F.value, OFFBOARD_F); } -TEST_F (CommandManagerTest, OffboardCommandMuxMinThrottle) +TEST_F(CommandManagerTest, OffboardCommandMuxMinThrottle) { stepFirmware(1100000); // Get past LAG_TIME rf.params_.set_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE, true); @@ -366,7 +358,7 @@ TEST_F (CommandManagerTest, OffboardCommandMuxMinThrottle) EXPECT_CLOSE(output.F.value, 0.0); } -TEST_F (CommandManagerTest, OffboardCommandMuxRollDeviation) +TEST_F(CommandManagerTest, OffboardCommandMuxRollDeviation) { stepFirmware(1100000); // Get past LAG_TIME rf.params_.set_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE, true); @@ -386,7 +378,7 @@ TEST_F (CommandManagerTest, OffboardCommandMuxRollDeviation) EXPECT_CLOSE(output.F.value, 0.0); } -TEST_F (CommandManagerTest, OffboardCommandMuxPitchDeviation) +TEST_F(CommandManagerTest, OffboardCommandMuxPitchDeviation) { stepFirmware(1100000); // Get past LAG_TIME rf.params_.set_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE, true); @@ -406,7 +398,7 @@ TEST_F (CommandManagerTest, OffboardCommandMuxPitchDeviation) EXPECT_CLOSE(output.F.value, 0.0); } -TEST_F (CommandManagerTest, OffboardCommandMuxYawrateDeviation) +TEST_F(CommandManagerTest, OffboardCommandMuxYawrateDeviation) { stepFirmware(1100000); // Get past LAG_TIME rf.params_.set_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE, true); @@ -426,7 +418,7 @@ TEST_F (CommandManagerTest, OffboardCommandMuxYawrateDeviation) EXPECT_CLOSE(output.F.value, 0.0); } -TEST_F (CommandManagerTest, OffboardCommandMuxLag) +TEST_F(CommandManagerTest, OffboardCommandMuxLag) { stepFirmware(1100000); // 1.1s Get past LAG_TIME rf.params_.set_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE, true); @@ -445,27 +437,27 @@ TEST_F (CommandManagerTest, OffboardCommandMuxLag) stepFirmware(500000); // 500 ms setOffboard(offboard_command); - output=rf.command_manager_.combined_control(); + output = rf.command_manager_.combined_control(); EXPECT_CLOSE(output.x.value, 0.0); // lag override = rf.command_manager_.get_rc_override(); EXPECT_EQ(override, 0x3E4); // Throttle and X stick overrides, plus stale offboard stepFirmware(600000); // 600 ms setOffboard(offboard_command); - output=rf.command_manager_.combined_control(); + output = rf.command_manager_.combined_control(); EXPECT_CLOSE(output.x.value, 0.0); // lag override = rf.command_manager_.get_rc_override(); EXPECT_EQ(override, 0x3E0); // Throttle override and stale offboard setOffboard(offboard_command); stepFirmware(20000); // 20 ms - output=rf.command_manager_.combined_control(); + output = rf.command_manager_.combined_control(); EXPECT_CLOSE(output.x.value, OFFBOARD_X); override = rf.command_manager_.get_rc_override(); EXPECT_EQ(override, 0x20); // Throttle override only } -TEST_F (CommandManagerTest, RCAttitudeOverrideSwitch) +TEST_F(CommandManagerTest, RCAttitudeOverrideSwitch) { stepFirmware(1100000); // 1.1s Get past LAG_TIME rf.params_.set_param_int(PARAM_RC_ATTITUDE_OVERRIDE_CHANNEL, 4); @@ -483,10 +475,9 @@ TEST_F (CommandManagerTest, RCAttitudeOverrideSwitch) stepFirmware(50000); // 50ms override = rf.command_manager_.get_rc_override(); EXPECT_EQ(override, 1); - } -TEST_F (CommandManagerTest, RCThrottleOverrideSwitch) +TEST_F(CommandManagerTest, RCThrottleOverrideSwitch) { stepFirmware(1100000); // 1.1s Get past LAG_TIME rf.params_.set_param_int(PARAM_RC_ATTITUDE_OVERRIDE_CHANNEL, -1); @@ -504,16 +495,15 @@ TEST_F (CommandManagerTest, RCThrottleOverrideSwitch) stepFirmware(50000); // 50ms override = rf.command_manager_.get_rc_override(); EXPECT_EQ(override, 2); - } -TEST_F (CommandManagerTest, StaleOffboardCommand) +TEST_F(CommandManagerTest, StaleOffboardCommand) { stepFirmware(1100000); // Get past LAG_TIME rf.params_.set_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE, true); setOffboard(offboard_command); - int timeout_us = rf.params_.get_param_int(PARAM_OFFBOARD_TIMEOUT)*1000; + int timeout_us = rf.params_.get_param_int(PARAM_OFFBOARD_TIMEOUT) * 1000; stepFirmware(timeout_us + 40000); control_t output = rf.command_manager_.combined_control(); @@ -522,7 +512,7 @@ TEST_F (CommandManagerTest, StaleOffboardCommand) EXPECT_CLOSE(output.x.value, 0.0); } -TEST_F (CommandManagerTest, PartialMux) +TEST_F(CommandManagerTest, PartialMux) { offboard_command.x.active = false; stepFirmware(1000000); @@ -538,7 +528,7 @@ TEST_F (CommandManagerTest, PartialMux) EXPECT_EQ(override, 0x60); // X channel stale and Throttle override } -TEST_F (CommandManagerTest, MixedTypes) +TEST_F(CommandManagerTest, MixedTypes) { offboard_command.x.type = RATE; stepFirmware(1000000); diff --git a/test/common.cpp b/test/common.cpp index 2c02283c..5f5de157 100644 --- a/test/common.cpp +++ b/test/common.cpp @@ -8,7 +8,7 @@ double quaternion_error(Eigen::Quaternionf q_eig, turbomath::Quaternion q) return 0; else { - Eigen::Vector3f v_tilde = atan2(q_tilde.vec().norm(), q_tilde.w())*q_tilde.vec()/q_tilde.vec().norm(); + Eigen::Vector3f v_tilde = atan2(q_tilde.vec().norm(), q_tilde.w()) * q_tilde.vec() / q_tilde.vec().norm(); return v_tilde.norm(); } } @@ -22,7 +22,7 @@ double quaternion_error(turbomath::Quaternion q0, turbomath::Quaternion q) return 0; else { - Eigen::Vector3f v_tilde = atan2(q_tilde.vec().norm(), q_tilde.w())*q_tilde.vec()/q_tilde.vec().norm(); + Eigen::Vector3f v_tilde = atan2(q_tilde.vec().norm(), q_tilde.w()) * q_tilde.vec() / q_tilde.vec().norm(); return v_tilde.norm(); } } diff --git a/test/common.h b/test/common.h index 802d0aac..a542f3e4 100644 --- a/test/common.h +++ b/test/common.h @@ -1,50 +1,52 @@ -#include - -#include +#include "eigen3/Eigen/Core" +#include "eigen3/Eigen/Dense" +#include "eigen3/Eigen/Geometry" +#include "test_board.h" #include "rosflight.h" -#include "test_board.h" -#include "eigen3/Eigen/Core" -#include "eigen3/Eigen/Geometry" -#include "eigen3/Eigen/Dense" +#include + +#include #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) #define EXPECT_SUPERCLOSE(x, y) EXPECT_NEAR(x, y, 0.0001) #define EXPECT_CLOSE(x, y) EXPECT_NEAR(x, y, 0.01) diff --git a/test/config_manager_test.cpp b/test/config_manager_test.cpp index bd19137f..5e81386f 100644 --- a/test/config_manager_test.cpp +++ b/test/config_manager_test.cpp @@ -1,11 +1,14 @@ -#include -#include +#include "config_manager.h" -#include "rosflight.h" +#include "configuration_enum.h" #include "mavlink.h" #include "test_board.h" -#include "configuration_enum.h" -#include "config_manager.h" + +#include "rosflight.h" + +#include + +#include using namespace rosflight_firmware; @@ -16,18 +19,13 @@ class ConfigManagerTest : public ::testing::Test Mavlink mavlink; ROSflight rf; - ConfigManagerTest() : - mavlink(board), - rf(board, mavlink) {} - void SetUp() override - { - rf.init(); - } + ConfigManagerTest() : mavlink(board), rf(board, mavlink) {} + void SetUp() override { rf.init(); } }; TEST_F(ConfigManagerTest, DefaultValues) { - for(device_t device{Configuration::FIRST_DEVICE}; device < Configuration::DEVICE_COUNT; ++device) + for (device_t device{Configuration::FIRST_DEVICE}; device < Configuration::DEVICE_COUNT; ++device) EXPECT_EQ(rf.config_manager_[device], 0); } @@ -39,8 +37,8 @@ TEST_F(ConfigManagerTest, SetValid) EXPECT_TRUE(response.successful); EXPECT_TRUE(response.reboot_required); EXPECT_EQ(std::string(reinterpret_cast(response.message)), "Succeed for testing"); - for(device_t device{Configuration::FIRST_DEVICE}; device < Configuration::DEVICE_COUNT; ++device) - if(device == changed_device) + for (device_t device{Configuration::FIRST_DEVICE}; device < Configuration::DEVICE_COUNT; ++device) + if (device == changed_device) EXPECT_EQ(rf.config_manager_[device], config); else EXPECT_EQ(rf.config_manager_[device], 0); @@ -54,6 +52,6 @@ TEST_F(ConfigManagerTest, SetInvalid) EXPECT_FALSE(response.successful); EXPECT_FALSE(response.reboot_required); EXPECT_EQ(std::string(reinterpret_cast(response.message)), "Fail for testing"); - for(device_t device{Configuration::FIRST_DEVICE}; device < Configuration::DEVICE_COUNT; ++device) + for (device_t device{Configuration::FIRST_DEVICE}; device < Configuration::DEVICE_COUNT; ++device) EXPECT_EQ(rf.config_manager_[device], 0); } diff --git a/test/estimator_test.cpp b/test/estimator_test.cpp index 0a510c2f..a91e78b7 100644 --- a/test/estimator_test.cpp +++ b/test/estimator_test.cpp @@ -1,9 +1,11 @@ #include "common.h" +#include "eigen3/unsupported/Eigen/MatrixFunctions" #include "math.h" -#include "rosflight.h" #include "mavlink.h" #include "test_board.h" -#include "eigen3/unsupported/Eigen/MatrixFunctions" + +#include "rosflight.h" + #include #include @@ -35,10 +37,7 @@ 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 { @@ -70,10 +69,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() { @@ -86,24 +82,24 @@ class EstimatorTest : public ::testing::Test 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_); - w[2] = z_amp_*sin(z_freq_/(2.0*M_PI)*t_); - integrate(q_, w, dt_/double(oversampling_factor_)); - t_ += dt_/double(oversampling_factor_); + w[0] = x_amp_ * sin(x_freq_ / (2.0 * M_PI) * t_); + w[1] = y_amp_ * sin(y_freq_ / (2.0 * M_PI) * t_); + w[2] = z_amp_ * sin(z_freq_ / (2.0 * M_PI) * t_); + integrate(q_, w, dt_ / double(oversampling_factor_)); + t_ += dt_ / double(oversampling_factor_); } simulateIMU(acc, gyro); extAttUpdate(); - board.set_imu(acc, gyro, t_*1e6); - board.set_time(t_*1e6); + board.set_imu(acc, gyro, t_ * 1e6); + board.set_time(t_ * 1e6); rf.run(); 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); + err_norm = std::abs(err_norm - 2.0 * M_PI); } max_error = (err_norm > max_error) ? err_norm : max_error; } @@ -112,14 +108,14 @@ class EstimatorTest : public ::testing::Test void integrate(Quaterniond& q, const Vector3d& _w, double dt) { - Vector3d w = _w*dt; + Vector3d w = _w * dt; Quaterniond w_exp; double w_norm = w.norm(); 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.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); @@ -128,9 +124,9 @@ class EstimatorTest : public ::testing::Test else { w_exp.w() = 1.0; - w_exp.x() = w(0)/2.0; - w_exp.y() = w(1)/2.0; - w_exp.z() = w(2)/2.0; + w_exp.x() = w(0) / 2.0; + w_exp.y() = w(1) / 2.0; + w_exp.z() = w(2) / 2.0; w_exp.normalize(); } q = q * w_exp; @@ -139,15 +135,15 @@ class EstimatorTest : public ::testing::Test void simulateIMU(float* acc, float* gyro) { - Vector3d y_acc = q_.inverse() * gravity; + Vector3d y_acc = q_.inverse() * gravity; acc[0] = y_acc.x(); acc[1] = y_acc.y(); acc[2] = y_acc.z(); // Create gyro measurement - gyro[0] = x_amp_*sin(x_freq_/(2.0*M_PI)*t_) + x_gyro_bias_; - gyro[1] = y_amp_*sin(y_freq_/(2.0*M_PI)*t_) + y_gyro_bias_; - gyro[2] = z_amp_*sin(z_freq_/(2.0*M_PI)*t_) + z_gyro_bias_; + gyro[0] = x_amp_ * sin(x_freq_ / (2.0 * M_PI) * t_) + x_gyro_bias_; + gyro[1] = y_amp_ * sin(y_freq_ / (2.0 * M_PI) * t_) + y_gyro_bias_; + gyro[2] = z_amp_ * sin(z_freq_ / (2.0 * M_PI) * t_) + z_gyro_bias_; } void extAttUpdate() @@ -165,17 +161,12 @@ class EstimatorTest : public ::testing::Test } } - double sign(double x) - { - return x < 0 ? -1 : 1; - } + double sign(double x) { return x < 0 ? -1 : 1; } Vector3d computeError() { - Quaterniond rf_quat(rf.estimator_.state().attitude.w, - rf.estimator_.state().attitude.x, - rf.estimator_.state().attitude.y, - rf.estimator_.state().attitude.z); + Quaterniond rf_quat(rf.estimator_.state().attitude.w, rf.estimator_.state().attitude.x, + rf.estimator_.state().attitude.y, rf.estimator_.state().attitude.z); Quaterniond err = q_ * rf_quat.inverse(); Vector3d v(err.x(), err.y(), err.z()); @@ -191,7 +182,7 @@ class EstimatorTest : public ::testing::Test Vector3d log_err; if (norm_v > 1e-4) { - log_err = 2.0 * std::atan(norm_v/w) * v / norm_v; + log_err = 2.0 * std::atan(norm_v / w) * v / norm_v; } else { @@ -201,11 +192,11 @@ class EstimatorTest : public ::testing::Test 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); + 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; @@ -228,20 +219,17 @@ class EstimatorTest : public ::testing::Test double xerr = x_gyro_bias_ - rf.estimator_.bias().x; double yerr = y_gyro_bias_ - rf.estimator_.bias().y; double zerr = z_gyro_bias_ - rf.estimator_.bias().z; - return std::sqrt(xerr*xerr + yerr*yerr + zerr*zerr); + return std::sqrt(xerr * xerr + yerr * yerr + zerr * zerr); } 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(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(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())); return rpy; } - }; TEST_F(EstimatorTest, LinearGyro) @@ -435,7 +423,6 @@ TEST_F(EstimatorTest, EstimateBiasAccel) #endif } - TEST_F(EstimatorTest, StaticExtAtt) { rf.params_.set_param_int(PARAM_FILTER_USE_ACC, false); @@ -486,48 +473,47 @@ TEST_F(EstimatorTest, StaticExtAtt) // This test is fixed by #357 TEST_F(EstimatorTest, MovingExtAtt) { - rf.params_.set_param_int(PARAM_FILTER_USE_ACC, false); - rf.params_.set_param_int(PARAM_FILTER_USE_QUAD_INT, true); - rf.params_.set_param_int(PARAM_FILTER_USE_MAT_EXP, true); - rf.params_.set_param_int(PARAM_ACC_ALPHA, 0); - rf.params_.set_param_int(PARAM_GYRO_XY_ALPHA, 0); - rf.params_.set_param_int(PARAM_GYRO_Z_ALPHA, 0); - rf.params_.set_param_int(PARAM_INIT_TIME, 0.0f); - - turbomath::Quaternion q_tweaked; - q_tweaked.from_RPY(0.2, 0.1, 0.0); - q_.w() = q_tweaked.w; - q_.x() = q_tweaked.x; - q_.y() = q_tweaked.y; - q_.z() = q_tweaked.z; - - x_freq_ = 2.0; - y_freq_ = 3.0; - z_freq_ = 0.5; - x_amp_ = 0.1; - y_amp_ = 0.2; - z_amp_ = -0.1; - - - tmax_ = 150.0; - x_gyro_bias_ = 0.01; - y_gyro_bias_ = -0.03; - z_gyro_bias_ = 0.01; - - oversampling_factor_ = 1; - - ext_att_update_rate_ = 3; + rf.params_.set_param_int(PARAM_FILTER_USE_ACC, false); + rf.params_.set_param_int(PARAM_FILTER_USE_QUAD_INT, true); + rf.params_.set_param_int(PARAM_FILTER_USE_MAT_EXP, true); + rf.params_.set_param_int(PARAM_ACC_ALPHA, 0); + rf.params_.set_param_int(PARAM_GYRO_XY_ALPHA, 0); + rf.params_.set_param_int(PARAM_GYRO_Z_ALPHA, 0); + rf.params_.set_param_int(PARAM_INIT_TIME, 0.0f); + + turbomath::Quaternion q_tweaked; + q_tweaked.from_RPY(0.2, 0.1, 0.0); + q_.w() = q_tweaked.w; + q_.x() = q_tweaked.x; + q_.y() = q_tweaked.y; + q_.z() = q_tweaked.z; + + x_freq_ = 2.0; + y_freq_ = 3.0; + z_freq_ = 0.5; + x_amp_ = 0.1; + y_amp_ = 0.2; + z_amp_ = -0.1; + + tmax_ = 150.0; + x_gyro_bias_ = 0.01; + y_gyro_bias_ = -0.03; + z_gyro_bias_ = 0.01; + + oversampling_factor_ = 1; + + ext_att_update_rate_ = 3; #ifdef DEBUG - initFile("movingExtAtt.bin"); + initFile("movingExtAtt.bin"); #endif - run(); + run(); - double error = computeError().norm(); - EXPECT_LE(error, 4e-3); - EXPECT_LE(biasError(), 2e-2); + double error = computeError().norm(); + EXPECT_LE(error, 4e-3); + EXPECT_LE(biasError(), 2e-2); #ifdef DEBUG - std::cout << "stateError = " << error << std::endl; - std::cout << "biasError = " << biasError() << std::endl; + std::cout << "stateError = " << error << std::endl; + std::cout << "biasError = " << biasError() << std::endl; #endif } diff --git a/test/parameters_test.cpp b/test/parameters_test.cpp index 7a9ac617..c68eb437 100644 --- a/test/parameters_test.cpp +++ b/test/parameters_test.cpp @@ -1,8 +1,10 @@ -#include -#include "test_board.h" #include "mavlink.h" +#include "test_board.h" + #include "rosflight.h" +#include + using namespace rosflight_firmware; #define EXPECT_PARAM_EQ_INT(id, value) EXPECT_EQ(value, rf.params_.get_param_int(id)) diff --git a/test/state_machine_test.cpp b/test/state_machine_test.cpp index f5b2b450..c0aab452 100644 --- a/test/state_machine_test.cpp +++ b/test/state_machine_test.cpp @@ -1,9 +1,9 @@ #include "common.h" - -#include "rosflight.h" #include "mavlink.h" -#include "test_board.h" #include "state_manager.h" +#include "test_board.h" + +#include "rosflight.h" using namespace rosflight_firmware; @@ -14,10 +14,7 @@ class StateMachineTest : public ::testing::Test Mavlink mavlink; ROSflight rf; - StateMachineTest() : - mavlink(board), - rf(board,mavlink) - {} + StateMachineTest() : mavlink(board), rf(board, mavlink) {} void SetUp() override { @@ -40,7 +37,6 @@ class StateMachineTest : public ::testing::Test rf.run(); } } - }; TEST_F(StateMachineTest, Init) @@ -75,9 +71,8 @@ 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; + uint32_t error = StateManager::ERROR_IMU_NOT_RESPONDING | StateManager::ERROR_TIME_GOING_BACKWARDS + | 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); @@ -87,9 +82,8 @@ 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; + uint32_t error = StateManager::ERROR_IMU_NOT_RESPONDING | StateManager::ERROR_TIME_GOING_BACKWARDS + | 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); @@ -100,26 +94,23 @@ 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; + uint32_t error = StateManager::ERROR_IMU_NOT_RESPONDING | StateManager::ERROR_TIME_GOING_BACKWARDS + | 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); EXPECT_EQ(rf.state_manager_.state().failsafe, false); - EXPECT_EQ(rf.state_manager_.state().error_codes, StateManager::ERROR_IMU_NOT_RESPONDING | - StateManager::ERROR_TIME_GOING_BACKWARDS); + EXPECT_EQ(rf.state_manager_.state().error_codes, + StateManager::ERROR_IMU_NOT_RESPONDING | StateManager::ERROR_TIME_GOING_BACKWARDS); EXPECT_EQ(rf.state_manager_.state().error, true); } TEST_F(StateMachineTest, ClearMultipleErrorsAtOnce) { - uint32_t error = StateManager::ERROR_IMU_NOT_RESPONDING | - StateManager::ERROR_TIME_GOING_BACKWARDS | - StateManager::ERROR_UNCALIBRATED_IMU; + uint32_t error = StateManager::ERROR_IMU_NOT_RESPONDING | StateManager::ERROR_TIME_GOING_BACKWARDS + | 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); @@ -128,9 +119,8 @@ 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; + uint32_t error = StateManager::ERROR_IMU_NOT_RESPONDING | StateManager::ERROR_TIME_GOING_BACKWARDS + | StateManager::ERROR_UNCALIBRATED_IMU; rf.state_manager_.set_error(error); rf.state_manager_.clear_error(error); EXPECT_EQ(rf.state_manager_.state().armed, false); @@ -139,7 +129,7 @@ TEST_F(StateMachineTest, ClearAllErrors) EXPECT_EQ(rf.state_manager_.state().error, false); } -TEST_F (StateMachineTest, DoNotArmIfError) +TEST_F(StateMachineTest, DoNotArmIfError) { // Now add, an error, and then try to arm rf.state_manager_.set_error(StateManager::ERROR_INVALID_MIXER); @@ -150,7 +140,7 @@ TEST_F (StateMachineTest, DoNotArmIfError) EXPECT_EQ(rf.state_manager_.state().error, true); } -TEST_F (StateMachineTest, ArmIfNoError) +TEST_F(StateMachineTest, ArmIfNoError) { rf.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM); EXPECT_EQ(rf.state_manager_.state().armed, true); @@ -159,7 +149,7 @@ TEST_F (StateMachineTest, ArmIfNoError) EXPECT_EQ(rf.state_manager_.state().error, false); } -TEST_F (StateMachineTest, ArmAndDisarm) +TEST_F(StateMachineTest, ArmAndDisarm) { rf.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM); EXPECT_EQ(rf.state_manager_.state().armed, true); @@ -171,7 +161,7 @@ TEST_F (StateMachineTest, ArmAndDisarm) EXPECT_EQ(rf.state_manager_.state().error, false); } -TEST_F (StateMachineTest, WaitForCalibrationToArm) +TEST_F(StateMachineTest, WaitForCalibrationToArm) { rf.params_.set_param_int(PARAM_CALIBRATE_GYRO_ON_ARM, true); // try to arm @@ -190,7 +180,7 @@ TEST_F (StateMachineTest, WaitForCalibrationToArm) EXPECT_EQ(rf.state_manager_.state().error, false); } -TEST_F (StateMachineTest, CalibrationFailedDontArm) +TEST_F(StateMachineTest, CalibrationFailedDontArm) { rf.params_.set_param_int(PARAM_CALIBRATE_GYRO_ON_ARM, true); rf.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM); @@ -202,7 +192,7 @@ TEST_F (StateMachineTest, CalibrationFailedDontArm) EXPECT_EQ(rf.state_manager_.state().error, false); } -TEST_F (StateMachineTest, ErrorDuringCalibrationDontArm) +TEST_F(StateMachineTest, ErrorDuringCalibrationDontArm) { rf.params_.set_param_int(PARAM_CALIBRATE_GYRO_ON_ARM, true); rf.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM); @@ -214,7 +204,7 @@ TEST_F (StateMachineTest, ErrorDuringCalibrationDontArm) EXPECT_EQ(rf.state_manager_.state().error, true); } -TEST_F (StateMachineTest, RCLostDuringCalibrationDontArm) +TEST_F(StateMachineTest, RCLostDuringCalibrationDontArm) { rf.params_.set_param_int(PARAM_CALIBRATE_GYRO_ON_ARM, true); rf.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM); @@ -226,7 +216,7 @@ TEST_F (StateMachineTest, RCLostDuringCalibrationDontArm) EXPECT_EQ(rf.state_manager_.state().error, true); } -TEST_F (StateMachineTest, ClearErrorStayDisarmed) +TEST_F(StateMachineTest, ClearErrorStayDisarmed) { rf.params_.set_param_int(PARAM_CALIBRATE_GYRO_ON_ARM, true); rf.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM); @@ -238,7 +228,7 @@ TEST_F (StateMachineTest, ClearErrorStayDisarmed) EXPECT_EQ(rf.state_manager_.state().error_codes, StateManager::ERROR_NONE); } -TEST_F (StateMachineTest, RecoverRCStayDisarmed) +TEST_F(StateMachineTest, RecoverRCStayDisarmed) { rf.params_.set_param_int(PARAM_CALIBRATE_GYRO_ON_ARM, true); rf.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM); @@ -250,7 +240,7 @@ TEST_F (StateMachineTest, RecoverRCStayDisarmed) EXPECT_EQ(rf.state_manager_.state().error_codes, StateManager::ERROR_NONE); } -TEST_F (StateMachineTest, SetErrorsWhileArmed) +TEST_F(StateMachineTest, SetErrorsWhileArmed) { rf.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM); EXPECT_EQ(rf.state_manager_.state().armed, true); @@ -262,7 +252,7 @@ TEST_F (StateMachineTest, SetErrorsWhileArmed) EXPECT_EQ(rf.state_manager_.state().error, true); } -TEST_F (StateMachineTest, ErrorsPersistWhenDisarmed) +TEST_F(StateMachineTest, ErrorsPersistWhenDisarmed) { rf.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM); rf.state_manager_.set_error(StateManager::ERROR_TIME_GOING_BACKWARDS); @@ -274,7 +264,7 @@ TEST_F (StateMachineTest, ErrorsPersistWhenDisarmed) EXPECT_EQ(rf.state_manager_.state().error, true); } -TEST_F (StateMachineTest, UnableToArmWithPersistentErrors) +TEST_F(StateMachineTest, UnableToArmWithPersistentErrors) { rf.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM); rf.state_manager_.set_error(StateManager::ERROR_TIME_GOING_BACKWARDS); @@ -286,7 +276,7 @@ TEST_F (StateMachineTest, UnableToArmWithPersistentErrors) EXPECT_EQ(rf.state_manager_.state().error, true); } -TEST_F (StateMachineTest, ArmIfThrottleLow) +TEST_F(StateMachineTest, ArmIfThrottleLow) { uint16_t rc_values[8]; for (int i = 0; i < 8; i++) @@ -300,7 +290,7 @@ TEST_F (StateMachineTest, ArmIfThrottleLow) EXPECT_EQ(true, rf.state_manager_.state().armed); } -TEST_F (StateMachineTest, ArmIfThrottleHighWithMinThrottle) +TEST_F(StateMachineTest, ArmIfThrottleHighWithMinThrottle) { rf.params_.set_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE, true); uint16_t rc_values[8]; @@ -317,7 +307,7 @@ TEST_F (StateMachineTest, ArmIfThrottleHighWithMinThrottle) EXPECT_EQ(rf.state_manager_.state().armed, false); } -TEST_F (StateMachineTest, DontArmIfThrottleHighWithoutMinThrottle) +TEST_F(StateMachineTest, DontArmIfThrottleHighWithoutMinThrottle) { rf.params_.set_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE, false); uint16_t rc_values[8]; @@ -334,7 +324,7 @@ TEST_F (StateMachineTest, DontArmIfThrottleHighWithoutMinThrottle) EXPECT_EQ(rf.state_manager_.state().armed, false); } -TEST_F (StateMachineTest, LostRCWhenDisarmNoFailsafe) +TEST_F(StateMachineTest, LostRCWhenDisarmNoFailsafe) { rf.state_manager_.set_event(StateManager::EVENT_RC_LOST); EXPECT_EQ(rf.state_manager_.state().armed, false); @@ -343,14 +333,14 @@ TEST_F (StateMachineTest, LostRCWhenDisarmNoFailsafe) EXPECT_EQ(rf.state_manager_.state().error, true); } -TEST_F (StateMachineTest, UnableToArmWithoutRC) +TEST_F(StateMachineTest, UnableToArmWithoutRC) { rf.state_manager_.set_event(StateManager::EVENT_RC_LOST); rf.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM); EXPECT_EQ(rf.state_manager_.state().armed, false); } -TEST_F (StateMachineTest, AbleToArmAfterRCRecovery) +TEST_F(StateMachineTest, AbleToArmAfterRCRecovery) { rf.state_manager_.set_event(StateManager::EVENT_RC_LOST); rf.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM); @@ -360,7 +350,7 @@ TEST_F (StateMachineTest, AbleToArmAfterRCRecovery) EXPECT_EQ(rf.state_manager_.state().armed, true); } -TEST_F (StateMachineTest, RCLostWhileArmedEnterFailsafe) +TEST_F(StateMachineTest, RCLostWhileArmedEnterFailsafe) { rf.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM); rf.state_manager_.set_event(StateManager::EVENT_RC_LOST); @@ -370,7 +360,7 @@ TEST_F (StateMachineTest, RCLostWhileArmedEnterFailsafe) EXPECT_EQ(rf.state_manager_.state().failsafe, true); } -TEST_F (StateMachineTest, DisarmWhileInFailsafeGoToError) +TEST_F(StateMachineTest, DisarmWhileInFailsafeGoToError) { rf.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM); rf.state_manager_.set_event(StateManager::EVENT_RC_LOST); @@ -381,7 +371,7 @@ TEST_F (StateMachineTest, DisarmWhileInFailsafeGoToError) EXPECT_EQ(rf.state_manager_.state().failsafe, true); } -TEST_F (StateMachineTest, RegainRCAfterFailsafe) +TEST_F(StateMachineTest, RegainRCAfterFailsafe) { rf.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM); rf.state_manager_.set_event(StateManager::EVENT_RC_LOST); @@ -392,7 +382,7 @@ TEST_F (StateMachineTest, RegainRCAfterFailsafe) EXPECT_EQ(rf.state_manager_.state().failsafe, false); } constexpr uint32_t StateManager::BackupData::ARM_MAGIC; // C++ is weird -TEST_F (StateMachineTest, NormalBoot) +TEST_F(StateMachineTest, NormalBoot) { board.backup_memory_clear(); rf.state_manager_.check_backup_memory(); @@ -451,7 +441,7 @@ TEST_F(StateMachineTest, CrashRecoveryInvalidArmMagic) { board.backup_memory_clear(); StateManager::BackupData data; - data.arm_flag = StateManager::BackupData::ARM_MAGIC-101; + data.arm_flag = StateManager::BackupData::ARM_MAGIC - 101; data.error_code = 1; data.reset_count = 1; data.finalize(); diff --git a/test/test_board.cpp b/test/test_board.cpp index 2509858e..713bef84 100644 --- a/test/test_board.cpp +++ b/test/test_board.cpp @@ -36,7 +36,6 @@ namespace rosflight_firmware { - void testBoard::set_rc(uint16_t *values) { for (int i = 0; i < 8; i++) @@ -66,24 +65,35 @@ void testBoard::set_imu(float *acc, float *gyro, uint64_t time_us) new_imu_ = true; } - // setup -void testBoard::init_board() +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, hardware_config_t configuration) {} 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; } +uint16_t testBoard::serial_bytes_available() +{ + return 0; +} +uint8_t testBoard::serial_read() +{ + return 0; +} void testBoard::serial_flush() {} // Hardware config @@ -91,10 +101,10 @@ bool testBoard::enable_device(device_t device, hardware_config_t configuration, { (void)configuration; (void)params; - switch(configuration) + switch (configuration) { case Configuration::SERIAL: - serial_init(0,0); + serial_init(0, 0); break; case Configuration::RC: rc_init(); @@ -110,7 +120,10 @@ const TestBoardConfigManager &testBoard::get_board_config_manager() const } // 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() { @@ -122,7 +135,6 @@ bool testBoard::new_imu_data() return false; } - bool testBoard::imu_read(float accel[3], float *temperature, float gyro[3], uint64_t *time) { for (int i = 0; i < 3; i++) @@ -138,7 +150,7 @@ bool testBoard::imu_read(float accel[3], float *temperature, float gyro[3], uint 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; @@ -149,7 +161,7 @@ bool testBoard::backup_memory_read(void *dest, size_t len) void testBoard::backup_memory_write(const void *src, size_t len) { - if(len > BACKUP_MEMORY_SIZE) + if (len > BACKUP_MEMORY_SIZE) len = BACKUP_MEMORY_SIZE; memcpy(backup_memory_, src, len); } @@ -164,21 +176,36 @@ void testBoard::backup_memory_clear() 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) {} -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) {} -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 { @@ -206,23 +233,34 @@ void testBoard::battery_current_set_multiplier(double multiplier) (void)multiplier; } -//GNSS is not supported on the test board -GNSSData testBoard::gnss_read() { return {}; } - -//GNSS is not supported on the test board -GNSSRaw testBoard::gnss_raw_read() { return {}; } +// GNSS is not supported on the test board +GNSSData testBoard::gnss_read() +{ + return {}; +} -//GNSS is not supported on the test board -bool testBoard::gnss_has_new_data() { return false; } +// GNSS is not supported on the test board +GNSSRaw testBoard::gnss_raw_read() +{ + return {}; +} +// GNSS is not supported on the test board +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() {} -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 ; + return static_cast(rc_values[channel] - 1000) / 1000.0; } void testBoard::pwm_write(uint8_t channel, float value) {} void testBoard::pwm_init(uint32_t refresh_rate, uint16_t idle_pwm) {} @@ -230,11 +268,15 @@ void testBoard::pwm_disable() {} // non-volatile memory void testBoard::memory_init() {} -bool testBoard::memory_read(void *dest, size_t len) { +bool testBoard::memory_read(void *dest, size_t len) +{ memset(dest, 0, len); return true; } -bool testBoard::memory_write(const void *src, 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 08750ea5..0c7006b9 100644 --- a/test/test_board.h +++ b/test/test_board.h @@ -38,10 +38,8 @@ namespace rosflight_firmware { - class testBoard : public Board { - private: uint16_t rc_values[8] = {1500, 1500, 1000, 1500, 1500, 1500, 1500, 1500}; uint64_t time_us_ = 0; @@ -54,29 +52,29 @@ class testBoard : public Board TestBoardConfigManager config_manager_; public: -// setup + // setup void init_board() override; void board_reset(bool bootloader) override; -// clock + // clock uint32_t clock_millis() override; uint64_t clock_micros() override; void clock_delay(uint32_t milliseconds) override; -// serial + // serial void serial_init(uint32_t baud_rate, hardware_config_t configuration); 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; -// Hardware config + // Hardware config bool enable_device(device_t device, hardware_config_t configuration, const Params ¶ms) override; - const TestBoardConfigManager & get_board_config_manager() const override; + const TestBoardConfigManager &get_board_config_manager() const override; -// sensors + // sensors void sensors_init() override; - uint16_t num_sensor_errors() ; + 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; @@ -112,22 +110,22 @@ class testBoard : public Board float battery_current_read() const override; void battery_current_set_multiplier(double multiplier) override; -// RC + // RC void rc_init(); bool rc_lost() override; float rc_read(uint8_t channel) override; -// PWM + // PWM void pwm_init(uint32_t refresh_rate, uint16_t idle_pwm) override; void pwm_disable() override; void pwm_write(uint8_t channel, float value) override; -// non-volatile memory + // 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; -// LEDs + // LEDs void led0_on() override; void led0_off() override; void led0_toggle() override; @@ -136,7 +134,7 @@ class testBoard : public Board void led1_off() override; void led1_toggle() override; -//Backup memory + // 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; @@ -147,7 +145,6 @@ class testBoard : public Board void set_rc(uint16_t *values); void set_time(uint64_t time_us); void set_pwm_lost(bool lost); - }; } // namespace rosflight_firmware diff --git a/test/test_board_config_manager.cpp b/test/test_board_config_manager.cpp index 2b8045dc..bb57acd3 100644 --- a/test/test_board_config_manager.cpp +++ b/test/test_board_config_manager.cpp @@ -1,4 +1,5 @@ #include "test_board_config_manager.h" + #include #include @@ -9,31 +10,37 @@ hardware_config_t TestBoardConfigManager::get_max_config(device_t device) const (void)device; return 0; // This is not needed to test other software } -ConfigManager::ConfigResponse TestBoardConfigManager::check_config_change(device_t device, hardware_config_t config, const ConfigManager &cm) const +ConfigManager::ConfigResponse TestBoardConfigManager::check_config_change(device_t device, + hardware_config_t config, + const ConfigManager &cm) const { (void)cm; // A couple variations are given for testing ConfigManager::ConfigResponse response; response.successful = true; response.reboot_required = true; - if(device == Configuration::SERIAL && config == 1) + if (device == Configuration::SERIAL && config == 1) { response.successful = false; response.reboot_required = false; - strcpy(reinterpret_cast(response.message), "Fail for testing"); + strcpy(reinterpret_cast(response.message), "Fail for testing"); return response; } - strcpy(reinterpret_cast(response.message), "Succeed for testing"); + strcpy(reinterpret_cast(response.message), "Succeed for testing"); return response; } -void TestBoardConfigManager::get_device_name(device_t device, char (&name)[BoardConfigManager::DEVICE_NAME_LENGTH]) const +void TestBoardConfigManager::get_device_name(device_t device, + char (&name)[BoardConfigManager::DEVICE_NAME_LENGTH]) const { std::string device_name = "device #" + std::to_string(static_cast(device)); - strcpy(reinterpret_cast(name), device_name.c_str()); + strcpy(reinterpret_cast(name), device_name.c_str()); } -void TestBoardConfigManager::get_config_name(device_t device, hardware_config_t config, char (&name)[BoardConfigManager::CONFIG_NAME_LENGTH]) const +void TestBoardConfigManager::get_config_name(device_t device, + hardware_config_t config, + char (&name)[BoardConfigManager::CONFIG_NAME_LENGTH]) const { - std::string config_name = "config " + std::to_string(static_cast(device)) + ","+std::to_string(static_cast(config)); - strcpy(reinterpret_cast(name), config_name.c_str()); -} + std::string config_name = + "config " + std::to_string(static_cast(device)) + "," + std::to_string(static_cast(config)); + strcpy(reinterpret_cast(name), config_name.c_str()); } +} // namespace rosflight_firmware diff --git a/test/test_board_config_manager.h b/test/test_board_config_manager.h index edf57799..32a23628 100644 --- a/test/test_board_config_manager.h +++ b/test/test_board_config_manager.h @@ -9,10 +9,14 @@ class TestBoardConfigManager : public BoardConfigManager { public: hardware_config_t get_max_config(device_t device) const override; - ConfigManager::ConfigResponse check_config_change(device_t device, hardware_config_t config, const ConfigManager &cm) const override; + ConfigManager::ConfigResponse check_config_change(device_t device, + hardware_config_t config, + const ConfigManager &cm) const override; void get_device_name(device_t device, char (&name)[BoardConfigManager::DEVICE_NAME_LENGTH]) const override; - void get_config_name(device_t device, hardware_config_t config, char (&name)[BoardConfigManager::CONFIG_NAME_LENGTH]) const override; + void get_config_name(device_t device, + hardware_config_t config, + char (&name)[BoardConfigManager::CONFIG_NAME_LENGTH]) const override; }; -} // rosflight_firmware +} // namespace rosflight_firmware #endif // TESTBOARDCONFIGMANAGER_H diff --git a/test/turbotrig_test.cpp b/test/turbotrig_test.cpp index 08c15659..9111b73d 100644 --- a/test/turbotrig_test.cpp +++ b/test/turbotrig_test.cpp @@ -31,69 +31,63 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#include "math.h" #include "common.h" -#include +#include "math.h" -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) -}; +#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::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)}; TEST(TurboMath, atan) { @@ -112,7 +106,6 @@ TEST(TurboMath, sin_cos) } } - TEST(TurboMath, atan2) { for (float i = -100.0; i <= 100.0; i += 0.1) @@ -140,23 +133,22 @@ TEST(TurboMath, asin) TEST(TurboMath, fastAlt) { - - //out of bounds + // out of bounds EXPECT_EQ(turbomath::alt(69681), 0.0); EXPECT_EQ(turbomath::alt(10700), 0.0); - //all valid int values + // 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); + 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 + // 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 } } - TEST(TurboMath, Vector) { for (int i = 0; i < 24; i++) @@ -187,17 +179,17 @@ TEST(TurboMath, Vector) EXPECT_VEC3_SUPERCLOSE((vec1 + vec2), (eig1 + eig2)); EXPECT_VEC3_SUPERCLOSE((vec1 - vec2), (eig1 - eig2)); - EXPECT_VEC3_SUPERCLOSE((vec1*5.0f), (eig1*5.0f)); - EXPECT_VEC3_SUPERCLOSE((vec1*-10.0f), (eig1*-10.0f)); + EXPECT_VEC3_SUPERCLOSE((vec1 * 5.0f), (eig1 * 5.0f)); + EXPECT_VEC3_SUPERCLOSE((vec1 * -10.0f), (eig1 * -10.0f)); vec1 *= -3.0f; - EXPECT_VEC3_SUPERCLOSE(vec1, (eig1*-3.0f)); + EXPECT_VEC3_SUPERCLOSE(vec1, (eig1 * -3.0f)); eig1 *= -3.0f; EXPECT_VEC3_SUPERCLOSE(vec1, eig1); - EXPECT_VEC3_SUPERCLOSE((vec1/-10.0f), (eig1/-10.0f)); + EXPECT_VEC3_SUPERCLOSE((vec1 / -10.0f), (eig1 / -10.0f)); vec1 /= -3.0f; - EXPECT_VEC3_SUPERCLOSE(vec1, (eig1/-3.0f)); + EXPECT_VEC3_SUPERCLOSE(vec1, (eig1 / -3.0f)); eig1 /= -3.0f; // Test Vector Dot Product @@ -208,14 +200,12 @@ TEST(TurboMath, Vector) } } - TEST(TurboMath, Quaternion) { - for (int i = 0; i < 24; i++) { turbomath::Quaternion quat1 = random_quaternions[i].normalize(); - turbomath::Quaternion quat2 = random_quaternions[i+1].normalize(); + turbomath::Quaternion quat2 = random_quaternions[i + 1].normalize(); Eigen::Quaternionf eig1(quat1.w, quat1.x, quat1.y, quat1.z); Eigen::Quaternionf eig2(quat2.w, quat2.x, quat2.y, quat2.z); @@ -224,23 +214,21 @@ 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)); + ASSERT_QUAT_SUPERCLOSE((quat1 * quat2), (eig2 * eig1)); ASSERT_QUAT_SUPERCLOSE(quat1.inverse(), eig1.inverse()); - // Test Quaternion Rotate turbomath::Vector vec1 = random_vectors[i]; Eigen::Vector3f veig1; veig1 << vec1.x, vec1.y, vec1.z; - // Test rotate_vector by rotating vector to new frame turbomath::Vector vec2 = quat1.rotate(vec1); - Eigen::Vector3f veig2 = veig1.transpose()*eig1.toRotationMatrix(); + Eigen::Vector3f veig2 = veig1.transpose() * eig1.toRotationMatrix(); EXPECT_VEC3_SUPERCLOSE(vec2, veig2); // compare with eigen // And rotate back @@ -270,14 +258,13 @@ TEST(TurboMath, QuatFromTwoVectors) { // Test the "quat_from_two_vectors" turbomath::Vector vec1(1.0f, 0.0f, 0.0f); - turbomath::Quaternion quat1(1.0f/sqrt(2.0f), 0.0f, 0.0f, 1.0f/sqrt(2.0f)); + turbomath::Quaternion quat1(1.0f / sqrt(2.0f), 0.0f, 0.0f, 1.0f / sqrt(2.0f)); turbomath::Vector vec2 = quat1.rotate(vec1); turbomath::Quaternion quat_test; quat_test.from_two_unit_vectors(vec2, vec1); ASSERT_TURBOQUAT_SUPERCLOSE(quat1, quat_test); - // A bunch of random (off-axes tests) for (int i = 0; i < 25; i++) {