Fixed an actual bug

include/board.h

@@ -39,6 +39,7 @@
 #include <cstdint>
 
 #include <sensors.h>
+#include <rc.h>
 
 namespace rosflight_firmware
 {

include/comm_manager.h

@@ -123,7 +123,7 @@ class CommManager : public ParamListenerInterface
   void send_attitude(void);
   void send_imu(void);
   void send_output_raw(void);
-  void send_rc_raw(void);
+
   void send_diff_pressure(void);
   void send_baro(void);
   void send_sonar(void);
@@ -159,6 +159,8 @@ class CommManager : public ParamListenerInterface
   void log(CommLinkInterface::LogSeverity severity, const char * fmt, ...);
   void log_message(CommLinkInterface::LogSeverity severity, char * text);
 
+
+  void send_rc_raw(void);
   void send_backup_data(const StateManager::BackupData & backup_data);
 };
 

include/estimator.h

@@ -44,17 +44,18 @@ namespace rosflight_firmware
 {
 class ROSflight;
 
-class Estimator : public ParamListenerInterface
-{
-
-public:
   typedef struct //__attribute__((__packed__))
   {
     uint64_t timestamp; // us, time of data read complete
     float q[4];         // quaternions
     float rate[3];
   } AttitudeStruct;
 
+class Estimator : public ParamListenerInterface
+{
+
+public:
+
   struct State
   {
     turbomath::Vector angular_velocity;

include/mixer.h

@@ -38,11 +38,14 @@
 #include <cstdint>
 
 #include <sensors.h>
+#include <rc.h>
 
 namespace rosflight_firmware
 {
 class ROSflight;
 
+typedef RcStruct MixerStruct;
+
 class Mixer : public ParamListenerInterface
 {
 
@@ -103,7 +106,8 @@ class Mixer : public ParamListenerInterface
 private:
   ROSflight & RF_;
 
-  RcStruct output_raw_ = {};
+  MixerStruct output_raw_ = {};
+
   //float raw_outputs_[NUM_TOTAL_OUTPUTS];
   float *raw_outputs_ = output_raw_.chan;
   float outputs_[NUM_TOTAL_OUTPUTS];
@@ -262,7 +266,7 @@ class Mixer : public ParamListenerInterface
   void param_change_callback(uint16_t param_id) override;
   void set_new_aux_command(aux_command_t new_aux_command);
   float * raw_outputs() { return raw_outputs_; }
-  RcStruct * get_output_raw(void) { return &output_raw_; }
+  MixerStruct * get_output_raw(void) { return &output_raw_; }
 };
 
 } // namespace rosflight_firmware

include/rc.h

@@ -41,6 +41,17 @@ namespace rosflight_firmware
 {
 class ROSflight;
 
+// 16 analog + 8 digital MUST BE > 14 (Mavlink message size is hardware to 14)
+#define RC_STRUCT_CHANNELS  24
+typedef struct //__attribute__((packed))
+{
+  uint64_t timestamp; // us, time of data read complete
+  uint8_t nChan;
+  float chan[RC_STRUCT_CHANNELS];
+  bool frameLost;
+  bool failsafeActivated;
+} RcStruct;
+
 class RC : public ParamListenerInterface
 {
 public:
@@ -72,7 +83,13 @@ class RC : public ParamListenerInterface
   bool new_command();
   void param_change_callback(uint16_t param_id) override;
 
+  RcStruct * get_rc(void);
+  float read_chan(uint8_t chan);
+  bool receive(void);
+
 private:
+  RcStruct rc_={};
+
   ROSflight & RF_;
 
   typedef struct

include/sensors.h

@@ -62,9 +62,6 @@ typedef struct //__attribute__((__packed__))
   float temperature;  // K
 } ImuStruct;
 
-// User version in estimator.h
-typedef Estimator::AttitudeStruct AttitudeStruct;
-
 typedef struct //__attribute__((__packed__))
 {
   uint64_t timestamp; // us, time of data read complete
@@ -100,17 +97,6 @@ typedef struct //__attribute__((packed))
   float temperature;  // K
 } MagStruct;
 
-// 16 analog + 8 digital MUST BE > 14 (Mavlink message size is hardware to 14)
-#define RC_STRUCT_CHANNELS  24
-typedef struct //__attribute__((packed))
-{
-  uint64_t timestamp; // us, time of data read complete
-  uint8_t nChan;
-  float chan[RC_STRUCT_CHANNELS];
-  bool frameLost;
-  bool failsafeActivated;
-} RcStruct;
-
 enum class GNSSFixType // quality from GGA
 {
   GNSS_FIX_TYPE_NO_FIX = 0,
@@ -181,7 +167,6 @@ typedef struct
   bool diff_pressure;
   bool sonar;
   bool battery;
-  bool rc;
 } got_flags;
 
 class ROSflight;
@@ -194,12 +179,9 @@ class Sensors : public ParamListenerInterface
   RangeStruct * get_sonar(void) { return &sonar_; }
   ImuStruct * get_imu(void) { return &imu_; }
   BatteryStruct * get_battery(void) { return &battery_; }
-  RcStruct * get_rc_(void) { return &rc_; }
   MagStruct * get_mag(void) { return &mag_; }
   GnssStruct * get_gnss(void) { return &gnss_; }
 
-  float read_rc_chan(uint8_t chan) { return rc_.chan[chan]; }
-
   Sensors(ROSflight & rosflight);
 
   void init();
@@ -220,7 +202,6 @@ class Sensors : public ParamListenerInterface
   RangeStruct sonar_ = {};
   ImuStruct imu_ = {};
   BatteryStruct battery_ = {};
-  RcStruct rc_ = {};
   MagStruct mag_ = {};
   GnssStruct gnss_ = {};
 

src/comm_manager.cpp

@@ -183,7 +183,7 @@ void CommManager::send_output_raw(void)
   comm_link_.send_output_raw(sysid_, *RF_.mixer_.get_output_raw());
 }
 
-void CommManager::send_rc_raw(void) { comm_link_.send_rc_raw(sysid_, *RF_.sensors_.get_rc_()); }
+void CommManager::send_rc_raw(void) { comm_link_.send_rc_raw(sysid_, *RF_.rc_.get_rc()); }
 
 void CommManager::send_diff_pressure(void)
 {
@@ -259,9 +259,6 @@ void CommManager::transmit(got_flags got)
   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;

src/rc.cpp

@@ -54,6 +54,14 @@ void RC::init_rc()
   init_switches();
 }
 
+RcStruct * RC::get_rc(void) { return &rc_; }
+float RC::read_chan(uint8_t chan) { return rc_.chan[chan]; }
+
+bool RC::receive(void)
+{
+  return RF_.board_.rc_read(&rc_);
+}
+
 void RC::param_change_callback(uint16_t param_id)
 {
   switch (param_id) {
@@ -172,7 +180,7 @@ bool RC::check_rc_lost()
   } else {
     // go into failsafe if we get an invalid RC command for any channel
     for (int8_t i = 0; i < RF_.params_.get_param_int(PARAM_RC_NUM_CHANNELS); i++) {
-      float pwm = RF_.sensors_.read_rc_chan(i);
+      float pwm = read_chan(i);
       if (pwm < -0.25 || pwm > 1.25) { failsafe = true; }
     }
   }
@@ -197,8 +205,8 @@ void RC::look_for_arm_disarm_signal()
   if (!switch_mapped(SWITCH_ARM)) {
     if (!RF_.state_manager_.state().armed) { // we are DISARMED
       // if left stick is down and to the right
-      if ((RF_.rc_.stick(STICK_F) < RF_.params_.get_param_float(PARAM_ARM_THRESHOLD))
-          && (RF_.rc_.stick(STICK_Z) > (1.0f - RF_.params_.get_param_float(PARAM_ARM_THRESHOLD)))) {
+      if ((stick(STICK_F) < RF_.params_.get_param_float(PARAM_ARM_THRESHOLD))
+          && (stick(STICK_Z) > (1.0f - RF_.params_.get_param_float(PARAM_ARM_THRESHOLD)))) {
         time_sticks_have_been_in_arming_position_ms += dt;
       } else {
         time_sticks_have_been_in_arming_position_ms = 0;
@@ -208,8 +216,8 @@ void RC::look_for_arm_disarm_signal()
       }
     } else { // we are ARMED
       // if left stick is down and to the left
-      if (RF_.rc_.stick(STICK_F) < RF_.params_.get_param_float(PARAM_ARM_THRESHOLD)
-          && RF_.rc_.stick(STICK_Z) < -(1.0f - RF_.params_.get_param_float(PARAM_ARM_THRESHOLD))) {
+      if (stick(STICK_F) < RF_.params_.get_param_float(PARAM_ARM_THRESHOLD)
+          && stick(STICK_Z) < -(1.0f - RF_.params_.get_param_float(PARAM_ARM_THRESHOLD))) {
         time_sticks_have_been_in_arming_position_ms += dt;
       } else {
         time_sticks_have_been_in_arming_position_ms = 0;
@@ -220,7 +228,7 @@ void RC::look_for_arm_disarm_signal()
       }
     }
   } else { // ARMING WITH SWITCH
-    if (RF_.rc_.switch_on(SWITCH_ARM)) {
+    if (switch_on(SWITCH_ARM)) {
       if (!RF_.state_manager_.state().armed) {
         RF_.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM);
       }
@@ -237,7 +245,7 @@ bool RC::run()
 
   // read and normalize stick values
   for (uint8_t channel = 0; channel < static_cast<uint8_t>(STICKS_COUNT); channel++) {
-    float pwm = RF_.sensors_.read_rc_chan(sticks[channel].channel);
+    float pwm = read_chan(sticks[channel].channel);
     if (sticks[channel].one_sided) { // generally only F is one_sided
       stick_values[channel] = pwm;
     } else {
@@ -248,7 +256,7 @@ bool RC::run()
   // read and interpret switch values
   for (uint8_t channel = 0; channel < static_cast<uint8_t>(SWITCHES_COUNT); channel++) {
     if (switches[channel].mapped) {
-      float pwm = RF_.sensors_.read_rc_chan(switches[channel].channel);
+      float pwm = read_chan(switches[channel].channel);
       if (switches[channel].direction < 0) {
         switch_values[channel] = pwm < 0.2;
       } else {

src/rosflight.cpp

@@ -131,7 +131,10 @@ void ROSflight::run()
   state_manager_.run();
 
   // Process latest received RC input data
-  if (got.rc) { rc_.run(); }
+  if (rc_.receive()) {
+    comm_manager_.send_rc_raw();
+    rc_.run();
+  }
 
   // Update commands (internal logic tells whether or not we should do anything or not)
   command_manager_.run();

src/sensors.cpp

@@ -169,9 +169,6 @@ got_flags Sensors::run()
   // BATTERY_MONITOR:
   got.battery = rf_.board_.battery_read(&battery_);
 
-  // RC
-  got.rc = rf_.board_.rc_read(&rc_);
-
   return got;
 }