mirror of https://github.com/ArduPilot/ardupilot
Plane: initial implementation of QRTL for quadplane RTL
This commit is contained in:
parent
460885c478
commit
4666b25258
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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) {
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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();
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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);
|
||||
|
||||
|
|
|
@ -65,7 +65,8 @@ enum FlightMode {
|
|||
QSTABILIZE = 17,
|
||||
QHOVER = 18,
|
||||
QLOITER = 19,
|
||||
QLAND = 20
|
||||
QLAND = 20,
|
||||
QRTL = 21
|
||||
};
|
||||
|
||||
// type of stick mixing enabled
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -255,6 +255,15 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = {
|
|||
// @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:
|
||||
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()) {
|
||||
|
|
|
@ -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;
|
||||
|
||||
|
|
|
@ -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;
|
||||
|
|
Loading…
Reference in New Issue