diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index c615fab1ca..60ae25ec1d 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -712,7 +712,8 @@ void Plane::update_flight_mode(void) case QSTABILIZE: case QHOVER: case QLOITER: - case QLAND: { + case QLAND: + case QRTL: { // set nav_roll and nav_pitch using sticks nav_roll_cd = channel_roll->norm_input() * roll_limit_cd; nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd); @@ -795,6 +796,7 @@ void Plane::update_navigation() case QHOVER: case QLOITER: case QLAND: + case QRTL: // nothing to do break; } diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index e880c5f2c6..a05f3c395c 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -144,6 +144,7 @@ void Plane::stabilize_stick_mixing_direct() control_mode == QHOVER || control_mode == QLOITER || control_mode == QLAND || + control_mode == QRTL || control_mode == TRAINING) { return; } @@ -167,6 +168,7 @@ void Plane::stabilize_stick_mixing_fbw() control_mode == QHOVER || control_mode == QLOITER || control_mode == QLAND || + control_mode == QRTL || control_mode == TRAINING || (control_mode == AUTO && g.auto_fbw_steer == 42)) { return; @@ -367,7 +369,8 @@ void Plane::stabilize() } else if (control_mode == QSTABILIZE || control_mode == QHOVER || control_mode == QLOITER || - control_mode == QLAND) { + control_mode == QLAND || + control_mode == QRTL) { quadplane.control_run(); } else { if (g.stick_mixing == STICK_MIXING_FBW && control_mode != STABILIZE) { diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index e51d019dc1..44a5dfce40 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -51,6 +51,7 @@ void Plane::send_heartbeat(mavlink_channel_t chan) case LOITER: case GUIDED: case CIRCLE: + case QRTL: base_mode = MAV_MODE_FLAG_GUIDED_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; // note that MAV_MODE_FLAG_AUTO_ENABLED does not match what @@ -203,6 +204,7 @@ void Plane::send_extended_status1(mavlink_channel_t chan) case LOITER: case GUIDED: case CIRCLE: + case QRTL: control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION; // attitude stabilisation control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_YAW_POSITION; // yaw position diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 81810a779f..2fd7dea7fd 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -832,7 +832,7 @@ private: void set_guided_WP(void); void init_home(); void update_home(); - void do_RTL(void); + void do_RTL(int32_t alt); bool verify_takeoff(); bool verify_loiter_unlim(); bool verify_loiter_time(); diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp index 2349007aef..14d8fa5de7 100644 --- a/ArduPlane/altitude.cpp +++ b/ArduPlane/altitude.cpp @@ -582,7 +582,9 @@ void Plane::rangefinder_height_update(void) (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH || flight_stage == AP_SpdHgtControl::FLIGHT_LAND_PREFLARE || flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL || - control_mode == QLAND) && + control_mode == QLAND || + control_mode == QRTL || + (control_mode == AUTO && plane.mission.get_current_nav_cmd().id == MAV_CMD_NAV_VTOL_LAND)) && g.rangefinder_landing) { rangefinder_state.in_use = true; gcs_send_text_fmt(MAV_SEVERITY_INFO, "Rangefinder engaged at %.2fm", (double)rangefinder_state.height_estimate); diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index a92a4debd0..12e65ddaaa 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -284,7 +284,7 @@ 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(cmd); + return quadplane.verify_vtol_land(); // do commands (always return true) case MAV_CMD_DO_CHANGE_SPEED: @@ -321,12 +321,12 @@ bool Plane::verify_command(const AP_Mission::Mission_Command& cmd) // Ret // Nav (Must) commands /********************************************************************************/ -void Plane::do_RTL(void) +void Plane::do_RTL(int32_t rtl_altitude) { auto_state.next_wp_no_crosstrack = true; auto_state.no_crosstrack = true; prev_WP_loc = current_loc; - next_WP_loc = rally.calc_best_rally_or_home_location(current_loc, get_RTL_altitude()); + next_WP_loc = rally.calc_best_rally_or_home_location(current_loc, rtl_altitude); setup_terrain_target_alt(next_WP_loc); set_target_altitude_location(next_WP_loc); diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index 251aa32408..9088dd165a 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -65,7 +65,8 @@ enum FlightMode { QSTABILIZE = 17, QHOVER = 18, QLOITER = 19, - QLAND = 20 + QLAND = 20, + QRTL = 21 }; // type of stick mixing enabled diff --git a/ArduPlane/events.cpp b/ArduPlane/events.cpp index 70cb2d402e..b12ced87fe 100644 --- a/ArduPlane/events.cpp +++ b/ArduPlane/events.cpp @@ -52,6 +52,7 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype) case CIRCLE: case RTL: case QLAND: + case QRTL: default: break; } @@ -109,6 +110,7 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype) case RTL: case QLAND: + case QRTL: default: break; } diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 88fa688156..1961e41ffb 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -254,6 +254,15 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @Increment: 0.1 // @User: Standard AP_GROUPINFO("WVANE_MINROLL", 34, QuadPlane, weathervane.min_roll, 1), + + // @Param: RTL_ALT + // @DisplayName: QRTL return altitude + // @Description: The altitude which QRTL mode heads to initially + // @Units: m + // @Range: 1 200 + // @Increment: 1 + // @User: Standard + AP_GROUPINFO("RTL_ALT", 35, QuadPlane, qrtl_alt, 15), AP_GROUPEND }; @@ -922,6 +931,10 @@ void QuadPlane::control_run(void) case QLOITER: case QLAND: control_loiter(); + break; + case QRTL: + control_qrtl(); + break; default: break; } @@ -956,6 +969,9 @@ bool QuadPlane::init_mode(void) case QLAND: init_land(); break; + case QRTL: + init_qrtl(); + break; default: break; } @@ -1031,6 +1047,7 @@ bool QuadPlane::in_vtol_mode(void) plane.control_mode == QHOVER || plane.control_mode == QLOITER || plane.control_mode == QLAND || + plane.control_mode == QRTL || in_vtol_auto()); } @@ -1189,7 +1206,18 @@ void QuadPlane::land_controller(void) switch (land.state) { case QLAND_POSITION1: case QLAND_POSITION2: - pos_control->set_alt_target_from_climb_rate(0, plane.G_Dt, false); + if (plane.control_mode == QRTL) { + plane.ahrs.get_position(plane.current_loc); + plane.auto_state.wp_proportion = location_path_proportion(plane.current_loc, + plane.prev_WP_loc, plane.next_WP_loc); + float target_altitude = linear_interpolate(plane.prev_WP_loc.alt, + plane.next_WP_loc.alt, + plane.auto_state.wp_proportion, + 0, 1); + pos_control->set_alt_target(target_altitude - plane.home.alt); + } else { + pos_control->set_alt_target_from_climb_rate(0, plane.G_Dt, false); + } break; case QLAND_DESCEND: { @@ -1319,6 +1347,31 @@ void QuadPlane::control_auto(const Location &loc) } +/* + handle QRTL mode + */ +void QuadPlane::control_qrtl(void) +{ + land_controller(); + if (land.state >= QLAND_POSITION2) { + // change target altitude to home alt + plane.next_WP_loc.alt = plane.home.alt; + verify_vtol_land(); + } else { + pos_control->set_alt_target(qrtl_alt*100UL); + } +} + +/* + handle QRTL mode + */ +void QuadPlane::init_qrtl(void) +{ + // use do_RTL() to setup next_WP_loc + plane.do_RTL(plane.home.alt + qrtl_alt*100UL); + plane.prev_WP_loc = plane.current_loc; +} + /* start a VTOL takeoff */ @@ -1418,7 +1471,7 @@ void QuadPlane::check_land_complete(void) /* check if a VTOL landing has completed */ -bool QuadPlane::verify_vtol_land(const AP_Mission::Mission_Command &cmd) +bool QuadPlane::verify_vtol_land(void) { if (!available()) { return true; @@ -1427,7 +1480,7 @@ bool QuadPlane::verify_vtol_land(const AP_Mission::Mission_Command &cmd) plane.auto_state.wp_distance < 2) { land.state = QLAND_DESCEND; plane.gcs_send_text(MAV_SEVERITY_INFO,"Land descend started"); - plane.set_next_WP(cmd.content.location); + plane.set_next_WP(plane.next_WP_loc); } if (should_relax()) { diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 2115cdc002..c24e46c7ad 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -62,7 +62,7 @@ public: bool do_vtol_takeoff(const AP_Mission::Mission_Command& cmd); bool do_vtol_land(const AP_Mission::Mission_Command& cmd); bool verify_vtol_takeoff(const AP_Mission::Mission_Command &cmd); - bool verify_vtol_land(const AP_Mission::Mission_Command &cmd); + bool verify_vtol_land(void); bool in_vtol_auto(void); bool in_vtol_mode(void); @@ -156,6 +156,9 @@ private: void control_loiter(void); void check_land_complete(void); + void init_qrtl(void); + void control_qrtl(void); + float assist_climb_rate_cms(void); // calculate desired yaw rate for assistance @@ -187,6 +190,9 @@ private: // landing speed in cm/s AP_Int16 land_speed_cms; + // QRTL start altitude, meters + AP_Int16 qrtl_alt; + // alt to switch to QLAND_FINAL AP_Float land_final_alt; diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index 77cfa76d10..a4cf3f9f2a 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -439,7 +439,7 @@ void Plane::set_mode(enum FlightMode mode) case RTL: auto_throttle_mode = true; prev_WP_loc = current_loc; - do_RTL(); + do_RTL(get_RTL_altitude()); break; case LOITER: @@ -462,6 +462,7 @@ void Plane::set_mode(enum FlightMode mode) case QHOVER: case QLOITER: case QLAND: + case QRTL: if (!quadplane.init_mode()) { control_mode = previous_mode; } else { @@ -507,6 +508,7 @@ bool Plane::mavlink_set_mode(uint8_t mode) case QHOVER: case QLOITER: case QLAND: + case QRTL: set_mode((enum FlightMode)mode); return true; } @@ -711,6 +713,9 @@ void Plane::print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode) case QLAND: port->print("QLand"); break; + case QRTL: + port->print("QRTL"); + break; default: port->printf("Mode(%u)", (unsigned)mode); break;