diff --git a/libraries/AP_Mount/AP_Mount_Siyi.cpp b/libraries/AP_Mount/AP_Mount_Siyi.cpp index 5244dd64e7c49..9984b20c9ac8e 100644 --- a/libraries/AP_Mount/AP_Mount_Siyi.cpp +++ b/libraries/AP_Mount/AP_Mount_Siyi.cpp @@ -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 diff --git a/libraries/AP_Mount/AP_Mount_Siyi.h b/libraries/AP_Mount/AP_Mount_Siyi.h index b388f72a8b471..7855942bcd916 100644 --- a/libraries/AP_Mount/AP_Mount_Siyi.h +++ b/libraries/AP_Mount/AP_Mount_Siyi.h @@ -136,6 +136,13 @@ class AP_Mount_Siyi : public AP_Mount_Backend 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();