diff --git a/boards/airbourne/Makefile b/boards/airbourne/Makefile index 7e8bf89a..766650b6 100644 --- a/boards/airbourne/Makefile +++ b/boards/airbourne/Makefile @@ -132,11 +132,13 @@ AIRBOURNE_SRCS = led.cpp \ analog_digital_converter.cpp \ analog_pin.cpp \ battery_monitor.cpp \ + ublox.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..56cd9220 160000 --- a/boards/airbourne/airbourne +++ b/boards/airbourne/airbourne @@ -1 +1 @@ -Subproject commit 2e18392f62926d1f83cc97f2073e8867166f5313 +Subproject commit 56cd9220c4a8756938854fc8429f902ae6683029 diff --git a/boards/airbourne/airbourne_board.cpp b/boards/airbourne/airbourne_board.cpp index af7d8c25..d542a184 100644 --- a/boards/airbourne/airbourne_board.cpp +++ b/boards/airbourne/airbourne_board.cpp @@ -41,12 +41,8 @@ void AirbourneBoard::init_board() led2_.init(LED2_GPIO, LED2_PIN); led1_.init(LED1_GPIO, LED1_PIN); - int_i2c_.init(&i2c_config[BARO_I2C]); - ext_i2c_.init(&i2c_config[EXTERNAL_I2C]); spi1_.init(&spi_config[MPU6000_SPI]); spi3_.init(&spi_config[FLASH_SPI]); - uart1_.init(&uart_config[UART1], 115200, UART::MODE_8N1); - uart3_.init(&uart_config[UART3], 115200, UART::MODE_8N1); backup_sram_init(); @@ -76,46 +72,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 +118,108 @@ 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: + UART *gnss_uart_ptr; + switch (configuration) + { + case AirbourneConfiguration::GNSS_UART1: + gnss_uart_ptr = &uart1_; + break; + case AirbourneConfiguration::GNSS_UART2: + gnss_uart_ptr = &uart2_; + break; + case AirbourneConfiguration::GNSS_UART3: + gnss_uart_ptr = &uart3_; + break; + case AirbourneConfiguration::GNSS_DISABLED: + default: + return true; + } + gnss_.init(gnss_uart_ptr); + 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 +227,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 +269,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 +298,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 +323,8 @@ bool AirbourneBoard::sonar_present() void AirbourneBoard::sonar_update() { - sonar_.update(); + if (sonar_.is_initialized()) + sonar_.update(); } float AirbourneBoard::sonar_read() @@ -245,88 +334,90 @@ float AirbourneBoard::sonar_read() bool AirbourneBoard::gnss_present() { - // return gnss_.present(); - return false; + gnss_.check_connection_status(); + return gnss_.present(); } void AirbourneBoard::gnss_update() {} bool AirbourneBoard::gnss_has_new_data() { - // return this->gnss_.new_data(); - return false; + return this->gnss_.new_data(); } // This method translates the UBLOX driver interface into the ROSFlight interface // If not gnss_has_new_data(), then this may return 0's for ECEF position data, // ECEF velocity data, or both GNSSData AirbourneBoard::gnss_read() { - // UBLOX::GNSSPVT gnss_pvt= gnss_.read(); - // UBLOX::GNSSPosECEF pos_ecef = gnss_.read_pos_ecef(); - // UBLOX::GNSSVelECEF vel_ecef = gnss_.read_vel_ecef(); + UBLOX::GNSSPVT gnss_pvt = gnss_.read(); + UBLOX::GNSSPosECEF pos_ecef = gnss_.read_pos_ecef(); + UBLOX::GNSSVelECEF vel_ecef = gnss_.read_vel_ecef(); + uint64_t timestamp = gnss_.get_last_pvt_timestamp(); GNSSData gnss = {}; - // gnss.time_of_week = gnss_pvt.time_of_week; - // gnss.time = gnss_pvt.time; - // gnss.nanos = gnss_pvt.nanos; - // gnss.lat = gnss_pvt.lat; - // gnss.lon = gnss_pvt.lon; - // gnss.height = gnss_pvt.height; - // gnss.vel_n = gnss_pvt.vel_n; - // gnss.vel_e = gnss_pvt.vel_e; - // gnss.vel_d = gnss_pvt.vel_d; - // gnss.h_acc = gnss_pvt.h_acc; - // gnss.v_acc = gnss_pvt.v_acc; - // //Does not include ECEF position data if the timestamp doesn't match - // //See UBLOX::new_data() for reasoning - // if (gnss.time_of_week == pos_ecef.time_of_week) - // { - // gnss.ecef.x = pos_ecef.x; - // gnss.ecef.y = pos_ecef.y; - // gnss.ecef.z = pos_ecef.z; - // gnss.ecef.p_acc = pos_ecef.p_acc; - // } - // //Does not include ECEF position data if the timestamp doesn't match - // //See UBLOX::new_data() for reasoning - // if (gnss.time_of_week == vel_ecef.time_of_week) - // { - // gnss.ecef.vx = vel_ecef.vx; - // gnss.ecef.vy = vel_ecef.vy; - // gnss.ecef.vz = vel_ecef.vz; - // gnss.ecef.s_acc = vel_ecef.s_acc; - // } - + gnss.time_of_week = gnss_pvt.time_of_week; + bool has_fix = (gnss_pvt.fix_type == UBLOX::FIX_TYPE_3D); + gnss.fix_type = has_fix ? GNSS_FIX_TYPE_FIX : GNSS_FIX_TYPE_NO_FIX; + gnss.time = gnss_pvt.time; + gnss.nanos = gnss_pvt.nanos; + gnss.lat = gnss_pvt.lat; + gnss.lon = gnss_pvt.lon; + gnss.height = gnss_pvt.height; + gnss.vel_n = gnss_pvt.vel_n; + gnss.vel_e = gnss_pvt.vel_e; + gnss.vel_d = gnss_pvt.vel_d; + gnss.h_acc = gnss_pvt.h_acc; + gnss.v_acc = gnss_pvt.v_acc; + // Does not include ECEF position data if the timestamp doesn't match + // See UBLOX::new_data() for reasoning + if (gnss.time_of_week == pos_ecef.time_of_week) + { + gnss.ecef.x = pos_ecef.x; + gnss.ecef.y = pos_ecef.y; + gnss.ecef.z = pos_ecef.z; + gnss.ecef.p_acc = pos_ecef.p_acc; + } + // Does not include ECEF position data if the timestamp doesn't match + // See UBLOX::new_data() for reasoning + if (gnss.time_of_week == vel_ecef.time_of_week) + { + gnss.ecef.vx = vel_ecef.vx; + gnss.ecef.vy = vel_ecef.vy; + gnss.ecef.vz = vel_ecef.vz; + gnss.ecef.s_acc = vel_ecef.s_acc; + } + gnss.rosflight_timestamp = timestamp; return gnss; } -GNSSRaw AirbourneBoard::gnss_raw_read() -{ - // UBLOX::NAV_PVT_t pvt = gnss_.read_raw(); - GNSSRaw raw = {}; - // raw.time_of_week = pvt.iTOW; - // raw.year = pvt.time.year; - // raw.month = pvt.time.month; - // raw.day = pvt.time.day; - // raw.hour = pvt.time.hour; - // raw.min = pvt.time.min; - // raw.sec = pvt.time.sec; - // raw.valid = pvt.time.valid; - // raw.t_acc = pvt.time.tAcc; - // raw.nano = pvt.time.nano; - // raw.fix_type = pvt.fixType; - // raw.num_sat = pvt.numSV; - // raw.lon = pvt.lon; - // raw.lat = pvt.lat; - // raw.height = pvt.height; - // raw.height_msl = pvt.hMSL; - // raw.h_acc = pvt.hAcc; - // raw.v_acc = pvt.vAcc; - // raw.vel_n = pvt.velN; - // raw.vel_e = pvt.velE; - // raw.vel_d = pvt.velD; - // raw.g_speed = pvt.gSpeed; - // raw.head_mot = pvt.headMot; - // raw.s_acc = pvt.sAcc; - // raw.head_acc = pvt.headAcc; - // raw.p_dop = pvt.pDOP; - // raw.rosflight_timestamp = gnss_.get_last_pvt_timestamp(); - return raw; +GNSSFull AirbourneBoard::gnss_full_read() +{ + UBLOX::NAV_PVT_t pvt = gnss_.read_full(); + GNSSFull full = {}; + full.time_of_week = pvt.iTOW; + full.year = pvt.time.year; + full.month = pvt.time.month; + full.day = pvt.time.day; + full.hour = pvt.time.hour; + full.min = pvt.time.min; + full.sec = pvt.time.sec; + full.valid = pvt.time.valid; + full.t_acc = pvt.time.tAcc; + full.nano = pvt.time.nano; + full.fix_type = pvt.fixType; + full.num_sat = pvt.numSV; + full.lon = pvt.lon; + full.lat = pvt.lat; + full.height = pvt.height; + full.height_msl = pvt.hMSL; + full.h_acc = pvt.hAcc; + full.v_acc = pvt.vAcc; + full.vel_n = pvt.velN; + full.vel_e = pvt.velE; + full.vel_d = pvt.velD; + full.g_speed = pvt.gSpeed; + full.head_mot = pvt.headMot; + full.s_acc = pvt.sAcc; + full.head_acc = pvt.headAcc; + full.p_dop = pvt.pDOP; + full.rosflight_timestamp = gnss_.get_last_pvt_timestamp(); + return full; } bool AirbourneBoard::battery_voltage_present() const diff --git a/boards/airbourne/airbourne_board.h b/boards/airbourne/airbourne_board.h index da2cdf25..657c10d8 100644 --- a/boards/airbourne/airbourne_board.h +++ b/boards/airbourne/airbourne_board.h @@ -33,10 +33,12 @@ #define ROSFLIGHT_FIRMWARE_AIRBOURNE_BOARD_H #include "M25P16.h" +#include "airbourne_board_config_manager.h" #include "analog_digital_converter.h" #include "analog_pin.h" #include "backup_sram.h" #include "battery_monitor.h" +#include "board.h" #include "hmc5883l.h" #include "i2c.h" #include "led.h" @@ -52,6 +54,7 @@ #include "spi.h" #include "system.h" #include "uart.h" +#include "ublox.h" #include "vcp.h" #include @@ -59,17 +62,17 @@ #include #include #include -// #include "ublox.h" - -#include "board.h" 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_; @@ -90,7 +93,7 @@ class AirbourneBoard : public Board M25P16 flash_; AnalogDigitalConverter battery_adc_; BatteryMonitor battery_monitor_; - // UBLOX gnss_; + UBLOX gnss_; enum SerialDevice : uint32_t { @@ -132,12 +135,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; @@ -176,9 +183,9 @@ class AirbourneBoard : public Board // GNSS GNSSData gnss_read() override; bool gnss_has_new_data() override; - GNSSRaw gnss_raw_read() override; + GNSSFull gnss_full_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..5477bab2 --- /dev/null +++ b/boards/airbourne/airbourne_board_config_manager.cpp @@ -0,0 +1,307 @@ +#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; + } + break; + 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; + } + break; + 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..49b79f8a 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_); } @@ -389,7 +410,7 @@ GNSSData BreezyBoard::gnss_read() } // GNSS is not supported on breezy boards -GNSSRaw BreezyBoard::gnss_raw_read() +GNSSFull BreezyBoard::gnss_full_read() { return {}; } diff --git a/boards/breezy/breezy_board.h b/boards/breezy/breezy_board.h index 3047bfd3..4b2a1e3f 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; @@ -134,11 +141,11 @@ class BreezyBoard : public Board GNSSData gnss_read() override; bool gnss_has_new_data() override; - GNSSRaw gnss_raw_read() override; + GNSSFull gnss_full_read() override; // 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..592cd902 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; @@ -154,38 +156,38 @@ void Mavlink::send_gnss(uint8_t system_id, const GNSSData &data) send_message(msg); } -void Mavlink::send_gnss_raw(uint8_t system_id, const GNSSRaw &raw) +void Mavlink::send_gnss_full(uint8_t system_id, const GNSSFull &full) { mavlink_message_t msg; - mavlink_rosflight_gnss_raw_t data = {}; - data.time_of_week = raw.time_of_week; - data.year = raw.year; - data.month = raw.month; - data.day = raw.day; - data.hour = raw.hour; - data.min = raw.min; - data.sec = raw.sec; - data.valid = raw.valid; - data.t_acc = raw.t_acc; - data.nano = raw.nano; - data.fix_type = raw.fix_type; - data.num_sat = raw.num_sat; - data.lon = raw.lon; - data.lat = raw.lat; - data.height = raw.height; - data.height_msl = raw.height_msl; - data.h_acc = raw.h_acc; - data.v_acc = raw.v_acc; - data.vel_n = raw.vel_n; - data.vel_e = raw.vel_e; - data.vel_d = raw.vel_d; - data.g_speed = raw.g_speed; - data.head_mot = raw.head_mot; - data.s_acc = raw.s_acc; - data.head_acc = raw.head_acc; - data.p_dop = raw.p_dop; - data.rosflight_timestamp = raw.rosflight_timestamp; - mavlink_msg_rosflight_gnss_raw_encode(system_id, compid_, &msg, &data); + mavlink_rosflight_gnss_full_t data = {}; + data.time_of_week = full.time_of_week; + data.year = full.year; + data.month = full.month; + data.day = full.day; + data.hour = full.hour; + data.min = full.min; + data.sec = full.sec; + data.valid = full.valid; + data.t_acc = full.t_acc; + data.nano = full.nano; + data.fix_type = full.fix_type; + data.num_sat = full.num_sat; + data.lon = full.lon; + data.lat = full.lat; + data.height = full.height; + data.height_msl = full.height_msl; + data.h_acc = full.h_acc; + data.v_acc = full.v_acc; + data.vel_n = full.vel_n; + data.vel_e = full.vel_e; + data.vel_d = full.vel_d; + data.g_speed = full.g_speed; + data.head_mot = full.head_mot; + data.s_acc = full.s_acc; + data.head_acc = full.head_acc; + data.p_dop = full.p_dop; + data.rosflight_timestamp = full.rosflight_timestamp; + mavlink_msg_rosflight_gnss_full_encode(system_id, compid_, &msg, &data); send_message(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; @@ -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..25066462 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, @@ -103,7 +118,7 @@ class Mavlink : public CommLinkInterface void send_timesync(uint8_t system_id, int64_t tc1, int64_t ts1) override; void send_version(uint8_t system_id, const char *const version) override; void send_gnss(uint8_t system_id, const GNSSData &data) override; - void send_gnss_raw(uint8_t system_id, const GNSSRaw &data) override; + void send_gnss_full(uint8_t system_id, const GNSSFull &full) override; void send_error_data(uint8_t system_id, const StateManager::BackupData &error_data) override; void send_battery_status(uint8_t system_id, float voltage, float current) override; @@ -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..b1b2abf5 160000 --- a/comms/mavlink/v1.0 +++ b/comms/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit 872592e9281826de5f62dcd44b85c83e4726b986 +Subproject commit b1b2abf5a8f2b65be183fe1e0fa93309f2abc2c6 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..acd2f04c 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 | @@ -121,7 +123,7 @@ This is a list of all ROSflight parameters, including their types, default value | STRM_SERVO | Rate of raw output stream | int | 50 | 0 | 490 | | STRM_RC | Rate of raw RC input stream | int | 50 | 0 | 50 | | STRM_GNSS | Maximum rate of GNSS data streaming. Higher values allow for lower latency| int | 1000 | 0 | 1000 | -| STRM_GNSS_RAW | Maximum rate of raw GNSS data streaming | int | 0 | 0 | 10 | +| STRM_GNSS_FULL | Maximum rate of fully detailed GNSS data streaming | int | 0 | 0 | 10 | | STRM_BATTERY | Rate of battery status stream | int | 0 | 0 | 50 | PARAM_MAX_CMD | saturation point for PID controller output | float | 1.0 | 0 | 1.0 | | PID_ROLL_RATE_P | Roll Rate Proportional Gain | float | 0.070f | 0.0 | 1000.0 | 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..2d02d775 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; @@ -95,7 +101,7 @@ class Board virtual GNSSData gnss_read() = 0; virtual bool gnss_has_new_data() = 0; - virtual GNSSRaw gnss_raw_read() = 0; + virtual GNSSFull gnss_full_read() = 0; virtual bool battery_voltage_present() const = 0; virtual float battery_voltage_read() const = 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..5e351541 100644 --- a/include/comm_manager.h +++ b/include/comm_manager.h @@ -63,7 +63,7 @@ class CommManager : public CommLinkInterface::ListenerInterface, public ParamLis STREAM_ID_SERVO_OUTPUT_RAW, STREAM_ID_GNSS, - STREAM_ID_GNSS_RAW, + STREAM_ID_GNSS_FULL, STREAM_ID_RC_RAW, STREAM_ID_LOW_PRIORITY, STREAM_COUNT @@ -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); @@ -153,7 +157,7 @@ class CommManager : public CommLinkInterface::ListenerInterface, public ParamLis void send_mag(void); void send_battery_status(void); void send_gnss(void); - void send_gnss_raw(void); + void send_gnss_full(void); void send_low_priority(void); // Debugging Utils @@ -161,6 +165,7 @@ class CommManager : public CommLinkInterface::ListenerInterface, public ParamLis // void send_named_command_struct(const char *const name, control_t command_struct); void send_next_param(void); + void send_next_config_info(void); Stream streams_[STREAM_COUNT] = { Stream(0, [this] { this->send_heartbeat(); }), Stream(0, [this] { this->send_status(); }), @@ -168,12 +173,12 @@ class CommManager : public CommLinkInterface::ListenerInterface, public ParamLis 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_gnss(); }), Stream(0, [this] { this->send_gnss_full(); }), Stream(0, [this] { this->send_rc_raw(); }), Stream(20000, [this] { this->send_low_priority(); })}; // the time of week stamp for the last sent GNSS message, to prevent re-sending uint32_t last_sent_gnss_tow_ = 0; - uint32_t last_sent_gnss_raw_tow_ = 0; + uint32_t last_sent_gnss_full_tow_ = 0; public: CommManager(ROSflight& rf, CommLinkInterface& comm_link); @@ -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..4920378b 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}, 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..7be459ba 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, @@ -183,7 +201,7 @@ class CommLinkInterface virtual void send_timesync(uint8_t system_id, int64_t tc1, int64_t ts1) = 0; virtual void send_version(uint8_t system_id, const char *const version) = 0; virtual void send_gnss(uint8_t system_id, const GNSSData &data) = 0; - virtual void send_gnss_raw(uint8_t system_id, const GNSSRaw &data) = 0; + virtual void send_gnss_full(uint8_t system_id, const GNSSFull &data) = 0; virtual void send_error_data(uint8_t system_id, const StateManager::BackupData &error_data) = 0; virtual void send_battery_status(uint8_t system_id, float voltage, float current) = 0; 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..bc8f6ea6 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 ***/ @@ -61,7 +60,7 @@ enum : uint16_t PARAM_STREAM_AIRSPEED_RATE, PARAM_STREAM_SONAR_RATE, PARAM_STREAM_GNSS_RATE, - PARAM_STREAM_GNSS_RAW_RATE, + PARAM_STREAM_GNSS_FULL_RATE, PARAM_STREAM_BATTERY_STATUS_RATE, PARAM_STREAM_OUTPUT_RAW_RATE, @@ -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/include/sensors.h b/include/sensors.h index 87082638..bdd0f6f8 100644 --- a/include/sensors.h +++ b/include/sensors.h @@ -43,13 +43,12 @@ namespace rosflight_firmware { -// Fix type, as defined in sensor_msgs/NavSatStatus enum GNSSFixType { - GNSS_FIX_TYPE_NO_FIX, // Unable to fix position - GNSS_FIX_TYPE_FIX, // Unaugmented fix - GNSS_FIX_TYPE_SBAS_FIX, // with satellite-based augmentation - GNSS_FIX_TYPE_GBAS_FIX // with ground-based augmentation + GNSS_FIX_TYPE_NO_FIX, + GNSS_FIX_TYPE_FIX, + GNSS_FIX_RTK_FLOAT, // The two RTK fix types are for possible future use. + GNSS_FIX_RTK_FIXED }; struct GNSSData @@ -86,7 +85,7 @@ struct GNSSData GNSSData() { memset(this, 0, sizeof(GNSSData)); } }; -struct GNSSRaw +struct GNSSFull { uint64_t time_of_week; uint16_t year; @@ -116,7 +115,7 @@ struct GNSSRaw uint16_t p_dop; uint64_t rosflight_timestamp; // microseconds, time stamp of last byte in the message - GNSSRaw() { memset(this, 0, sizeof(GNSSRaw)); } + GNSSFull() { memset(this, 0, sizeof(GNSSFull)); } }; class ROSflight; @@ -149,7 +148,7 @@ class Sensors : public ParamListenerInterface bool gnss_new_data = false; float gps_CNO = 0; // What is this? bool gnss_present = false; - GNSSRaw gnss_raw; + GNSSFull gnss_full; turbomath::Vector mag = {0, 0, 0}; 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..3bdd49cb 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); @@ -91,7 +92,7 @@ void CommManager::init() set_streaming_rate(STREAM_ID_BARO, PARAM_STREAM_BARO_RATE); set_streaming_rate(STREAM_ID_SONAR, PARAM_STREAM_SONAR_RATE); set_streaming_rate(STREAM_ID_GNSS, PARAM_STREAM_GNSS_RATE); - set_streaming_rate(STREAM_ID_GNSS_RAW, PARAM_STREAM_GNSS_RAW_RATE); + set_streaming_rate(STREAM_ID_GNSS_FULL, PARAM_STREAM_GNSS_FULL_RATE); set_streaming_rate(STREAM_ID_MAG, PARAM_STREAM_MAG_RATE); set_streaming_rate(STREAM_ID_BATTERY_STATUS, PARAM_STREAM_BATTERY_STATUS_RATE); set_streaming_rate(STREAM_ID_SERVO_OUTPUT_RAW, PARAM_STREAM_OUTPUT_RAW_RATE); @@ -131,8 +132,8 @@ void CommManager::param_change_callback(uint16_t param_id) case PARAM_STREAM_GNSS_RATE: set_streaming_rate(STREAM_ID_GNSS, param_id); break; - case PARAM_STREAM_GNSS_RAW_RATE: - set_streaming_rate(STREAM_ID_GNSS_RAW, param_id); + case PARAM_STREAM_GNSS_FULL_RATE: + set_streaming_rate(STREAM_ID_GNSS_FULL, param_id); break; case PARAM_STREAM_MAG_RATE: set_streaming_rate(STREAM_ID_MAG, param_id); @@ -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) { @@ -544,16 +590,16 @@ void CommManager::send_gnss(void) } } -void CommManager::send_gnss_raw() +void CommManager::send_gnss_full() { - const GNSSRaw& gnss_raw = RF_.sensors_.data().gnss_raw; + const GNSSFull& gnss_full = RF_.sensors_.data().gnss_full; if (RF_.sensors_.data().gnss_present) { - if (gnss_raw.time_of_week != last_sent_gnss_raw_tow_) + if (gnss_full.time_of_week != last_sent_gnss_full_tow_) { - comm_link_.send_gnss_raw(sysid_, RF_.sensors_.data().gnss_raw); - last_sent_gnss_raw_tow_ = gnss_raw.time_of_week; + comm_link_.send_gnss_full(sysid_, RF_.sensors_.data().gnss_full); + last_sent_gnss_full_tow_ = gnss_full.time_of_week; } } } @@ -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/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..e650153f 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,15 +119,15 @@ 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_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 @@ -131,11 +136,11 @@ void Params::set_defaults(void) 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_GNSS_FULL_RATE, "STRM_GNSS_FULL", 10); //Rate of GNSS full stream (Hz) | 0 | 10 + init_param_int(PARAM_STREAM_BATTERY_STATUS_RATE, "STRM_BATTERY", 10); //Rate of battery status 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,24 +155,27 @@ 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 ***/ @@ -227,7 +235,6 @@ void Params::set_defaults(void) /************************/ /*** RC CONFIGURATION ***/ /************************/ - init_param_int(PARAM_RC_TYPE, "RC_TYPE", 0); // Type of RC input 0 - PPM, 1 - SBUS | 0 | 1 init_param_int(PARAM_RC_X_CHANNEL, "RC_X_CHN", 0); // RC input channel mapped to x-axis commands [0 - indexed] | 0 | 3 init_param_int(PARAM_RC_Y_CHANNEL, "RC_Y_CHN", 1); // RC input channel mapped to y-axis commands [0 - indexed] | 0 | 3 init_param_int(PARAM_RC_Z_CHANNEL, "RC_Z_CHN", 3); // RC input channel mapped to z-axis commands [0 - indexed] | 0 | 3 @@ -268,7 +275,6 @@ void Params::set_defaults(void) 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 ***/ /********************/ @@ -297,9 +303,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 +317,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/src/sensors.cpp b/src/sensors.cpp index 6ef034ce..9b733c0c 100644 --- a/src/sensors.cpp +++ b/src/sensors.cpp @@ -150,7 +150,7 @@ void Sensors::update_other_sensors() data_.gnss_new_data = true; rf_.board_.gnss_update(); this->data_.gnss_data = rf_.board_.gnss_read(); - this->data_.gnss_raw = rf_.board_.gnss_raw_read(); + this->data_.gnss_full = rf_.board_.gnss_full_read(); } break; 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..69386fbf 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" 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..a3bd7b7f 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() @@ -218,7 +240,7 @@ GNSSData testBoard::gnss_read() } // GNSS is not supported on the test board -GNSSRaw testBoard::gnss_raw_read() +GNSSFull testBoard::gnss_full_read() { return {}; } @@ -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..6076ff06 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(); @@ -93,7 +99,7 @@ class testBoard : public Board bool gnss_present() override { return false; } void gnss_update() override {} GNSSData gnss_read() override; - GNSSRaw gnss_raw_read() override; + GNSSFull gnss_full_read() override; bool gnss_has_new_data() override; bool battery_voltage_present() const override; @@ -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