Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_TECS: take load factor into account in min airspeed 

this will push up minimum airspeed when turning
This commit is contained in:
Andrew Tridgell 2014-11-12 13:32:46 +11:00
parent ba312856ea
commit bf591b0008
2 changed files with 12 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_TECS/AP_TECS.cpp
View File

@ -242,7 +242,7 @@ void AP_TECS::update_50hz(float hgt_afe)
}
void AP_TECS::_update_speed(void)
void AP_TECS::_update_speed(float load_factor)
{
// Calculate time in seconds since last update
uint32_t now = hal.scheduler->micros();
@ -254,7 +254,10 @@ void AP_TECS::_update_speed(void)
float EAS2TAS = _ahrs.get_EAS2TAS();
_TAS_dem = _EAS_dem * EAS2TAS;
_TASmax = aparm.airspeed_max * EAS2TAS;
_TASmin = aparm.airspeed_min * EAS2TAS;
_TASmin = aparm.airspeed_min * EAS2TAS * load_factor;
if (_TASmax < _TASmin) {
_TASmax = _TASmin;
}
if (_landAirspeed >= 0 && _ahrs.airspeed_sensor_enabled() &&
(_flight_stage == FLIGHT_LAND_APPROACH || _flight_stage== FLIGHT_LAND_FINAL)) {
_TAS_dem = _landAirspeed * EAS2TAS;
@ -307,7 +310,7 @@ void AP_TECS::_update_speed_demand(void)
_TAS_dem = _TASmin;
}
// Constrain speed demand
// Constrain speed demand, taking into account the load factor
_TAS_dem = constrain_float(_TAS_dem, _TASmin, _TASmax);
// calculate velocity rate limits based on physical performance limits
@ -708,7 +711,8 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
enum FlightStage flight_stage,
int32_t ptchMinCO_cd,
int16_t throttle_nudge,
float hgt_afe)
float hgt_afe,
float load_factor)
{
// Calculate time in seconds since last update
uint32_t now = hal.scheduler->micros();
@ -716,7 +720,7 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
_update_pitch_throttle_last_usec = now;
// Update the speed estimate using a 2nd order complementary filter
_update_speed();
_update_speed(load_factor);
// Convert inputs
_hgt_dem = hgt_dem_cm * 0.01f;

5
libraries/AP_TECS/AP_TECS.h
View File

@ -50,7 +50,8 @@ public:
enum FlightStage flight_stage,
int32_t ptchMinCO_cd,
int16_t throttle_nudge,
float hgt_afe);
float hgt_afe,
float load_factor);
// demanded throttle in percentage
// should return 0 to 100
@ -241,7 +242,7 @@ private:
uint8_t _flare_counter;
// Update the airspeed internal state using a second order complementary filter
void _update_speed(void);
void _update_speed(float load_factor);
// Update the demanded airspeed
void _update_speed_demand(void);