zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Hack to fix external vision pos offset compensation

master
devbharat 9 years ago committed by Paul Riseborough
parent
d3bad9fdb0
commit
d16f413b55
1 changed files with 4 additions and 3 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 7
      EKF/ekf.cpp

7
EKF/ekf.cpp
Unescape View File

@ -313,9 +313,10 @@ bool Ekf::update()
// correct position and height for offset relative to IMU // correct position and height for offset relative to IMU
Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body; Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
_ev_sample_delayed.posNED(0) -= pos_offset_earth(0); // warnx("pos_offset_earth %lf %lf %lf", (double)pos_offset_earth(0)*100, (double)pos_offset_earth(1)*100, (double)pos_offset_earth(2)*100);
_ev_sample_delayed.posNED(1) -= pos_offset_earth(1); _ev_sample_delayed.posNED(0) += pos_offset_earth(1); // XXX FIX IT Hack After "_R_to_earth * pos_offset_body" x and y offset seem to be switched!
_ev_sample_delayed.posNED(2) -= pos_offset_earth(2); // + or - ? _ev_sample_delayed.posNED(1) -= pos_offset_earth(0); // XXX FIX IT Hack After "_R_to_earth * pos_offset_body" x and y offset seem to be switched!
_ev_sample_delayed.posNED(2) -= pos_offset_earth(2);
} }
// use external vision yaw observation // use external vision yaw observation
if (_control_status.flags.ev_yaw) { if (_control_status.flags.ev_yaw) {

Write Preview
Loading…
Cancel
Save