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

AP_VisualOdom: pre_arm check fix for camera attitude

Browse Source
This commit is contained in:
Randy Mackay 2021-06-02 14:44:41 +09:00
parent ab5d4da776
commit f8d39e65ec
1 changed files with 3 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp
View File

@ -237,18 +237,16 @@ bool AP_VisualOdom_IntelT265::pre_arm_check(char *failure_msg, uint8_t failure_m
return false; return false;
} }
// get angular difference between AHRS and camera attitude
Vector3f angle_diff;
_attitude_last.angular_difference(ahrs_quat).to_axis_angle(angle_diff);
// check if roll and pitch is different by > 10deg (using NED so cannot determine whether roll or pitch specifically) // check if roll and pitch is different by > 10deg (using NED so cannot determine whether roll or pitch specifically)
const float rp_diff_deg = degrees(safe_sqrt(sq(angle_diff.x)+sq(angle_diff.y))); const float rp_diff_deg = degrees(ahrs_quat.roll_pitch_difference(_attitude_last));
if (rp_diff_deg > 10.0f) { if (rp_diff_deg > 10.0f) {
hal.util->snprintf(failure_msg, failure_msg_len, "roll/pitch diff %4.1f deg (>10)",(double)rp_diff_deg); hal.util->snprintf(failure_msg, failure_msg_len, "roll/pitch diff %4.1f deg (>10)",(double)rp_diff_deg);
return false; return false;
} }
// check if yaw is different by > 10deg // check if yaw is different by > 10deg
Vector3f angle_diff;
ahrs_quat.angular_difference(_attitude_last).to_axis_angle(angle_diff);
const float yaw_diff_deg = degrees(fabsf(angle_diff.z)); const float yaw_diff_deg = degrees(fabsf(angle_diff.z));
if (yaw_diff_deg > 10.0f) { if (yaw_diff_deg > 10.0f) {
hal.util->snprintf(failure_msg, failure_msg_len, "yaw diff %4.1f deg (>10)",(double)yaw_diff_deg); hal.util->snprintf(failure_msg, failure_msg_len, "yaw diff %4.1f deg (>10)",(double)yaw_diff_deg);