Merge pull request #468 from rosflight/jacob/use-current-battery-in-mixer

Use battery voltage as max voltage in mixer

Author: JMoore5353

include/sensors.h

@@ -122,6 +122,7 @@ class Sensors : public ParamListenerInterface
   void correct_mag(void);
   void correct_baro(void);
   void correct_diff_pressure(void);
+  void lpf_battery(void);
   void update_battery_monitor_multipliers(void);
 
   // IMU calibration
@@ -151,6 +152,8 @@ class Sensors : public ParamListenerInterface
   // Battery Monitor
   float battery_voltage_alpha_{0.995};
   float battery_current_alpha_{0.995};
+  float previous_battery_voltage_{0.0};
+  float previous_battery_current_{0.0};
 };
 
 } // namespace rosflight_firmware

src/controller.cpp

@@ -87,6 +87,9 @@ void Controller::calculate_max_thrust()
   float KQ = KV;
   int num_motors = RF_.params_.get_param_int(PARAM_NUM_MOTORS);
 
+  // Note: As voltage drops during flight, this max voltage stays constant.
+  // This means that a constant throttle setting will result in a constant
+  // output thrust command (if converting from throttle to thrust).
   float V_max = RF_.params_.get_param_float(PARAM_VOLT_MAX);
   float a = R * rho * pow(D, 5.0) * CQ / (4 * pow(M_PI, 2.0) * KQ);
   float b = KV;

src/mixer.cpp

@@ -121,7 +121,6 @@ void Mixer::param_change_callback(uint16_t param_id)
     case PARAM_PROP_CT: C_T_ = RF_.params_.get_param_float(PARAM_PROP_CT); break;
     case PARAM_PROP_CQ: C_Q_ = RF_.params_.get_param_float(PARAM_PROP_CQ); break;
     case PARAM_NUM_MOTORS: num_motors_ = RF_.params_.get_param_int(PARAM_NUM_MOTORS); break;
-    case PARAM_VOLT_MAX: V_max_ = RF_.params_.get_param_float(PARAM_VOLT_MAX); break;
     case PARAM_MOTOR_PWM_SEND_RATE:
       init_PWM();
       break;
@@ -265,7 +264,6 @@ void Mixer::update_parameters()
   C_T_ = RF_.params_.get_param_float(PARAM_PROP_CT);
   C_Q_ = RF_.params_.get_param_float(PARAM_PROP_CQ);
   num_motors_ = RF_.params_.get_param_int(PARAM_NUM_MOTORS);
-  V_max_ = RF_.params_.get_param_float(PARAM_VOLT_MAX);
 }
 
 Mixer::mixer_t Mixer::invert_mixer(const mixer_t* mixer_to_invert)
@@ -479,7 +477,8 @@ float Mixer::mix_multirotor_with_motor_parameters(Controller::Output commands)
         + R_ * i_0_ + K_V_ * sqrt(omega_squared);
 
       // Convert desired V_in setting to a throttle setting
-      outputs_[i] = V_in / V_max_;
+      BatteryStruct * batt = RF_.sensors_.get_battery();
+      outputs_[i] = V_in / batt->voltage;
 
       // Save off the largest control output (for motors) if it is greater than 1.0 for future scaling
       if (abs(outputs_[i]) > max_output) { max_output = abs(outputs_[i]); }

src/sensors.cpp

@@ -52,7 +52,9 @@ const float Sensors::DIFF_PRESSURE_MAX_CALIBRATION_VARIANCE = 100.0; // standard
 
 Sensors::Sensors(ROSflight & rosflight)
     : rf_(rosflight)
-{}
+{
+  previous_battery_voltage_ = rf_.params_.get_param_float(PARAM_VOLT_MAX);
+}
 
 void Sensors::init()
 {
@@ -169,7 +171,9 @@ got_flags Sensors::run()
   got.sonar = rf_.board_.sonar_read(&sonar_);
 
   // BATTERY_MONITOR:
-  got.battery = rf_.board_.battery_read(&battery_);
+  if ((got.battery = rf_.board_.battery_read(&battery_))) {
+    lpf_battery();
+  }
 
   return got;
 }
@@ -477,6 +481,14 @@ void Sensors::correct_diff_pressure()
     * sqrt((fabs(diff_pressure_.pressure) / (0.5 * 1.225)));
 }
 
+void Sensors::lpf_battery()
+{
+  battery_.voltage = battery_.voltage * (1.0 - battery_voltage_alpha_) + previous_battery_voltage_ * battery_voltage_alpha_;
+  battery_.current = battery_.current * (1.0 - battery_current_alpha_) + previous_battery_current_ * battery_current_alpha_;
+  previous_battery_voltage_ = battery_.voltage;
+  previous_battery_current_ = battery_.current;
+}
+
 void Sensors::update_battery_monitor_multipliers()
 {
   float voltage_multiplier = this->rf_.params_.get_param_float(PARAM_BATTERY_VOLTAGE_MULTIPLIER);