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

Plane: added relative_altitude helper functions 

prevents some code duplication
This commit is contained in:
Andrew Tridgell 2013-07-10 13:40:13 +10:00
parent 66d60953df
commit 7aa360b176
4 changed files with 23 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduPlane/ArduPlane.pde
View File

@ -785,7 +785,7 @@ 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)
{ {
// Call TECS 50Hz update // Call TECS 50Hz update
SpdHgt_Controller->update_50hz((current_loc.alt - home.alt) * 0.01f); SpdHgt_Controller->update_50hz(relative_altitude());
} }
} }
@ -1255,7 +1255,7 @@ static void update_alt()
(control_mode==AUTO && takeoff_complete == false), (control_mode==AUTO && takeoff_complete == false),
takeoff_pitch_cd, takeoff_pitch_cd,
throttle_nudge, throttle_nudge,
float(hgt_afe_cm)*0.01f); relative_altitude());
if (g.log_bitmask & MASK_LOG_TECS) { if (g.log_bitmask & MASK_LOG_TECS) {
Log_Write_TECS_Tuning(); Log_Write_TECS_Tuning();
} }

4
ArduPlane/Attitude.pde
View File

@ -257,7 +257,7 @@ static void stabilize()
see if we should zero the attitude controller integrators. see if we should zero the attitude controller integrators.
*/ */
if (channel_throttle->control_in == 0 && if (channel_throttle->control_in == 0 &&
abs(current_loc.alt - home.alt) < 500 && relative_altitude_abs_cm() < 500 &&
fabs(barometer.get_climb_rate()) < 0.5f && fabs(barometer.get_climb_rate()) < 0.5f &&
g_gps->ground_speed < 300) { g_gps->ground_speed < 300) {
// we are low, with no climb rate, and zero throttle, and very // we are low, with no climb rate, and zero throttle, and very
@ -511,7 +511,7 @@ static bool suppress_throttle(void)
return false; return false;
} }
if (labs(home.alt - current_loc.alt) >= 1000) { if (relative_altitude_abs_cm() >= 1000) {
// we're more than 10m from the home altitude // we're more than 10m from the home altitude
throttle_suppressed = false; throttle_suppressed = false;
return false; return false;

4
ArduPlane/GCS_Mavlink.pde
View File

@ -264,8 +264,8 @@ static void NOINLINE send_location(mavlink_channel_t chan)
fix_time, fix_time,
current_loc.lat, // in 1E7 degrees current_loc.lat, // in 1E7 degrees
current_loc.lng, // in 1E7 degrees current_loc.lng, // in 1E7 degrees
g_gps->altitude * 10, // millimeters above sea level g_gps->altitude * 10, // millimeters above sea level
(current_loc.alt-home.alt) * 10, // millimeters above ground relative_altitude() * 1.0e3, // millimeters above ground
g_gps->velocity_north() * 100, // X speed cm/s (+ve North) g_gps->velocity_north() * 100, // X speed cm/s (+ve North)
g_gps->velocity_east() * 100, // Y speed cm/s (+ve East) g_gps->velocity_east() * 100, // Y speed cm/s (+ve East)
g_gps->velocity_down() * -100, // Z speed cm/s (+ve up) g_gps->velocity_down() * -100, // Z speed cm/s (+ve up)

17
ArduPlane/navigation.pde
View File

@ -193,3 +193,20 @@ static void setup_glide_slope(void)
break; break;
} }
} }
/*
return relative altitude in meters (relative to home)
*/
static float relative_altitude(void)
{
return (current_loc.alt - home.alt) * 0.01f;
}
/*
return relative altitude in centimeters, absolute value
*/
static int32_t relative_altitude_abs_cm(void)
{
return labs(current_loc.alt - home.alt);
}