aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Plane: Add support for a loiter to alt approach to VTOL landing

Browse Source
This commit is contained in:
Michael du Breuil 2018-09-24 21:11:26 -07:00 committed by Andrew Tridgell
parent 35be68348d
commit 8e906e0ba0
4 changed files with 118 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
ArduPlane/Plane.h
View File

@ -355,6 +355,19 @@ private:
uint32_t AFS_last_valid_rc_ms;
} failsafe;
enum Landing_ApproachStage {
LOITER_TO_ALT,
WAIT_FOR_BREAKOUT,
APPROACH_LINE,
VTOL_LANDING,
};
// Landing
struct {
enum Landing_ApproachStage approach_stage;
float approach_direction_deg;
} vtol_approach_s;
bool any_failsafe_triggered() {
return failsafe.state != FAILSAFE_NONE || battery.has_failsafed() || failsafe.adsb;
}
@ -1015,6 +1028,7 @@ private:
void do_takeoff(const AP_Mission::Mission_Command& cmd);
void do_nav_wp(const AP_Mission::Mission_Command& cmd);
void do_land(const AP_Mission::Mission_Command& cmd);
void do_landing_vtol_approach(const AP_Mission::Mission_Command& cmd);
void loiter_set_direction_wp(const AP_Mission::Mission_Command& cmd);
void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd);
void do_loiter_turns(const AP_Mission::Mission_Command& cmd);
@ -1025,6 +1039,7 @@ private:
void do_vtol_takeoff(const AP_Mission::Mission_Command& cmd);
void do_vtol_land(const AP_Mission::Mission_Command& cmd);
bool verify_nav_wp(const AP_Mission::Mission_Command& cmd);
bool verify_landing_vtol_approach(const AP_Mission::Mission_Command& cmd);
void do_wait_delay(const AP_Mission::Mission_Command& cmd);
void do_within_distance(const AP_Mission::Mission_Command& cmd);
void do_change_speed(const AP_Mission::Mission_Command& cmd);

93
ArduPlane/commands_logic.cpp
View File

