Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-10 18:03:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Landing: Support absolute altitude deepstalls

Browse Source
This commit is contained in:
Michael du Breuil 2017-11-13 10:25:46 -07:00 committed by Tom Pittenger
parent 964d64a623
commit 012ddbefac
2 changed files with 26 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
libraries/AP_Landing/AP_Landing_Deepstall.cpp
View File

@ -159,6 +159,13 @@ void AP_Landing_Deepstall::do_land(const AP_Mission::Mission_Command& cmd, const
// load the landing point in, the rest of path building is deferred for a better wind estimate // load the landing point in, the rest of path building is deferred for a better wind estimate
memcpy(&landing_point, &cmd.content.location, sizeof(Location)); memcpy(&landing_point, &cmd.content.location, sizeof(Location));
if (!landing_point.flags.relative_alt && !landing_point.flags.terrain_alt) {
approach_alt_offset = cmd.p1;
landing_point.alt += approach_alt_offset * 100;
} else {
approach_alt_offset = 0.0f;
}
} }
// currently identical to the slope aborts // currently identical to the slope aborts
@ -190,7 +197,7 @@ bool AP_Landing_Deepstall::verify_land(const Location &prev_WP_loc, Location &ne
case DEEPSTALL_STAGE_ESTIMATE_WIND: case DEEPSTALL_STAGE_ESTIMATE_WIND:
{ {
landing.nav_controller->update_loiter(landing_point, landing.aparm.loiter_radius, landing_point.flags.loiter_ccw ? -1 : 1); landing.nav_controller->update_loiter(landing_point, landing.aparm.loiter_radius, landing_point.flags.loiter_ccw ? -1 : 1);
if (!landing.nav_controller->reached_loiter_target() || (fabsf(height) > DEEPSTALL_LOITER_ALT_TOLERANCE)) { if (!landing.nav_controller->reached_loiter_target() || (fabsf(height - approach_alt_offset) > DEEPSTALL_LOITER_ALT_TOLERANCE)) {
// wait until the altitude is correct before considering a breakout // wait until the altitude is correct before considering a breakout
return false; return false;
} }
@ -218,7 +225,7 @@ bool AP_Landing_Deepstall::verify_land(const Location &prev_WP_loc, Location &ne
if (loiter_sum_cd < 36000) { if (loiter_sum_cd < 36000) {
build_approach_path(false); build_approach_path(false);
} }
if (!verify_breakout(current_loc, arc_entry, height)) { if (!verify_breakout(current_loc, arc_entry, height - approach_alt_offset)) {
int32_t target_bearing = landing.nav_controller->target_bearing_cd(); int32_t target_bearing = landing.nav_controller->target_bearing_cd();
int32_t delta = wrap_180_cd(target_bearing - last_target_bearing); int32_t delta = wrap_180_cd(target_bearing - last_target_bearing);
if (delta > 0) { // only accumulate turns in the correct direction if (delta > 0) { // only accumulate turns in the correct direction
@ -255,10 +262,20 @@ bool AP_Landing_Deepstall::verify_land(const Location &prev_WP_loc, Location &ne
Location entry_point; Location entry_point;
landing.nav_controller->update_waypoint(arc_exit, extended_approach); landing.nav_controller->update_waypoint(arc_exit, extended_approach);
float relative_alt_D; float height_above_target;
landing.ahrs.get_relative_position_D_home(relative_alt_D); if (is_zero(approach_alt_offset)) {
landing.ahrs.get_relative_position_D_home(height_above_target);
height_above_target = -height_above_target;
} else {
Location position;
if (landing.ahrs.get_position(position)) {
height_above_target = (position.alt - landing_point.alt + approach_alt_offset * 100) * 1e-2f;
} else {
height_above_target = approach_alt_offset;
}
}
const float travel_distance = predict_travel_distance(landing.ahrs.wind_estimate(), -relative_alt_D, false); const float travel_distance = predict_travel_distance(landing.ahrs.wind_estimate(), height_above_target, false);
memcpy(&entry_point, &landing_point, sizeof(Location)); memcpy(&entry_point, &landing_point, sizeof(Location));
location_update(entry_point, target_heading_deg + 180.0, travel_distance); location_update(entry_point, target_heading_deg + 180.0, travel_distance);
@ -270,7 +287,7 @@ bool AP_Landing_Deepstall::verify_land(const Location &prev_WP_loc, Location &ne
} }
return false; return false;
} }
predict_travel_distance(landing.ahrs.wind_estimate(), -relative_alt_D, true); predict_travel_distance(landing.ahrs.wind_estimate(), height_above_target, true);
stage = DEEPSTALL_STAGE_LAND; stage = DEEPSTALL_STAGE_LAND;
stall_entry_time = AP_HAL::millis(); stall_entry_time = AP_HAL::millis();
@ -470,7 +487,8 @@ void AP_Landing_Deepstall::build_approach_path(bool use_current_heading)
//extend the approach point to 1km away so that there is always a navigational target //extend the approach point to 1km away so that there is always a navigational target
location_update(extended_approach, target_heading_deg, 1000.0); location_update(extended_approach, target_heading_deg, 1000.0);
float expected_travel_distance = predict_travel_distance(wind, landing_point.alt * 0.01f, false); float expected_travel_distance = predict_travel_distance(wind, is_zero(approach_alt_offset) ? landing_point.alt * 0.01f : approach_alt_offset,
false);
float approach_extension_m = expected_travel_distance + approach_extension; float approach_extension_m = expected_travel_distance + approach_extension;
float loiter_radius_m_abs = fabsf(loiter_radius); float loiter_radius_m_abs = fabsf(loiter_radius);
// an approach extensions must be at least half the loiter radius, or the aircraft has a // an approach extensions must be at least half the loiter radius, or the aircraft has a

1
libraries/AP_Landing/AP_Landing_Deepstall.h
View File

@ -87,6 +87,7 @@ private:
float crosstrack_error; // current crosstrack error float crosstrack_error; // current crosstrack error
float predicted_travel_distance; // distance the aircraft is perdicted to travel during deepstall float predicted_travel_distance; // distance the aircraft is perdicted to travel during deepstall
bool hold_level; // locks the target yaw rate of the aircraft to 0, serves to hold the aircraft level at all times bool hold_level; // locks the target yaw rate of the aircraft to 0, serves to hold the aircraft level at all times
float approach_alt_offset; // approach altitude offset
//public AP_Landing interface //public AP_Landing interface
void do_land(const AP_Mission::Mission_Command& cmd, const float relative_altitude); void do_land(const AP_Mission::Mission_Command& cmd, const float relative_altitude);