aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_VisualOdom: fix handle_vision_position_delta_msg

Browse Source 
position and angle deltas were swapped when passed into the EKF
time passed into the EKF was scaled as usec when it should be sec
This commit is contained in:
Randy Mackay 2020-04-08 09:33:35 +09:00
parent c18fce6714
commit dc2e0f328b
1 changed files with 4 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp
View File

@ -47,15 +47,16 @@ void AP_VisualOdom_MAV::handle_vision_position_delta_msg(const mavlink_message_t
_last_update_ms = now_ms;
// send to EKF
const float time_delta_sec = packet.time_delta_usec / 1000000.0f;
AP::ahrs_navekf().writeBodyFrameOdom(packet.confidence,
angle_delta,
position_delta,
packet.time_delta_usec,
angle_delta,
time_delta_sec,
now_ms,
_frontend.get_pos_offset());
// log sensor data
AP::logger().Write_VisualOdom(packet.time_delta_usec / 1000000.0f,
AP::logger().Write_VisualOdom(time_delta_sec,
angle_delta,
position_delta,
packet.confidence);