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

Plane: pass in adjusted height above field to TECS

Browse Source
This commit is contained in:
Paul Riseborough 2013-07-09 09:25:08 +10:00 committed by Andrew Tridgell
parent 9c431b4a04
commit c34803db13
1 changed files with 20 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
ArduPlane/ArduPlane.pde
View File

@ -454,9 +454,6 @@ static struct {
////////////////////////////////////////////////////////////////////////////////
AP_Airspeed airspeed;
////////////////////////////////////////////////////////////////////////////////
// Altitude Sensor variables
////////////////////////////////////////////////////////////////////////////////
// flight mode specific
////////////////////////////////////////////////////////////////////////////////
@ -785,10 +782,23 @@ static void fast_loop()
*/
static void update_speed_height(void)
{
if (g.alt_control_algorithm == ALT_CONTROL_TECS &&
auto_throttle_mode && !throttle_suppressed) {
if (g.alt_control_algorithm == ALT_CONTROL_TECS && auto_throttle_mode && !throttle_suppressed)
{
// height above field elevation in cm
float hgt_afe_cm;
// calculate the height above field elevation in centimeters
if (barometer.healthy)
{
hgt_afe_cm = (1 - g.altitude_mix) * (g_gps->altitude - home.alt);
hgt_afe_cm += g.altitude_mix * read_barometer();
}
else if (g_gps->status() >= GPS::GPS_OK_FIX_3D)
{
hgt_afe_cm = g_gps->altitude - home.alt;
}
// Call TECS 50Hz update
SpdHgt_Controller->update_50hz();
SpdHgt_Controller->update_50hz(hgt_afe_cm*0.01f);
}
}
@ -1251,7 +1261,8 @@ static void update_alt()
// Update the speed & height controller states
if (g.alt_control_algorithm == ALT_CONTROL_TECS &&
auto_throttle_mode && !throttle_suppressed) {
SpdHgt_Controller->update_pitch_throttle(target_altitude_cm - home.alt, target_airspeed_cm,
SpdHgt_Controller->update_pitch_throttle(target_altitude_cm - home.alt + (int32_t(g.alt_offset)*100),
target_airspeed_cm,
(control_mode==AUTO && takeoff_complete == false),
takeoff_pitch_cd);
if (g.log_bitmask & MASK_LOG_TECS) {