Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: added QLAND mode

Browse Source 
for VTOL landing. Use for failsafe as well
This commit is contained in:
Andrew Tridgell 2016-03-09 18:20:41 +11:00
parent 927efa90f8
commit f4ccf94dfc
10 changed files with 76 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
ArduPlane/ArduPlane.cpp
View File

@ -544,10 +544,7 @@ void Plane::update_flight_mode(void)
}
// ensure we are fly-forward
if (effective_mode == QSTABILIZE ||
effective_mode == QHOVER ||
effective_mode == QLOITER ||
quadplane.in_vtol_auto()) {
if (quadplane.in_vtol_mode()) {
ahrs.set_fly_forward(false);
} else {
ahrs.set_fly_forward(true);
@ -709,7 +706,8 @@ void Plane::update_flight_mode(void)
case QSTABILIZE:
case QHOVER:
case QLOITER: {
case QLOITER:
case QLAND: {
// 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);
@ -789,6 +787,7 @@ void Plane::update_navigation()
case QSTABILIZE:
case QHOVER:
case QLOITER:
case QLAND:
// nothing to do
break;
}
@ -936,9 +935,7 @@ void Plane::update_flight_stage(void)
} else {
set_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL);
}
} else if (control_mode == QSTABILIZE ||
control_mode == QHOVER ||
control_mode == QLOITER ||
} else if (quadplane.in_vtol_mode() ||
quadplane.in_assisted_flight()) {
set_flight_stage(AP_SpdHgtControl::FLIGHT_VTOL);
} else {

10
ArduPlane/Attitude.cpp
View File

@ -143,6 +143,7 @@ void Plane::stabilize_stick_mixing_direct()
control_mode == QSTABILIZE ||
control_mode == QHOVER ||
control_mode == QLOITER ||
control_mode == QLAND ||
control_mode == TRAINING) {
return;
}
@ -165,6 +166,7 @@ void Plane::stabilize_stick_mixing_fbw()
control_mode == QSTABILIZE ||
control_mode == QHOVER ||
control_mode == QLOITER ||
control_mode == QLAND ||
control_mode == TRAINING ||
(control_mode == AUTO && g.auto_fbw_steer)) {
return;
@ -364,7 +366,8 @@ void Plane::stabilize()
stabilize_acro(speed_scaler);
} else if (control_mode == QSTABILIZE ||
control_mode == QHOVER ||
control_mode == QLOITER) {
control_mode == QLOITER ||
control_mode == QLAND) {
quadplane.control_run();
} else {
if (g.stick_mixing == STICK_MIXING_FBW && control_mode != STABILIZE) {
@ -994,10 +997,7 @@ void Plane::set_servos(void)
guided_throttle_passthru) {
// manual pass through of throttle while in GUIDED
channel_throttle->radio_out = channel_throttle->radio_in;
} else if (control_mode == QSTABILIZE ||
control_mode == QHOVER ||
control_mode == QLOITER ||
quadplane.in_vtol_auto()) {
} else if (quadplane.in_vtol_mode()) {
// no forward throttle for now
channel_throttle->servo_out = 0;
channel_throttle->calc_pwm();

2
ArduPlane/GCS_Mavlink.cpp
View File

@ -42,6 +42,7 @@ void Plane::send_heartbeat(mavlink_channel_t chan)
case QSTABILIZE:
case QHOVER:
case QLOITER:
case QLAND:
case CRUISE:
base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED;
break;
@ -178,6 +179,7 @@ void Plane::send_extended_status1(mavlink_channel_t chan)
case AUTOTUNE:
case QSTABILIZE:
case QHOVER:
case QLAND:
case QLOITER:
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

1
ArduPlane/Plane.h
View File

@ -204,6 +204,7 @@ private:
float initial_correction;
uint32_t last_correction_time_ms;
uint8_t in_range_count;
float height_estimate;
} rangefinder_state;
#endif

9
ArduPlane/altitude.cpp
View File

@ -565,7 +565,7 @@ void Plane::rangefinder_height_update(void)
}
// correct the range for attitude (multiply by DCM.c.z, which
// is cos(roll)*cos(pitch))
height_estimate = distance * ahrs.get_rotation_body_to_ned().c.z;
rangefinder_state.height_estimate = distance * ahrs.get_rotation_body_to_ned().c.z;
// we consider ourselves to be fully in range when we have 10
// good samples (0.2s) that are different by 5% of the maximum
@ -580,11 +580,12 @@ void Plane::rangefinder_height_update(void)
rangefinder_state.in_range = true;
if (!rangefinder_state.in_use &&
(flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH ||
flight_stage == AP_SpdHgtControl::FLIGHT_LAND_PREFLARE ||
flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL) &&
flight_stage == AP_SpdHgtControl::FLIGHT_LAND_PREFLARE ||
flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL ||
control_mode == QLAND) &&
g.rangefinder_landing) {
rangefinder_state.in_use = true;
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Rangefinder engaged at %.2fm", (double)height_estimate);
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Rangefinder engaged at %.2fm", (double)rangefinder_state.height_estimate);
}
}
} else {

3
ArduPlane/defines.h
View File

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

10
ArduPlane/events.cpp
View File

@ -29,9 +29,10 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype)
case QSTABILIZE:
case QLOITER:
case QHOVER:
failsafe.saved_mode = control_mode;
failsafe.saved_mode_set = 1;
set_mode(QHOVER);
set_mode(QLAND);
break;
case AUTO:
@ -50,7 +51,7 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype)
case CIRCLE:
case RTL:
case QHOVER:
case QLAND:
default:
break;
}
@ -87,8 +88,9 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype)
break;
case QSTABILIZE:
case QHOVER:
case QLOITER:
set_mode(QHOVER);
set_mode(QLAND);
break;
case AUTO:
@ -106,7 +108,7 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype)
break;
case RTL:
case QHOVER:
case QLAND:
default:
break;
}

