add rc_override reason struct to command manager (from issue #371)

Author: JMoore5353

include/command_manager.h

@@ -86,7 +86,7 @@ class CommandManager : public ParamListenerInterface
     control_channel_t * combined;
   } mux_t;
 
-  mux_t muxes[6] = {{&rc_command_.Qx, &offboard_command_.Qx, &combined_command_.Qx},
+  mux_t muxes_[6] = {{&rc_command_.Qx, &offboard_command_.Qx, &combined_command_.Qx},
                     {&rc_command_.Qy, &offboard_command_.Qy, &combined_command_.Qy},
                     {&rc_command_.Qz, &offboard_command_.Qz, &combined_command_.Qz},
                     {&rc_command_.Fx, &offboard_command_.Fx, &combined_command_.Fx},
@@ -146,40 +146,96 @@ class CommandManager : public ParamListenerInterface
     MUX_FX,
     MUX_FY,
     MUX_FZ,
+    NUM_MUX_CHANNELS
+  };
+
+  enum RCOverrideReason : uint16_t
+  {
+    OVERRIDE_NO_OVERRIDE = 0x0,
+    OVERRIDE_ATT_SWITCH = 0x1,
+    OVERRIDE_THR_SWITCH = 0x2,
+    OVERRIDE_X = 0x4,
+    OVERRIDE_Y = 0x8,
+    OVERRIDE_Z = 0x10,
+    OVERRIDE_T = 0x20,
+    OVERRIDE_OFFBOARD_X_INACTIVE = 0x40,
+    OVERRIDE_OFFBOARD_Y_INACTIVE = 0x80,
+    OVERRIDE_OFFBOARD_Z_INACTIVE = 0x100,
+    OVERRIDE_OFFBOARD_T_INACTIVE = 0x200,
   };
 
   typedef struct
   {
     RC::Stick rc_channel;
     uint32_t last_override_time;
-  } rc_stick_override_t;
-
-  rc_stick_override_t rc_stick_override_[3] = {{RC::STICK_X, 0},
-                                               {RC::STICK_Y, 0},
-                                               {RC::STICK_Z, 0}};
+    RCOverrideReason stick_override_reason;
+    RCOverrideReason offboard_inactive_override_reason;
+    uint16_t override_mask;
+  } channel_override_t;
+
+  channel_override_t channel_override_[6] = {
+      {RC::STICK_X, 0, OVERRIDE_X, OVERRIDE_OFFBOARD_X_INACTIVE, X_OVERRIDDEN},
+      {RC::STICK_Y, 0, OVERRIDE_Y, OVERRIDE_OFFBOARD_Y_INACTIVE, Y_OVERRIDDEN},
+      {RC::STICK_Z, 0, OVERRIDE_Z, OVERRIDE_OFFBOARD_Z_INACTIVE, Z_OVERRIDDEN},
+      {RC::STICK_F, 0, OVERRIDE_T, OVERRIDE_OFFBOARD_T_INACTIVE, T_OVERRIDDEN},
+      {RC::STICK_F, 0, OVERRIDE_T, OVERRIDE_OFFBOARD_T_INACTIVE, T_OVERRIDDEN},
+      {RC::STICK_F, 0, OVERRIDE_T, OVERRIDE_OFFBOARD_T_INACTIVE,
+        T_OVERRIDDEN} // Note that throttle overriding works a bit differently
+  };
 
   ROSflight & RF_;
 
   bool new_command_;
-  uint8_t rc_override_;
+  uint16_t rc_override_;
 
   control_t & failsafe_command_;
 
   void param_change_callback(uint16_t param_id) override;
   void init_failsafe();
 
-  uint8_t do_roll_pitch_yaw_muxing(MuxChannel channel);
-  uint8_t do_throttle_muxing(void);
-  void do_min_throttle_muxing();
+  /**
+   * @brief Checks which channels are overridden
+   * @details There are many reasons that a channel could be overriden. These reasons include:
+   *  - A stick is deflected
+   *  - The commanded throttle is less than the RC throttle, and the MIN_THROTTLE parameter is set
+   *  - The attitude or throttle override switch is flipped
+   *  - The onboard computer has not sent any commands recently
+   * The returned bitfield indicates which reasons have caused an override.
+   * By anding with a constant such as X_OVERRIDDEN, you can check if a specific channel is overridden.
+   * @return A bitfield, with overriden reasons indicated
+   */
+  uint16_t determine_override_status();
+  uint16_t check_if_attitude_channel_is_overriden_by_rc(MuxChannel channel);
+  uint16_t check_if_throttle_channel_is_overriden_by_rc();
+  void do_muxing(uint16_t rc_override);
+  void do_channel_muxing(MuxChannel channel, uint16_t rc_override);
 
   void interpret_rc(void);
   bool stick_deviated(MuxChannel channel);
 
 public:
