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

TECS: added height update_pitch call

Browse Source
This commit is contained in:
Paul Riseborough 2013-07-10 13:32:13 +10:00 committed by Andrew Tridgell
parent 971d36f06b
commit 66d60953df
3 changed files with 12 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/AP_SpdHgtControl/AP_SpdHgtControl.h
View File

@ -45,7 +45,8 @@ public:
int32_t EAS_dem_cm, int32_t EAS_dem_cm,
bool climbOutDem, bool climbOutDem,
int32_t ptchMinCO_cd, int32_t ptchMinCO_cd,
int16_t throttle_nudge) = 0; int16_t throttle_nudge,
float hgt_afe) = 0;
// demanded throttle in percentage // demanded throttle in percentage
// should return 0 to 100 // should return 0 to 100

13
libraries/AP_TECS/AP_TECS.cpp
View File

@ -164,7 +164,7 @@ void AP_TECS::update_50hz(float hgt_afe)
// If more than 1 second has elapsed since last update then reset the integrator state // If more than 1 second has elapsed since last update then reset the integrator state
// to the measured height // to the measured height
if (DT > 1.0) { if (DT > 1.0) {
_integ3_state = _baro->get_altitude(); _integ3_state = hgt_afe;
} else { } else {
_integ3_state = _integ3_state + integ3_input*DT; _integ3_state = _integ3_state + integ3_input*DT;
} }
@ -534,7 +534,7 @@ void AP_TECS::_update_pitch(void)
_last_pitch_dem = _pitch_dem; _last_pitch_dem = _pitch_dem;
} }
void AP_TECS::_initialise_states(int32_t ptchMinCO_cd) void AP_TECS::_initialise_states(int32_t ptchMinCO_cd, float hgt_afe)
{ {
// Initialise states and variables if DT > 1 second or in climbout // Initialise states and variables if DT > 1 second or in climbout
if (_DT > 1.0) if (_DT > 1.0)
@ -543,7 +543,7 @@ void AP_TECS::_initialise_states(int32_t ptchMinCO_cd)
_integ7_state = 0.0f; _integ7_state = 0.0f;
_last_throttle_dem = aparm.throttle_cruise * 0.01f; _last_throttle_dem = aparm.throttle_cruise * 0.01f;
_last_pitch_dem = _ahrs->pitch; _last_pitch_dem = _ahrs->pitch;
_hgt_dem_adj_last = _baro->get_altitude(); _hgt_dem_adj_last = hgt_afe;
_hgt_dem_adj = _hgt_dem_adj_last; _hgt_dem_adj = _hgt_dem_adj_last;
_hgt_dem_prev = _hgt_dem_adj_last; _hgt_dem_prev = _hgt_dem_adj_last;
_hgt_dem_in_old = _hgt_dem_adj_last; _hgt_dem_in_old = _hgt_dem_adj_last;
@ -558,7 +558,7 @@ void AP_TECS::_initialise_states(int32_t ptchMinCO_cd)
{ {
_PITCHminf = 0.000174533f * ptchMinCO_cd; _PITCHminf = 0.000174533f * ptchMinCO_cd;
_THRminf = _THRmaxf - 0.01f; _THRminf = _THRmaxf - 0.01f;
_hgt_dem_adj_last = _baro->get_altitude(); _hgt_dem_adj_last = hgt_afe;
_hgt_dem_adj = _hgt_dem_adj_last; _hgt_dem_adj = _hgt_dem_adj_last;
_hgt_dem_prev = _hgt_dem_adj_last; _hgt_dem_prev = _hgt_dem_adj_last;
_TAS_dem_last = _TAS_dem; _TAS_dem_last = _TAS_dem;
@ -580,7 +580,8 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
int32_t EAS_dem_cm, int32_t EAS_dem_cm,
bool climbOutDem, bool climbOutDem,
int32_t ptchMinCO_cd, int32_t ptchMinCO_cd,
int16_t throttle_nudge) int16_t throttle_nudge,
float hgt_afe)
{ {
// 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();
@ -600,7 +601,7 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
_climbOutDem = climbOutDem; _climbOutDem = climbOutDem;
// initialise selected states and variables if DT > 1 second or in climbout // initialise selected states and variables if DT > 1 second or in climbout
_initialise_states(ptchMinCO_cd); _initialise_states(ptchMinCO_cd, hgt_afe);
// Calculate Specific Total Energy Rate Limits // Calculate Specific Total Energy Rate Limits
_update_STE_rate_lim(); _update_STE_rate_lim();

5
libraries/AP_TECS/AP_TECS.h
View File

@ -49,7 +49,8 @@ public:
int32_t EAS_dem_cm, int32_t EAS_dem_cm,
bool climbOutDem, bool climbOutDem,
int32_t ptchMinCO_cd, int32_t ptchMinCO_cd,
int16_t throttle_nudge); int16_t throttle_nudge,
float hgt_afe);
// demanded throttle in percentage // demanded throttle in percentage
// should return 0 to 100 // should return 0 to 100
@ -256,7 +257,7 @@ private:
void _update_pitch(void); void _update_pitch(void);
// Initialise states and variables // Initialise states and variables
void _initialise_states(int32_t ptchMinCO_cd); void _initialise_states(int32_t ptchMinCO_cd, float hgt_afe);
// Calculate specific total energy rate limits // Calculate specific total energy rate limits
void _update_STE_rate_lim(void); void _update_STE_rate_lim(void);