mirror of https://github.com/ArduPilot/ardupilot
AP_Motors: add and use AP_BATTERY_ENABLED
This commit is contained in:
parent
b48b26ddb8
commit
eda6d0b6a0
|
@ -359,6 +359,7 @@ void AP_Motors6DOF::output_armed_stabilizing()
|
|||
}
|
||||
}
|
||||
|
||||
#if AP_BATTERY_ENABLED
|
||||
const AP_BattMonitor &battery = AP::battery();
|
||||
|
||||
// Current limiting
|
||||
|
@ -384,6 +385,7 @@ void AP_Motors6DOF::output_armed_stabilizing()
|
|||
batt_current_ratio = predicted_current_ratio;
|
||||
}
|
||||
_output_limited += (_dt / (_dt + _batt_current_time_constant)) * (1 - batt_current_ratio);
|
||||
#endif
|
||||
|
||||
_output_limited = constrain_float(_output_limited, 0.0f, 1.0f);
|
||||
|
||||
|
|
|
@ -345,6 +345,7 @@ void AP_MotorsMulticopter::update_throttle_filter()
|
|||
// return current_limit as a number from 0 ~ 1 in the range throttle_min to throttle_max
|
||||
float AP_MotorsMulticopter::get_current_limit_max_throttle()
|
||||
{
|
||||
#if AP_BATTERY_ENABLED
|
||||
AP_BattMonitor &battery = AP::battery();
|
||||
|
||||
const uint8_t batt_idx = thr_lin.get_battery_index();
|
||||
|
@ -376,6 +377,9 @@ float AP_MotorsMulticopter::get_current_limit_max_throttle()
|
|||
|
||||
// limit max throttle
|
||||
return get_throttle_hover() + ((1.0 - get_throttle_hover()) * _throttle_limit);
|
||||
#else
|
||||
return 1.0;
|
||||
#endif
|
||||
}
|
||||
|
||||
#if HAL_LOGGING_ENABLED
|
||||
|
|
|
@ -151,6 +151,7 @@ float Thrust_Linearization::remove_thrust_curve_and_volt_scaling(float throttle)
|
|||
// update_lift_max from battery voltage - used for voltage compensation
|
||||
void Thrust_Linearization::update_lift_max_from_batt_voltage()
|
||||
{
|
||||
#if AP_BATTERY_ENABLED
|
||||
// sanity check battery_voltage_min is not too small
|
||||
// if disabled or misconfigured exit immediately
|
||||
float _batt_voltage = motors.has_option(AP_Motors::MotorOptions::BATT_RAW_VOLTAGE) ? AP::battery().voltage(batt_idx) : AP::battery().voltage_resting_estimate(batt_idx);
|
||||
|
@ -177,6 +178,7 @@ void Thrust_Linearization::update_lift_max_from_batt_voltage()
|
|||
// calculate lift max
|
||||
float thrust_curve_expo = constrain_float(curve_expo, -1.0, 1.0);
|
||||
lift_max = batt_voltage_filt.get() * (1 - thrust_curve_expo) + thrust_curve_expo * batt_voltage_filt.get() * batt_voltage_filt.get();
|
||||
#endif
|
||||
}
|
||||
|
||||
// return gain scheduling gain based on voltage and air density
|
||||
|
|
Loading…
Reference in New Issue