From d571f9a1c8da9e6be4ba021ea5e61ef200f9d1df Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 25 Apr 2023 20:19:15 +0900 Subject: [PATCH 1/6] mavlink: update to latest --- modules/mavlink | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/mavlink b/modules/mavlink index 9c39822c0f6be..ade0217f053c6 160000 --- a/modules/mavlink +++ b/modules/mavlink @@ -1 +1 @@ -Subproject commit 9c39822c0f6bef4e7ecb5c4f95979f06c23f763d +Subproject commit ade0217f053c69822909da76e232e114ed514d52 From e885fbbb62fb11cdd5b15d4efb80b384982019ec Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 10 May 2023 11:26:30 +0900 Subject: [PATCH 2/6] AP_Mount: fix send_gimbal_manager_information instance --- libraries/AP_Mount/AP_Mount_Backend.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index a654015a12e0b..2f89d4f2a2092 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -162,7 +162,7 @@ void AP_Mount_Backend::send_gimbal_manager_information(mavlink_channel_t chan) mavlink_msg_gimbal_manager_information_send(chan, AP_HAL::millis(), // autopilot system time get_gimbal_manager_capability_flags(), // bitmap of gimbal manager capability flags - _instance, // gimbal device id + _instance + 1, // gimbal device id radians(_params.roll_angle_min), // roll_min in radians radians(_params.roll_angle_max), // roll_max in radians radians(_params.pitch_angle_min), // pitch_min in radians From c6932dd4bb631e094a53b32d79f9813b41d99f31 Mon Sep 17 00:00:00 2001 From: davidsastresas Date: Mon, 8 May 2023 01:07:14 +0200 Subject: [PATCH 3/6] 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 25d2cbbab66b4..1d6620d943d50 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 759e66df597f3..ec4302a2ca94d 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -149,7 +149,7 @@ class AP_Mount 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 @@ class AP_Mount // 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 @@ class AP_Mount 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 2f89d4f2a2092..ee87bcc51be2c 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 aae60825c2f67..cc928988fa9f7 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -86,6 +86,10 @@ class AP_Mount_Backend // 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 @@ class AP_Mount_Backend // 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 @@ class AP_Mount_Backend 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 From ccebbd3d31d2096fb208b7f89b8c7f52ea73b7fe Mon Sep 17 00:00:00 2001 From: davidsastresas Date: Tue, 9 May 2023 21:25:33 +0900 Subject: [PATCH 4/6] GCS_MAVLink: support gimbal_manager_status and do_gimbal_manager_configure Co-authored-by: Randy Mackay --- libraries/GCS_MAVLink/GCS.h | 3 ++- libraries/GCS_MAVLink/GCS_Common.cpp | 36 ++++++++++++++++++++-------- libraries/GCS_MAVLink/ap_message.h | 1 + 3 files changed, 29 insertions(+), 11 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 940c46f98d2c7..07eeb68c6e099 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -372,6 +372,7 @@ class GCS_MAVLINK void send_vibration() const; void send_gimbal_device_attitude_status() const; void send_gimbal_manager_information() const; + void send_gimbal_manager_status() const; void send_named_float(const char *name, float value) const; void send_home_position() const; void send_gps_global_origin() const; @@ -642,7 +643,7 @@ class GCS_MAVLINK MAV_RESULT handle_command_do_set_roi_sysid(const mavlink_command_long_t &packet); MAV_RESULT handle_command_do_set_roi_none(); - virtual MAV_RESULT handle_command_mount(const mavlink_command_long_t &packet); + virtual MAV_RESULT handle_command_mount(const mavlink_command_long_t &packet, const mavlink_message_t &msg); MAV_RESULT handle_command_mag_cal(const mavlink_command_long_t &packet); virtual MAV_RESULT handle_command_long_packet(const mavlink_command_long_t &packet); MAV_RESULT handle_command_camera(const mavlink_command_long_t &packet); diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 0f8b9c9b6da2c..59641dd8ea57c 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -990,6 +990,7 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c { MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS, MSG_GIMBAL_DEVICE_ATTITUDE_STATUS}, { MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE, MSG_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE}, { MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION, MSG_GIMBAL_MANAGER_INFORMATION}, + { MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS, MSG_GIMBAL_MANAGER_STATUS}, #endif #if AP_OPTICALFLOW_ENABLED { MAVLINK_MSG_ID_OPTICAL_FLOW, MSG_OPTICAL_FLOW}, @@ -4540,14 +4541,14 @@ MAV_RESULT GCS_MAVLINK::handle_command_accelcal_vehicle_pos(const mavlink_comman } #endif // HAL_INS_ACCELCAL_ENABLED -MAV_RESULT GCS_MAVLINK::handle_command_mount(const mavlink_command_long_t &packet) +MAV_RESULT GCS_MAVLINK::handle_command_mount(const mavlink_command_long_t &packet, const mavlink_message_t &msg) { #if HAL_MOUNT_ENABLED AP_Mount *mount = AP::mount(); if (mount == nullptr) { return MAV_RESULT_UNSUPPORTED; } - return mount->handle_command_long(packet); + return mount->handle_command_long(packet, msg); #else return MAV_RESULT_UNSUPPORTED; #endif @@ -4678,12 +4679,6 @@ MAV_RESULT GCS_MAVLINK::handle_command_long_packet(const mavlink_command_long_t break; #endif - case MAV_CMD_DO_MOUNT_CONFIGURE: - case MAV_CMD_DO_MOUNT_CONTROL: - case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW: - result = handle_command_mount(packet); - break; - #if AP_MAVLINK_MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES_ENABLED case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: { result = handle_command_request_autopilot_capabilities(packet); @@ -4892,6 +4887,14 @@ void GCS_MAVLINK::handle_command_long(const mavlink_message_t &msg) result = handle_command_preflight_calibration(packet, msg); break; + case MAV_CMD_DO_MOUNT_CONFIGURE: + case MAV_CMD_DO_MOUNT_CONTROL: + case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW: + case MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE: + // some mount commands require the source sysid and compid + result = handle_command_mount(packet, msg); + break; + default: result = handle_command_long_packet(packet); break; @@ -5334,9 +5337,7 @@ void GCS_MAVLINK::send_gimbal_device_attitude_status() const } mount->send_gimbal_device_attitude_status(chan); } -#endif -#if HAL_MOUNT_ENABLED void GCS_MAVLINK::send_gimbal_manager_information() const { AP_Mount *mount = AP::mount(); @@ -5345,6 +5346,15 @@ void GCS_MAVLINK::send_gimbal_manager_information() const } mount->send_gimbal_manager_information(chan); } + +void GCS_MAVLINK::send_gimbal_manager_status() const +{ + AP_Mount *mount = AP::mount(); + if (mount == nullptr) { + return; + } + mount->send_gimbal_manager_status(chan); +} #endif void GCS_MAVLINK::send_set_position_target_global_int(uint8_t target_system, uint8_t target_component, const Location& loc) @@ -5675,6 +5685,12 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) #if HAL_MOUNT_ENABLED CHECK_PAYLOAD_SIZE(GIMBAL_MANAGER_INFORMATION); send_gimbal_manager_information(); +#endif + break; + case MSG_GIMBAL_MANAGER_STATUS: +#if HAL_MOUNT_ENABLED + CHECK_PAYLOAD_SIZE(GIMBAL_MANAGER_STATUS); + send_gimbal_manager_status(); #endif break; case MSG_OPTICAL_FLOW: diff --git a/libraries/GCS_MAVLink/ap_message.h b/libraries/GCS_MAVLink/ap_message.h index 9179c4b90b072..c11d75f81eb88 100644 --- a/libraries/GCS_MAVLink/ap_message.h +++ b/libraries/GCS_MAVLink/ap_message.h @@ -51,6 +51,7 @@ enum ap_message : uint8_t { MSG_CAMERA_FEEDBACK, MSG_GIMBAL_DEVICE_ATTITUDE_STATUS, MSG_GIMBAL_MANAGER_INFORMATION, + MSG_GIMBAL_MANAGER_STATUS, MSG_OPTICAL_FLOW, MSG_MAG_CAL_PROGRESS, MSG_MAG_CAL_REPORT, From 566a44299fbd029d6703394f99402b36a7056510 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 10 May 2023 13:07:17 +0900 Subject: [PATCH 5/6] Copter: handle_command_mount override accepts msg mavlink msg required to extra caller's sysid/compid --- ArduCopter/GCS_Mavlink.cpp | 4 ++-- ArduCopter/GCS_Mavlink.h | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 184a8f60155e6..a3e750b4dff1e 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -762,7 +762,7 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_int_packet(const mavlink_command_i } #if HAL_MOUNT_ENABLED -MAV_RESULT GCS_MAVLINK_Copter::handle_command_mount(const mavlink_command_long_t &packet) +MAV_RESULT GCS_MAVLINK_Copter::handle_command_mount(const mavlink_command_long_t &packet, const mavlink_message_t &msg) { switch (packet.command) { case MAV_CMD_DO_MOUNT_CONTROL: @@ -775,7 +775,7 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_mount(const mavlink_command_long_t default: break; } - return GCS_MAVLINK::handle_command_mount(packet); + return GCS_MAVLINK::handle_command_mount(packet, msg); } #endif diff --git a/ArduCopter/GCS_Mavlink.h b/ArduCopter/GCS_Mavlink.h index e804974ead27b..8dfa2cc119eb5 100644 --- a/ArduCopter/GCS_Mavlink.h +++ b/ArduCopter/GCS_Mavlink.h @@ -30,7 +30,7 @@ class GCS_MAVLINK_Copter : public GCS_MAVLINK MAV_RESULT handle_command_do_set_roi(const Location &roi_loc) override; MAV_RESULT handle_preflight_reboot(const mavlink_command_long_t &packet, const mavlink_message_t &msg) override; #if HAL_MOUNT_ENABLED - MAV_RESULT handle_command_mount(const mavlink_command_long_t &packet) override; + MAV_RESULT handle_command_mount(const mavlink_command_long_t &packet, const mavlink_message_t &msg) override; #endif MAV_RESULT handle_command_int_packet(const mavlink_command_int_t &packet) override; MAV_RESULT handle_command_long_packet(const mavlink_command_long_t &packet) override; From 49014a55465726769860e48567c715fa60f2232a Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 10 May 2023 13:07:33 +0900 Subject: [PATCH 6/6] Blimp: remove unused handle_command_mount override --- Blimp/GCS_Mavlink.cpp | 10 ---------- Blimp/GCS_Mavlink.h | 1 - 2 files changed, 11 deletions(-) diff --git a/Blimp/GCS_Mavlink.cpp b/Blimp/GCS_Mavlink.cpp index 515a2870b6b9c..ed97aaa8971e1 100644 --- a/Blimp/GCS_Mavlink.cpp +++ b/Blimp/GCS_Mavlink.cpp @@ -479,16 +479,6 @@ MAV_RESULT GCS_MAVLINK_Blimp::handle_command_int_packet(const mavlink_command_in } } -MAV_RESULT GCS_MAVLINK_Blimp::handle_command_mount(const mavlink_command_long_t &packet) -{ - // if the mount doesn't do pan control then yaw the entire vehicle instead: - switch (packet.command) { - default: - break; - } - return GCS_MAVLINK::handle_command_mount(packet); -} - MAV_RESULT GCS_MAVLINK_Blimp::handle_command_long_packet(const mavlink_command_long_t &packet) { switch (packet.command) { diff --git a/Blimp/GCS_Mavlink.h b/Blimp/GCS_Mavlink.h index 3e2d67d7d4015..036fe4066645d 100644 --- a/Blimp/GCS_Mavlink.h +++ b/Blimp/GCS_Mavlink.h @@ -26,7 +26,6 @@ class GCS_MAVLINK_Blimp : public GCS_MAVLINK void send_position_target_global_int() override; MAV_RESULT handle_command_do_set_roi(const Location &roi_loc) override; - MAV_RESULT handle_command_mount(const mavlink_command_long_t &packet) override; MAV_RESULT handle_command_int_packet(const mavlink_command_int_t &packet) override; MAV_RESULT handle_command_long_packet(const mavlink_command_long_t &packet) override; MAV_RESULT handle_command_int_do_reposition(const mavlink_command_int_t &packet);