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 2e18392f..1d52f046 160000 --- a/boards/airbourne/airbourne +++ b/boards/airbourne/airbourne @@ -1 +1 @@ -Subproject commit 2e18392f62926d1f83cc97f2073e8867166f5313 +Subproject commit 1d52f046b4ca328fda9b0dd047de237b43f1b128 diff --git a/boards/airbourne/airbourne_board.cpp b/boards/airbourne/airbourne_board.cpp index af7d8c25..935205c7 100644 --- a/boards/airbourne/airbourne_board.cpp +++ b/boards/airbourne/airbourne_board.cpp @@ -76,46 +76,39 @@ 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) + vcp_.init(); // VCP is always initialized, so that if UART is mistakenly enabled, it can still be used + 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 AirbourneConfiguration::SERIAL_VCP: current_serial_ = &vcp_; - secondary_serial_device_ = SERIAL_DEVICE_VCP; + break; + case AirbourneConfiguration::SERIAL_UART1: + current_serial_ = &uart1_; + uart1_.init(&uart_config[UART1], baud_rate); + break; + case AirbourneConfiguration::SERIAL_UART2: + current_serial_ = &uart2_; + uart2_.init(&uart_config[UART2], baud_rate); + break; + case AirbourneConfiguration::SERIAL_UART3: + current_serial_ = &uart3_; + uart3_.init(&uart_config[UART3], baud_rate); + break; } } void AirbourneBoard::serial_write(const uint8_t *src, size_t len) { + if (vcp_.connected()) + current_serial_ = &vcp_; current_serial_->write(src, 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(); } @@ -129,6 +122,92 @@ void AirbourneBoard::serial_flush() current_serial_->flush(); } +// Resources +bool AirbourneBoard::enable_device(device_t device, hardware_config_t configuration, const Params ¶ms) +{ + switch (device) + { + 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 Configuration::RC: + switch (configuration) + { + case AirbourneConfiguration::RC_PPM: + rc_init(RC_TYPE_PPM); + break; + case AirbourneConfiguration::RC_SBUS: + rc_init(RC_TYPE_SBUS); + break; + default: + return false; + } + return true; + case Configuration::AIRSPEED: + if (configuration == AirbourneConfiguration::AIRSPEED_I2C2) + { + if (!ext_i2c_.is_initialized()) + ext_i2c_.init(&i2c_config[EXTERNAL_I2C]); + airspeed_.init(&ext_i2c_); + } + break; + case Configuration::GNSS: + // GNSS is currently disabled + break; + case Configuration::SONAR: + 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 == 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); + battery_adc_.init(battery_monitor_config.adc); + battery_monitor_.init(battery_monitor_config, &battery_adc_, voltage_multiplier, current_multiplier); + } + break; + 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_); + } + break; + 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_); + } + break; + default: + return false; + } + return false; +} + +AirbourneBoardConfigManager const &AirbourneBoard::get_board_config_manager() const +{ + return board_config_manager_; +} + // sensors void AirbourneBoard::sensors_init() { @@ -136,14 +215,7 @@ void AirbourneBoard::sensors_init() { } // wait for sensors to boot up imu_.init(&spi1_); - - 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() @@ -185,23 +257,25 @@ bool AirbourneBoard::mag_present() void AirbourneBoard::mag_update() { - mag_.update(); + if (mag_.is_initialized()) + 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(); } void AirbourneBoard::baro_update() { - baro_.update(); + if (baro_.is_initialized()) + baro_.update(); } void AirbourneBoard::baro_read(float *pressure, float *temperature) @@ -212,7 +286,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() @@ -235,7 +311,8 @@ bool AirbourneBoard::sonar_present() void AirbourneBoard::sonar_update() { - sonar_.update(); + if (sonar_.is_initialized()) + sonar_.update(); } float AirbourneBoard::sonar_read() diff --git a/boards/airbourne/airbourne_board.h b/boards/airbourne/airbourne_board.h index da2cdf25..484f7fcc 100644 --- a/boards/airbourne/airbourne_board.h +++ b/boards/airbourne/airbourne_board.h @@ -61,6 +61,7 @@ #include // #include "ublox.h" +#include "airbourne_board_config_manager.h" #include "board.h" namespace rosflight_firmware @@ -68,8 +69,11 @@ namespace rosflight_firmware class AirbourneBoard : public Board { private: + AirbourneBoardConfigManager board_config_manager_; + VCP vcp_; UART uart1_; + UART uart2_; UART uart3_; Serial *current_serial_; // A pointer to the serial stream currently in use. I2C int_i2c_; @@ -132,12 +136,16 @@ 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 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 + bool enable_device(device_t device, hardware_config_t configuration, const Params ¶ms) override; + AirbourneBoardConfigManager const &get_board_config_manager() const override; + // sensors void sensors_init() override; uint16_t num_sensor_errors() override; @@ -178,7 +186,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/airbourne/airbourne_board_config_manager.cpp b/boards/airbourne/airbourne_board_config_manager.cpp new file mode 100644 index 00000000..84a9364a --- /dev/null +++ b/boards/airbourne/airbourne_board_config_manager.cpp @@ -0,0 +1,305 @@ +#include "airbourne_board_config_manager.h" + +#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() {} + +hardware_config_t AirbourneBoardConfigManager::get_max_config(device_t device) const +{ + 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 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) + { + strcpy(resp.message, "Device not found"); + return resp; + } + + if (config > AirbourneBoardConfigManager::max_configs[device]) + { + strcpy(resp.message, "Configuration not found"); + return resp; + } + if (config == cm[device]) + { + strcpy(resp.message, "Configuration already set. No change required"); + resp.successful = true; + resp.reboot_required = false; + return resp; + } + + 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: + 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) + { + for (device_t other_device{Configuration::FIRST_DEVICE}; other_device != Configuration::DEVICE_COUNT; + ++other_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 + continue; + if (port == get_port(other_device, cm[other_device])) + { + conflict_device = other_device; + break; + } + } + } + break; + case Configuration::AIRSPEED: + case Configuration::SONAR: + if (cm[Configuration::GNSS] == AirbourneConfiguration::GNSS_UART3) + conflict_device = Configuration::GNSS; + if (cm[Configuration::SERIAL] == AirbourneConfiguration::GNSS_UART3) + conflict_device = Configuration::SERIAL; + break; + default: + break; + } + if (conflict_device != Configuration::DEVICE_COUNT) + { + switch (conflict_device) + { + case Configuration::SERIAL: + strcpy(resp.message, "Port is used by serial."); + break; + case Configuration::RC: + strcpy(resp.message, "Port is used by RC."); + break; + case Configuration::AIRSPEED: + strcpy(resp.message, "Port is used by airspeed sensor."); + break; + case Configuration::GNSS: + strcpy(resp.message, "Port is used by GNSS receiver."); + break; + case Configuration::SONAR: + 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(resp.message, "Port is used by battery monitor."); + break; + case Configuration::BAROMETER: + strcpy(resp.message, "Port is used by barometer."); + break; + case Configuration::MAGNETOMETER: + strcpy(resp.message, "Port is used by magnetometer."); + break; + default: + strcpy(resp.message, "Other error."); + break; + } + return resp; + } + resp.successful = true; + resp.reboot_required = true; + 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) + { + case Configuration::SERIAL: + strcpy(name, "Serial"); + break; + case Configuration::RC: + strcpy(name, "RC"); + break; + case Configuration::AIRSPEED: + strcpy(name, "Airspeed"); + break; + case Configuration::GNSS: + strcpy(name, "GNSS"); + break; + case Configuration::SONAR: + strcpy(name, "Sonar"); + break; + case Configuration::BATTERY_MONITOR: + strcpy(name, "Battery Monitor"); + break; + case Configuration::BAROMETER: + strcpy(name, "Baro"); + break; + case Configuration::MAGNETOMETER: + strcpy(name, "Mag"); + break; + default: + strcpy(name, "Error/Unsupported"); + break; + } +} +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 + switch (device) + { + case Configuration::SERIAL: + switch (config) + { + case AirbourneConfiguration::SERIAL_VCP: + strcpy(name, "VCP over USB"); + break; + case AirbourneConfiguration::SERIAL_UART1: + strcpy(name, "UART1 on Main"); + break; + case AirbourneConfiguration::SERIAL_UART2: + strcpy(name, "UART2 on Flex-IO"); + break; + case AirbourneConfiguration::SERIAL_UART3: + strcpy(name, "UART3 on Flexi"); + break; + } + break; + case Configuration::RC: + if (config == AirbourneConfiguration::RC_PPM) + strcpy(name, "PPM on Flex-IO"); + else if (config == AirbourneConfiguration::RC_SBUS) + strcpy(name, "SBUS on Main"); + break; + case Configuration::AIRSPEED: + if (config == AirbourneConfiguration::AIRSPEED_DISABLED) + strcpy(name, "Disabled"); + else if (config == AirbourneConfiguration::AIRSPEED_I2C2) + strcpy(name, "I2C2 on Flexi"); + break; + case Configuration::GNSS: + switch (config) + { + case AirbourneConfiguration::GNSS_DISABLED: + strcpy(name, "Disabled"); + break; + case AirbourneConfiguration::GNSS_UART1: + strcpy(name, "UART1 on main"); + break; + case AirbourneConfiguration::GNSS_UART2: + strcpy(name, "UART2 on Flex-Io"); + break; + case AirbourneConfiguration::GNSS_UART3: + strcpy(name, "UART3 on Flexi"); + break; + } + break; + case Configuration::SONAR: + if (config == AirbourneConfiguration::SONAR_DISABLED) + strcpy(name, "Disabled"); + else if (config == AirbourneConfiguration::SONAR_I2C2) + strcpy(name, "I2C2 on Flexi"); + break; + case Configuration::BATTERY_MONITOR: + if (config == AirbourneConfiguration::BATTERY_MONITOR_DISABLED) + strcpy(name, "Disabled"); + else if (config == AirbourneConfiguration::BATTERY_MONITOR_ADC3) + strcpy(name, "ADC3 on Power"); + break; + case Configuration::BAROMETER: + if (config == AirbourneConfiguration::BAROMETER_DISABLED) + strcpy(name, "Disabled"); + else if (config == AirbourneConfiguration::BAROMETER_ONBOARD) + 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 mag"); + break; + default: + strcpy(name, "Invalid device"); + } +} +AirbourneBoardConfigManager::Port AirbourneBoardConfigManager::get_port(uint8_t device, uint8_t config) const +{ + switch (device) + { + case Configuration::SERIAL: + switch (config) + { + case AirbourneConfiguration::SERIAL_VCP: + return USB_PORT; + case AirbourneConfiguration::SERIAL_UART1: + return MAIN_PORT; + case AirbourneConfiguration::SERIAL_UART2: + return FLEX_IO_PORT; + case AirbourneConfiguration::SERIAL_UART3: + return FLEXI_PORT; + } + case Configuration::GNSS: + switch (config) + { + case AirbourneConfiguration::GNSS_DISABLED: + return NO_PORT; + case AirbourneConfiguration::GNSS_UART1: + return MAIN_PORT; + case AirbourneConfiguration::GNSS_UART2: + return FLEX_IO_PORT; + case AirbourneConfiguration::GNSS_UART3: + return FLEXI_PORT; + } + case Configuration::RC: + if (config == AirbourneConfiguration::RC_PPM) + return FLEX_IO_PORT; + 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 == AirbourneConfiguration::SONAR_I2C2) + return FLEXI_PORT; + break; + case Configuration::BATTERY_MONITOR: + 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 == AirbourneConfiguration::BAROMETER_ONBOARD) + return INTERNAL_I2C; + } + return NO_PORT; +} +} // namespace 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..23e60b4a --- /dev/null +++ b/boards/airbourne/airbourne_board_config_manager.h @@ -0,0 +1,39 @@ +#ifndef AIRBOURNE_BOARD_CONFIG_MANAGER_H +#define AIRBOURNE_BOARD_CONFIG_MANAGER_H + +#include "airbourne_configuration_enum.h" +#include "board_config_manager.h" + +namespace rosflight_firmware +{ +class ROSflight; +class AirbourneBoard; +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; + 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, + 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 + INTERNAL_I2C + }; + Port get_port(uint8_t device, uint8_t config) const; // Get the port used by a given configuration +}; +} // namespace rosflight_firmware + +#endif // AIRBOURNE_BOARD_CONFIG_MANAGER_H diff --git a/boards/airbourne/airbourne_configuration_enum.h b/boards/airbourne/airbourne_configuration_enum.h new file mode 100644 index 00000000..117a8e72 --- /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 AirbourneConfiguration +} // namespace rosflight_firmware +#endif // AIRBOURNE_CONFIGURATION_ENUM_H 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 699c4ee7..8ee3ce21 100644 --- a/boards/breezy/breezy_board.cpp +++ b/boards/breezy/breezy_board.cpp @@ -78,9 +78,8 @@ 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) { - (void)dev; Serial1 = uartOpen(USART1, NULL, baud_rate, MODE_RXTX); } @@ -107,6 +106,29 @@ void BreezyBoard::serial_flush() return; } +bool BreezyBoard::enable_device(device_t device, hardware_config_t configuration, const Params ¶ms) +{ + (void)configuration; + switch (device) + { + case Configuration::RC: + rc_init(); + break; + case Configuration::SERIAL: + serial_init(params.get_param_int(PARAM_BAUD_RATE)); + break; + default: + break; + } + + return true; +} + +const BreezyBoardConfigManager &BreezyBoard::get_board_config_manager() const +{ + return config_manager_; +} + // sensors void BreezyBoard::sensors_init() @@ -330,9 +352,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 3047bfd3..cef767bf 100644 --- a/boards/breezy/breezy_board.h +++ b/boards/breezy/breezy_board.h @@ -42,6 +42,8 @@ extern "C" } #include "board.h" +#include "breezy_board_config_manager.h" +#include "configuration_enum.h" #include "sensors.h" namespace rosflight_firmware @@ -77,6 +79,7 @@ class BreezyBoard : public Board bool new_imu_data_; uint64_t imu_time_us_; + BreezyBoardConfigManager config_manager_; public: BreezyBoard(); @@ -91,12 +94,16 @@ 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); 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; + const BreezyBoardConfigManager &get_board_config_manager() const override; + // sensors void sensors_init() override; uint16_t num_sensor_errors() override; @@ -138,7 +145,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(); 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 new file mode 100644 index 00000000..e47279e2 --- /dev/null +++ b/boards/breezy/breezy_board_config_manager.cpp @@ -0,0 +1,43 @@ +#include "breezy_board_config_manager.h" + +#include + +namespace rosflight_firmware +{ +hardware_config_t BreezyBoardConfigManager::get_max_config(device_t device) const +{ + (void)device; + return 0; +} + +ConfigManager::ConfigResponse BreezyBoardConfigManager::check_config_change(device_t device, + hardware_config_t config, + const ConfigManager &cm) const +{ + (void)device; + (void)config; + (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, + 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)device; + (void)config; + strcpy(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..88616f73 --- /dev/null +++ b/boards/breezy/breezy_board_config_manager.h @@ -0,0 +1,22 @@ +#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) 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 + +#endif // BREEZYBOARDCONFIGMANAGER_H diff --git a/comms/mavlink/mavlink.cpp b/comms/mavlink/mavlink.cpp index 36566976..51ede484 100644 --- a/comms/mavlink/mavlink.cpp +++ b/comms/mavlink/mavlink.cpp @@ -38,9 +38,8 @@ namespace rosflight_firmware { Mavlink::Mavlink(Board &board) : board_(board) {} -void Mavlink::init(uint32_t baud_rate, uint32_t dev) +void Mavlink::init() { - board_.serial_init(baud_rate, dev); initialized_ = true; } @@ -110,6 +109,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; @@ -266,6 +268,46 @@ 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_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); + send_message(msg); +} + +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)); + 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]) +{ + mavlink_message_t msg; + 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; @@ -290,7 +332,7 @@ void Mavlink::send_sonar(uint8_t system_id, 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, @@ -425,6 +467,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); @@ -541,6 +586,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) @@ -572,6 +636,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 cd000636..7bc1ba5d 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, @@ -85,6 +85,21 @@ 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_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, @@ -94,7 +109,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, @@ -121,6 +136,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..95b44529 160000 --- a/comms/mavlink/v1.0 +++ b/comms/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit 872592e9281826de5f62dcd44b85c83e4726b986 +Subproject commit 95b44529c2cd87d59b09b90ec22d976dfa6ecaaa diff --git a/docs/developer-guide/code-architecture.md b/docs/developer-guide/code-architecture.md index 87bb696b..dafef1cb 100644 --- a/docs/developer-guide/code-architecture.md +++ b/docs/developer-guide/code-architecture.md @@ -86,12 +86,21 @@ The operation of the state manager is defined by the following finite state mach The state manager also includes functionality for recovering from hard faults. In the case of a hard fault, the firmware writes a small amount of data to backup memory then reboots. This backup memory location is checked and then cleared after every reboot. The backup memory includes the armed state of the flight controller. On reboot, the firmware will initialize then, if this armed-state flag is set, immediately transition back into the armed state. This functionality allows for continued RC control in the case of a hard fault. Hard faults are not expected with the stable firmware code base, but this feature adds an additional layer of safety if experimental changes are being made to the firmware itself. +### 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. 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. 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 new file mode 100644 index 00000000..480e88cb --- /dev/null +++ b/docs/user-guide/firmware-configuration.md @@ -0,0 +1,150 @@ +# Firmware Configuration + +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 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: +``` +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: '' +``` +## 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. + +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 `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 configuration #0. +###OpenPilot Revolution (Revo) Configurations +####Serial +See [Using Secondary Serial Links](hardware-setup.md#using-secondary-serial-links) for more details on this setting. + +| 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|Multiple I2C devices can share the port.| + +####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|Multiple I2C devices can share the port.| + +####Battery Monitor +| Configuration | Number | port | Notes | +| ------------- | ------ | ---- | ----- | +|Disabled|0|None|| +|ADC3 on Power|1|PWR / Sonar|| + +####Baro (Barometer) + +| Configuration | Number | port | Notes | +| ------------- | ------ | ---- | ----- | +|Disabled|0|None|| +|Onboard Baro|1|None|| + +####Mag (Magnetometer) + +| Configuration | Number | port | Notes | +| ------------- | ------ | ---- | ----- | +|Disabled|0|None|| +|Onboard Mag|1|None|| 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/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/overview.md b/docs/user-guide/overview.md index 5ffe41a4..ab0f2eef 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 8bbda704..6bee9423 100644 --- a/docs/user-guide/parameter-configuration.md +++ b/docs/user-guide/parameter-configuration.md @@ -45,18 +45,21 @@ 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 `memory_write`. This also saves firmware configurations. ``` -rosservice call /param_write +rosservice call /memory_write ``` + `rosflight_io` should then respond with + ``` [ INFO] [1491672597.123201952]: Param write succeeded [ INFO] [1491672597.123452908]: Onboard parameters have been saved ``` + !!! important It is highly recommended that you write parameters before arming and flying the vehicle. Among other things, this will ensure that in the rare case that a hard fault is encountered and the flight controller must reboot during flight, the correct configuration will be loaded on reboot. @@ -108,7 +111,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 | 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. diff --git a/include/board.h b/include/board.h index 47e33ab9..6c260bea 100644 --- a/include/board.h +++ b/include/board.h @@ -32,6 +32,9 @@ #ifndef ROSFLIGHT_FIRMWARE_BOARD_H #define ROSFLIGHT_FIRMWARE_BOARD_H +#include "board_config_manager.h" +#include "configuration_enum.h" +#include "param.h" #include "sensors.h" #include "state_manager.h" @@ -60,12 +63,15 @@ 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_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; + virtual const BoardConfigManager &get_board_config_manager() const = 0; + // sensors virtual void sensors_init() = 0; virtual uint16_t num_sensor_errors() = 0; @@ -106,7 +112,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/board_config_manager.h b/include/board_config_manager.h new file mode 100644 index 00000000..db8ce331 --- /dev/null +++ b/include/board_config_manager.h @@ -0,0 +1,62 @@ +#ifndef BOARD_CONFIG_MANAGER_H + +#define BOARD_CONFIG_MANAGER_H + +#include "config_manager.h" +#include "configuration_enum.h" + +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 + * @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. + * @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 + */ + 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. + * 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; + /** + * @brief Returns the name of a configuration + * @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 + */ + virtual void get_config_name(device_t device, hardware_config_t config, char (&name)[CONFIG_NAME_LENGTH]) const = 0; +}; +} // namespace rosflight_firmware + +#endif // BOARD_CONFIG_MANAGER_H diff --git a/include/comm_manager.h b/include/comm_manager.h index ff9b43b0..1dbef25f 100644 --- a/include/comm_manager.h +++ b/include/comm_manager.h @@ -82,6 +82,8 @@ class CommManager : public CommLinkInterface::ListenerInterface, public ParamLis ROSflight& RF_; CommLinkInterface& comm_link_; uint8_t send_params_index_; + device_t send_device_info_index_{Configuration::DEVICE_COUNT}; + hardware_config_t send_config_info_index_{0}; bool initialized_ = false; bool connected_ = false; @@ -140,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); @@ -161,15 +165,16 @@ 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(); }), 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; @@ -183,6 +188,10 @@ 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 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/command_manager.h b/include/command_manager.h index 0ba8d8de..85494a7f 100644 --- a/include/command_manager.h +++ b/include/command_manager.h @@ -84,9 +84,9 @@ class CommandManager : public ParamListenerInterface // clang-format off control_t rc_command_ = {0, - {false, ANGLE, 0.0}, - {false, ANGLE, 0.0}, - {false, RATE, 0.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}, @@ -124,28 +124,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; - - rc_stick_override_t rc_stick_override_[3] = {{RC::STICK_X, 0}, {RC::STICK_Y, 0}, {RC::STICK_Z, 0}}; + RCOverrideReason stick_override_reason; + RCOverrideReason offboard_inactive_override_reason; + 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 + }; 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 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. + * 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); @@ -154,7 +192,19 @@ class CommandManager : public ParamListenerInterface CommandManager(ROSflight &_rf); void init(); bool run(); - bool rc_override_active(); + /** + * @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); void set_new_rc_command(control_t new_rc_command); diff --git a/include/config_manager.h b/include/config_manager.h new file mode 100644 index 00000000..1058804d --- /dev/null +++ b/include/config_manager.h @@ -0,0 +1,89 @@ +#ifndef HARDWARE_CONFIG_H +#define HARDWARE_CONFIG_H + +#include "configuration_enum.h" + +namespace rosflight_firmware +{ +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 + * @sa Configuration::device_t + */ +class ConfigManager +{ +public: + struct __attribute__((packed)) Config + { + uint32_t checksum; + hardware_config_t config[Configuration::DEVICE_COUNT]; + }; + + static constexpr int CONFIG_RESPONSE_MESSAGE_LENGTH = 50; + /** + * @brief A struct to hold responses to attempts to change configurations + */ + 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 */ + }; + + ConfigManager(ROSflight &RF, Config &config); + /** + * @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 + */ + bool init(); + /** + * @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; + /** + * @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: + 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 +}; +} // namespace rosflight_firmware +#endif // HARDWARE_CONFIG_H diff --git a/include/configuration_enum.h b/include/configuration_enum.h new file mode 100644 index 00000000..44420af1 --- /dev/null +++ b/include/configuration_enum.h @@ -0,0 +1,51 @@ +#ifndef CONFIGURATION_ENUM_H +#define CONFIGURATION_ENUM_H + +#include + +/** + * Test + */ +namespace rosflight_firmware +{ +/** + * The namespace for Configuration options + */ +namespace Configuration +{ +/** + * @brief An enumeration of configurable devices + */ +enum device_t : uint8_t +{ + SERIAL, + RC, + AIRSPEED, + GNSS, + SONAR, + BATTERY_MONITOR, + BAROMETER, + MAGNETOMETER, + DEVICE_COUNT /**(0)}; +/** + * @brief Allows incrementing device_t's for use in for loops. Stops incrementing past DEVICE_COUNT + */ +inline device_t& operator++(device_t& dev) +{ + uint8_t return_value = dev; + return_value++; + if (return_value > DEVICE_COUNT) + return_value--; + dev = static_cast(return_value); + 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/interface/comm_link.h b/include/interface/comm_link.h index cc4490fd..861d80d5 100644 --- a/include/interface/comm_link.h +++ b/include/interface/comm_link.h @@ -66,7 +66,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 @@ -124,9 +125,11 @@ 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; + virtual void init() = 0; virtual void receive() = 0; // send functions @@ -165,6 +168,21 @@ 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_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, @@ -174,7 +192,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, diff --git a/include/memory_manager.h b/include/memory_manager.h new file mode 100644 index 00000000..ad55a970 --- /dev/null +++ b/include/memory_manager.h @@ -0,0 +1,55 @@ +#ifndef MEMORY_MANAGER_H +#define MEMORY_MANAGER_H + +#include "config_manager.h" +#include "param.h" + +namespace rosflight_firmware +{ +class ROSflight; +class MemoryManager +{ +public: + struct PersistentMemory + { + Params::params_t params; + ConfigManager::Config config; + }; + + 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: + ROSflight &RF_; + PersistentMemory memory_; + bool ready_{false}; +}; +} // namespace rosflight_firmware + +#endif // MEMORY_MANAGER_H diff --git a/include/param.h b/include/param.h index 8b80d9d9..4c0aded9 100644 --- a/include/param.h +++ b/include/param.h @@ -45,7 +45,6 @@ enum : uint16_t /*** HARDWARE CONFIGURATION ***/ /******************************/ PARAM_BAUD_RATE = 0, - PARAM_SERIAL_DEVICE, /*****************************/ /*** MAVLINK CONFIGURATION ***/ @@ -156,7 +155,6 @@ enum : uint16_t /************************/ /*** RC CONFIGURATION ***/ /************************/ - PARAM_RC_TYPE, PARAM_RC_X_CHANNEL, PARAM_RC_Y_CHANNEL, PARAM_RC_Z_CHANNEL, @@ -239,6 +237,7 @@ class Params int32_t ivalue; }; +public: typedef struct { uint32_t version; @@ -253,8 +252,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); @@ -264,7 +264,7 @@ class Params size_t num_listeners_; public: - Params(ROSflight &_rf); + Params(ROSflight &_rf, params_t ¶m_struct); // function declarations @@ -293,10 +293,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 9b2927bc..bcb7bdaf 100644 --- a/include/rosflight.h +++ b/include/rosflight.h @@ -38,8 +38,10 @@ #include "board.h" #include "comm_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" @@ -56,8 +58,9 @@ class ROSflight ROSflight(Board& board, CommLinkInterface& comm_link); Board& board_; - CommManager comm_manager_; + MemoryManager memory_manager_; + CommManager comm_manager_; Params params_; CommandManager command_manager_; @@ -67,6 +70,7 @@ class ROSflight RC rc_; Sensors sensors_; StateManager state_manager_; + ConfigManager config_manager_; uint32_t loop_time_us; 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 diff --git a/scripts/rosflight.mk b/scripts/rosflight.mk index af1a6bb1..ca89f073 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 551b087a..f1a1d90c 100644 --- a/src/comm_manager.cpp +++ b/src/comm_manager.cpp @@ -76,11 +76,12 @@ CommManager::CommManager(ROSflight& rf, CommLinkInterface& comm_link) : RF_(rf), // 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; + send_device_info_index_ = Configuration::DEVICE_COUNT; + send_config_info_index_ = 0; update_system_id(PARAM_SYSTEM_ID); set_streaming_rate(STREAM_ID_HEARTBEAT, PARAM_STREAM_HEARTBEAT_RATE); @@ -250,7 +251,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(); @@ -279,6 +280,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; } } @@ -396,6 +400,48 @@ void CommManager::heartbeat_callback(void) this->send_heartbeat(); } +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::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) + send_config_value(static_cast(device)); +} +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) +{ + 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, Configuration::DEVICE_COUNT); +} + +void CommManager::send_config_info(device_t device, hardware_config_t config) +{ + 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); +} + +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) { @@ -440,7 +486,7 @@ void CommManager::send_status(void) 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_.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()); } @@ -561,6 +607,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()) @@ -606,6 +653,22 @@ void CommManager::send_next_param(void) } } +void CommManager::send_next_config_info(void) +{ + if (send_device_info_index_ < Configuration::DEVICE_COUNT) + { + if (send_config_info_index_ == 0) + 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_)) + { + ++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/command_manager.cpp b/src/command_manager.cpp index 11134625..f4da1db3 100644 --- a/src/command_manager.cpp +++ b/src/command_manager.cpp @@ -64,6 +64,7 @@ CommandManager::CommandManager(ROSflight &_rf) : RF_(_rf), failsafe_command_(mul void CommandManager::init() { init_failsafe(); + rc_override_ = determine_override_status(); } void CommandManager::param_change_callback(uint16_t param_id) @@ -150,82 +151,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[channel].onboard->active) - { - 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[channel].combined = override_this_channel ? *muxes[channel].rc : *muxes[channel].onboard; - return override_this_channel; + 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; } -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() +uint16_t CommandManager::get_rc_override() { return rc_override_; } @@ -260,7 +235,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) @@ -283,10 +258,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_) diff --git a/src/config_manager.cpp b/src/config_manager.cpp new file mode 100644 index 00000000..2c0bde6d --- /dev/null +++ b/src/config_manager.cpp @@ -0,0 +1,90 @@ +#include "config_manager.h" + +#include "rosflight.h" + +namespace rosflight_firmware +{ +ConfigManager::ConfigManager(ROSflight &RF, Config &config) : RF_{RF}, config_{config} {} + +bool ConfigManager::init() +{ + if (!read()) + fill_defaults(); + return true; +} + +bool ConfigManager::configure_devices() const +{ + bool success = true; + 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; +} + +ConfigManager::ConfigResponse ConfigManager::attempt_set_configuration(device_t device, uint8_t config) +{ + 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; +} +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) const +{ + return config_.config[device]; +} +uint8_t ConfigManager::operator[](device_t device) const +{ + return get_configuration(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; + 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; +} + +void ConfigManager::fill_defaults() +{ + memset(config_.config, 0, sizeof(config_.config)); +} +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); + 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_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); +} + +} // namespace rosflight_firmware diff --git a/src/memory_manager.cpp b/src/memory_manager.cpp new file mode 100644 index 00000000..c257caa3 --- /dev/null +++ b/src/memory_manager.cpp @@ -0,0 +1,21 @@ +#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(PersistentMemory)); + return ready_; +} +bool MemoryManager::write_memory() +{ + RF_.params_.prepare_write(); + RF_.config_manager_.prepare_write(); + return RF_.board_.memory_write(&memory_, sizeof(PersistentMemory)); +} + +} // namespace rosflight_firmware diff --git a/src/mixer.cpp b/src/mixer.cpp index 8dd5756f..c19e39de 100644 --- a/src/mixer.cpp +++ b/src/mixer.cpp @@ -55,7 +55,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 5f8ac391..94bef8bc 100644 --- a/src/param.cpp +++ b/src/param.cpp @@ -56,7 +56,13 @@ namespace rosflight_firmware { -Params::Params(ROSflight &_rf) : RF_(_rf), 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) @@ -98,13 +104,12 @@ 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(); } } @@ -114,28 +119,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_SERIAL_DEVICE, "SERIAL_DEVICE", 0); // Serial Port (for supported devices) | 0 | 3 + 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 ***/ @@ -150,76 +155,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 @@ -227,15 +257,19 @@ 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 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 +277,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,7 +332,9 @@ 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 } // clang-format on @@ -297,9 +346,8 @@ void Params::set_listeners(ParamListenerInterface *const listeners[], size_t num 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; @@ -312,17 +360,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/rc.cpp b/src/rc.cpp index df58f4e1..91375f48 100644 --- a/src/rc.cpp +++ b/src/rc.cpp @@ -47,7 +47,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(); } @@ -56,9 +55,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: diff --git a/src/rosflight.cpp b/src/rosflight.cpp index 09647da8..8feddee7 100644 --- a/src/rosflight.cpp +++ b/src/rosflight.cpp @@ -37,15 +37,17 @@ 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), mixer_(*this), rc_(*this), sensors_(*this), - state_manager_(*this) + state_manager_(*this), + config_manager_(*this, memory_manager_.get_config()) { comm_link.set_listener(&comm_manager_); params_.set_listeners(param_listeners_, num_param_listeners_); @@ -58,7 +60,14 @@ void ROSflight::init() state_manager_.init(); // Read EEPROM to get initial params + memory_manager_.read_memory(); + + // Prepare to initialize devices params_.init(); + config_manager_.init(); + + // Initialize devices + config_manager_.configure_devices(); // Initialize Mixer mixer_.init(); diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 57486408..09c80725 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,10 +53,12 @@ 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 estimator_test.cpp parameters_test.cpp + config_manager_test.cpp ) target_link_libraries(unit_tests ${GTEST_LIBRARIES} pthread) diff --git a/test/command_manager_test.cpp b/test/command_manager_test.cpp index cf87743b..ab27ffbc 100644 --- a/test/command_manager_test.cpp +++ b/test/command_manager_test.cpp @@ -1,3 +1,4 @@ +#include "cmath" #include "common.h" #include "mavlink.h" #include "test_board.h" @@ -43,10 +44,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 < 8; i++) - { - rc_values[i] = 1500; - } + 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); @@ -77,7 +76,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); @@ -331,6 +330,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); @@ -348,6 +351,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); @@ -364,6 +371,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); @@ -380,6 +391,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); @@ -396,6 +411,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)); @@ -404,33 +423,81 @@ 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, 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, 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, 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); // 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) @@ -443,6 +510,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); } @@ -454,10 +523,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) diff --git a/test/config_manager_test.cpp b/test/config_manager_test.cpp new file mode 100644 index 00000000..5e81386f --- /dev/null +++ b/test/config_manager_test.cpp @@ -0,0 +1,57 @@ +#include "config_manager.h" + +#include "configuration_enum.h" +#include "mavlink.h" +#include "test_board.h" + +#include "rosflight.h" + +#include + +#include + +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{Configuration::FIRST_DEVICE}; 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::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"); + 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); +} + +TEST_F(ConfigManagerTest, SetInvalid) +{ + device_t changed_device = Configuration::SERIAL; + hardware_config_t config = 1; + 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"); + for (device_t device{Configuration::FIRST_DEVICE}; device < Configuration::DEVICE_COUNT; ++device) + EXPECT_EQ(rf.config_manager_[device], 0); +} diff --git a/test/parameters_test.cpp b/test/parameters_test.cpp index 90a2aacc..c68eb437 100644 --- a/test/parameters_test.cpp +++ b/test/parameters_test.cpp @@ -49,7 +49,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 aeaeb55f..713bef84 100644 --- a/test/test_board.cpp +++ b/test/test_board.cpp @@ -84,7 +84,7 @@ uint64_t testBoard::clock_micros() 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() { @@ -96,6 +96,28 @@ uint8_t testBoard::serial_read() } void testBoard::serial_flush() {} +// Hardware config +bool testBoard::enable_device(device_t device, hardware_config_t configuration, const Params ¶ms) +{ + (void)configuration; + (void)params; + switch (configuration) + { + case Configuration::SERIAL: + serial_init(0, 0); + break; + case Configuration::RC: + rc_init(); + break; + } + + return true; +} + +const TestBoardConfigManager &testBoard::get_board_config_manager() const +{ + return config_manager_; +} // sensors void testBoard::sensors_init() {} uint16_t testBoard::num_sensor_errors() @@ -231,7 +253,7 @@ bool testBoard::gnss_has_new_data() // 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_; @@ -248,7 +270,8 @@ void testBoard::pwm_disable() {} void testBoard::memory_init() {} bool testBoard::memory_read(void *dest, size_t len) { - return false; + memset(dest, 0, len); + return true; } bool testBoard::memory_write(const void *src, size_t len) { diff --git a/test/test_board.h b/test/test_board.h index bf0f95f9..0c7006b9 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 { @@ -48,6 +49,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 @@ -60,12 +62,16 @@ 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); 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; + const TestBoardConfigManager &get_board_config_manager() const override; + // sensors void sensors_init() override; uint16_t num_sensor_errors(); @@ -105,7 +111,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 new file mode 100644 index 00000000..bb57acd3 --- /dev/null +++ b/test/test_board_config_manager.cpp @@ -0,0 +1,46 @@ +#include "test_board_config_manager.h" + +#include +#include + +namespace rosflight_firmware +{ +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 +{ + (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) + { + response.successful = false; + response.reboot_required = 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, + 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, + 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()); +} +} // namespace rosflight_firmware diff --git a/test/test_board_config_manager.h b/test/test_board_config_manager.h new file mode 100644 index 00000000..32a23628 --- /dev/null +++ b/test/test_board_config_manager.h @@ -0,0 +1,22 @@ +#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) 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 +#endif // TESTBOARDCONFIGMANAGER_H