Reworked sensor and RC acquisition

include/board.h

@@ -70,45 +70,48 @@ class Board
   virtual void sensors_init() = 0;
   virtual uint16_t num_sensor_errors() = 0;
 
-  virtual bool new_imu_data() = 0;
+  // IMU
+  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;
 
+  // Mag
   virtual bool mag_present() = 0;
-  virtual void mag_update() = 0;
-  virtual void mag_read(float mag[3]) = 0;
+  virtual bool mag_has_new_data() = 0;
+  virtual bool mag_read(float mag[3]) = 0;
 
+  // Baro
   virtual bool baro_present() = 0;
-  virtual void baro_update() = 0;
-  virtual void baro_read(float *pressure, float *temperature) = 0;
+  virtual bool baro_has_new_data() = 0;
+  virtual bool baro_read(float *pressure, float *temperature) = 0;
 
+  // Pitot
   virtual bool diff_pressure_present() = 0;
-  virtual void diff_pressure_update() = 0;
-  virtual void diff_pressure_read(float *diff_pressure, float *temperature) = 0;
+  virtual bool diff_pressure_has_new_data() = 0;
+  virtual bool diff_pressure_read(float *diff_pressure, float *temperature) = 0;
 
+  // Sonar
   virtual bool sonar_present() = 0;
-  virtual void sonar_update() = 0;
-  virtual float sonar_read() = 0;
+  virtual bool sonar_has_new_data() = 0;
+  virtual bool sonar_read(float *range) = 0;
 
+  // GPS
   virtual bool gnss_present() = 0;
-  virtual void gnss_update() = 0;
-
-  virtual GNSSData gnss_read() = 0;
   virtual bool gnss_has_new_data() = 0;
-  virtual GNSSFull gnss_full_read() = 0;
+  virtual bool gnss_read(GNSSData *gnss, GNSSFull *gnss_full) = 0;
 
-  virtual bool battery_voltage_present() const = 0;
-  virtual float battery_voltage_read() const = 0;
+  // Battery
+  virtual bool battery_present() = 0;
+  virtual bool battery_has_new_data() = 0;
+  virtual bool battery_read(float *voltage, float *current) = 0;
   virtual void battery_voltage_set_multiplier(double multiplier) = 0;
-
-  virtual bool battery_current_present() const = 0;
-  virtual float battery_current_read() const = 0;
   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;
+  virtual bool rc_has_new_data() = 0;
+  virtual float rc_read(uint8_t chan) = 0;
 
   // PWM
   virtual void pwm_init(uint32_t refresh_rate, uint16_t idle_pwm) = 0;

include/comm_manager.h

@@ -163,15 +163,6 @@ class CommManager : public CommLinkInterface::ListenerInterface, public ParamLis
 
   void send_next_param(void);
 
-  Stream streams_[STREAM_COUNT] = {
-      Stream(0, [this] { this->send_heartbeat(); }),      Stream(0, [this] { this->send_status(); }),
-      Stream(0, [this] { this->send_attitude(); }),       Stream(0, [this] { this->send_imu(); }),
-      Stream(0, [this] { this->send_diff_pressure(); }),  Stream(0, [this] { this->send_baro(); }),
-      Stream(0, [this] { this->send_sonar(); }),          Stream(0, [this] { this->send_mag(); }),
-      Stream(0, [this] { this->send_battery_status(); }), Stream(0, [this] { this->send_output_raw(); }),
-      Stream(0, [this] { this->send_gnss(); }),           Stream(0, [this] { this->send_gnss_full(); }),
-      Stream(0, [this] { this->send_rc_raw(); }),         Stream(20000, [this] { this->send_low_priority(); })};
-
   // the time of week stamp for the last sent GNSS message, to prevent re-sending
   uint32_t last_sent_gnss_tow_ = 0;
   uint32_t last_sent_gnss_full_tow_ = 0;
@@ -182,9 +173,8 @@ class CommManager : public CommLinkInterface::ListenerInterface, public ParamLis
   void init();
   void param_change_callback(uint16_t param_id) override;
   void receive(void);
