Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 07:13:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Mount: fix SiYi gimbal upside-down facing not working

Browse Source
This commit is contained in:
Asif Khan 2023-06-08 16:09:18 +05:30 committed by Randy Mackay
parent bd5355973d
commit caf35b71a6
2 changed files with 24 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
libraries/AP_Mount/AP_Mount_Siyi.cpp
View File

@ -391,22 +391,21 @@ void AP_Mount_Siyi::process_packet()
break;
case SiyiCommandId::ACQUIRE_GIMBAL_CONFIG_INFO: {
if (_parsed_msg.data_bytes_received != 5 && // ZR10 firmware version reply is 5 bytes
_parsed_msg.data_bytes_received != 7) { // A8 firmware version reply is 7 bytes
#if AP_MOUNT_SIYI_DEBUG
unexpected_len = true;
#endif
break;
// update gimbal's mounting direction
if (_parsed_msg.data_bytes_received > 5) {
_gimbal_mounting_dir = (_msg_buff[_msg_buff_data_start+5] == 2) ? GimbalMountingDirection::UPSIDE_DOWN : GimbalMountingDirection::NORMAL;
}
// update recording state and warn user of mismatch
const bool recording = _msg_buff[_msg_buff_data_start+3] > 0;
if (recording != _last_record_video) {
gcs().send_text(MAV_SEVERITY_INFO, "Siyi: recording %s", recording ? "ON" : "OFF");
}
_last_record_video = recording;
debug("GimConf hdr:%u rec:%u foll:%u", (unsigned)_msg_buff[_msg_buff_data_start+1],
(unsigned)_msg_buff[_msg_buff_data_start+3],
(unsigned)_msg_buff[_msg_buff_data_start+4]);
debug("GimConf hdr:%u rec:%u foll:%u mntdir:%u", (unsigned)_msg_buff[_msg_buff_data_start+1],
(unsigned)_msg_buff[_msg_buff_data_start+3],
(unsigned)_msg_buff[_msg_buff_data_start+4],
(unsigned)_msg_buff[_msg_buff_data_start+5]);
break;
}
@ -588,13 +587,20 @@ void AP_Mount_Siyi::send_target_angles(float pitch_rad, float yaw_rad, bool yaw_
return;
}
// if gimbal mounting direction is 2 i.e. upside down, then transform the angles
Vector3f current_angle_transformed = _current_angle_rad;
if (_gimbal_mounting_dir == GimbalMountingDirection::UPSIDE_DOWN) {
current_angle_transformed.y = -wrap_PI(_current_angle_rad.y + M_PI);
current_angle_transformed.z = -_current_angle_rad.z;
}
// use simple P controller to convert pitch angle error (in radians) to a target rate scalar (-100 to +100)
const float pitch_err_rad = (pitch_rad - _current_angle_rad.y);
const float pitch_err_rad = (pitch_rad - current_angle_transformed.y);
const float pitch_rate_scalar = constrain_float(100.0 * pitch_err_rad * AP_MOUNT_SIYI_PITCH_P / AP_MOUNT_SIYI_RATE_MAX_RADS, -100, 100);
// convert yaw angle to body-frame the use simple P controller to convert yaw angle error to a target rate scalar (-100 to +100)
const float yaw_bf_rad = yaw_is_ef ? wrap_PI(yaw_rad - AP::ahrs().yaw) : yaw_rad;
const float yaw_err_rad = (yaw_bf_rad - _current_angle_rad.z);
const float yaw_err_rad = (yaw_bf_rad - current_angle_transformed.z);
const float yaw_rate_scalar = constrain_float(100.0 * yaw_err_rad * AP_MOUNT_SIYI_YAW_P / AP_MOUNT_SIYI_RATE_MAX_RADS, -100, 100);
// rotate gimbal. pitch_rate and yaw_rate are scalars in the range -100 ~ +100

7
libraries/AP_Mount/AP_Mount_Siyi.h
View File

@ -136,6 +136,13 @@ private:
ZR10
} _hardware_model;
// gimbal mounting method/direction
enum class GimbalMountingDirection : uint8_t {
UNDEFINED = 0,
NORMAL = 1,
UPSIDE_DOWN = 2,
} _gimbal_mounting_dir;
// reading incoming packets from gimbal and confirm they are of the correct format
// results are held in the _parsed_msg structure
void read_incoming_packets();