diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 929964b973..444e301018 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -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 { diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 2cff2ec516..ebcd7d8385 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -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(); diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 264edb7f8e..043bed091d 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -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 diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index a0193b8bfa..e84da17036 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -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 diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp index 0734300492..9512ccf8a9 100644 --- a/ArduPlane/altitude.cpp +++ b/ArduPlane/altitude.cpp @@ -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 { diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index 938fc813ec..16a16b7b02 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -66,7 +66,8 @@ enum FlightMode { INITIALISING = 16, QSTABILIZE = 17, QHOVER = 18, - QLOITER = 19 + QLOITER = 19, + QLAND = 20 }; // type of stick mixing enabled diff --git a/ArduPlane/events.cpp b/ArduPlane/events.cpp index 8ad3b90e83..70cb2d402e 100644 --- a/ArduPlane/events.cpp +++ b/ArduPlane/events.cpp @@ -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; } diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 777b4654af..8cfe906fe1 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -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; } diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index bcf1cad182..4dcaa80b1d 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -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); diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index 97b5fb8bc6..4708dd5e58 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -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;