diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 3e0a2227aa..6fe2982a9f 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -1347,6 +1347,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; } + case MAV_CMD_DO_AUTOTUNE_ENABLE: + // param1 : enable/disable + plane.autotune_enable(packet.param1); + break; + default: break; } diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 2b09ae9737..0d76bd0a6d 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -751,6 +751,7 @@ private: void reset_control_switch(); void autotune_start(void); void autotune_restore(void); + void autotune_enable(bool enable); bool fly_inverted(void); void failsafe_short_on_event(enum failsafe_state fstype); void failsafe_long_on_event(enum failsafe_state fstype); diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 870e1b0fa0..593f17d95e 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -145,6 +145,9 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd) } } #endif + + case MAV_CMD_DO_AUTOTUNE_ENABLE: + autotune_enable(cmd.p1); break; #if CAMERA == ENABLED @@ -267,6 +270,7 @@ bool Plane::verify_command(const AP_Mission::Mission_Command& cmd) // Ret case MAV_CMD_DO_INVERTED_FLIGHT: case MAV_CMD_DO_LAND_START: case MAV_CMD_DO_FENCE_ENABLE: + case MAV_CMD_DO_AUTOTUNE_ENABLE: return true; default: diff --git a/ArduPlane/control_modes.cpp b/ArduPlane/control_modes.cpp index 88c61cd26f..589afab33d 100644 --- a/ArduPlane/control_modes.cpp +++ b/ArduPlane/control_modes.cpp @@ -146,6 +146,18 @@ void Plane::autotune_restore(void) pitchController.autotune_restore(); } +/* + enable/disable autotune for AUTO modes + */ +void Plane::autotune_enable(bool enable) +{ + if (enable) { + autotune_start(); + } else { + autotune_restore(); + } +} + /* are we flying inverted? */