mirror of https://github.com/ArduPilot/ardupilot
Plane: added relative_altitude helper functions
prevents some code duplication
This commit is contained in:
parent
66d60953df
commit
7aa360b176
|
@ -785,7 +785,7 @@ static void update_speed_height(void)
|
|||
if (g.alt_control_algorithm == ALT_CONTROL_TECS && auto_throttle_mode && !throttle_suppressed)
|
||||
{
|
||||
// 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),
|
||||
takeoff_pitch_cd,
|
||||
throttle_nudge,
|
||||
float(hgt_afe_cm)*0.01f);
|
||||
relative_altitude());
|
||||
if (g.log_bitmask & MASK_LOG_TECS) {
|
||||
Log_Write_TECS_Tuning();
|
||||
}
|
||||
|
|
|
@ -257,7 +257,7 @@ static void stabilize()
|
|||
see if we should zero the attitude controller integrators.
|
||||
*/
|
||||
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 &&
|
||||
g_gps->ground_speed < 300) {
|
||||
// we are low, with no climb rate, and zero throttle, and very
|
||||
|
@ -511,7 +511,7 @@ static bool suppress_throttle(void)
|
|||
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
|
||||
throttle_suppressed = false;
|
||||
return false;
|
||||
|
|
|
@ -264,8 +264,8 @@ static void NOINLINE send_location(mavlink_channel_t chan)
|
|||
fix_time,
|
||||
current_loc.lat, // in 1E7 degrees
|
||||
current_loc.lng, // in 1E7 degrees
|
||||
g_gps->altitude * 10, // millimeters above sea level
|
||||
(current_loc.alt-home.alt) * 10, // millimeters above ground
|
||||
g_gps->altitude * 10, // millimeters above sea level
|
||||
relative_altitude() * 1.0e3, // millimeters above ground
|
||||
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_down() * -100, // Z speed cm/s (+ve up)
|
||||
|
|
|
@ -193,3 +193,20 @@ static void setup_glide_slope(void)
|
|||
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…
Reference in New Issue