Rewored sensor and RC acquisition

Previously, sensors and RC hardware would collect and store data at a rate that was independent of how often the core rosflight code checked for sensor data. This commit changes that, making the core rosflight code be dependent on the hardware's aquisition rate, syncronizing the two.

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 uint8_t 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 uint8_t 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 uint8_t 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 uint8_t 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 uint8_t sonar_has_new_data() = 0;
+  virtual bool sonar_read(float *range) =0;
 
+  // GPS
   virtual bool gnss_present() = 0;
-  virtual void gnss_update() = 0;
+  virtual uint8_t gnss_has_new_data() = 0;
+  virtual bool gnss_read( GNSSData *gnss, GNSSFull *gnss_full) = 0;
 
-  virtual GNSSData gnss_read() = 0;
-  virtual bool gnss_has_new_data() = 0;
-  virtual GNSSFull gnss_full_read() = 0;
-
-  virtual bool battery_voltage_present() const = 0;
-  virtual float battery_voltage_read() const = 0;
+  // Battery
+  virtual bool battery_present()  = 0;
+  virtual uint8_t 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 uint8_t 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,20 @@
 
 namespace rosflight_firmware
 {
+
+typedef struct
+{
+  uint8_t imu;
+  uint8_t gnss;
+  uint8_t gnss_full;
+  uint8_t baro;
+  uint8_t mag;
+  uint8_t diff_pressure;
+  uint8_t sonar;
+  uint8_t battery;
+  uint8_t rc;
+} got_flags;
+
 enum GNSSFixType
 {
   GNSS_FIX_TYPE_NO_FIX,
@@ -145,11 +159,10 @@ 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};
 
     bool baro_present = false;
@@ -169,7 +182,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 +201,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,49 @@ 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
+  {
+    send_imu();
+    send_attitude();
+    static uint64_t ro_count=0;
+    if(!((ro_count++)%8)) send_output_raw();      // Raw output at 400Hz/8 = 50Hz
+   }
+
+  if(got.diff_pressure)   send_diff_pressure();   // Pitot sensor
+  if(got.baro)            send_baro();            // Baro altimeter
+  if(got.mag)             send_mag();             // Manetometer
+  if(got.sonar)           send_sonar();           // Height above ground sensor (not enabled)
+  if(got.battery)         send_battery_status();  // Battery V & I
+  if(got.gnss)            send_gnss();            // GPS data (GNSS Packed)
+  if(got.gnss_full)       send_gnss_full();       // GPS full data (not needed)
+  if(got.rc) // report at half the S.Bus rate.
   {
-    streams_[i].stream(time_us);
+    static uint64_t rc_count=0;
+    if(!((rc_count++)%2)) send_rc_raw();      	// RC (S.Bus) inputs, scaled 1000-2000
   }
-  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));
+  {
+  	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

@@ -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

@@ -265,28 +265,19 @@ 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;
 
-  // read and normalize stick values
   for (uint8_t channel = 0; channel < static_cast<uint8_t>(STICKS_COUNT); channel++)
   {
-    float pwm = RF_.board_.rc_read(sticks[channel].channel);
     if (sticks[channel].one_sided) // generally only F is one_sided
     {
-      stick_values[channel] = pwm;
+      stick_values[channel] = RF_.board_.rc_read(sticks[channel].channel);
     }
     else
     {
-      stick_values[channel] = 2.0 * (pwm - 0.5);
+      stick_values[channel] = 2.0 * (RF_.board_.rc_read(sticks[channel].channel) - 0.5);
     }
   }