Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 09:58:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_AHRS: use SIM_ODOM_ENABLE to enable visual odom in SITL

Browse Source
This commit is contained in:
Andrew Tridgell 2017-05-01 10:09:57 +10:00
parent 67404e9660
commit 28030dd85c
1 changed files with 22 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -300,25 +300,28 @@ void AP_AHRS_NavEKF::update_SITL(void)
_accel_ef_ekf_blended = _accel_ef_ekf[0];
}
// use SITL states to write body frame odometry data at 20Hz
uint32_t timeStamp_ms = AP_HAL::millis();
if (timeStamp_ms - _last_body_odm_update_ms > 50) {
const float quality = 100.0f;
const Vector3f posOffset = Vector3f(0.0f,0.0f,0.0f);
float delTime = 0.001f*(timeStamp_ms - _last_body_odm_update_ms);
_last_body_odm_update_ms = timeStamp_ms;
timeStamp_ms -= (timeStamp_ms - _last_body_odm_update_ms)/2; // correct for first order hold average delay
Vector3f delAng = Vector3f(radians(fdm.rollRate),
radians(fdm.pitchRate),
radians(fdm.yawRate));
delAng *= delTime;
// rotate earth velocity into body frame and calculate delta position
Matrix3f Tbn;
Tbn.from_euler(radians(fdm.rollDeg),radians(fdm.pitchDeg),radians(fdm.yawDeg));
Vector3f earth_vel = Vector3f(fdm.speedN,fdm.speedE,fdm.speedD);
Vector3f delPos = Tbn.transposed() * (earth_vel * delTime);
// write to EKF
EKF3.writeBodyFrameOdom(quality, delPos, delAng, delTime, timeStamp_ms, posOffset);
if (_sitl->odom_enable) {
// use SITL states to write body frame odometry data at 20Hz
uint32_t timeStamp_ms = AP_HAL::millis();
if (timeStamp_ms - _last_body_odm_update_ms > 50) {
const float quality = 100.0f;
const Vector3f posOffset = Vector3f(0.0f,0.0f,0.0f);
float delTime = 0.001f*(timeStamp_ms - _last_body_odm_update_ms);
_last_body_odm_update_ms = timeStamp_ms;
timeStamp_ms -= (timeStamp_ms - _last_body_odm_update_ms)/2; // correct for first order hold average delay
Vector3f delAng = Vector3f(radians(fdm.rollRate),
radians(fdm.pitchRate),
radians(fdm.yawRate));
delAng *= delTime;
// rotate earth velocity into body frame and calculate delta position
Matrix3f Tbn;
Tbn.from_euler(radians(fdm.rollDeg),radians(fdm.pitchDeg),radians(fdm.yawDeg));
Vector3f earth_vel = Vector3f(fdm.speedN,fdm.speedE,fdm.speedD);
Vector3f delPos = Tbn.transposed() * (earth_vel * delTime);
// write to EKF
EKF3.writeBodyFrameOdom(quality, delPos, delAng, delTime, timeStamp_ms, posOffset);
}
}
}
#endif // CONFIG_HAL_BOARD