Browse Source

AP_DAL: avoid logging unused IMU data

zr-v5.1
Andrew Tridgell 4 years ago
parent
commit
fabac94952
  1. 8
      libraries/AP_DAL/AP_DAL_InertialSensor.cpp

8
libraries/AP_DAL/AP_DAL_InertialSensor.cpp

@ -29,15 +29,19 @@ void AP_DAL_InertialSensor::start_frame() @@ -29,15 +29,19 @@ void AP_DAL_InertialSensor::start_frame()
log_RISI &RISI = _RISI[i];
const log_RISI old_RISI = RISI;
// accel stuff
// accel data
RISI.use_accel = ins.use_accel(i);
if (RISI.use_accel) {
RISI.get_delta_velocity_ret = ins.get_delta_velocity(i, RISI.delta_velocity);
RISI.delta_velocity_dt = ins.get_delta_velocity_dt(i);
}
// gryo stuff
// gryo data
RISI.use_gyro = ins.use_gyro(i);
if (RISI.use_gyro) {
RISI.delta_angle_dt = ins.get_delta_angle_dt(i);
RISI.get_delta_angle_ret = ins.get_delta_angle(i, RISI.delta_angle);
}
update_filtered(i);

Loading…
Cancel
Save