Browse Source

Plane: added relative_altitude helper functions

prevents some code duplication
master
Andrew Tridgell 12 years ago
parent
commit
7aa360b176
  1. 4
      ArduPlane/ArduPlane.pde
  2. 4
      ArduPlane/Attitude.pde
  3. 2
      ArduPlane/GCS_Mavlink.pde
  4. 17
      ArduPlane/navigation.pde

4
ArduPlane/ArduPlane.pde

@ -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

@ -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;

2
ArduPlane/GCS_Mavlink.pde

@ -265,7 +265,7 @@ static void NOINLINE send_location(mavlink_channel_t chan)
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

@ -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);
}

Loading…
Cancel
Save