diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index d8ba80849b..80b2ae2919 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -1605,6 +1605,34 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg) break; } + case MAVLINK_MSG_ID_MANUAL_CONTROL: + { + if (msg->sysid != plane.g.sysid_my_gcs) break; // Only accept control from our gcs + + mavlink_manual_control_t packet; + mavlink_msg_manual_control_decode(msg, &packet); + + bool override_active = false; + int16_t roll = (packet.y == INT16_MAX) ? 0 : plane.channel_roll->get_radio_min() + (plane.channel_roll->get_radio_max() - plane.channel_roll->get_radio_min()) * (packet.y + 1000) / 2000.0f; + int16_t pitch = (packet.x == INT16_MAX) ? 0 : plane.channel_pitch->get_radio_min() + (plane.channel_pitch->get_radio_max() - plane.channel_pitch->get_radio_min()) * (-packet.x + 1000) / 2000.0f; + int16_t throttle = (packet.z == INT16_MAX) ? 0 : plane.channel_throttle->get_radio_min() + (plane.channel_throttle->get_radio_max() - plane.channel_throttle->get_radio_min()) * (packet.z) / 1000.0f; + int16_t yaw = (packet.r == INT16_MAX) ? 0 : plane.channel_rudder->get_radio_min() + (plane.channel_rudder->get_radio_max() - plane.channel_rudder->get_radio_min()) * (packet.r + 1000) / 2000.0f; + + override_active |= hal.rcin->set_override(uint8_t(plane.rcmap.roll() - 1), roll); + override_active |= hal.rcin->set_override(uint8_t(plane.rcmap.pitch() - 1), pitch); + override_active |= hal.rcin->set_override(uint8_t(plane.rcmap.throttle() - 1), throttle); + override_active |= hal.rcin->set_override(uint8_t(plane.rcmap.yaw() - 1), yaw); + + if (override_active) { + plane.failsafe.last_valid_rc_ms = AP_HAL::millis(); + plane.failsafe.AFS_last_valid_rc_ms = plane.failsafe.last_valid_rc_ms; + } + + // a manual control message is considered to be a 'heartbeat' from the ground station for failsafe purposes + plane.failsafe.last_heartbeat_ms = AP_HAL::millis(); + break; + } + case MAVLINK_MSG_ID_HEARTBEAT: { // We keep track of the last time we received a heartbeat from