diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index d3cc0cb490..f05e7430eb 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -142,6 +142,8 @@ void AP_Mount_Backend::send_gimbal_device_attitude_status(mavlink_channel_t chan if (!get_attitude_quaternion(att_quat)) { return; } + Vector3f ang_velocity { nanf(""), nanf(""), nanf("") }; + IGNORE_RETURN(get_angular_velocity(ang_velocity)); // construct quaternion array const float quat_array[4] = {att_quat.q1, att_quat.q2, att_quat.q3, att_quat.q4}; @@ -152,9 +154,9 @@ void AP_Mount_Backend::send_gimbal_device_attitude_status(mavlink_channel_t chan 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) + ang_velocity.x, // roll axis angular velocity (NaN for unknown) + ang_velocity.y, // pitch axis angular velocity (NaN for unknown) + ang_velocity.z, // yaw axis angular velocity (NaN for unknown) 0, // failure flags (not supported) std::numeric_limits::quiet_NaN(), // delta_yaw (NaN for unknonw) std::numeric_limits::quiet_NaN(), // delta_yaw_velocity (NaN for unknonw) diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index 0503d02ae6..1c873e48e6 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -67,6 +67,9 @@ public: // yaw is in body-frame. virtual bool get_attitude_quaternion(Quaternion& att_quat) = 0; + // get angular velocity of mount. Only available on some backends + virtual bool get_angular_velocity(Vector3f& rates) { return false; } + // get mount's mode enum MAV_MOUNT_MODE get_mode() const { return _mode; } diff --git a/libraries/AP_Mount/AP_Mount_Siyi.cpp b/libraries/AP_Mount/AP_Mount_Siyi.cpp index bbe829d476..0cdc907f9d 100644 --- a/libraries/AP_Mount/AP_Mount_Siyi.cpp +++ b/libraries/AP_Mount/AP_Mount_Siyi.cpp @@ -99,7 +99,7 @@ void AP_Mount_Siyi::update() _last_attitude_send_ms = now_ms; send_attitude(); } - + // run zoom control update_zoom_control(); @@ -540,9 +540,9 @@ void AP_Mount_Siyi::process_packet() _current_angle_rad.z = -radians((int16_t)UINT16_VALUE(_msg_buff[_msg_buff_data_start+1], _msg_buff[_msg_buff_data_start]) * 0.1); // yaw angle _current_angle_rad.y = radians((int16_t)UINT16_VALUE(_msg_buff[_msg_buff_data_start+3], _msg_buff[_msg_buff_data_start+2]) * 0.1); // pitch angle _current_angle_rad.x = radians((int16_t)UINT16_VALUE(_msg_buff[_msg_buff_data_start+5], _msg_buff[_msg_buff_data_start+4]) * 0.1); // roll angle - //const float yaw_rate_degs = -(int16_t)UINT16_VALUE(_msg_buff[_msg_buff_data_start+7], _msg_buff[_msg_buff_data_start+6]) * 0.1; // yaw rate - //const float pitch_rate_deg = (int16_t)UINT16_VALUE(_msg_buff[_msg_buff_data_start+9], _msg_buff[_msg_buff_data_start+8]) * 0.1; // pitch rate - //const float roll_rate_deg = (int16_t)UINT16_VALUE(_msg_buff[_msg_buff_data_start+11], _msg_buff[_msg_buff_data_start+10]) * 0.1; // roll rate + _current_rates_rads.z = -radians((int16_t)UINT16_VALUE(_msg_buff[_msg_buff_data_start+7], _msg_buff[_msg_buff_data_start+6]) * 0.1); // yaw rate + _current_rates_rads.y = radians((int16_t)UINT16_VALUE(_msg_buff[_msg_buff_data_start+9], _msg_buff[_msg_buff_data_start+8]) * 0.1); // pitch rate + _current_rates_rads.x = radians((int16_t)UINT16_VALUE(_msg_buff[_msg_buff_data_start+11], _msg_buff[_msg_buff_data_start+10]) * 0.1); // roll rate break; } diff --git a/libraries/AP_Mount/AP_Mount_Siyi.h b/libraries/AP_Mount/AP_Mount_Siyi.h index 54f4d2b909..2245e4ca0d 100644 --- a/libraries/AP_Mount/AP_Mount_Siyi.h +++ b/libraries/AP_Mount/AP_Mount_Siyi.h @@ -93,6 +93,12 @@ protected: // get attitude as a quaternion. returns true on success bool get_attitude_quaternion(Quaternion& att_quat) override; + // get angular velocity of mount. Only available on some backends + bool get_angular_velocity(Vector3f& rates) override { + rates = _current_rates_rads; + return true; + } + private: // serial protocol command ids @@ -309,6 +315,7 @@ private: // actual attitude received from gimbal Vector3f _current_angle_rad; // current angles in radians received from gimbal (x=roll, y=pitch, z=yaw) + Vector3f _current_rates_rads; // current angular rates in rad/s (x=roll, y=pitch, z=yaw) uint32_t _last_current_angle_rad_ms; // system time _current_angle_rad was updated uint32_t _last_req_current_angle_rad_ms; // system time that this driver last requested current angle