Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-11 17:13:56 -03:00
Code Issues Packages Projects Releases Wiki Activity

AP_TECS: changed land_spdwgt to be sliding scale

Browse Source
This commit is contained in:
Tom Pittenger 2016-01-08 16:13:08 -08:00 committed by Andrew Tridgell
parent 7978872e32
commit db8a2f7e8b
2 changed files with 16 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_TECS/AP_TECS.cpp
View File

@ -669,7 +669,13 @@ void AP_TECS::_update_pitch(void)
} else if ( _underspeed || _flight_stage == AP_TECS::FLIGHT_TAKEOFF || _flight_stage == AP_TECS::FLIGHT_LAND_ABORT) {
SKE_weighting = 2.0f;
} else if (_flight_stage == AP_TECS::FLIGHT_LAND_APPROACH || _flight_stage == AP_TECS::FLIGHT_LAND_FINAL) {
SKE_weighting = constrain_float(_spdWeightLand, 0.0f, 2.0f);
if (_spdWeightLand < 0) {
// use sliding scale from normal weight down to zero at landing
float scaled_weight = _spdWeight * (1.0f - _path_proportion);
SKE_weighting = constrain_float(scaled_weight, 0.0f, 2.0f);
} else {
SKE_weighting = constrain_float(_spdWeightLand, 0.0f, 2.0f);
}
}
float SPE_weighting = 2.0f - SKE_weighting;

10
libraries/AP_TECS/AP_TECS.h
View File

@ -91,7 +91,12 @@ public:
float get_height_rate_demand(void) const {
return _hgt_rate_dem;
}
// set path_proportion
void set_path_proportion(float path_proportion) {
_path_proportion = constrain_float(path_proportion, 0.0f, 1.0f);
}
// this supports the TECS_* user settable parameters
static const struct AP_Param::GroupInfo var_info[];
@ -271,6 +276,9 @@ private:
// counter for demanded sink rate on land final
uint8_t _flare_counter;
// percent traveled along the previous and next waypoints
float _path_proportion;
// Update the airspeed internal state using a second order complementary filter
void _update_speed(float load_factor);