-  void stream();
+  void stream(got_flags got);
   void send_param_value(uint16_t param_id);
-  void set_streaming_rate(uint8_t stream_id, int16_t param_id);
   void update_status();
   void log(CommLinkInterface::LogSeverity severity, const char* fmt, ...);
 

include/param.h

@@ -51,21 +51,6 @@ enum : uint16_t
   /*** MAVLINK CONFIGURATION ***/
   /*****************************/
   PARAM_SYSTEM_ID,
-  PARAM_STREAM_HEARTBEAT_RATE,
-  PARAM_STREAM_STATUS_RATE,
-
-  PARAM_STREAM_ATTITUDE_RATE,
-  PARAM_STREAM_IMU_RATE,
-  PARAM_STREAM_MAG_RATE,
-  PARAM_STREAM_BARO_RATE,
-  PARAM_STREAM_AIRSPEED_RATE,
-  PARAM_STREAM_SONAR_RATE,
-  PARAM_STREAM_GNSS_RATE,
-  PARAM_STREAM_GNSS_FULL_RATE,
-  PARAM_STREAM_BATTERY_STATUS_RATE,
-
-  PARAM_STREAM_OUTPUT_RAW_RATE,
-  PARAM_STREAM_RC_RAW_RATE,
 
   /********************************/
   /*** CONTROLLER CONFIGURATION ***/

include/sensors.h

@@ -43,6 +43,19 @@
 
 namespace rosflight_firmware
 {
+typedef struct
+{
+  bool imu;
+  bool gnss;
+  bool gnss_full;
+  bool baro;
+  bool mag;
+  bool diff_pressure;
+  bool sonar;
+  bool battery;
+  bool rc;
+} got_flags;
+
 enum GNSSFixType
 {
   GNSS_FIX_TYPE_NO_FIX,
@@ -145,10 +158,9 @@ class Sensors : public ParamListenerInterface
     bool sonar_range_valid = false;
 
     GNSSData gnss_data;
-    bool gnss_new_data = false;
+    GNSSFull gnss_full;
     float gps_CNO = 0; // What is this?
     bool gnss_present = false;
-    GNSSFull gnss_full;
 
     turbomath::Vector mag = {0, 0, 0};
 
@@ -169,7 +181,7 @@ class Sensors : public ParamListenerInterface
 
   // function declarations
   void init();
-  bool run();
+  got_flags run();
   void param_change_callback(uint16_t param_id) override;
 
   // Calibration Functions
@@ -188,6 +200,8 @@ class Sensors : public ParamListenerInterface
     return true;
   }
 
+  got_flags got;
+
 private:
   static const float BARO_MAX_CHANGE_RATE;
   static const float BARO_SAMPLE_RATE;

src/comm_manager.cpp

@@ -83,19 +83,6 @@ void CommManager::init()
   send_params_index_ = PARAMS_COUNT;
 
   update_system_id(PARAM_SYSTEM_ID);
-  set_streaming_rate(STREAM_ID_HEARTBEAT, PARAM_STREAM_HEARTBEAT_RATE);
-  set_streaming_rate(STREAM_ID_STATUS, PARAM_STREAM_STATUS_RATE);
-  set_streaming_rate(STREAM_ID_IMU, PARAM_STREAM_IMU_RATE);
-  set_streaming_rate(STREAM_ID_ATTITUDE, PARAM_STREAM_ATTITUDE_RATE);
-  set_streaming_rate(STREAM_ID_DIFF_PRESSURE, PARAM_STREAM_AIRSPEED_RATE);
-  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_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);
-  set_streaming_rate(STREAM_ID_RC_RAW, PARAM_STREAM_RC_RAW_RATE);
 
   initialized_ = true;
 }
@@ -107,45 +94,6 @@ void CommManager::param_change_callback(uint16_t param_id)
   case PARAM_SYSTEM_ID:
     update_system_id(param_id);
     break;
