diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index fb8f95c52d..9076d9c481 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -562,9 +562,15 @@ void Plane::update_flight_mode(void) handle_auto_mode(); break; + case GUIDED: + if (auto_state.vtol_mode && quadplane.available()) { + quadplane.guided_update(); + break; + } + // fall through + case RTL: case LOITER: - case GUIDED: calc_nav_roll(); calc_nav_pitch(); calc_throttle(); diff --git a/ArduPlane/commands.cpp b/ArduPlane/commands.cpp index ef8f9e6f53..309c1a563f 100644 --- a/ArduPlane/commands.cpp +++ b/ArduPlane/commands.cpp @@ -93,6 +93,12 @@ void Plane::set_guided_WP(void) setup_glide_slope(); setup_turn_angle(); + // reset loiter start time. + loiter.start_time_ms = 0; + + // start in non-VTOL mode + auto_state.vtol_mode = false; + loiter_angle_reset(); } diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp index e00fad991f..848c03581d 100644 --- a/ArduPlane/navigation.cpp +++ b/ArduPlane/navigation.cpp @@ -172,7 +172,11 @@ void Plane::update_loiter(uint16_t radius) } } - if (loiter.start_time_ms == 0 && + if (loiter.start_time_ms != 0 && + quadplane.available() && + quadplane.guided_mode != 0) { + auto_state.vtol_mode = true; + } else if (loiter.start_time_ms == 0 && control_mode == AUTO && !auto_state.no_crosstrack && get_distance(current_loc, next_WP_loc) > radius*2) { @@ -192,6 +196,10 @@ void Plane::update_loiter(uint16_t radius) // starting a loiter in GUIDED means we just reached the target point gcs_send_mission_item_reached_message(0); } + if (quadplane.available() && + quadplane.guided_mode != 0) { + quadplane.guided_start(); + } } } } diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 52701cfa8b..f562fd0f38 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -1060,12 +1060,19 @@ bool QuadPlane::handle_do_vtol_transition(enum MAV_VTOL_STATE state) */ bool QuadPlane::in_vtol_auto(void) { - if (!enable || plane.control_mode != AUTO) { + if (!enable) { + return false; + } + if (plane.control_mode != AUTO && + plane.control_mode != GUIDED) { return false; } if (plane.auto_state.vtol_mode) { return true; } + if (plane.control_mode == GUIDED) { + return false; + } switch (plane.mission.get_current_nav_cmd().id) { case MAV_CMD_NAV_VTOL_LAND: case MAV_CMD_NAV_VTOL_TAKEOFF: @@ -1690,3 +1697,23 @@ float QuadPlane::get_weathervane_yaw_rate_cds(void) // authority of the weathervane controller return weathervane.last_output * (yaw_rate_max/2) * 100; } + +/* + start guided mode control + */ +void QuadPlane::guided_start(void) +{ + poscontrol.state = QPOS_POSITION1; + poscontrol.speed_scale = 0; + setup_target_position(); + poscontrol.slow_descent = (plane.current_loc.alt > plane.next_WP_loc.alt); +} + +/* + update guided mode control + */ +void QuadPlane::guided_update(void) +{ + // run VTOL position controller + vtol_position_controller(); +} diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 5c284e6e53..34c3b61256 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -182,6 +182,9 @@ private: // setup correct aux channels for frame class void setup_default_channels(uint8_t num_motors); + + void guided_start(void); + void guided_update(void); AP_Int16 transition_time_ms;