+  static constexpr uint16_t X_OVERRIDDEN{OVERRIDE_ATT_SWITCH | OVERRIDE_X | OVERRIDE_OFFBOARD_X_INACTIVE};
+  static constexpr uint16_t Y_OVERRIDDEN{OVERRIDE_ATT_SWITCH | OVERRIDE_Y | OVERRIDE_OFFBOARD_Y_INACTIVE};
+  static constexpr uint16_t Z_OVERRIDDEN{OVERRIDE_ATT_SWITCH | OVERRIDE_Z | OVERRIDE_OFFBOARD_Z_INACTIVE};
+  static constexpr uint16_t T_OVERRIDDEN{OVERRIDE_THR_SWITCH | OVERRIDE_T | OVERRIDE_OFFBOARD_T_INACTIVE};
+  static constexpr uint16_t ATTITUDE_OVERRIDDEN{X_OVERRIDDEN | Y_OVERRIDDEN | Z_OVERRIDDEN};
+
   CommandManager(ROSflight & _rf);
   void init();
   bool run();
-  uint8_t rc_override_active();
+  /**
+   * @brief Checks which channels are overridden, and why
+   * @details There are many reasons that a channel could be overriden. These reasons include:
+   *  - A stick is deflected
+   *  - The commanded throttle is less than the RC throttle, and the MIN_THROTTLE parameter is set
+   *  - The attitude or throttle override switch is flipped
+   *  - The onboard computer has not sent any commands recently
+   * The returned bitfield indicates which reasons have caused an override.
+   * Because c++ can use integers as booleans, this function can be treated as providing a boolean
+   * This value is updated if a new RC command is available
+   * @return A bitfield, with overriden reasons indicated
+   */
+  uint16_t get_rc_override();
   bool offboard_control_active();
   void set_new_offboard_command(control_t new_offboard_command);
   void set_new_rc_command(control_t new_rc_command);

include/mixer.h

@@ -34,6 +34,7 @@
 
 #include "param_listener.h"
 #include "controller.h"
+#include "command_manager.h"
 
 #include <eigen/Eigen/Dense>
 #include <eigen/Eigen/SVD>

src/comm_manager.cpp

@@ -373,7 +373,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_.command_manager_.get_rc_override(), RF_.command_manager_.offboard_control_active(),
     RF_.state_manager_.state().error_codes, control_mode, RF_.board_.sensors_errors_count(),
     RF_.get_loop_time_us());
 }

src/command_manager.cpp

