5
0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-05 23:48:31 -04:00

GCS_MAVLink: add virtual send_winch_status

This commit is contained in:
Randy Mackay 2020-07-22 20:05:20 +09:00
parent 6ea3c2a091
commit 1a3ef62b84
3 changed files with 9 additions and 0 deletions
libraries/GCS_MAVLink

View File

@ -265,6 +265,7 @@ public:
void send_set_position_target_global_int(uint8_t target_system, uint8_t target_component, const Location& loc);
void send_rpm() const;
void send_generator_status() const;
virtual void send_winch_status() const {};
// lock a channel, preventing use by MAVLink
void lock(bool _lock) {

View File

@ -43,6 +43,7 @@
#include <AP_EFI/AP_EFI.h>
#include <AP_Proximity/AP_Proximity.h>
#include <AP_Scripting/AP_Scripting.h>
#include <AP_Winch/AP_Winch.h>
#include <stdio.h>
@ -807,6 +808,7 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c
{ MAVLINK_MSG_ID_AUTOPILOT_VERSION, MSG_AUTOPILOT_VERSION},
{ MAVLINK_MSG_ID_EFI_STATUS, MSG_EFI_STATUS},
{ MAVLINK_MSG_ID_GENERATOR_STATUS, MSG_GENERATOR_STATUS},
{ MAVLINK_MSG_ID_WINCH_STATUS, MSG_WINCH_STATUS},
};
for (uint8_t i=0; i<ARRAY_SIZE(map); i++) {
@ -4764,6 +4766,11 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id)
break;
}
case MSG_WINCH_STATUS:
CHECK_PAYLOAD_SIZE(WINCH_STATUS);
send_winch_status();
break;
default:
// try_send_message must always at some stage return true for
// a message, or we will attempt to infinitely retry the

View File

@ -74,5 +74,6 @@ enum ap_message : uint8_t {
MSG_AUTOPILOT_VERSION,
MSG_EFI_STATUS,
MSG_GENERATOR_STATUS,
MSG_WINCH_STATUS,
MSG_LAST // MSG_LAST must be the last entry in this enum
};