fix: initialize batt voltage mult to 1 to avoid divide by zero

Author: JMoore5353

src/mixer.cpp

@@ -112,6 +112,8 @@ void Mixer::param_change_callback(uint16_t param_id)
   } else switch (param_id) {
     case PARAM_PRIMARY_MIXER:
     case PARAM_SECONDARY_MIXER:
+    case PARAM_USE_MOTOR_PARAMETERS:
+    case PARAM_RC_F_AXIS:
       init_mixing();
       break;
     case PARAM_MOTOR_RESISTANCE: R_ = RF_.params_.get_param_float(PARAM_MOTOR_RESISTANCE); break;
@@ -454,7 +456,7 @@ float Mixer::mix_multirotor_without_motor_parameters(Controller::Output commands
 float Mixer::mix_multirotor_with_motor_parameters(Controller::Output commands)
 {
   // Mix the inputs
-  float max_output = 1.0;
+  float max_output{1.0};
 
   float rho = RF_.sensors_.rho();
 
@@ -467,17 +469,29 @@ float Mixer::mix_multirotor_with_motor_parameters(Controller::Output commands)
                             commands.Qx * (*mixer_to_use_.Qx)[i] +
                             commands.Qy * (*mixer_to_use_.Qy)[i] +
                             commands.Qz * (*mixer_to_use_.Qz)[i];
-      
+
       // Ensure that omega_squared is non-negative
       if (omega_squared < 0.0) { omega_squared = 0.0; }
 
       // Ch. 4, setting equation for torque produced by a propeller equal to Eq. 4.19
       // Note that we assume constant advance ratio, leading to constant torque and thrust constants.
+      if (K_Q_ < 0.0000001) {
+        RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR,
+                              "Divide by K_Q, which is zero! Skipping");
+        return max_output;
+      }
       float V_in = rho * pow(D_, 5.0) / (4.0 * pow(M_PI, 2.0)) * omega_squared * C_Q_ * R_ / K_Q_
         + R_ * i_0_ + K_V_ * sqrt(omega_squared);
 
       // Convert desired V_in setting to a throttle setting
       BatteryStruct * batt = RF_.sensors_.get_battery();
+      if (batt->voltage < 0.0001) {
+        RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR,
+                              "Divide by battery voltage, which is near zero!");
+        RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR,
+                              "Is battery multiplier set correctly?");
+        return max_output;
+      }
       outputs_[i] = V_in / batt->voltage;
 
       // Save off the largest control output (for motors) if it is greater than 1.0 for future scaling
@@ -527,7 +541,7 @@ void Mixer::mix_multirotor()
   }
 
   // Mix the outputs based on if a custom mixer (i.e. with motor parameters) is selected.
-  float max_output;
+  float max_output{0.0};
   if (RF_.params_.get_param_int(PARAM_USE_MOTOR_PARAMETERS)) {
     max_output = mix_multirotor_with_motor_parameters(commands);
   } else {

src/param.cpp

@@ -426,8 +426,8 @@ void Params::set_defaults(void)
   /*****************************/
   /*** BATTERY MONITOR SETUP ***/
   /*****************************/
-  init_param_float(PARAM_BATTERY_VOLTAGE_MULTIPLIER, "BATT_VOLT_MULT", 0.0f); // Multiplier for the voltage sensor | 0 | inf
-  init_param_float(PARAM_BATTERY_CURRENT_MULTIPLIER, "BATT_CURR_MULT", 0.0f); // Multiplier for the current sensor | 0 | inf
+  init_param_float(PARAM_BATTERY_VOLTAGE_MULTIPLIER, "BATT_VOLT_MULT", 1.0f); // Multiplier for the voltage sensor | 0 | inf
+  init_param_float(PARAM_BATTERY_CURRENT_MULTIPLIER, "BATT_CURR_MULT", 1.0f); // Multiplier for the current sensor | 0 | inf
   init_param_float(PARAM_BATTERY_VOLTAGE_ALPHA, "BATT_VOLT_ALPHA", 0.995f);   // Alpha value for the low pass filter on the reported battery voltage | 0 | 1
   init_param_float(PARAM_BATTERY_CURRENT_ALPHA, "BATT_CURR_ALPHA", 0.995f);   // Alpha value for the low pass filter on the reported battery current | 0 | 1
 

test/parameters_test.cpp

@@ -217,7 +217,7 @@ TEST(Parameters, DefaultParameters)
   EXPECT_PARAM_EQ_FLOAT(PARAM_FC_ROLL, 0.0f); // roll angle (deg) of flight controller wrt aircraft body | 0 | 360
   EXPECT_PARAM_EQ_FLOAT(PARAM_FC_PITCH, 0.0f); // pitch angle (deg) of flight controller wrt aircraft body | 0 | 360
   EXPECT_PARAM_EQ_FLOAT(PARAM_FC_YAW, 0.0f); // yaw angle (deg) of flight controller wrt aircraft body | 0 | 360
-  EXPECT_PARAM_EQ_FLOAT(PARAM_BATTERY_VOLTAGE_MULTIPLIER, 0.0f); // Multiplier for the voltage sensor | 0 | inf
-  EXPECT_PARAM_EQ_FLOAT(PARAM_BATTERY_CURRENT_MULTIPLIER, 0.0f); // Multiplier for the current sensor | 0 | inf
+  EXPECT_PARAM_EQ_FLOAT(PARAM_BATTERY_VOLTAGE_MULTIPLIER, 1.0f); // Multiplier for the voltage sensor | 0 | inf
+  EXPECT_PARAM_EQ_FLOAT(PARAM_BATTERY_CURRENT_MULTIPLIER, 1.0f); // Multiplier for the current sensor | 0 | inf
   EXPECT_PARAM_EQ_INT(PARAM_OFFBOARD_TIMEOUT, 100); // Timeout in milliseconds for offboard commands, after which RC override is activated | 0 | 100000
 }