Merge branch 'main' into 432-new-mixing-system

include/board.h

@@ -54,6 +54,13 @@ class Board
   virtual void init_board() = 0;
   virtual void board_reset(bool bootloader) = 0;
 
+  virtual void sensors_init(void) = 0;
+  virtual uint16_t sensors_errors_count() = 0;
+
+  virtual uint16_t sensors_init_message_count() =  0;
+  virtual bool sensors_init_message_good(uint16_t i) = 0;
+  virtual uint16_t sensors_init_message(char *message, uint16_t size, uint16_t i) = 0;
+
   // clock
   virtual uint32_t clock_millis() = 0;
   virtual uint64_t clock_micros() = 0;
@@ -67,11 +74,8 @@ class Board
   virtual uint8_t serial_read() = 0;
   virtual void serial_flush() = 0;
 
-  // sensors
-  virtual void sensors_init() = 0;
-  virtual uint16_t num_sensor_errors() = 0;
-
   // IMU
+  virtual bool imu_present() = 0;
   virtual bool imu_has_new_data() = 0;
   virtual bool imu_read(float accel[3], float * temperature, float gyro[3], uint64_t * time) = 0;
   virtual void imu_not_responding_error() = 0;

include/comm_manager.h

@@ -162,7 +162,6 @@ class CommManager : public CommLinkInterface::ListenerInterface, public ParamLis
 
   // Debugging Utils
   void send_named_value_int(const char * const name, int32_t value);
-  //    void send_named_command_struct(const char *const name, control_t command_struct);
 
   void send_next_param(void);
 
@@ -180,8 +179,8 @@ class CommManager : public CommLinkInterface::ListenerInterface, public ParamLis
   void send_param_value(uint16_t param_id);
   void update_status();
   void log(CommLinkInterface::LogSeverity severity, const char * fmt, ...);
+  void log_message(CommLinkInterface::LogSeverity severity, char * text);
 
-  void send_parameter_list();
   void send_named_value_float(const char * const name, float value);
 
   void send_backup_data(const StateManager::BackupData & backup_data);

include/interface/comm_link.h

@@ -32,7 +32,6 @@
 #ifndef ROSFLIGHT_FIRMWARE_COMM_LINK_H
 #define ROSFLIGHT_FIRMWARE_COMM_LINK_H
 
-#include "board.h"
 #include "param.h"
 #include "sensors.h"
 #include "state_manager.h"

include/sensors.h

@@ -146,7 +146,7 @@ class Sensors : public ParamListenerInterface
     float imu_temperature = 0;
     uint64_t imu_time = 0;
 
-    float diff_pressure_velocity = 0;
+    float diff_pressure_ias = 0;
     float diff_pressure = 0;
     float diff_pressure_temp = 0;
 
@@ -271,7 +271,6 @@ class Sensors : public ParamListenerInterface
 
   // Baro Calibration
   bool baro_calibrated_ = false;
-  float ground_pressure_ = 0.0f;
   uint16_t baro_calibration_count_ = 0;
   uint32_t last_baro_cal_iter_ms_ = 0;
   float baro_calibration_mean_ = 0.0f;

src/comm_manager.cpp

@@ -129,8 +129,6 @@ void CommManager::param_request_list_callback(uint8_t target_system)
   if (target_system == sysid_) { send_params_index_ = 0; }
 }
 