-  case PARAM_STREAM_HEARTBEAT_RATE:
-    set_streaming_rate(STREAM_ID_HEARTBEAT, param_id);
-    break;
-  case PARAM_STREAM_STATUS_RATE:
-    set_streaming_rate(STREAM_ID_STATUS, param_id);
-    break;
-  case PARAM_STREAM_IMU_RATE:
-    set_streaming_rate(STREAM_ID_IMU, param_id);
-    break;
-  case PARAM_STREAM_ATTITUDE_RATE:
-    set_streaming_rate(STREAM_ID_ATTITUDE, param_id);
-    break;
-  case PARAM_STREAM_AIRSPEED_RATE:
-    set_streaming_rate(STREAM_ID_DIFF_PRESSURE, param_id);
-    break;
-  case PARAM_STREAM_BARO_RATE:
-    set_streaming_rate(STREAM_ID_BARO, param_id);
-    break;
-  case PARAM_STREAM_SONAR_RATE:
-    set_streaming_rate(STREAM_ID_SONAR, param_id);
-    break;
-  case PARAM_STREAM_GNSS_RATE:
-    set_streaming_rate(STREAM_ID_GNSS, param_id);
-    break;
-  case PARAM_STREAM_GNSS_FULL_RATE:
-    set_streaming_rate(STREAM_ID_GNSS_FULL, param_id);
-    break;
-  case PARAM_STREAM_MAG_RATE:
-    set_streaming_rate(STREAM_ID_MAG, param_id);
-    break;
-  case PARAM_STREAM_OUTPUT_RAW_RATE:
-    set_streaming_rate(STREAM_ID_SERVO_OUTPUT_RAW, param_id);
-    break;
-  case PARAM_STREAM_RC_RAW_RATE:
-    set_streaming_rate(STREAM_ID_RC_RAW, param_id);
-    break;
-  case PARAM_STREAM_BATTERY_STATUS_RATE:
-    set_streaming_rate(STREAM_ID_BATTERY_STATUS, param_id);
-    break;
   default:
     // do nothing
     break;
@@ -390,10 +338,6 @@ void CommManager::heartbeat_callback(void)
     comm_link_.send_error_data(sysid_, backup_data_buffer_);
     have_backup_data_ = false;
   }
-
-  /// JSJ: I don't think we need this
-  // respond to heartbeats with a heartbeat
-  this->send_heartbeat();
 }
 
 // function definitions
@@ -572,19 +516,61 @@ void CommManager::send_low_priority(void)
 }
 
 // function definitions
-void CommManager::stream()
+void CommManager::stream(got_flags got)
 {
   uint64_t time_us = RF_.board_.clock_micros();
-  for (int i = 0; i < STREAM_COUNT; i++)
+
+  // Send out data
+
+  if (got.imu) // Nominally 400Hz
   {
-    streams_[i].stream(time_us);
+    send_imu();
+    send_attitude();
+    static uint64_t ro_count = 0;
+    if (!((ro_count++) % 8))
+      send_output_raw(); // Raw output at 400Hz/8 = 50Hz
   }
-  RF_.board_.serial_flush();
-}
 
-void CommManager::set_streaming_rate(uint8_t stream_id, int16_t param_id)
-{
-  streams_[stream_id].set_rate(RF_.params_.get_param_int(param_id));
+  // Pitot sensor
+  if (got.diff_pressure)
+    send_diff_pressure();
+  // Baro altitude
+  if (got.baro)
+    send_baro();
+  // Magnetometer
+  if (got.mag)
+    send_mag();
+  // Height above ground sensor (not enabled)
+  if (got.sonar)
+    send_sonar();
+  // Battery V & I
+  if (got.battery)
+    send_battery_status();
+  // GPS data (GNSS Packed)
+  if (got.gnss)
+    send_gnss();
+  // GPS full data (not needed)
+  if (got.gnss_full)
+    send_gnss_full();
+  if (got.rc)
+    send_rc_raw();
+
+  {
+    static uint64_t next_heartbeat = 0, next_status = 0;
+
+    if ((time_us) / 1000000 >= next_heartbeat) // 1 Hz
+    {
+      send_heartbeat();
+      next_heartbeat = time_us / 1000000 + 1;
+    }
+    if ((time_us) / 100000 >= next_status) // 10 Hz
+    {
+      send_status();
+      next_status = time_us / 100000 + 1;
+    }
+  }
+
+  send_low_priority(); // parameter values and logging messages
 }
 
 void CommManager::send_named_value_int(const char* const name, int32_t value)

