From 007c96a3d8afa82fb58af92c7e54d9e95294bf4b Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Sun, 8 Mar 2015 20:47:28 +1030 Subject: [PATCH] AP_Motors: Low throttle compensation setters --- libraries/AP_Motors/AP_Motors_Class.cpp | 17 +++++++++++++++++ libraries/AP_Motors/AP_Motors_Class.h | 9 +++++++++ 2 files changed, 26 insertions(+) diff --git a/libraries/AP_Motors/AP_Motors_Class.cpp b/libraries/AP_Motors/AP_Motors_Class.cpp index e773fc3f5c..2d27eb98bf 100644 --- a/libraries/AP_Motors/AP_Motors_Class.cpp +++ b/libraries/AP_Motors/AP_Motors_Class.cpp @@ -179,6 +179,9 @@ void AP_Motors::output() // calc filtered battery voltage and lift_max update_lift_max_from_batt_voltage(); + // move throttle_low_comp towards desired throttle low comp + update_throttle_low_comp(); + // output to motors if (_flags.armed ) { output_armed(); @@ -318,3 +321,17 @@ void AP_Motors::update_battery_resistance() } } } + +// update_throttle_low_comp - slew set_throttle_low_comp to requested value +void AP_Motors::update_throttle_low_comp() +{ + // slew _throttle_low_comp to _throttle_low_comp_desired + if (_throttle_low_comp < _throttle_low_comp_desired) { + // increase quickly (i.e. from 0.1 to 0.9 in 0.8 seconds) + _throttle_low_comp += min(1.0f/_loop_rate, _throttle_low_comp_desired-_throttle_low_comp); + } else if (_throttle_low_comp > _throttle_low_comp_desired) { + // reduce more slowly (from 0.9 to 0.1 in 1.8 seconds) + _throttle_low_comp -= min(0.5f/_loop_rate, _throttle_low_comp-_throttle_low_comp_desired); + } + _throttle_low_comp = constrain_float(_throttle_low_comp, 0.1f, 1.0f); +} diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index 151939262c..5355ea4c0b 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/libraries/AP_Motors/AP_Motors_Class.h @@ -156,6 +156,11 @@ public: // set_current - set current to be used for output scaling virtual void set_current(float current){ _batt_current = current; } + // set_throttle_low_comp - set desired throttle_low_comp (actual throttle_low_comp is slewed towards this value over 1~2 seconds) + // low values favour pilot/autopilot throttle over attitude control, high values favour attitude control over throttle + // has no effect when throttle is above hover throttle + void set_throttle_low_comp(float throttle_low_comp) { _throttle_low_comp_desired = throttle_low_comp; } + // get_lift_max - get maximum lift ratio float get_lift_max() { return _lift_max; } @@ -211,6 +216,9 @@ protected: // update_battery_resistance - calculate battery resistance when throttle is above hover_out void update_battery_resistance(); + // update_throttle_low_comp - updates thr_low_comp value towards the target + void update_throttle_low_comp(); + // get_voltage_comp_gain - return battery voltage compensation gain float get_voltage_comp_gain() const { return 1.0f/_lift_max; } @@ -247,6 +255,7 @@ protected: int16_t _max_throttle; // the maximum throttle to be sent to the motors (sometimes limited by slow start) int16_t _hover_out; // the estimated hover throttle as pct * 10 (i.e. 0 ~ 1000) int16_t _spin_when_armed_ramped;// equal to _spin_when_armed parameter but slowly ramped up from zero + float _throttle_low_comp_desired; // desired throttle_low_comp value, actual throttle_low_comp is slewed towards this value over 1~2 seconds // battery voltage compensation variables float _batt_voltage; // latest battery voltage reading