@ -95,8 +95,17 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
return quadplane.do_vtol_takeoff(cmd);
case MAV_CMD_NAV_VTOL_LAND:
crash_state.is_crashed = false;
return quadplane.do_vtol_land(cmd);
if (quadplane.options & QuadPlane::OPTION_MISSION_LAND_FW_APPROACH) {
// the user wants to approach the landing in a fixed wing flight mode
// the waypoint will be used as a loiter_to_alt
// after which point the plane will compute the optimal into the wind direction
// and fly in on that direction towards the landing waypoint
// it will then transition to VTOL and do a normal quadplane landing
do_landing_vtol_approach(cmd);
break;
} else {
return quadplane.do_vtol_land(cmd);
}
// Conditional commands
@ -261,7 +270,13 @@ bool Plane::verify_command(const AP_Mission::Mission_Command& cmd) // Ret
return quadplane.verify_vtol_takeoff(cmd);
case MAV_CMD_NAV_VTOL_LAND:
return quadplane.verify_vtol_land();
if ((quadplane.options & QuadPlane::OPTION_MISSION_LAND_FW_APPROACH) && !verify_landing_vtol_approach(cmd)) {
// verify_landing_vtol_approach will return true once we have completed the approach,
// in which case we fall over to normal vtol landing code
return false;
} else {
return quadplane.verify_vtol_land();
}
// Conditional commands
@ -401,6 +416,15 @@ void Plane::do_land(const AP_Mission::Mission_Command& cmd)
#endif
}
void Plane::do_landing_vtol_approach(const AP_Mission::Mission_Command& cmd)
{
do_loiter_to_alt(cmd);
vtol_approach_s.approach_stage = LOITER_TO_ALT;
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_LAND);
}
void Plane::loiter_set_direction_wp(const AP_Mission::Mission_Command& cmd)
{
if (cmd.content.location.flags.loiter_ccw) {
@ -943,6 +967,69 @@ void Plane::exit_mission_callback()
}
}
bool Plane::verify_landing_vtol_approach(const AP_Mission::Mission_Command &cmd)
{
switch (vtol_approach_s.approach_stage) {
case LOITER_TO_ALT:
if (verify_loiter_to_alt()) {
Vector3f wind = ahrs.wind_estimate();
vtol_approach_s.approach_direction_deg = degrees(atan2f(-wind.y, -wind.x));
gcs().send_text(MAV_SEVERITY_INFO, "Selected an approach path of %.1f", (double)vtol_approach_s.approach_direction_deg);
// select target approach direction
// select detransition distance (add in extra distance if the approach does not fit in the required space)
// validate turn
vtol_approach_s.approach_stage = WAIT_FOR_BREAKOUT;
}
break;
case WAIT_FOR_BREAKOUT:
{
const float breakout_direction_rad = radians(wrap_180(vtol_approach_s.approach_direction_deg + 270));
nav_controller->update_loiter(cmd.content.location, aparm.loiter_radius, cmd.content.location.flags.loiter_ccw ? -1 : 1);
// breakout when within 5 degrees of the opposite direction
if (fabsf(ahrs.yaw - breakout_direction_rad) < radians(5.0f)) {
gcs().send_text(MAV_SEVERITY_INFO, "Starting VTOL land approach path");
vtol_approach_s.approach_stage = APPROACH_LINE;
set_next_WP(cmd.content.location);
// fallthrough
} else {
break;
}
FALLTHROUGH;
}
case APPROACH_LINE:
{
// project an apporach path
Location start = cmd.content.location;
Location end = cmd.content.location;
// project a 1km waypoint to either side of the landing location
location_update(start, vtol_approach_s.approach_direction_deg + 180, 1000);
location_update(end, vtol_approach_s.approach_direction_deg, 1000);
nav_controller->update_waypoint(start, end);
// check if we should move on to the next waypoint
Location breakout_loc = cmd.content.location;
location_update(breakout_loc, vtol_approach_s.approach_direction_deg + 180, quadplane.stopping_distance());
if(location_passed_point(current_loc, start, breakout_loc)) {
vtol_approach_s.approach_stage = VTOL_LANDING;
quadplane.do_vtol_land(cmd);
// fallthrough
} else {
break;
}
FALLTHROUGH;
}
case VTOL_LANDING:
// nothing to do here, we should be into the quadplane landing code
return true;
}
return false;
}
bool Plane::verify_loiter_heading(bool init)
{
if (quadplane.in_vtol_auto()) {

14
ArduPlane/quadplane.cpp
View File

@ -328,7 +328,7 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = {
// @Param: OPTIONS
// @DisplayName: quadplane options
// @Description: This provides a set of additional control options for quadplanes. LevelTransition means that the wings should be held level to within LEVEL_ROLL_LIMIT degrees during transition to fixed wing flight, and the vehicle will not use the vertical lift motors to climb during the transition. If AllowFWTakeoff bit is not set then fixed wing takeoff on quadplanes will instead perform a VTOL takeoff. If AllowFWLand bit is not set then fixed wing land on quadplanes will instead perform a VTOL land. If respect takeoff frame is not set the vehicle will interpret all takeoff waypoints as an altitude above the corrent position.
// @Bitmask: 0:LevelTransition,1:AllowFWTakeoff,2:AllowFWLand,3:Respect takeoff frame types
// @Bitmask: 0:LevelTransition,1:AllowFWTakeoff,2:AllowFWLand,3:Respect takeoff frame types,4:Use a fixed wing approach for VTOL landings
AP_GROUPINFO("OPTIONS", 58, QuadPlane, options, 0),
AP_SUBGROUPEXTENSION("",59, QuadPlane, var_info2),
@ -1711,7 +1711,6 @@ bool QuadPlane::in_vtol_auto(void) const
}
uint16_t id = plane.mission.get_current_nav_cmd().id;
switch (id) {
case MAV_CMD_NAV_VTOL_LAND:
case MAV_CMD_NAV_VTOL_TAKEOFF:
return true;
case MAV_CMD_NAV_LOITER_UNLIM:
@ -1721,6 +1720,7 @@ bool QuadPlane::in_vtol_auto(void) const
return plane.auto_state.vtol_loiter;
case MAV_CMD_NAV_TAKEOFF:
return is_vtol_takeoff(id);
case MAV_CMD_NAV_VTOL_LAND:
case MAV_CMD_NAV_LAND:
return is_vtol_land(id);
default:
@ -2057,12 +2057,12 @@ void QuadPlane::control_auto(const Location &loc)
takeoff_controller();
}
break;
case MAV_CMD_NAV_VTOL_LAND:
case MAV_CMD_NAV_LAND:
if (is_vtol_land(id)) {
vtol_position_controller();
}
break;
case MAV_CMD_NAV_VTOL_LAND:
case MAV_CMD_NAV_LOITER_UNLIM:
case MAV_CMD_NAV_LOITER_TIME:
case MAV_CMD_NAV_LOITER_TURNS:
@ -2164,6 +2164,8 @@ bool QuadPlane::do_vtol_land(const AP_Mission::Mission_Command& cmd)
throttle_wait = false;
landing_detect.lower_limit_start_ms = 0;
set_alt_target_current();
plane.crash_state.is_crashed = false;
// also update nav_controller for status output
plane.nav_controller->update_waypoint(plane.prev_WP_loc, plane.next_WP_loc);
@ -2562,7 +2564,11 @@ bool QuadPlane::is_vtol_takeoff(uint16_t id) const
bool QuadPlane::is_vtol_land(uint16_t id) const
{
if (id == MAV_CMD_NAV_VTOL_LAND) {
return true;
if (options & QuadPlane::OPTION_MISSION_LAND_FW_APPROACH) {
return plane.vtol_approach_s.approach_stage == Plane::Landing_ApproachStage::VTOL_LANDING;
} else {
return true;
}
}
if (id == MAV_CMD_NAV_LAND && available() && (options & OPTION_ALLOW_FW_LAND) == 0) {
// treat fixed wing land as VTOL land

3
ArduPlane/quadplane.h
View File

@ -1,3 +1,5 @@
#pragma once
#include <AP_Motors/AP_Motors.h>
#include <AC_PID/AC_PID.h>
#include <AC_AttitudeControl/AC_AttitudeControl_Multi.h> // Attitude control library
@ -454,6 +456,7 @@ private:
OPTION_ALLOW_FW_TAKEOFF=(1<<1),
OPTION_ALLOW_FW_LAND=(1<<2),
OPTION_RESPECT_TAKEOFF_FRAME=(1<<3),
OPTION_MISSION_LAND_FW_APPROACH=(1<<4),
};
/*