Browse Source

Plane: add VTOL POS1 notification to QRTL mode

gps-1.3.1
Hwurzburg 3 years ago committed by Andrew Tridgell
parent
commit
4e731bf4e2
  1. 3
      ArduPlane/quadplane.cpp

3
ArduPlane/quadplane.cpp

@ -2079,11 +2079,12 @@ void QuadPlane::run_xy_controller(void) @@ -2079,11 +2079,12 @@ void QuadPlane::run_xy_controller(void)
*/
void QuadPlane::poscontrol_init_approach(void)
{
const float dist = plane.current_loc.get_distance(plane.next_WP_loc);
if ((options & OPTION_DISABLE_APPROACH) != 0) {
// go straight to QPOS_POSITION1
poscontrol.set_state(QPOS_POSITION1);
gcs().send_text(MAV_SEVERITY_INFO,"VTOL Position1 d=%.1f", dist);
} else if (poscontrol.get_state() != QPOS_APPROACH) {
const float dist = plane.current_loc.get_distance(plane.next_WP_loc);
gcs().send_text(MAV_SEVERITY_INFO,"VTOL approach d=%.1f", dist);
poscontrol.set_state(QPOS_APPROACH);
poscontrol.thrust_loss_start_ms = 0;

Loading…
Cancel
Save