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 // Calculate time in seconds since last update
uint32_t now = hal.scheduler->micros(); uint32_t now = hal.scheduler->micros();
@ -254,7 +254,10 @@ void AP_TECS::_update_speed(void)
float EAS2TAS = _ahrs.get_EAS2TAS(); float EAS2TAS = _ahrs.get_EAS2TAS();
_TAS_dem = _EAS_dem * EAS2TAS; _TAS_dem = _EAS_dem * EAS2TAS;
_TASmax = aparm.airspeed_max * 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() && if (_landAirspeed >= 0 && _ahrs.airspeed_sensor_enabled() &&
(_flight_stage == FLIGHT_LAND_APPROACH || _flight_stage== FLIGHT_LAND_FINAL)) { (_flight_stage == FLIGHT_LAND_APPROACH || _flight_stage== FLIGHT_LAND_FINAL)) {
_TAS_dem = _landAirspeed * EAS2TAS; _TAS_dem = _landAirspeed * EAS2TAS;
@ -307,7 +310,7 @@ void AP_TECS::_update_speed_demand(void)
_TAS_dem = _TASmin; _TAS_dem = _TASmin;
} }
// Constrain speed demand // Constrain speed demand, taking into account the load factor
_TAS_dem = constrain_float(_TAS_dem, _TASmin, _TASmax); _TAS_dem = constrain_float(_TAS_dem, _TASmin, _TASmax);
// calculate velocity rate limits based on physical performance limits // 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, enum FlightStage flight_stage,
int32_t ptchMinCO_cd, int32_t ptchMinCO_cd,
int16_t throttle_nudge, int16_t throttle_nudge,
float hgt_afe) float hgt_afe,
float load_factor)
{ {
// Calculate time in seconds since last update // Calculate time in seconds since last update
uint32_t now = hal.scheduler->micros(); 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_pitch_throttle_last_usec = now;
// Update the speed estimate using a 2nd order complementary filter // Update the speed estimate using a 2nd order complementary filter
_update_speed(); _update_speed(load_factor);
// Convert inputs // Convert inputs
_hgt_dem = hgt_dem_cm * 0.01f; _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, enum FlightStage flight_stage,
int32_t ptchMinCO_cd, int32_t ptchMinCO_cd,
int16_t throttle_nudge, int16_t throttle_nudge,
float hgt_afe); float hgt_afe,
float load_factor);
// demanded throttle in percentage // demanded throttle in percentage
// should return 0 to 100 // should return 0 to 100
@ -241,7 +242,7 @@ private:
uint8_t _flare_counter; uint8_t _flare_counter;
// Update the airspeed internal state using a second order complementary filter // 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 // Update the demanded airspeed
void _update_speed_demand(void); void _update_speed_demand(void);