Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Plane: initial implementation of QRTL for quadplane RTL

This commit is contained in:
Andrew Tridgell 2016-04-29 15:31:08 +10:00
parent 460885c478
commit 4666b25258
11 changed files with 89 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduPlane/ArduPlane.cpp
View File

@ -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;
}

5
ArduPlane/Attitude.cpp
View File

@ -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) {

2
ArduPlane/GCS_Mavlink.cpp
View File

@ -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

2
ArduPlane/Plane.h
View File

@ -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();

4
ArduPlane/altitude.cpp
View File

@ -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);

6
ArduPlane/commands_logic.cpp
View File

@ -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);

3
ArduPlane/defines.h
View File

@ -65,7 +65,8 @@ enum FlightMode {
QSTABILIZE = 17,
QHOVER = 18,
QLOITER = 19,
QLAND = 20
QLAND = 20,
QRTL = 21
};
// type of stick mixing enabled

2
ArduPlane/events.cpp
View File

@ -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;
}

59
ArduPlane/quadplane.cpp
View File

@ -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()) {

8
ArduPlane/quadplane.h
View File

@ -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;

7
ArduPlane/system.cpp
View File

@ -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;