From cbec7ee47bbe8e87ebe3f48bd6c9cf9a8fff38da Mon Sep 17 00:00:00 2001 From: davidsastresas Date: Mon, 8 May 2023 01:07:14 +0200 Subject: [PATCH] AP_Mount: support gimbal_manager_status and do_gimbal_manager_configure Co-authored-by: Randy Mackay --- libraries/AP_Mount/AP_Mount.cpp | 35 +++++++++++++++- libraries/AP_Mount/AP_Mount.h | 6 ++- libraries/AP_Mount/AP_Mount_Backend.cpp | 55 +++++++++++++++++++++++++ libraries/AP_Mount/AP_Mount_Backend.h | 14 +++++++ 4 files changed, 108 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index 25d2cbbab6..1d6620d943 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -337,6 +337,26 @@ MAV_RESULT AP_Mount::handle_command_do_gimbal_manager_pitchyaw(const mavlink_com return MAV_RESULT_FAILED; } +// handle mav_cmd_do_gimbal_manager_configure for deconflicting different mavlink message senders +MAV_RESULT AP_Mount::handle_command_do_gimbal_manager_configure(const mavlink_command_long_t &packet, const mavlink_message_t &msg) +{ + AP_Mount_Backend *backend; + + // check gimbal device id. 0 is primary, 1 is 1st gimbal, 2 is 2nd gimbal, etc + const uint8_t instance = packet.param7; + if (instance == 0) { + backend = get_primary(); + } else { + backend = get_instance(instance - 1); + } + + if (backend == nullptr) { + return MAV_RESULT_FAILED; + } + + return backend->handle_command_do_gimbal_manager_configure(packet, msg); +} + void AP_Mount::handle_gimbal_manager_set_attitude(const mavlink_message_t &msg){ mavlink_gimbal_manager_set_attitude_t packet; mavlink_msg_gimbal_manager_set_attitude_decode(&msg,&packet); @@ -402,7 +422,7 @@ void AP_Mount::handle_gimbal_manager_set_attitude(const mavlink_message_t &msg){ } } -MAV_RESULT AP_Mount::handle_command_long(const mavlink_command_long_t &packet) +MAV_RESULT AP_Mount::handle_command_long(const mavlink_command_long_t &packet, const mavlink_message_t &msg) { switch (packet.command) { case MAV_CMD_DO_MOUNT_CONFIGURE: @@ -411,6 +431,8 @@ MAV_RESULT AP_Mount::handle_command_long(const mavlink_command_long_t &packet) return handle_command_do_mount_control(packet); case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW: return handle_command_do_gimbal_manager_pitchyaw(packet); + case MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE: + return handle_command_do_gimbal_manager_configure(packet, msg); default: return MAV_RESULT_UNSUPPORTED; } @@ -485,6 +507,17 @@ void AP_Mount::send_gimbal_manager_information(mavlink_channel_t chan) } } +// send a GIMBAL_MANAGER_STATUS message to GCS +void AP_Mount::send_gimbal_manager_status(mavlink_channel_t chan) +{ + // call send_gimbal_device_attitude_status for each instance + for (uint8_t instance=0; instancesend_gimbal_manager_status(chan); + } + } +} + // get mount's current attitude in euler angles in degrees. yaw angle is in body-frame // returns true on success bool AP_Mount::get_attitude_euler(uint8_t instance, float& roll_deg, float& pitch_deg, float& yaw_bf_deg) diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index 759e66df59..ec4302a2ca 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -149,7 +149,7 @@ public: void set_target_sysid(uint8_t instance, uint8_t sysid); // mavlink message handling: - MAV_RESULT handle_command_long(const mavlink_command_long_t &packet); + MAV_RESULT handle_command_long(const mavlink_command_long_t &packet, const mavlink_message_t &msg); void handle_param_value(const mavlink_message_t &msg); void handle_message(mavlink_channel_t chan, const mavlink_message_t &msg); @@ -159,6 +159,9 @@ public: // send a GIMBAL_MANAGER_INFORMATION message to GCS void send_gimbal_manager_information(mavlink_channel_t chan); + // send a GIMBAL_MANAGER_STATUS message to GCS + void send_gimbal_manager_status(mavlink_channel_t chan); + // get mount's current attitude in euler angles in degrees. yaw angle is in body-frame // returns true on success bool get_attitude_euler(uint8_t instance, float& roll_deg, float& pitch_deg, float& yaw_bf_deg); @@ -218,6 +221,7 @@ private: MAV_RESULT handle_command_do_mount_configure(const mavlink_command_long_t &packet); MAV_RESULT handle_command_do_mount_control(const mavlink_command_long_t &packet); MAV_RESULT handle_command_do_gimbal_manager_pitchyaw(const mavlink_command_long_t &packet); + MAV_RESULT handle_command_do_gimbal_manager_configure(const mavlink_command_long_t &packet, const mavlink_message_t &msg); void handle_gimbal_manager_set_attitude(const mavlink_message_t &msg); void handle_global_position_int(const mavlink_message_t &msg); void handle_gimbal_device_information(const mavlink_message_t &msg); diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index 2f89d4f2a2..ee87bcc51b 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -171,6 +171,25 @@ void AP_Mount_Backend::send_gimbal_manager_information(mavlink_channel_t chan) radians(_params.yaw_angle_max)); // yaw_max in radians } +// send a GIMBAL_MANAGER_STATUS message to GCS +void AP_Mount_Backend::send_gimbal_manager_status(mavlink_channel_t chan) +{ + uint32_t flags = GIMBAL_MANAGER_FLAGS_ROLL_LOCK | GIMBAL_MANAGER_FLAGS_PITCH_LOCK; + + if (_yaw_lock) { + flags |= GIMBAL_MANAGER_FLAGS_PITCH_LOCK; + } + + mavlink_msg_gimbal_manager_status_send(chan, + AP_HAL::millis(), // autopilot system time + flags, // bitmap of gimbal manager flags + _instance + 1, // gimbal device id + mavlink_control_id.sysid, // primary control system id + mavlink_control_id.compid, // primary control component id + 0, // secondary control system id + 0); // secondary control component id +} + // process MOUNT_CONTROL messages received from GCS. deprecated. void AP_Mount_Backend::handle_mount_control(const mavlink_mount_control_t &packet) { @@ -267,6 +286,42 @@ MAV_RESULT AP_Mount_Backend::handle_command_do_mount_control(const mavlink_comma } } +// handle do_gimbal_manager_configure. Returns MAV_RESULT_ACCEPTED on success +// requires original message in order to extract caller's sysid and compid +MAV_RESULT AP_Mount_Backend::handle_command_do_gimbal_manager_configure(const mavlink_command_long_t &packet, const mavlink_message_t &msg) +{ + // sanity check param1 and param2 values + if ((packet.param1 < -3) || (packet.param1 > UINT8_MAX) || (packet.param2 < -3) || (packet.param2 > UINT8_MAX)) { + return MAV_RESULT_FAILED; + } + + // convert negative packet1 and packet2 values + int16_t new_sysid = packet.param1; + switch (new_sysid) { + case -1: + // leave unchanged + break; + case -2: + // set itself in control + mavlink_control_id.sysid = msg.sysid; + mavlink_control_id.compid = msg.compid; + break; + case -3: + // remove control if currently in control + if ((mavlink_control_id.sysid == msg.sysid) && (mavlink_control_id.compid == msg.compid)) { + mavlink_control_id.sysid = 0; + mavlink_control_id.compid = 0; + } + break; + default: + mavlink_control_id.sysid = packet.param1; + mavlink_control_id.compid = packet.param2; + break; + } + + return MAV_RESULT_ACCEPTED; +} + // handle a GLOBAL_POSITION_INT message bool AP_Mount_Backend::handle_global_position_int(uint8_t msg_sysid, const mavlink_global_position_int_t &packet) { diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index aae60825c2..cc928988fa 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -86,6 +86,10 @@ public: // handle do_mount_control command. Returns MAV_RESULT_ACCEPTED on success MAV_RESULT handle_command_do_mount_control(const mavlink_command_long_t &packet); + + // handle do_gimbal_manager_configure. Returns MAV_RESULT_ACCEPTED on success + // requires original message in order to extract caller's sysid and compid + MAV_RESULT handle_command_do_gimbal_manager_configure(const mavlink_command_long_t &packet, const mavlink_message_t &msg); // process MOUNT_CONFIGURE messages received from GCS. deprecated. void handle_mount_configure(const mavlink_mount_configure_t &msg); @@ -102,6 +106,9 @@ public: // send a GIMBAL_MANAGER_INFORMATION message to GCS void send_gimbal_manager_information(mavlink_channel_t chan); + // send a GIMBAL_MANAGER_STATUS message to GCS + void send_gimbal_manager_status(mavlink_channel_t chan); + // handle a GIMBAL_REPORT message virtual void handle_gimbal_report(mavlink_channel_t chan, const mavlink_message_t &msg) {} @@ -229,6 +236,13 @@ protected: bool _target_sysid_location_set;// true if _target_sysid has been set uint32_t _last_warning_ms; // system time of last warning sent to GCS + + // structure holding mavlink sysid and compid of controller of this gimbal + // see MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE and GIMBAL_MANAGER_STATUS + struct { + uint8_t sysid; + uint8_t compid; + } mavlink_control_id; }; #endif // HAL_MOUNT_ENABLED