Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Mount: added gimbal rates for SIYI

Browse Source 
this helps diagnose issues with gyro bias versus control when camera
is spinning
This commit is contained in:
Andrew Tridgell 2023-11-16 09:36:30 +11:00
parent 7472f76336
commit 8ff2fa4fd9
4 changed files with 19 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_Mount/AP_Mount_Backend.cpp
View File

@ -142,6 +142,8 @@ void AP_Mount_Backend::send_gimbal_device_attitude_status(mavlink_channel_t chan
if (!get_attitude_quaternion(att_quat)) { if (!get_attitude_quaternion(att_quat)) {
return; return;
} }
Vector3f ang_velocity { nanf(""), nanf(""), nanf("") };
IGNORE_RETURN(get_angular_velocity(ang_velocity));
// construct quaternion array // construct quaternion array
const float quat_array[4] = {att_quat.q1, att_quat.q2, att_quat.q3, att_quat.q4}; 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 AP_HAL::millis(), // autopilot system time
get_gimbal_device_flags(), get_gimbal_device_flags(),
quat_array, // attitude expressed as quaternion quat_array, // attitude expressed as quaternion
std::numeric_limits<double>::quiet_NaN(), // roll axis angular velocity (NaN for unknown) ang_velocity.x, // roll axis angular velocity (NaN for unknown)
std::numeric_limits<double>::quiet_NaN(), // pitch axis angular velocity (NaN for unknown) ang_velocity.y, // pitch axis angular velocity (NaN for unknown)
std::numeric_limits<double>::quiet_NaN(), // yaw axis angular velocity (NaN for unknown) ang_velocity.z, // yaw axis angular velocity (NaN for unknown)
0, // failure flags (not supported) 0, // failure flags (not supported)
std::numeric_limits<double>::quiet_NaN(), // delta_yaw (NaN for unknonw) std::numeric_limits<double>::quiet_NaN(), // delta_yaw (NaN for unknonw)
std::numeric_limits<double>::quiet_NaN(), // delta_yaw_velocity (NaN for unknonw) std::numeric_limits<double>::quiet_NaN(), // delta_yaw_velocity (NaN for unknonw)

3
libraries/AP_Mount/AP_Mount_Backend.h
View File

@ -67,6 +67,9 @@ public:
// yaw is in body-frame. // yaw is in body-frame.
virtual bool get_attitude_quaternion(Quaternion& att_quat) = 0; 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 // get mount's mode
enum MAV_MOUNT_MODE get_mode() const { return _mode; } enum MAV_MOUNT_MODE get_mode() const { return _mode; }

8
libraries/AP_Mount/AP_Mount_Siyi.cpp
View File

@ -99,7 +99,7 @@ void AP_Mount_Siyi::update()
_last_attitude_send_ms = now_ms; _last_attitude_send_ms = now_ms;
send_attitude(); send_attitude();
} }
// run zoom control // run zoom control
update_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.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.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 _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 _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
//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 _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
//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.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; break;
} }

7
libraries/AP_Mount/AP_Mount_Siyi.h
View File

@ -93,6 +93,12 @@ protected:
// get attitude as a quaternion. returns true on success // get attitude as a quaternion. returns true on success
bool get_attitude_quaternion(Quaternion& att_quat) override; 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: private:
// serial protocol command ids // serial protocol command ids
@ -309,6 +315,7 @@ private:
// actual attitude received from gimbal // actual attitude received from gimbal
Vector3f _current_angle_rad; // current angles in radians received from gimbal (x=roll, y=pitch, z=yaw) 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_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 uint32_t _last_req_current_angle_rad_ms; // system time that this driver last requested current angle