mirror of https://github.com/ArduPilot/ardupilot
AP_VisualOdom: remove sensor position from call to ahrs writeExtNavData
This commit is contained in:
parent
79afc70cdb
commit
2164293cc0
|
@ -44,7 +44,7 @@ void AP_VisualOdom_IntelT265::handle_vision_position_estimate(uint64_t remote_ti
|
||||||
// send attitude and position to EKF
|
// send attitude and position to EKF
|
||||||
const float posErr = 0; // parameter required?
|
const float posErr = 0; // parameter required?
|
||||||
const float angErr = 0; // parameter required?
|
const float angErr = 0; // parameter required?
|
||||||
AP::ahrs().writeExtNavData(_frontend.get_pos_offset(), pos, att, posErr, angErr, time_ms, get_reset_timestamp_ms(reset_counter));
|
AP::ahrs().writeExtNavData(pos, att, posErr, angErr, time_ms, get_reset_timestamp_ms(reset_counter));
|
||||||
|
|
||||||
// calculate euler orientation for logging
|
// calculate euler orientation for logging
|
||||||
float roll;
|
float roll;
|
||||||
|
|
|
@ -71,7 +71,7 @@ void AP_VisualOdom_MAV::handle_vision_position_estimate(uint64_t remote_time_us,
|
||||||
// send attitude and position to EKF
|
// send attitude and position to EKF
|
||||||
const float posErr = 0; // parameter required?
|
const float posErr = 0; // parameter required?
|
||||||
const float angErr = 0; // parameter required?
|
const float angErr = 0; // parameter required?
|
||||||
AP::ahrs().writeExtNavData(_frontend.get_pos_offset(), pos, attitude, posErr, angErr, time_ms, get_reset_timestamp_ms(reset_counter));
|
AP::ahrs().writeExtNavData(pos, attitude, posErr, angErr, time_ms, get_reset_timestamp_ms(reset_counter));
|
||||||
|
|
||||||
// calculate euler orientation for logging
|
// calculate euler orientation for logging
|
||||||
float roll;
|
float roll;
|
||||||
|
|
Loading…
Reference in New Issue