-void CommManager::send_parameter_list() { send_params_index_ = 0; }
-
 void CommManager::param_request_read_callback(uint8_t target_system, const char * const param_name,
                                               int16_t param_index)
 {
@@ -339,10 +337,15 @@ void CommManager::log(CommLinkInterface::LogSeverity severity, const char * fmt,
   // Convert the format string to a raw char array
   va_list args;
   va_start(args, fmt);
-  char text[LOG_MSG_SIZE];
-  vsnprintf(text, LOG_MSG_SIZE, fmt, args);
+  char message[LOG_MSG_SIZE];
+  vsnprintf(message, LOG_MSG_SIZE, fmt, args);
   va_end(args);
 
+  log_message(severity, message);
+}
+
+void CommManager::log_message(CommLinkInterface::LogSeverity severity, char * text)
+{
   if (initialized_ && connected_) {
     comm_link_.send_log_message(sysid_, severity, text);
   } else {
@@ -372,7 +375,7 @@ void CommManager::send_status(void)
   comm_link_.send_status(
     sysid_, RF_.state_manager_.state().armed, RF_.state_manager_.state().failsafe,
     RF_.command_manager_.rc_override_active(), RF_.command_manager_.offboard_control_active(),
-    RF_.state_manager_.state().error_codes, control_mode, RF_.board_.num_sensor_errors(),
+    RF_.state_manager_.state().error_codes, control_mode, RF_.board_.sensors_errors_count(),
     RF_.get_loop_time_us());
 }
 
@@ -412,7 +415,7 @@ void CommManager::send_rc_raw(void)
 
 void CommManager::send_diff_pressure(void)
 {
-  comm_link_.send_diff_pressure(sysid_, RF_.sensors_.data().diff_pressure_velocity,
+  comm_link_.send_diff_pressure(sysid_, RF_.sensors_.data().diff_pressure_ias,
                                 RF_.sensors_.data().diff_pressure,
                                 RF_.sensors_.data().diff_pressure_temp);
 }

src/sensors.cpp

@@ -66,9 +66,6 @@ void Sensors::init()
 
   next_sensor_to_update_ = BAROMETER;
 
-  float alt = rf_.params_.get_param_float(PARAM_GROUND_LEVEL);
-  ground_pressure_ = 101325.0f * static_cast<float>(pow((1 - 2.25694e-5 * alt), 5.2553));
-
   int_start_us_ = rf_.board_.clock_micros();
 
   this->update_battery_monitor_multipliers();
@@ -464,17 +461,19 @@ void Sensors::calibrate_baro()
       // else reset cal variables and start over
       if (baro_calibration_var_ < BARO_MAX_CALIBRATION_VARIANCE) {
         rf_.params_.set_param_float(PARAM_BARO_BIAS, baro_calibration_mean_);
+        // set ground altitude to be pressure altitude at PARAM_BARO_BIAS
+        rf_.params_.set_param_float(PARAM_GROUND_LEVEL,turbomath::alt(rf_.params_.get_param_float(PARAM_BARO_BIAS)));
         baro_calibrated_ = true;
-        rf_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_INFO, "Baro Cal successful!");
+        rf_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_INFO, "Baro ground pressure cal successful!");
       } else {
         rf_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR,
-                              "Too much movement for barometer cal");
+                              "Too much movement for barometer ground pressure cal");
       }
       baro_calibration_mean_ = 0.0f;
       baro_calibration_var_ = 0.0f;
       baro_calibration_count_ = 0;
     } else if (baro_calibration_count_ > SENSOR_CAL_DELAY_CYCLES) {
-      float measurement = data_.baro_pressure - ground_pressure_;
+      float measurement = data_.baro_pressure;
       float delta = measurement - baro_calibration_mean_;
       baro_calibration_mean_ += delta / (baro_calibration_count_ - SENSOR_CAL_DELAY_CYCLES);
       float delta2 = measurement - baro_calibration_mean_;
@@ -553,20 +552,17 @@ void Sensors::correct_mag(void)
 void Sensors::correct_baro(void)
 {
   if (!baro_calibrated_) { calibrate_baro(); }
-  data_.baro_pressure -= rf_.params_.get_param_float(PARAM_BARO_BIAS);
-  data_.baro_altitude =
-    turbomath::alt(data_.baro_pressure) - rf_.params_.get_param_float(PARAM_GROUND_LEVEL);
+  data_.baro_altitude = turbomath::alt(data_.baro_pressure);
 }
 
 void Sensors::correct_diff_pressure()
 {
   if (!diff_pressure_calibrated_) { calibrate_diff_pressure(); }
   data_.diff_pressure -= rf_.params_.get_param_float(PARAM_DIFF_PRESS_BIAS);
 
-  float atm = 101325.0f;
-  if (data_.baro_present) { atm = data_.baro_pressure; }
-  data_.diff_pressure_velocity = turbomath::fsign(data_.diff_pressure) * 24.574f
-    / turbomath::inv_sqrt((turbomath::fabs(data_.diff_pressure) * data_.diff_pressure_temp / atm));
+  // compute indicated air speed
+  data_.diff_pressure_ias = turbomath::fsign(data_.diff_pressure)
+    * sqrt((fabs(data_.diff_pressure)/(0.5*1.225)));
 }
 
 void Sensors::update_battery_monitor_multipliers()

test/test_board.cpp

@@ -73,8 +73,13 @@ uint8_t testBoard::serial_read() { return 0; }
 void testBoard::serial_flush() {}
 
 // sensors
-void testBoard::sensors_init() {}
-uint16_t testBoard::num_sensor_errors() { return 0; }
+void testBoard::sensors_init() {};
+uint16_t testBoard::sensors_errors_count() {return 0;}
+uint16_t testBoard::sensors_init_message_count() {return 0;}
+uint16_t testBoard::sensors_init_message(char * message, uint16_t size, uint16_t i) {return 0;}
+bool testBoard::sensors_init_message_good(uint16_t i) {return false;}
+
+bool testBoard::imu_present() {return false;}
 
 bool testBoard::imu_has_new_data()
 {

test/test_board.h

@@ -69,8 +69,12 @@ class testBoard : public Board
 
   // sensors
   void sensors_init() override;
-  uint16_t num_sensor_errors() override;
+  uint16_t sensors_errors_count() override;
+  uint16_t sensors_init_message_count() override;
+  uint16_t sensors_init_message(char * message, uint16_t size, uint16_t i) override;
+  bool sensors_init_message_good(uint16_t i) override;
 
+  bool imu_present() override;
   bool imu_has_new_data() override;
   bool imu_read(float accel[3], float * temperature, float gyro[3], uint64_t * time) override;
   void imu_not_responding_error() override;