diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 08682dc973..ddb9f08185 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -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); diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 5b614d03b1..37671fb6f0 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -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()) { diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 7c71260247..b100b18b43 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -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 diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 35e3fb34f0..8536aaddbb 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -1,3 +1,5 @@ +#pragma once + #include #include #include // 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), }; /*