Browse Source

AP_NavEKF: rely on delta_velocity and delta_angles always being available

master
Andrew Tridgell 10 years ago
parent
commit
c9988d8b37
  1. 12
      libraries/AP_NavEKF/AP_NavEKF.cpp

12
libraries/AP_NavEKF/AP_NavEKF.cpp

@ -4037,12 +4037,8 @@ bool NavEKF::readDeltaVelocity(uint8_t ins_index, Vector3f &dVel, float &dVel_dt @@ -4037,12 +4037,8 @@ bool NavEKF::readDeltaVelocity(uint8_t ins_index, Vector3f &dVel, float &dVel_dt
const AP_InertialSensor &ins = _ahrs->get_ins();
if (ins_index < ins.get_accel_count()) {
if (ins.get_delta_velocity(ins_index,dVel)) {
dVel_dt = ins.get_delta_velocity_dt(ins_index);
} else {
dVel = ins.get_accel(ins_index) * dtIMUactual;
dVel_dt = dtIMUactual;
}
ins.get_delta_velocity(ins_index,dVel);
dVel_dt = ins.get_delta_velocity_dt(ins_index);
return true;
}
return false;
@ -4052,9 +4048,7 @@ bool NavEKF::readDeltaAngle(uint8_t ins_index, Vector3f &dAng) { @@ -4052,9 +4048,7 @@ bool NavEKF::readDeltaAngle(uint8_t ins_index, Vector3f &dAng) {
const AP_InertialSensor &ins = _ahrs->get_ins();
if (ins_index < ins.get_gyro_count()) {
if (!ins.get_delta_angle(ins_index,dAng)) {
dAng = ins.get_gyro(ins_index) * dtIMUactual;
}
ins.get_delta_angle(ins_index,dAng);
return true;
}
return false;

Loading…
Cancel
Save