From 96ae9cef141423a748bce9b7faa6b17e8e9c74dd Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 21 Mar 2018 14:40:53 +0900 Subject: [PATCH] Rover: send correct mav-type for boats --- APMrover2/GCS_Mavlink.cpp | 3 ++- APMrover2/system.cpp | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp index d8966b30ee..7e33acb66d 100644 --- a/APMrover2/GCS_Mavlink.cpp +++ b/APMrover2/GCS_Mavlink.cpp @@ -54,7 +54,8 @@ void Rover::send_heartbeat(mavlink_channel_t chan) // indicate we have set a custom mode base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; - gcs().chan(chan-MAVLINK_COMM_0).send_heartbeat(MAV_TYPE_GROUND_ROVER, + gcs().chan(chan-MAVLINK_COMM_0).send_heartbeat( + is_boat() ? MAV_TYPE_SURFACE_BOAT : MAV_TYPE_GROUND_ROVER, base_mode, control_mode->mode_number(), system_status); diff --git a/APMrover2/system.cpp b/APMrover2/system.cpp index a22007c9e2..90b1b7179b 100644 --- a/APMrover2/system.cpp +++ b/APMrover2/system.cpp @@ -80,7 +80,7 @@ void Rover::init_ardupilot() // setup frsky telemetry #if FRSKY_TELEM_ENABLED == ENABLED - frsky_telemetry.init(serial_manager, fwver.fw_string, MAV_TYPE_GROUND_ROVER); + frsky_telemetry.init(serial_manager, fwver.fw_string, (is_boat() ? MAV_TYPE_SURFACE_BOAT : MAV_TYPE_GROUND_ROVER)); #endif #if LOGGING_ENABLED == ENABLED