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: align sensor displays yaw shift in 0 to 360 range

Browse Source
This commit is contained in:
Randy Mackay 2020-04-09 17:43:47 +09:00
parent 9fd39f3768
commit 1ba036169f
1 changed files with 4 additions and 4 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

@ -53,7 +53,7 @@ void AP_VisualOdom_IntelT265::handle_vision_position_estimate(uint64_t remote_ti
att.to_euler(roll, pitch, yaw); att.to_euler(roll, pitch, yaw);
// log sensor data // log sensor data
AP::logger().Write_VisualPosition(remote_time_us, time_ms, x, y, z, degrees(roll), degrees(pitch), degrees(yaw)); AP::logger().Write_VisualPosition(remote_time_us, time_ms, x, y, z, degrees(roll), degrees(pitch), wrap_360(degrees(yaw)));
// store corrected attitude for use in pre-arm checks // store corrected attitude for use in pre-arm checks
_attitude_last = att; _attitude_last = att;
@ -122,11 +122,11 @@ bool AP_VisualOdom_IntelT265::align_sensor_to_vehicle(const Vector3f &position,
// trim yaw by difference between ahrs and sensor yaw // trim yaw by difference between ahrs and sensor yaw
Vector3f angle_diff; Vector3f angle_diff;
ahrs_quat.angular_difference(att_corrected).to_axis_angle(angle_diff); ahrs_quat.angular_difference(att_corrected).to_axis_angle(angle_diff);
const float yaw_trim_orig = _yaw_trim;
_yaw_trim = angle_diff.z; _yaw_trim = angle_diff.z;
gcs().send_text(MAV_SEVERITY_CRITICAL, "VisualOdom: yaw shifted %d to %d deg", gcs().send_text(MAV_SEVERITY_CRITICAL, "VisualOdom: yaw shifted %d to %d deg",
//(int)degrees(_yaw_trim - yaw_trim_orig), (int)degrees(_yaw_trim - yaw_trim_orig),
(int)degrees(_yaw_trim), (int)wrap_360(degrees(sens_yaw + _yaw_trim)));
(int)degrees(sens_yaw + _yaw_trim));
// convert _yaw_trim to _yaw_rotation to speed up processing later // convert _yaw_trim to _yaw_rotation to speed up processing later
_yaw_rotation.from_euler(0.0f, 0.0f, _yaw_trim); _yaw_rotation.from_euler(0.0f, 0.0f, _yaw_trim);