@@ -38,33 +38,17 @@
 
 namespace rosflight_firmware
 {
-typedef enum
-{
-  ATT_MODE_RATE,
-  ATT_MODE_ANGLE
-} att_mode_t;
-
-typedef struct
-{
-  RC::Stick rc_channel;
-  uint32_t last_override_time;
-} rc_stick_override_t;
-
-rc_stick_override_t rc_stick_override[] = {{RC::STICK_X, 0}, {RC::STICK_Y, 0}, {RC::STICK_Z, 0}};
-
-typedef struct
-{
-  control_channel_t * rc;
-  control_channel_t * onboard;
-  control_channel_t * combined;
-} mux_t;
 
 CommandManager::CommandManager(ROSflight & _rf)
     : RF_(_rf)
     , failsafe_command_(multirotor_failsafe_command_)
 {}
 
-void CommandManager::init() { init_failsafe(); }
+void CommandManager::init()
+{
+  init_failsafe();
+  rc_override_ = determine_override_status();
+}
 
 void CommandManager::param_change_callback(uint16_t param_id)
 {
@@ -124,27 +108,22 @@ void CommandManager::interpret_rc(void)
   rc_command_.Qz.value = RF_.rc_.stick(RC::STICK_Z);
 
   // Load the RC command based on the axis associated with the RC F command
+  rc_command_.Fx.value = 0.0;
+  rc_command_.Fy.value = 0.0;
+  rc_command_.Fz.value = 0.0;
   switch (static_cast<rc_f_axis_t>(RF_.params_.get_param_int(PARAM_RC_F_AXIS))) {
-    case X_AXIS:   // RC F = X axis
+    case X_AXIS:
       rc_command_.Fx.value = RF_.rc_.stick(RC::STICK_F);
-      rc_command_.Fy.value = 0.0;
-      rc_command_.Fz.value = 0.0;
       break;
-    case Y_AXIS:   // RC F = Y axis
-      rc_command_.Fx.value = 0.0;
+    case Y_AXIS:
       rc_command_.Fy.value = RF_.rc_.stick(RC::STICK_F);
-      rc_command_.Fz.value = 0.0;
       break;
     case Z_AXIS:
-      rc_command_.Fx.value = 0.0;
-      rc_command_.Fy.value = 0.0;
       rc_command_.Fz.value = RF_.rc_.stick(RC::STICK_F);
       break;
     default:
       RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_WARNING,
           "Invalid RC F axis. Defaulting to z-axis.");
-      rc_command_.Fx.value = 0.0;
-      rc_command_.Fy.value = 0.0;
       rc_command_.Fz.value = RF_.rc_.stick(RC::STICK_F);
       break;
   }
@@ -199,50 +178,60 @@ bool CommandManager::stick_deviated(MuxChannel channel)
   uint32_t now = RF_.board_.clock_millis();
 
   // if we are still in the lag time, return true
-  if (now < rc_stick_override_[channel].last_override_time
+  if (now < channel_override_[channel].last_override_time
         + RF_.params_.get_param_int(PARAM_OVERRIDE_LAG_TIME)) {
     return true;
   } else {
-    if (fabsf(RF_.rc_.stick(rc_stick_override_[channel].rc_channel))
+    if (fabsf(RF_.rc_.stick(channel_override_[channel].rc_channel))
         > RF_.params_.get_param_float(PARAM_RC_OVERRIDE_DEVIATION)) {
-      rc_stick_override_[channel].last_override_time = now;
+      channel_override_[channel].last_override_time = now;
       return true;
     }
     return false;
   }
 }
 
-uint8_t CommandManager::do_roll_pitch_yaw_muxing(MuxChannel channel)
+uint16_t CommandManager::determine_override_status()
 {
-  bool override_this_channel = false;
   // Check if the override switch exists and is triggered, or if the sticks have deviated enough to
   // trigger an override
-  if ((RF_.rc_.switch_mapped(RC::SWITCH_ATT_OVERRIDE) && RF_.rc_.switch_on(RC::SWITCH_ATT_OVERRIDE))
-      || stick_deviated(channel)) {
-    override_this_channel = true;
-  } else { // Otherwise only have RC override if the offboard channel is inactive
-    if (muxes[channel].onboard->active) {
-      override_this_channel = false;
-    } else {
-      override_this_channel = true;
-    }
+  uint16_t rc_override{0};
+  if (RF_.rc_.switch_mapped(RC::SWITCH_ATT_OVERRIDE) && RF_.rc_.switch_on(RC::SWITCH_ATT_OVERRIDE)) {
+    rc_override |= OVERRIDE_ATT_SWITCH;
   }
-  // set the combined channel output depending on whether RC is overriding for this channel or not
-  *muxes[channel].combined = override_this_channel ? *muxes[channel].rc : *muxes[channel].onboard;
-  if (override_this_channel) return 1U;
-  else return 0;
+  if (RF_.rc_.switch_mapped(RC::SWITCH_THROTTLE_OVERRIDE) && RF_.rc_.switch_on(RC::SWITCH_THROTTLE_OVERRIDE)) {
+    rc_override |= OVERRIDE_THR_SWITCH;
+  }
+
+  rc_override |= check_if_attitude_channel_is_overriden_by_rc(MUX_QX);
+  rc_override |= check_if_attitude_channel_is_overriden_by_rc(MUX_QY);
+  rc_override |= check_if_attitude_channel_is_overriden_by_rc(MUX_QZ);
+  rc_override |= check_if_throttle_channel_is_overriden_by_rc();
+
+  return rc_override;
 }
 
-uint8_t CommandManager::do_throttle_muxing(void)
+uint16_t CommandManager::check_if_attitude_channel_is_overriden_by_rc(MuxChannel channel)
+{
+  uint16_t rc_overrides{0};
+  if (stick_deviated(channel)) {
+    rc_overrides |= channel_override_[channel].stick_override_reason;
+  }
+  if (!(muxes_[channel].onboard->active)) {
+    rc_overrides |= channel_override_[channel].offboard_inactive_override_reason;
+  }
+  return rc_overrides;
+}
+
+uint16_t CommandManager::check_if_throttle_channel_is_overriden_by_rc()
 {
-  bool override_this_channel = false;
   MuxChannel selected_channel;
-  // Determine which channel to check based on which axis the RC channel corresponds to
+  // Determine which channel to check based on which axis the RC F channel corresponds to
   switch (static_cast<rc_f_axis_t>(RF_.params_.get_param_int(PARAM_RC_F_AXIS))) {
-    case X_AXIS: 
+    case X_AXIS:
       selected_channel = MUX_FX;
       break;
-    case Y_AXIS: 
+    case Y_AXIS:
       selected_channel = MUX_FY;
       break;
     case Z_AXIS:
@@ -255,43 +244,38 @@ uint8_t CommandManager::do_throttle_muxing(void)
       break;
   }
 
-  // Check if the override switch exists and is triggered
-  if (RF_.rc_.switch_mapped(RC::SWITCH_THROTTLE_OVERRIDE)
-      && RF_.rc_.switch_on(RC::SWITCH_THROTTLE_OVERRIDE)) {
-    override_this_channel = true;
-  } else { // Otherwise check if the offboard throttle channel is active, if it isn't, have RC override
-    if (muxes[selected_channel].onboard->active) {
-      // Check if the parameter flag is set to have us always take the smaller throttle
-      if (RF_.params_.get_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE)) {
-        override_this_channel = (muxes[selected_channel].rc->value < muxes[selected_channel].onboard->value);
-      } else {
-        override_this_channel = false;
-      }
-    } else {
-      override_this_channel = true;
+  uint16_t rc_overrides{0};
+  if (!(muxes_[selected_channel].onboard->active)) {
+    rc_overrides |= OVERRIDE_OFFBOARD_T_INACTIVE;
+  }
+  if (RF_.params_.get_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE)) {
+    if (muxes_[selected_channel].rc->value < muxes_[selected_channel].onboard->value) {
+      rc_overrides |= OVERRIDE_T;
     }
   }
+  return rc_overrides;
+}
 
-  // Set the combined channel output depending on whether RC is overriding for this channel or not
-  // Either RC overrides all force inputs, or none
-  if (override_this_channel) {
-    *muxes[MUX_FX].combined = *muxes[MUX_FX].rc;
-    *muxes[MUX_FY].combined = *muxes[MUX_FY].rc;
-    *muxes[MUX_FZ].combined = *muxes[MUX_FZ].rc;
-  } else {
-    *muxes[MUX_FX].combined = *muxes[MUX_FX].onboard;
-    *muxes[MUX_FY].combined = *muxes[MUX_FY].onboard;
-    *muxes[MUX_FZ].combined = *muxes[MUX_FZ].onboard;
+void CommandManager::do_muxing(uint16_t rc_override)
+{
+  for (uint8_t channel{0}; channel < NUM_MUX_CHANNELS; ++channel) {
+    do_channel_muxing(static_cast<MuxChannel>(channel), rc_override);
   }
-  return override_this_channel;
 }
 
-uint8_t CommandManager::rc_override_active() { return rc_override_; }
+void CommandManager::do_channel_muxing(MuxChannel channel, uint16_t rc_override )
+{
+  bool override_this_channel = (rc_override & channel_override_[channel].override_mask);
+  // set the combined channel output depending on whether RC is overriding for this channel or not
+  *muxes_[channel].combined = override_this_channel ? *muxes_[channel].rc : *muxes_[channel].onboard;
+}
+
+uint16_t CommandManager::get_rc_override() { return rc_override_; }
 
 bool CommandManager::offboard_control_active()
 {
-  for (int i = 0; i < 4; i++) {
-    if (muxes[i].onboard->active) { return true; }
+  for (uint8_t channel{0}; channel < NUM_MUX_CHANNELS; ++channel) {
+    if (muxes_[channel].onboard->active) { return true; }
   }
   return false;
 }
@@ -316,7 +300,7 @@ void CommandManager::override_combined_command_with_rc()
 
 bool CommandManager::run()
 {
-  bool last_rc_override = rc_override_active();
+  uint16_t last_rc_override = get_rc_override();
 
   // Check for and apply failsafe command
   if (RF_.state_manager_.state().failsafe) {
@@ -338,21 +322,19 @@ bool CommandManager::run()
     }
 
     // Perform muxing
-    rc_override_ = do_roll_pitch_yaw_muxing(MUX_QX);
-    rc_override_ |= do_roll_pitch_yaw_muxing(MUX_QY);
-    rc_override_ |= do_roll_pitch_yaw_muxing(MUX_QZ);
-    rc_override_ += 2U*do_throttle_muxing();
+    rc_override_ = determine_override_status();
+    do_muxing(rc_override_);
 
     // Light to indicate override
-    if (rc_override_active()) {
+    if (rc_override_) {
       RF_.board_.led0_on();
     } else {
       RF_.board_.led0_off();
     }
   }
 
   // There was a change in rc_override state
-  if (last_rc_override != rc_override_active()) { RF_.comm_manager_.update_status(); }
+  if (last_rc_override != get_rc_override()) { RF_.comm_manager_.update_status(); }
   return true;
 }