src/param.cpp

@@ -49,10 +49,10 @@
 #endif
 
 // Uncomment to view contents of GIT_VERSION_HASH and GIT_VERSION STRING
-//#define STRINGIFY(s) XSTRINGIFY(s)
-//#define XSTRINGIFY(s) #s
-//#pragma message( "GIT_VERSION_HASH: " STRINGIFY(GIT_VERSION_HASH))
-//#pragma message( "GIT_VERSION_STRING: " GIT_VERSION_STRING)
+// #define STRINGIFY(s) XSTRINGIFY(s)
+// #define XSTRINGIFY(s) #s
+// #pragma message( "GIT_VERSION_HASH: " STRINGIFY(GIT_VERSION_HASH))
+// #pragma message( "GIT_VERSION_STRING: " GIT_VERSION_STRING)
 
 namespace rosflight_firmware
 {
@@ -121,21 +121,6 @@ void Params::set_defaults(void)
   /*** MAVLINK CONFIGURATION ***/
   /*****************************/
   init_param_int(PARAM_SYSTEM_ID, "SYS_ID", 1); // Mavlink System ID  | 1 | 255
-  init_param_int(PARAM_STREAM_HEARTBEAT_RATE, "STRM_HRTBT", 1); // Rate of heartbeat stream (Hz) | 0 | 1000
-  init_param_int(PARAM_STREAM_STATUS_RATE, "STRM_STATUS", 10); // Rate of status stream (Hz) | 0 | 1000
-
-  init_param_int(PARAM_STREAM_ATTITUDE_RATE, "STRM_ATTITUDE", 200); // Rate of attitude stream (Hz) | 0 | 1000
-  init_param_int(PARAM_STREAM_IMU_RATE, "STRM_IMU", 250); // Rate of IMU stream (Hz) | 0 | 1000
-  init_param_int(PARAM_STREAM_MAG_RATE, "STRM_MAG", 50); // Rate of magnetometer stream (Hz) | 0 | 75
-  init_param_int(PARAM_STREAM_BARO_RATE, "STRM_BARO", 50); // Rate of barometer stream (Hz) | 0 | 100
-  init_param_int(PARAM_STREAM_AIRSPEED_RATE, "STRM_AIRSPEED", 50); // Rate of airspeed stream (Hz) | 0 |  50
-  init_param_int(PARAM_STREAM_SONAR_RATE, "STRM_SONAR", 40); // Rate of sonar stream (Hz) | 0 | 40
-  init_param_int(PARAM_STREAM_GNSS_RATE, "STRM_GNSS", 1000); // Maximum rate of GNSS stream (Hz) | 0 | 10
-  init_param_int(PARAM_STREAM_GNSS_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
 
   /********************************/
   /*** CONTROLLER CONFIGURATION ***/

src/rc.cpp

@@ -263,15 +263,6 @@ void RC::look_for_arm_disarm_signal()
 
 bool RC::run()
 {
-  uint32_t now = RF_.board_.clock_millis();
-
-  // if it has been more than 20ms then look for new RC values and parse them
-  if (now - last_rc_receive_time < 20)
-  {
-    return false;
-  }
-  last_rc_receive_time = now;
-
   // Check for rc lost
   if (check_rc_lost())
     return false;
@@ -295,13 +286,14 @@ bool RC::run()
   {
     if (switches[channel].mapped)
     {
+      float pwm = RF_.board_.rc_read(switches[channel].channel);
       if (switches[channel].direction < 0)
       {
-        switch_values[channel] = RF_.board_.rc_read(switches[channel].channel) < 0.2;
+        switch_values[channel] = pwm < 0.2;
       }
       else
       {
-        switch_values[channel] = RF_.board_.rc_read(switches[channel].channel) >= 0.8;
+        switch_values[channel] = pwm >= 0.8;
       }
     }
     else