Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-26 18:48:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_AHRS: simulate body frame odometry sensor

Browse Source
This commit is contained in:
priseborough 2017-03-22 09:38:50 +11:00 committed by Randy Mackay
parent 3b82f60b08
commit 90cd46f4d7
2 changed files with 28 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -261,10 +261,14 @@ void AP_AHRS_NavEKF::update_SITL(void)
{
if (_sitl == nullptr) {
_sitl = (SITL::SITL *)AP_Param::find_object("SIM_");
if (_sitl == nullptr) {
return;
}
}
if (_sitl && active_EKF_type() == EKF_TYPE_SITL) {
const struct SITL::sitl_fdm &fdm = _sitl->state;
const struct SITL::sitl_fdm &fdm = _sitl->state;
if (active_EKF_type() == EKF_TYPE_SITL) {
roll = radians(fdm.rollDeg);
pitch = radians(fdm.pitchDeg);
yaw = radians(fdm.yawDeg);
@ -286,6 +290,27 @@ void AP_AHRS_NavEKF::update_SITL(void)
fdm.zAccel);
}
_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);
}
}
#endif // CONFIG_HAL_BOARD

1
libraries/AP_AHRS/AP_AHRS_NavEKF.h
View File

@ -283,6 +283,7 @@ private:
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
SITL::SITL *_sitl;
uint32_t _last_body_odm_update_ms = 0;
void update_SITL(void);
#endif
};