59
ArduPlane/quadplane.cpp
View File

@ -569,6 +569,14 @@ void QuadPlane::init_loiter(void)
init_throttle_wait();
}
void QuadPlane::init_land(void)
{
init_loiter();
throttle_wait = false;
land_state = QLAND_DESCEND;
motors_lower_limit_start_ms = 0;
}
// helper for is_flying()
bool QuadPlane::is_flying(void)
@ -644,8 +652,23 @@ void QuadPlane::control_loiter()
plane.nav_roll_cd = wp_nav->get_roll();
plane.nav_pitch_cd = wp_nav->get_pitch();
// update altitude target and call position controller
pos_control->set_alt_target_from_climb_rate_ff(get_pilot_desired_climb_rate_cms(), plane.G_Dt, false);
if (plane.control_mode == QLAND) {
if (land_state == QLAND_DESCEND) {
if (plane.g.rangefinder_landing && plane.rangefinder_state.in_range) {
if (plane.rangefinder_state.height_estimate < land_final_alt) {
land_state = QLAND_FINAL;
}
} else if (plane.adjusted_relative_altitude_cm() < land_final_alt*100) {
land_state = QLAND_FINAL;
}
}
float descent_rate = (land_state == QLAND_FINAL)?land_speed_cms:wp_nav->get_speed_down();
pos_control->set_alt_target_from_climb_rate(-descent_rate, plane.G_Dt, true);
check_land_complete();
} else {
// update altitude target and call position controller
pos_control->set_alt_target_from_climb_rate_ff(get_pilot_desired_climb_rate_cms(), plane.G_Dt, false);
}
pos_control->update_z_controller();
}
@ -848,12 +871,7 @@ void QuadPlane::update(void)
return;
}
bool quad_mode = (plane.control_mode == QSTABILIZE ||
plane.control_mode == QHOVER ||
plane.control_mode == QLOITER ||
in_vtol_auto());
if (!quad_mode) {
if (!in_vtol_mode()) {
update_transition();
} else {
assisted_flight = false;
@ -898,8 +916,8 @@ void QuadPlane::control_run(void)
control_hover();
break;
case QLOITER:
case QLAND:
control_loiter();
break;
default:
break;
}
@ -931,6 +949,9 @@ bool QuadPlane::init_mode(void)
case QLOITER:
init_loiter();
break;
case QLAND:
init_land();
break;
default:
break;
}
@ -997,6 +1018,7 @@ bool QuadPlane::in_vtol_mode(void)
return (plane.control_mode == QSTABILIZE ||
plane.control_mode == QHOVER ||
plane.control_mode == QLOITER ||
plane.control_mode == QLAND ||
in_vtol_auto());
}
@ -1220,6 +1242,17 @@ bool QuadPlane::verify_vtol_takeoff(const AP_Mission::Mission_Command &cmd)
return true;
}
void QuadPlane::check_land_complete(void)
{
if (land_state == QLAND_FINAL &&
(motors_lower_limit_start_ms != 0 &&
millis() - motors_lower_limit_start_ms > 5000)) {
plane.disarm_motors();
land_state = QLAND_COMPLETE;
plane.gcs_send_text(MAV_SEVERITY_INFO,"Land complete");
}
}
/*
check if a VTOL landing has completed
*/
@ -1247,12 +1280,6 @@ bool QuadPlane::verify_vtol_land(const AP_Mission::Mission_Command &cmd)
plane.gcs_send_text(MAV_SEVERITY_INFO,"Land final started");
}
if (land_state == QLAND_FINAL &&
(motors_lower_limit_start_ms != 0 &&
millis() - motors_lower_limit_start_ms > 5000)) {
plane.disarm_motors();
land_state = QLAND_COMPLETE;
plane.gcs_send_text(MAV_SEVERITY_INFO,"Land complete");
}
check_land_complete();
return false;
}

2
ArduPlane/quadplane.h
View File

@ -118,7 +118,9 @@ private:
void control_hover(void);
void init_loiter(void);
void init_land(void);
void control_loiter(void);
void check_land_complete(void);
float assist_climb_rate_cms(void);

5
ArduPlane/system.cpp
View File

@ -461,6 +461,7 @@ void Plane::set_mode(enum FlightMode mode)
case QSTABILIZE:
case QHOVER:
case QLOITER:
case QLAND:
if (!quadplane.init_mode()) {
control_mode = previous_mode;
} else {
@ -505,6 +506,7 @@ bool Plane::mavlink_set_mode(uint8_t mode)
case QSTABILIZE:
case QHOVER:
case QLOITER:
case QLAND:
set_mode((enum FlightMode)mode);
return true;
}
@ -704,6 +706,9 @@ void Plane::print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode)
case QLOITER:
port->print("QLoiter");
break;
case QLAND:
port->print("QLand");
break;
default:
port->printf("Mode(%u)", (unsigned)mode);
break;