diff --git a/AntennaTracker/GCS_Mavlink.cpp b/AntennaTracker/GCS_Mavlink.cpp index 7db3bc6e2a..fc9373aa19 100644 --- a/AntennaTracker/GCS_Mavlink.cpp +++ b/AntennaTracker/GCS_Mavlink.cpp @@ -15,12 +15,14 @@ * pattern below when adding any new messages */ -void Tracker::send_heartbeat(mavlink_channel_t chan) +MAV_TYPE GCS_MAVLINK_Tracker::frame_type() const { - uint8_t base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; - uint8_t system_status = MAV_STATE_ACTIVE; - uint32_t custom_mode = control_mode; + return MAV_TYPE_ANTENNA_TRACKER; +} +MAV_MODE GCS_MAVLINK_Tracker::base_mode() const +{ + uint8_t base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; // work out the base_mode. This value is not very useful // for APM, but we calculate it as best we can so a generic // MAVLink enabled ground station can work out something about @@ -29,7 +31,7 @@ void Tracker::send_heartbeat(mavlink_channel_t chan) // only get useful information from the custom_mode, which maps to // the APM flight mode and has a well defined meaning in the // ArduPlane documentation - switch (control_mode) { + switch (tracker.control_mode) { case MANUAL: base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; break; @@ -48,7 +50,6 @@ void Tracker::send_heartbeat(mavlink_channel_t chan) break; case INITIALISING: - system_status = MAV_STATE_CALIBRATING; break; } @@ -57,10 +58,20 @@ void Tracker::send_heartbeat(mavlink_channel_t chan) base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; } - gcs().chan(chan-MAVLINK_COMM_0).send_heartbeat(MAV_TYPE_ANTENNA_TRACKER, - base_mode, - custom_mode, - system_status); + return (MAV_MODE)base_mode; +} + +uint32_t GCS_MAVLINK_Tracker::custom_mode() const +{ + return tracker.control_mode; +} + +MAV_STATE GCS_MAVLINK_Tracker::system_status() const +{ + if (tracker.control_mode == INITIALISING) { + return MAV_STATE_CALIBRATING; + } + return MAV_STATE_ACTIVE; } void Tracker::send_attitude(mavlink_channel_t chan) @@ -167,7 +178,7 @@ bool GCS_MAVLINK_Tracker::try_send_message(enum ap_message id) case MSG_HEARTBEAT: CHECK_PAYLOAD_SIZE(HEARTBEAT); last_heartbeat_time = AP_HAL::millis(); - tracker.send_heartbeat(chan); + send_heartbeat(); return true; case MSG_ATTITUDE: diff --git a/AntennaTracker/GCS_Mavlink.h b/AntennaTracker/GCS_Mavlink.h index 8545332b1f..c2a131ff38 100644 --- a/AntennaTracker/GCS_Mavlink.h +++ b/AntennaTracker/GCS_Mavlink.h @@ -39,4 +39,8 @@ private: void handle_change_alt_request(AP_Mission::Mission_Command &cmd) override; bool try_send_message(enum ap_message id) override; + MAV_TYPE frame_type() const override; + MAV_MODE base_mode() const override; + uint32_t custom_mode() const override; + MAV_STATE system_status() const override; }; diff --git a/AntennaTracker/Tracker.h b/AntennaTracker/Tracker.h index 1d35e62ec6..9568852469 100644 --- a/AntennaTracker/Tracker.h +++ b/AntennaTracker/Tracker.h @@ -202,7 +202,6 @@ private: void one_second_loop(); void ten_hz_logging_loop(); - void send_heartbeat(mavlink_channel_t chan); void send_attitude(mavlink_channel_t chan); void send_extended_status1(mavlink_channel_t chan); void send_location(mavlink_channel_t chan);