From b4979e2cfa7f2ea93de3acaa8ea27a1c58fc960d Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 11 Jul 2022 17:07:22 +0900 Subject: [PATCH] AP_Mount: replace send-mount-status with send-gimbal-device-attitude-status --- libraries/AP_Mount/AP_Mount.cpp | 8 ++-- libraries/AP_Mount/AP_Mount.h | 4 +- libraries/AP_Mount/AP_Mount_Alexmos.cpp | 12 ++++-- libraries/AP_Mount/AP_Mount_Alexmos.h | 6 ++- libraries/AP_Mount/AP_Mount_Backend.cpp | 38 +++++++++++++++++++ libraries/AP_Mount/AP_Mount_Backend.h | 13 ++++++- libraries/AP_Mount/AP_Mount_Gremsy.cpp | 24 +++--------- libraries/AP_Mount/AP_Mount_Gremsy.h | 8 ++-- libraries/AP_Mount/AP_Mount_SToRM32.cpp | 8 ++-- libraries/AP_Mount/AP_Mount_SToRM32.h | 6 ++- .../AP_Mount/AP_Mount_SToRM32_serial.cpp | 8 ++-- libraries/AP_Mount/AP_Mount_SToRM32_serial.h | 6 ++- libraries/AP_Mount/AP_Mount_Servo.cpp | 11 +++--- libraries/AP_Mount/AP_Mount_Servo.h | 6 ++- libraries/AP_Mount/AP_Mount_SoloGimbal.cpp | 13 +++---- libraries/AP_Mount/AP_Mount_SoloGimbal.h | 11 ++++-- 16 files changed, 117 insertions(+), 65 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index b00b8f87a2..a8b7feafd4 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -742,13 +742,13 @@ void AP_Mount::handle_mount_control(const mavlink_message_t &msg) _backends[_primary]->handle_mount_control(packet); } -/// Return mount status information -void AP_Mount::send_mount_status(mavlink_channel_t chan) +// send a GIMBAL_DEVICE_ATTITUDE_STATUS message to GCS +void AP_Mount::send_gimbal_device_attitude_status(mavlink_channel_t chan) { - // call send_mount_status for each instance + // call send_gimbal_device_attitude_status for each instance for (uint8_t instance=0; instancesend_mount_status(chan); + _backends[instance]->send_gimbal_device_attitude_status(chan); } } } diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index a6687eefbf..3ba8f79100 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -146,8 +146,8 @@ public: void handle_param_value(const mavlink_message_t &msg); void handle_message(mavlink_channel_t chan, const mavlink_message_t &msg); - // send a MOUNT_STATUS message to GCS: - void send_mount_status(mavlink_channel_t chan); + // send a GIMBAL_DEVICE_ATTITUDE_STATUS message to GCS + void send_gimbal_device_attitude_status(mavlink_channel_t chan); // run pre-arm check. returns false on failure and fills in failure_msg // any failure_msg returned will not include a prefix diff --git a/libraries/AP_Mount/AP_Mount_Alexmos.cpp b/libraries/AP_Mount/AP_Mount_Alexmos.cpp index 3da8a9f9d4..0bc0d860f1 100644 --- a/libraries/AP_Mount/AP_Mount_Alexmos.cpp +++ b/libraries/AP_Mount/AP_Mount_Alexmos.cpp @@ -102,15 +102,19 @@ bool AP_Mount_Alexmos::has_pan_control() const return _gimbal_3axis; } -// send_mount_status - called to allow mounts to send their status to GCS using the MOUNT_STATUS message -void AP_Mount_Alexmos::send_mount_status(mavlink_channel_t chan) +// get attitude as a quaternion. returns true on success +bool AP_Mount_Alexmos::get_attitude_quaternion(Quaternion& att_quat) { if (!_initialised) { - return; + return false; } + // request attitude from gimbal get_angles(); - mavlink_msg_mount_status_send(chan, 0, 0, _current_angle.y*100, _current_angle.x*100, _current_angle.z*100, _mode); + + // construct quaternion + att_quat.from_euler(radians(_current_angle.x), radians(_current_angle.y), radians(_current_angle.z)); + return true; } /* diff --git a/libraries/AP_Mount/AP_Mount_Alexmos.h b/libraries/AP_Mount/AP_Mount_Alexmos.h index 1c5a781847..1958b25dec 100644 --- a/libraries/AP_Mount/AP_Mount_Alexmos.h +++ b/libraries/AP_Mount/AP_Mount_Alexmos.h @@ -81,8 +81,10 @@ public: // has_pan_control - returns true if this mount can control its pan (required for multicopters) bool has_pan_control() const override; - // send_mount_status - called to allow mounts to send their status to GCS via MAVLink - void send_mount_status(mavlink_channel_t chan) override; +protected: + + // get attitude as a quaternion. returns true on success + bool get_attitude_quaternion(Quaternion& att_quat) override; private: diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index 1222ff7749..70a09166f6 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -65,6 +65,34 @@ void AP_Mount_Backend::handle_mount_configure(const mavlink_mount_configure_t &p _state._stab_pan.set(packet.stab_yaw); } +// send a GIMBAL_DEVICE_ATTITUDE_STATUS message to GCS +void AP_Mount_Backend::send_gimbal_device_attitude_status(mavlink_channel_t chan) +{ + if (suppress_heartbeat()) { + // block heartbeat from transmitting to the GCS + GCS_MAVLINK::disable_channel_routing(chan); + } + + Quaternion att_quat; + if (!get_attitude_quaternion(att_quat)) { + return; + } + + // construct quaternion array + const float quat_array[4] = {att_quat.q1, att_quat.q2, att_quat.q3, att_quat.q4}; + + mavlink_msg_gimbal_device_attitude_status_send(chan, + 0, // target system + 0, // target component + AP_HAL::millis(), // autopilot system time + get_gimbal_device_flags(), + quat_array, // attitude expressed as quaternion + std::numeric_limits::quiet_NaN(), // roll axis angular velocity (NaN for unknown) + std::numeric_limits::quiet_NaN(), // pitch axis angular velocity (NaN for unknown) + std::numeric_limits::quiet_NaN(), // yaw axis angular velocity (NaN for unknown) + 0); // failure flags (not supported) +} + // process MOUNT_CONTROL messages received from GCS. deprecated. void AP_Mount_Backend::handle_mount_control(const mavlink_mount_control_t &packet) { @@ -310,6 +338,16 @@ void AP_Mount_Backend::update_angle_target_from_rate(const MountTarget& rate_rad } } +// helper function to provide GIMBAL_DEVICE_FLAGS for use in GIMBAL_DEVICE_ATTITUDE_STATUS message +uint16_t AP_Mount_Backend::get_gimbal_device_flags() const +{ + const uint16_t flags = (get_mode() == MAV_MOUNT_MODE_RETRACT ? GIMBAL_DEVICE_FLAGS_RETRACT : 0) | + (get_mode() == MAV_MOUNT_MODE_NEUTRAL ? GIMBAL_DEVICE_FLAGS_NEUTRAL : 0) | + GIMBAL_DEVICE_FLAGS_ROLL_LOCK | // roll angle is always earth-frame + GIMBAL_DEVICE_FLAGS_PITCH_LOCK; // pitch angle is always earth-frame, yaw_angle is always body-frame + return flags; +} + // get angle targets (in radians) to home location // returns true on success, false on failure bool AP_Mount_Backend::get_angle_target_to_home(MountTarget& angle_rad) const diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index c97d9962c5..65c7512977 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -82,8 +82,8 @@ public: // process MOUNT_CONTROL messages received from GCS. deprecated. void handle_mount_control(const mavlink_mount_control_t &packet); - // send_mount_status - called to allow mounts to send their status to GCS via MAVLink - virtual void send_mount_status(mavlink_channel_t chan) = 0; + // send a GIMBAL_DEVICE_ATTITUDE_STATUS message to GCS + void send_gimbal_device_attitude_status(mavlink_channel_t chan); // handle a GIMBAL_REPORT message virtual void handle_gimbal_report(mavlink_channel_t chan, const mavlink_message_t &msg) {} @@ -115,6 +115,12 @@ protected: bool yaw_is_ef; }; + // returns true if mavlink heartbeat should be suppressed for this gimbal (only used by Solo gimbal) + virtual bool suppress_heartbeat() const { return false; } + + // get attitude as a quaternion. returns true on success + virtual bool get_attitude_quaternion(Quaternion& att_quat) = 0; + // get pilot input (in the range -1 to +1) received through RC void get_rc_input(float& roll_in, float& pitch_in, float& yaw_in) const; @@ -153,6 +159,9 @@ protected: // assumes a 50hz update rate void update_angle_target_from_rate(const MountTarget& rate_rad, MountTarget& angle_rad) const; + // helper function to provide GIMBAL_DEVICE_FLAGS for use in GIMBAL_DEVICE_ATTITUDE_STATUS message + uint16_t get_gimbal_device_flags() const; + AP_Mount &_frontend; // reference to the front end which holds parameters AP_Mount::mount_state &_state; // references to the parameters and state for this backend uint8_t _instance; // this instance's number diff --git a/libraries/AP_Mount/AP_Mount_Gremsy.cpp b/libraries/AP_Mount/AP_Mount_Gremsy.cpp index a845157cc2..1a490605c7 100644 --- a/libraries/AP_Mount/AP_Mount_Gremsy.cpp +++ b/libraries/AP_Mount/AP_Mount_Gremsy.cpp @@ -121,31 +121,17 @@ bool AP_Mount_Gremsy::healthy() const return true; } -// send_mount_status - called to allow mounts to send their status to GCS using the MOUNT_STATUS message -void AP_Mount_Gremsy::send_mount_status(mavlink_channel_t chan) +// get attitude as a quaternion. returns true on success +bool AP_Mount_Gremsy::get_attitude_quaternion(Quaternion& att_quat) { - // check we have space for the message - if (!HAVE_PAYLOAD_SPACE(chan, GIMBAL_DEVICE_ATTITUDE_STATUS)) { - return; - } - // check we have received an updated message if (_gimbal_device_attitude_status.time_boot_ms == _sent_gimbal_device_attitude_status_ms) { - return; + return false; } _sent_gimbal_device_attitude_status_ms = _gimbal_device_attitude_status.time_boot_ms; - // forward on message to GCS - mavlink_msg_gimbal_device_attitude_status_send(chan, - 0, // target system - 0, // target component - AP_HAL::millis(), // autopilot system time - _gimbal_device_attitude_status.flags, - _gimbal_device_attitude_status.q, - _gimbal_device_attitude_status.angular_velocity_x, - _gimbal_device_attitude_status.angular_velocity_y, - _gimbal_device_attitude_status.angular_velocity_z, - _gimbal_device_attitude_status.failure_flags); + att_quat = _gimbal_device_attitude_status.q; + return true; } // search for gimbal in GCS_MAVLink routing table diff --git a/libraries/AP_Mount/AP_Mount_Gremsy.h b/libraries/AP_Mount/AP_Mount_Gremsy.h index 31cc7c2baf..01e57c4b07 100644 --- a/libraries/AP_Mount/AP_Mount_Gremsy.h +++ b/libraries/AP_Mount/AP_Mount_Gremsy.h @@ -36,15 +36,17 @@ public: // has_pan_control bool has_pan_control() const override { return true; } - // send_mount_status - void send_mount_status(mavlink_channel_t chan) override; - // handle GIMBAL_DEVICE_INFORMATION message void handle_gimbal_device_information(const mavlink_message_t &msg) override; // handle GIMBAL_DEVICE_ATTITUDE_STATUS message void handle_gimbal_device_attitude_status(const mavlink_message_t &msg) override; +protected: + + // get attitude as a quaternion. returns true on success + bool get_attitude_quaternion(Quaternion& att_quat) override; + private: // search for gimbal in GCS_MAVLink routing table diff --git a/libraries/AP_Mount/AP_Mount_SToRM32.cpp b/libraries/AP_Mount/AP_Mount_SToRM32.cpp index 1ca4872553..c5ca95590b 100644 --- a/libraries/AP_Mount/AP_Mount_SToRM32.cpp +++ b/libraries/AP_Mount/AP_Mount_SToRM32.cpp @@ -123,11 +123,11 @@ void AP_Mount_SToRM32::set_mode(enum MAV_MOUNT_MODE mode) _mode = mode; } -// send_mount_status - called to allow mounts to send their status to GCS using the MOUNT_STATUS message -void AP_Mount_SToRM32::send_mount_status(mavlink_channel_t chan) +// get attitude as a quaternion. returns true on success +bool AP_Mount_SToRM32::get_attitude_quaternion(Quaternion& att_quat) { - // return target angles as gimbal's actual attitude. To-Do: retrieve actual gimbal attitude and send these instead - mavlink_msg_mount_status_send(chan, 0, 0, degrees(_angle_rad.pitch)*100, degrees(_angle_rad.roll)*100, degrees(get_bf_yaw_angle(_angle_rad))*100, _mode); + att_quat.from_euler(_angle_rad.roll, _angle_rad.pitch, get_bf_yaw_angle(_angle_rad)); + return true; } // search for gimbal in GCS_MAVLink routing table diff --git a/libraries/AP_Mount/AP_Mount_SToRM32.h b/libraries/AP_Mount/AP_Mount_SToRM32.h index ef9c747703..b8bd402844 100644 --- a/libraries/AP_Mount/AP_Mount_SToRM32.h +++ b/libraries/AP_Mount/AP_Mount_SToRM32.h @@ -35,8 +35,10 @@ public: // set_mode - sets mount's mode void set_mode(enum MAV_MOUNT_MODE mode) override; - // send_mount_status - called to allow mounts to send their status to GCS using the MOUNT_STATUS message - void send_mount_status(mavlink_channel_t chan) override; +protected: + + // get attitude as a quaternion. returns true on success + bool get_attitude_quaternion(Quaternion& att_quat) override; private: diff --git a/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp b/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp index 7e7d2d417b..44c4227760 100644 --- a/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp +++ b/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp @@ -152,11 +152,11 @@ void AP_Mount_SToRM32_serial::set_mode(enum MAV_MOUNT_MODE mode) _mode = mode; } -// send_mount_status - called to allow mounts to send their status to GCS using the MOUNT_STATUS message -void AP_Mount_SToRM32_serial::send_mount_status(mavlink_channel_t chan) +// get attitude as a quaternion. returns true on success +bool AP_Mount_SToRM32_serial::get_attitude_quaternion(Quaternion& att_quat) { - // return target angles as gimbal's actual attitude. - mavlink_msg_mount_status_send(chan, 0, 0, _current_angle.y, _current_angle.x, _current_angle.z, _mode); + att_quat.from_euler(radians(_current_angle.x * 0.01f), radians(_current_angle.y * 0.01f), radians(_current_angle.z * 0.01f)); + return true; } bool AP_Mount_SToRM32_serial::can_send(bool with_control) { diff --git a/libraries/AP_Mount/AP_Mount_SToRM32_serial.h b/libraries/AP_Mount/AP_Mount_SToRM32_serial.h index 1fb49c1cc5..fcf85c0409 100644 --- a/libraries/AP_Mount/AP_Mount_SToRM32_serial.h +++ b/libraries/AP_Mount/AP_Mount_SToRM32_serial.h @@ -38,8 +38,10 @@ public: // set_mode - sets mount's mode void set_mode(enum MAV_MOUNT_MODE mode) override; - // send_mount_status - called to allow mounts to send their status to GCS using the MOUNT_STATUS message - void send_mount_status(mavlink_channel_t chan) override; +protected: + + // get attitude as a quaternion. returns true on success + bool get_attitude_quaternion(Quaternion& att_quat) override; private: diff --git a/libraries/AP_Mount/AP_Mount_Servo.cpp b/libraries/AP_Mount/AP_Mount_Servo.cpp index 825409b27d..daaa8b812d 100644 --- a/libraries/AP_Mount/AP_Mount_Servo.cpp +++ b/libraries/AP_Mount/AP_Mount_Servo.cpp @@ -120,14 +120,15 @@ bool AP_Mount_Servo::has_pan_control() const return SRV_Channels::function_assigned(_pan_idx); } -// private methods - -// send_mount_status - called to allow mounts to send their status to GCS using the MOUNT_STATUS message -void AP_Mount_Servo::send_mount_status(mavlink_channel_t chan) +// get attitude as a quaternion. returns true on success +bool AP_Mount_Servo::get_attitude_quaternion(Quaternion& att_quat) { - mavlink_msg_mount_status_send(chan, 0, 0, _angle_bf_output_deg.y*100, _angle_bf_output_deg.x*100, _angle_bf_output_deg.z*100, _mode); + att_quat.from_euler(radians(_angle_bf_output_deg.x), radians(_angle_bf_output_deg.y), radians(_angle_bf_output_deg.z)); + return true; } +// private methods + // update body-frame angle outputs from earth-frame angle targets void AP_Mount_Servo::update_angle_outputs(const MountTarget& angle_rad) { diff --git a/libraries/AP_Mount/AP_Mount_Servo.h b/libraries/AP_Mount/AP_Mount_Servo.h index 1b97c53d36..11b142dc33 100644 --- a/libraries/AP_Mount/AP_Mount_Servo.h +++ b/libraries/AP_Mount/AP_Mount_Servo.h @@ -39,8 +39,10 @@ public: // returns true if this mount can control its pan (required for multicopters) bool has_pan_control() const override; - // send_mount_status - called to allow mounts to send their status to GCS using the MOUNT_STATUS message - void send_mount_status(mavlink_channel_t chan) override; +protected: + + // get attitude as a quaternion. returns true on success + bool get_attitude_quaternion(Quaternion& att_quat) override; private: diff --git a/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp b/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp index 7a41457391..89e9dbeb2c 100644 --- a/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp +++ b/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp @@ -115,15 +115,14 @@ void AP_Mount_SoloGimbal::set_mode(enum MAV_MOUNT_MODE mode) _mode = mode; } -// send_mount_status - called to allow mounts to send their status to GCS using the MOUNT_STATUS message -void AP_Mount_SoloGimbal::send_mount_status(mavlink_channel_t chan) +// get attitude as a quaternion. returns true on success +bool AP_Mount_SoloGimbal::get_attitude_quaternion(Quaternion& att_quat) { - if (_gimbal.aligned()) { - mavlink_msg_mount_status_send(chan, 0, 0, degrees(_angle_rad.roll)*100, degrees(_angle_rad.pitch)*100, degrees(get_bf_yaw_angle(_angle_rad))*100, _mode); + if (!_gimbal.aligned()) { + return false; } - - // block heartbeat from transmitting to the GCS - GCS_MAVLINK::disable_channel_routing(chan); + att_quat.from_euler(_angle_rad.roll, _angle_rad.pitch, get_bf_yaw_angle(_angle_rad)); + return true; } /* diff --git a/libraries/AP_Mount/AP_Mount_SoloGimbal.h b/libraries/AP_Mount/AP_Mount_SoloGimbal.h index c891256bdf..4b704d0cb2 100644 --- a/libraries/AP_Mount/AP_Mount_SoloGimbal.h +++ b/libraries/AP_Mount/AP_Mount_SoloGimbal.h @@ -35,9 +35,6 @@ public: // set_mode - sets mount's mode void set_mode(enum MAV_MOUNT_MODE mode) override; - // send_mount_status - called to allow mounts to send their status to GCS using the MOUNT_STATUS message - void send_mount_status(mavlink_channel_t chan) override; - // handle a GIMBAL_REPORT message void handle_gimbal_report(mavlink_channel_t chan, const mavlink_message_t &msg) override; void handle_gimbal_torque_report(mavlink_channel_t chan, const mavlink_message_t &msg); @@ -45,6 +42,14 @@ public: void update_fast() override; +protected: + + // returns true if heart beat should be suppressed for this gimbal + bool suppress_heartbeat() const override { return true; } + + // get attitude as a quaternion. returns true on success + bool get_attitude_quaternion(Quaternion& att_quat) override; + private: // internal variables bool _initialised; // true once the driver has been initialised