From e595da41e61506911d639babf8396166696971d1 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 10 Jun 2022 17:24:04 +0900 Subject: [PATCH] AP_Mount: gremsy uses gimb-dev-att-send to retract gimbal this replaces the MAV_CMD_USER_1 method --- libraries/AP_Mount/AP_Mount_Gremsy.cpp | 38 +++++++++++++------------- libraries/AP_Mount/AP_Mount_Gremsy.h | 6 ++-- 2 files changed, 22 insertions(+), 22 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount_Gremsy.cpp b/libraries/AP_Mount/AP_Mount_Gremsy.cpp index 1440aca0f3..6824c8ed94 100644 --- a/libraries/AP_Mount/AP_Mount_Gremsy.cpp +++ b/libraries/AP_Mount/AP_Mount_Gremsy.cpp @@ -28,7 +28,7 @@ void AP_Mount_Gremsy::update() // move mount to a "retracted" position. We disable motors case MAV_MOUNT_MODE_RETRACT: // handled below - enable_motors(false); + send_gimbal_device_retract(); break; // move mount to a neutral position, typically pointing forward @@ -237,6 +237,24 @@ bool AP_Mount_Gremsy::start_sending_attitude_to_gimbal() return (res == MAV_RESULT_ACCEPTED); } +// send GIMBAL_DEVICE_SET_ATTITUDE to gimbal to command gimbal to retract (aka relax) +void AP_Mount_Gremsy::send_gimbal_device_retract() const +{ + // check we have space for the message + if (!HAVE_PAYLOAD_SPACE(_chan, GIMBAL_DEVICE_SET_ATTITUDE)) { + return; + } + + // send command_long command containing a do_mount_control command + const float quat_array[4] = {NAN, NAN, NAN, NAN}; + mavlink_msg_gimbal_device_set_attitude_send(_chan, + _sysid, // target system + _compid, // target component + GIMBAL_DEVICE_FLAGS_RETRACT, // gimbal device flags + quat_array, // attitude as a quaternion + 0, 0, 0); // angular velocities +} + // send GIMBAL_DEVICE_SET_ATTITUDE to gimbal to control rate // earth_frame should be true if yaw_rads target is an earth frame rate, false if body_frame void AP_Mount_Gremsy::send_gimbal_device_set_rate(float roll_rads, float pitch_rads, float yaw_rads, bool earth_frame) const @@ -290,22 +308,4 @@ void AP_Mount_Gremsy::send_gimbal_device_set_attitude(float roll_rad, float pitc NAN, NAN, NAN); // angular velocities } -// turn motors on/off -void AP_Mount_Gremsy::enable_motors(bool on) const -{ - // check we have space for the message - if (!HAVE_PAYLOAD_SPACE(_chan, COMMAND_LONG)) { - return; - } - - // send COMPMAND_LONG with MAV_CMD_USER_1 command with Param7 populated to turn on/off motor - mavlink_msg_command_long_send(_chan, - _sysid, - _compid, - MAV_CMD_USER_1, - 0, // confirmation of zero means this is the first time this message has been sent - 0, 0, 0, 0, 0, 0, // param1 to param6 unused - on ? 0x01 : 0x00);// 0=OFF, 1=ON -} - #endif // HAL_MOUNT_ENABLED diff --git a/libraries/AP_Mount/AP_Mount_Gremsy.h b/libraries/AP_Mount/AP_Mount_Gremsy.h index a1239fe5f7..29ce2c2ddd 100644 --- a/libraries/AP_Mount/AP_Mount_Gremsy.h +++ b/libraries/AP_Mount/AP_Mount_Gremsy.h @@ -54,6 +54,9 @@ private: // returns true on success, false on failure to start sending bool start_sending_attitude_to_gimbal(); + // send GIMBAL_DEVICE_SET_ATTITUDE to gimbal to command gimbal to retract (aka relax) + void send_gimbal_device_retract() const; + // send GIMBAL_DEVICE_SET_ATTITUDE to gimbal to control rate // earth_frame should be true if yaw_rads target is an earth frame rate, false if body_frame void send_gimbal_device_set_rate(float roll_rads, float pitch_rads, float yaw_rads, bool earth_frame) const; @@ -62,9 +65,6 @@ private: // earth_frame should be true if yaw_rad target is an earth frame angle, false if body_frame void send_gimbal_device_set_attitude(float roll_rad, float pitch_rad, float yaw_rad, bool earth_frame) const; - // turn motors on/off - void enable_motors(bool on) const; - // internal variables bool _found_gimbal; // true once a MAVLink enabled gimbal has been found bool _got_device_info; // true once gimbal has provided device info