aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Motors: allow use of raw voltage for battery compensation driven by MOT_OPTIONS

Browse Source
This commit is contained in:
Andy Piper 2023-03-16 23:19:29 +00:00 committed by Andrew Tridgell
parent 070d159b17
commit 6248a657c8
3 changed files with 28 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_Motors/AP_MotorsMulticopter.cpp
View File

@ -215,6 +215,13 @@ const AP_Param::GroupInfo AP_MotorsMulticopter::var_info[] = {
// @User: Advanced
AP_GROUPINFO("SAFE_TIME", 42, AP_MotorsMulticopter, _safe_time, AP_MOTORS_SAFE_TIME_DEFAULT),
// @Param: OPTIONS
// @DisplayName: Motor options
// @Description: Motor options
// @Bitmask: 0:Voltage compensation uses raw voltage
// @User: Advanced
AP_GROUPINFO("OPTIONS", 43, AP_MotorsMulticopter, _options, 0),
AP_GROUPEND
};
@ -362,7 +369,6 @@ float AP_MotorsMulticopter::get_current_limit_max_throttle()
return get_throttle_hover() + ((1.0 - get_throttle_hover()) * _throttle_limit);
}
// 10hz logging of voltage scaling and max trust
void AP_MotorsMulticopter::Log_Write()
{

8
libraries/AP_Motors/AP_Motors_Class.h
View File

@ -277,6 +277,11 @@ public:
// write log, to be called at 10hz
virtual void Log_Write() {};
enum MotorOptions : uint8_t {
BATT_RAW_VOLTAGE = (1 << 0U)
};
bool has_option(MotorOptions option) { return _options.get() & uint8_t(option); }
protected:
// output functions that should be overloaded by child classes
virtual void output_armed_stabilizing() = 0;
@ -340,6 +345,9 @@ protected:
bool _thrust_balanced; // true when output thrust is well balanced
float _thrust_boost_ratio; // choice between highest and second highest motor output for output mixing (0 ~ 1). Zero is normal operation
// motor options
AP_Int16 _options;
MAV_TYPE _mav_type; // MAV_TYPE_GENERIC = 0;
enum pwm_type { PWM_TYPE_NORMAL = 0,

20
libraries/AP_Motors/AP_Motors_Thrust_Linearization.cpp
View File

@ -83,8 +83,9 @@ void Thrust_Linearization::update_lift_max_from_batt_voltage()
{
// sanity check battery_voltage_min is not too small
// if disabled or misconfigured exit immediately
float _batt_voltage_resting_estimate = AP::battery().voltage_resting_estimate(batt_idx);
if ((batt_voltage_max <= 0) || (batt_voltage_min >= batt_voltage_max) || (_batt_voltage_resting_estimate < 0.25 * batt_voltage_min)) {
float _batt_voltage = motors.has_option(AP_Motors::MotorOptions::BATT_RAW_VOLTAGE) ? AP::battery().voltage(batt_idx) : AP::battery().voltage_resting_estimate(batt_idx);
if ((batt_voltage_max <= 0) || (batt_voltage_min >= batt_voltage_max) || (_batt_voltage < 0.25 * batt_voltage_min)) {
batt_voltage_filt.reset(1.0);
lift_max = 1.0;
return;
@ -92,15 +93,20 @@ void Thrust_Linearization::update_lift_max_from_batt_voltage()
batt_voltage_min.set(MAX(batt_voltage_min, batt_voltage_max * 0.6));
// contrain resting voltage estimate (resting voltage is actual voltage with sag removed based on current draw and resistance)
_batt_voltage_resting_estimate = constrain_float(_batt_voltage_resting_estimate, batt_voltage_min, batt_voltage_max);
// constrain resting voltage estimate (resting voltage is actual voltage with sag removed based on current draw and resistance)
_batt_voltage = constrain_float(_batt_voltage, batt_voltage_min, batt_voltage_max);
// filter at 0.5 Hz
float batt_voltage_flittered = batt_voltage_filt.apply(_batt_voltage_resting_estimate / batt_voltage_max, motors.get_dt());
if (!motors.has_option(AP_Motors::MotorOptions::BATT_RAW_VOLTAGE)) {
// filter at 0.5 Hz
batt_voltage_filt.apply(_batt_voltage / batt_voltage_max, motors.get_dt());
} else {
// reset is equivalent to no filtering
batt_voltage_filt.reset(_batt_voltage / batt_voltage_max);
}
// calculate lift max
float thrust_curve_expo = constrain_float(curve_expo, -1.0, 1.0);
lift_max = batt_voltage_flittered * (1 - thrust_curve_expo) + thrust_curve_expo * batt_voltage_flittered * batt_voltage_flittered;
lift_max = batt_voltage_filt.get() * (1 - thrust_curve_expo) + thrust_curve_expo * batt_voltage_filt.get() * batt_voltage_filt.get();
}
// return gain scheduling gain based on voltage and air density