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

TECS: added support for throttle nudging

Browse Source
This commit is contained in:
Paul Riseborough 2013-07-09 20:50:37 +10:00 committed by Andrew Tridgell
parent c34803db13
commit 2c5db9a165
3 changed files with 19 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_SpdHgtControl/AP_SpdHgtControl.h
View File

@ -41,7 +41,11 @@ public:
// Update of the pitch and throttle demands // Update of the pitch and throttle demands
// Should be called at 10Hz or faster // Should be called at 10Hz or faster
virtual void update_pitch_throttle(int32_t hgt_dem_cm, int32_t EAS_dem_cm, bool climbOutDem, int32_t ptchMinCO_cd) = 0; virtual void update_pitch_throttle( int32_t hgt_dem_cm,
int32_t EAS_dem_cm,
bool climbOutDem,
int32_t ptchMinCO_cd,
int16_t throttle_nudge) = 0;
// demanded throttle in percentage // demanded throttle in percentage
// should return 0 to 100 // should return 0 to 100

12
libraries/AP_TECS/AP_TECS.cpp
View File

@ -424,10 +424,10 @@ void AP_TECS::_update_throttle(void)
_throttle_dem = constrain_float(_throttle_dem, _THRminf, _THRmaxf); _throttle_dem = constrain_float(_throttle_dem, _THRminf, _THRmaxf);
} }
void AP_TECS::_update_throttle_option(void) void AP_TECS::_update_throttle_option(int16_t throttle_nudge)
{ {
// Calculate throttle demand by interpolating between pitch and throttle limits // Calculate throttle demand by interpolating between pitch and throttle limits
float nomThr = aparm.throttle_cruise * 0.01f; float nomThr = (aparm.throttle_cruise + throttle_nudge)* 0.01f;
if (_climbOutDem) if (_climbOutDem)
{ {
_throttle_dem = _THRmaxf; _throttle_dem = _THRmaxf;
@ -576,7 +576,11 @@ void AP_TECS::_update_STE_rate_lim(void)
_STEdot_min = - _minSinkRate * GRAVITY_MSS; _STEdot_min = - _minSinkRate * GRAVITY_MSS;
} }
void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm, int32_t EAS_dem_cm, bool climbOutDem, int32_t ptchMinCO_cd) void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
int32_t EAS_dem_cm,
bool climbOutDem,
int32_t ptchMinCO_cd,
int16_t throttle_nudge)
{ {
// 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();
@ -617,7 +621,7 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm, int32_t EAS_dem_cm, bool
if (_ahrs->airspeed_sensor_enabled()) { if (_ahrs->airspeed_sensor_enabled()) {
_update_throttle(); _update_throttle();
} else { } else {
_update_throttle_option(); _update_throttle_option(throttle_nudge);
} }
// Detect bad descent due to demanded airspeed being too high // Detect bad descent due to demanded airspeed being too high

8
libraries/AP_TECS/AP_TECS.h
View File

@ -45,7 +45,11 @@ public:
void update_50hz(float hgt_afe); void update_50hz(float hgt_afe);
// Update the control loop calculations // Update the control loop calculations
void update_pitch_throttle(int32_t hgt_dem_cm, int32_t EAS_dem_cm, bool climbOutDem, int32_t ptchMinCO_cd); void update_pitch_throttle(int32_t hgt_dem_cm,
int32_t EAS_dem_cm,
bool climbOutDem,
int32_t ptchMinCO_cd,
int16_t throttle_nudge);
// demanded throttle in percentage // demanded throttle in percentage
// should return 0 to 100 // should return 0 to 100
@ -243,7 +247,7 @@ private:
void _update_throttle(void); void _update_throttle(void);
// Update Demanded Throttle Non-Airspeed // Update Demanded Throttle Non-Airspeed
void _update_throttle_option(void); void _update_throttle_option(int16_t throttle_nudge);
// Detect Bad Descent // Detect Bad Descent
void _detect_bad_descent(void); void _detect_bad_descent(void);