minor code cleanup

Author: JMoore5353

include/controller.h

@@ -70,6 +70,7 @@ class Controller : public ParamListenerInterface
   void calculate_max_thrust();
   void calculate_equilbrium_torque_from_rc();
   void param_change_callback(uint16_t param_id) override;
+  bool is_throttle_high(float threshold);
 
 private:
   class PID

include/mixer.h

@@ -278,7 +278,7 @@ class Mixer : public ParamListenerInterface
   float R_;                      // Motor resistance
   float rho_;                    // Air density
   float K_V_;                    // Motor back-emf constant
-  float K_Q_ = 0.01706;          // Motor torque constant
+  float K_Q_;                    // Motor torque constant
   float i_0_;                    // Motor no-load current
   float D_;                      // Propeller diameter
   float C_T_;                    // Thrust coefficient

src/command_manager.cpp

@@ -114,7 +114,6 @@ void CommandManager::init_failsafe()
 void CommandManager::interpret_rc(void)
 {
   // get initial, unscaled RC values
-  // TODO: Adjust this to choose the channel that the RC thottle corresponds to
   rc_command_.Qx.value = RF_.rc_.stick(RC::STICK_X);
   rc_command_.Qy.value = RF_.rc_.stick(RC::STICK_Y);
   rc_command_.Qz.value = RF_.rc_.stick(RC::STICK_Z);
@@ -307,7 +306,6 @@ bool CommandManager::run()
     if (RF_.board_.clock_millis()
         > offboard_command_.stamp_ms + RF_.params_.get_param_int(PARAM_OFFBOARD_TIMEOUT)) {
       // If it has been longer than 100 ms, then disable the offboard control
-      // TODO: Check to make sure the FX and FY commands can be set to true
       offboard_command_.Fx.active = false;
       offboard_command_.Fy.active = false;
       offboard_command_.Fz.active = false;

src/controller.cpp

@@ -101,6 +101,12 @@ void Controller::calculate_max_thrust()
   max_thrust_ = rho * pow(D, 4.0) * CT * pow(omega, 2.0) / (4 * pow(M_PI, 2.0)) * num_motors;
 }
 
+bool Controller::is_throttle_high(float threshold) {
+  return  RF_.command_manager_.combined_control().Fx.value > threshold ||
+          RF_.command_manager_.combined_control().Fy.value > threshold ||
+          RF_.command_manager_.combined_control().Fz.value > threshold;
+}
+
 void Controller::run()
 {
   // Time calculation
@@ -117,10 +123,8 @@ void Controller::run()
   prev_time_us_ = RF_.estimator_.state().timestamp_us;
 
   // Check if integrators should be updated
-  //! @todo better way to figure out if throttle is high
-  // TODO: fix this... Needs to be checked based on the throttle channel (not necessarily Fz)
   bool update_integrators = (RF_.state_manager_.state().armed)
-    && (RF_.command_manager_.combined_control().Fz.value > 0.1f) && dt_us < 10000;
+    && is_throttle_high(0.1f) && dt_us < 10000;
 
   // Run the PID loops
   Controller::Output pid_output = run_pid_loops(

src/mixer.cpp

@@ -805,17 +805,17 @@ void Mixer::mix_fixedwing()
   Controller::Output commands = RF_.controller_.output();
 
   // Reverse fixed-wing channels just before mixing if we need to
-  if (RF_.params_.get_param_int(PARAM_FIXED_WING)) {
-    commands.Qx *= RF_.params_.get_param_int(PARAM_AILERON_REVERSE) ? -1 : 1;
-    commands.Qy *= RF_.params_.get_param_int(PARAM_ELEVATOR_REVERSE) ? -1 : 1;
-    commands.Qz *= RF_.params_.get_param_int(PARAM_RUDDER_REVERSE) ? -1 : 1;
-  }
+  commands.Qx *= RF_.params_.get_param_int(PARAM_AILERON_REVERSE) ? -1 : 1;
+  commands.Qy *= RF_.params_.get_param_int(PARAM_ELEVATOR_REVERSE) ? -1 : 1;
+  commands.Qz *= RF_.params_.get_param_int(PARAM_RUDDER_REVERSE) ? -1 : 1;
 
   // Mix the outputs
   for (uint8_t i = 0; i < NUM_MIXER_OUTPUTS; i++) {
     if ((*mixer_to_use_.output_type)[i] != NONE) {
       // Matrix multiply to mix outputs
       outputs_[i] = commands.Fx * (*mixer_to_use_.Fx)[i] +
+                    commands.Fy * (*mixer_to_use_.Fy)[i] +
+                    commands.Fz * (*mixer_to_use_.Fz)[i] +
                     commands.Qx * (*mixer_to_use_.Qx)[i] +
                     commands.Qy * (*mixer_to_use_.Qy)[i] +
                     commands.Qz * (*mixer_to_use_.Qz)[i];