Plane: added quadplane version of is_flying()

This commit is contained in:
Andrew Tridgell 2016-01-01 21:39:36 +11:00
parent f068a8c913
commit 418464ab8c
5 changed files with 58 additions and 9 deletions

View File

@ -486,7 +486,7 @@ void Plane::handle_auto_mode(void)
nav_cmd_id = auto_rtl_command.id;
}
if (auto_state.vtol_mode) {
if (quadplane.in_vtol_auto()) {
quadplane.control_auto(next_WP_loc);
} else if (nav_cmd_id == MAV_CMD_NAV_TAKEOFF ||
(nav_cmd_id == MAV_CMD_NAV_LAND && flight_stage == AP_SpdHgtControl::FLIGHT_LAND_ABORT)) {
@ -541,7 +541,7 @@ void Plane::update_flight_mode(void)
if (effective_mode == QSTABILIZE ||
effective_mode == QHOVER ||
effective_mode == QLOITER ||
(effective_mode == AUTO && auto_state.vtol_mode)) {
quadplane.in_vtol_auto()) {
ahrs.set_fly_forward(false);
} else {
ahrs.set_fly_forward(true);
@ -867,7 +867,7 @@ void Plane::update_flight_stage(void)
// Update the speed & height controller states
if (auto_throttle_mode && !throttle_suppressed) {
if (control_mode==AUTO) {
if (auto_state.vtol_mode) {
if (quadplane.in_vtol_auto()) {
set_flight_stage(AP_SpdHgtControl::FLIGHT_VTOL);
} else if (auto_state.takeoff_complete == false) {
set_flight_stage(AP_SpdHgtControl::FLIGHT_TAKEOFF);

View File

@ -638,6 +638,11 @@ bool Plane::suppress_throttle(void)
}
}
if (quadplane.is_flying()) {
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Throttle enabled VTOL");
throttle_suppressed = false;
}
// throttle remains suppressed
return true;
}
@ -931,7 +936,7 @@ void Plane::set_servos(void)
} else if (control_mode == QSTABILIZE ||
control_mode == QHOVER ||
control_mode == QLOITER ||
(control_mode == AUTO && auto_state.vtol_mode)) {
quadplane.in_vtol_auto()) {
// no forward throttle for now
channel_throttle->servo_out = 0;
channel_throttle->calc_pwm();

View File

@ -125,6 +125,10 @@ void Plane::update_is_flying_5Hz(void)
isFlyingProbability = (0.85f * isFlyingProbability) + (0.15f * (float)is_flying_bool);
}
if (quadplane.is_flying()) {
is_flying_bool = true;
}
/*
update last_flying_ms so we always know how long we have not
been flying for. This helps for crash detection and auto-disarm

View File

@ -440,6 +440,19 @@ void QuadPlane::init_loiter(void)
init_throttle_wait();
}
// helper for is_flying()
bool QuadPlane::is_flying(void)
{
if (!available()) {
return false;
}
if (motors->get_throttle() > 200 && !motors->limit.throttle_lower) {
return true;
}
return false;
}
// crude landing detector to prevent tipover
bool QuadPlane::should_relax(void)
{
@ -677,7 +690,7 @@ void QuadPlane::update(void)
bool quad_mode = (plane.control_mode == QSTABILIZE ||
plane.control_mode == QHOVER ||
plane.control_mode == QLOITER ||
(plane.control_mode == AUTO && plane.auto_state.vtol_mode));
in_vtol_auto());
if (!quad_mode) {
update_transition();
@ -793,6 +806,26 @@ bool QuadPlane::handle_do_vtol_transition(const mavlink_command_long_t &packet)
return MAV_RESULT_FAILED;
}
/*
are we in a VTOL auto state?
*/
bool QuadPlane::in_vtol_auto(void)
{
if (plane.control_mode != AUTO) {
return false;
}
if (plane.auto_state.vtol_mode) {
return true;
}
switch (plane.mission.get_current_nav_cmd().id) {
case MAV_CMD_NAV_VTOL_LAND:
case MAV_CMD_NAV_VTOL_TAKEOFF:
return true;
default:
return false;
}
}
/*
handle auto-mode when auto_state.vtol_mode is true
*/
@ -828,7 +861,13 @@ void QuadPlane::control_auto(const Location &loc)
switch (plane.mission.get_current_nav_cmd().id) {
case MAV_CMD_NAV_VTOL_LAND:
pos_control->set_alt_target_from_climb_rate_ff(-50, plane.G_Dt, true);
if (plane.auto_state.wp_distance > 2) {
pos_control->set_alt_target_from_climb_rate_ff(0, plane.G_Dt, true);
} else if (plane.current_loc.alt > plane.next_WP_loc.alt+5*100) {
pos_control->set_alt_target_with_slew(wp_nav->get_loiter_target().z, plane.ins.get_loop_delta_t());
} else {
pos_control->set_alt_target_from_climb_rate_ff(-30, plane.G_Dt, false);
}
break;
case MAV_CMD_NAV_VTOL_TAKEOFF:
pos_control->set_alt_target_with_slew(wp_nav->get_loiter_target().z, plane.ins.get_loop_delta_t());
@ -852,7 +891,6 @@ bool QuadPlane::do_vtol_takeoff(const AP_Mission::Mission_Command& cmd)
plane.prev_WP_loc = plane.current_loc;
plane.next_WP_loc = cmd.content.location;
plane.next_WP_loc.alt = plane.current_loc.alt + cmd.content.location.alt;
plane.auto_state.vtol_mode = true;
throttle_wait = false;
// also update nav_controller for status output
@ -871,7 +909,6 @@ bool QuadPlane::do_vtol_land(const AP_Mission::Mission_Command& cmd)
}
plane.prev_WP_loc = plane.current_loc;
plane.next_WP_loc = cmd.content.location;
plane.auto_state.vtol_mode = true;
throttle_wait = false;
land_complete = false;
@ -892,7 +929,6 @@ bool QuadPlane::verify_vtol_takeoff(const AP_Mission::Mission_Command &cmd)
if (plane.current_loc.alt+threshold < plane.next_WP_loc.alt) {
return false;
}
plane.auto_state.vtol_mode = false;
transition_state = TRANSITION_AIRSPEED_WAIT;
return true;
}

View File

@ -46,6 +46,10 @@ public:
bool do_vtol_land(const AP_Mission::Mission_Command& cmd);
bool verify_vtol_takeoff(const AP_Mission::Mission_Command &cmd);
bool verify_vtol_land(void);
bool in_vtol_auto(void);
// vtol help for is_flying()
bool is_flying(void);
private:
AP_AHRS_NavEKF &ahrs;