Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 15:23:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: split landing code into landing.pde

Browse Source 
this is in preparation for more complex landing code using the
rangefinder
This commit is contained in:
Andrew Tridgell 2014-08-12 11:21:35 +10:00
parent 7def71d43a
commit dad95648a0
3 changed files with 83 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
ArduPlane/ArduPlane.pde
View File

@ -546,6 +546,9 @@ static struct {
// turn angle for next leg of mission
float next_turn_angle;
// filtered sink rate for landing
float land_sink_rate;
} auto_state = {
takeoff_complete : true,
land_complete : false,
@ -556,7 +559,8 @@ static struct {
takeoff_pitch_cd : 0,
highest_airspeed : 0,
initial_pitch_cd : 0,
next_turn_angle : 90.0f
next_turn_angle : 90.0f,
land_sink_rate : 0
};
// true if we are in an auto-throttle mode, which means
@ -1200,6 +1204,7 @@ static void handle_auto_mode(void)
// are for takeoff and landing
steer_state.hold_course_cd = -1;
auto_state.land_complete = false;
auto_state.land_sink_rate = 0;
calc_nav_roll();
calc_nav_pitch();
calc_throttle();

68
ArduPlane/commands_logic.pde
View File

@ -32,6 +32,7 @@ start_command(const AP_Mission::Mission_Command& cmd)
if (AP_Mission::is_nav_cmd(cmd)) {
// set land_complete to false to stop us zeroing the throttle
auto_state.land_complete = false;
auto_state.land_sink_rate = 0;
// set takeoff_complete to true so we don't add extra evevator
// except in a takeoff
@ -369,73 +370,6 @@ static bool verify_takeoff()
}
}
/*
update navigation for landing
*/
static bool verify_land()
{
// we don't 'verify' landing in the sense that it never completes,
// so we don't verify command completion. Instead we use this to
// adjust final landing parameters
float height = height_above_target();
// calculate the sink rate.
float sink_rate, ground_speed;
Vector3f vel;
if (ahrs.get_velocity_NED(vel)) {
sink_rate = vel.z;
ground_speed = pythagorous2(vel.x, vel.y);
} else {
sink_rate = gps.velocity().z;
ground_speed = gps.ground_speed();
}
/* Set land_complete (which starts the flare) under 3 conditions:
1) we are within LAND_FLARE_ALT meters of the landing altitude
2) we are within LAND_FLARE_SEC of the landing point
horizontally
3) we are within LAND_FLARE_SEC of the landing point vertically
*/
if (wp_distance <= g.land_flare_sec * ground_speed ||
height <= g.land_flare_alt ||
height <= -sink_rate * g.land_flare_sec) {
if (!auto_state.land_complete) {
gcs_send_text_fmt(PSTR("Flare %.1fm sink=%.2f speed=%.1f"), height, sink_rate, ground_speed);
}
auto_state.land_complete = true;
if (gps.ground_speed() < 3) {
// reload any airspeed or groundspeed parameters that may have
// been set for landing. We don't do this till ground
// speed drops below 3.0 m/s as otherwise we will change
// target speeds too early.
g.airspeed_cruise_cm.load();
g.min_gndspeed_cm.load();
aparm.throttle_cruise.load();
}
}
/*
when landing we keep the L1 navigation waypoint 200m ahead. This
prevents sudden turns if we overshoot the landing point
*/
struct Location land_WP_loc = next_WP_loc;
int32_t land_bearing_cd = get_bearing_cd(prev_WP_loc, next_WP_loc);
location_update(land_WP_loc,
land_bearing_cd*0.01f,
get_distance(prev_WP_loc, current_loc) + 200);
nav_controller->update_waypoint(prev_WP_loc, land_WP_loc);
/*
we always return false as a landing mission item never
completes - we stay on this waypoint unless the GCS commands us
to change mission item or reset the mission
*/
return false;
}
/*
update navigation for normal mission waypoints. Return true when the
waypoint is complete

76
ArduPlane/landing.pde Normal file
View File

@ -0,0 +1,76 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
landing logic
*/
/*
update navigation for landing. Called when on landing approach or
final flare
*/
static bool verify_land()
{
// we don't 'verify' landing in the sense that it never completes,
// so we don't verify command completion. Instead we use this to
// adjust final landing parameters
float height = height_above_target();
// calculate the sink rate.
float sink_rate;
Vector3f vel;
if (ahrs.get_velocity_NED(vel)) {
sink_rate = vel.z;
} else if (gps.status() >= AP_GPS::GPS_OK_FIX_3D && gps.have_vertical_velocity()) {
sink_rate = gps.velocity().z;
} else {
sink_rate = -barometer.get_climb_rate();
}
// low pass the sink rate to take some of the noise out
auto_state.land_sink_rate = 0.8f * auto_state.land_sink_rate + 0.2f*sink_rate;
/* Set land_complete (which starts the flare) under 2 conditions:
1) we are within LAND_FLARE_ALT meters of the landing altitude
2) we are within LAND_FLARE_SEC of the landing point vertically
by the calculated sink rate
*/
if (height <= g.land_flare_alt ||
height <= -auto_state.land_sink_rate * g.land_flare_sec) {
if (!auto_state.land_complete) {
gcs_send_text_fmt(PSTR("Flare %.1fm sink=%.2f speed=%.1f"),
height, auto_state.land_sink_rate, gps.ground_speed());
}
auto_state.land_complete = true;
if (gps.ground_speed() < 3) {
// reload any airspeed or groundspeed parameters that may have
// been set for landing. We don't do this till ground
// speed drops below 3.0 m/s as otherwise we will change
// target speeds too early.
g.airspeed_cruise_cm.load();
g.min_gndspeed_cm.load();
aparm.throttle_cruise.load();
}
}
/*
when landing we keep the L1 navigation waypoint 200m ahead. This
prevents sudden turns if we overshoot the landing point
*/
struct Location land_WP_loc = next_WP_loc;
int32_t land_bearing_cd = get_bearing_cd(prev_WP_loc, next_WP_loc);
location_update(land_WP_loc,
land_bearing_cd*0.01f,
get_distance(prev_WP_loc, current_loc) + 200);
nav_controller->update_waypoint(prev_WP_loc, land_WP_loc);
/*
we always return false as a landing mission item never
completes - we stay on this waypoint unless the GCS commands us
to change mission item or reset the mission
*/
return false;
}