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

AP_InertialSensor: support fast sampling in SITL

Browse Source 
this allows testing of fast sample logs for FFT
This commit is contained in:
Andrew Tridgell 2019-01-08 07:15:02 +11:00
parent 8fe39e8784
commit a84af85689
1 changed files with 15 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp
View File

@ -42,6 +42,12 @@ bool AP_InertialSensor_SITL::init_sensor(void)
AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SITL, i, 1, DEVTYPE_SITL));
accel_instance[i] = _imu.register_accel(accel_sample_hz[i],
AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SITL, i, 2, DEVTYPE_SITL));
if (enable_fast_sampling(accel_instance[i])) {
_set_accel_raw_sample_rate(accel_instance[i], accel_sample_hz[i]*4);
}
if (enable_fast_sampling(gyro_instance[i])) {
_set_gyro_raw_sample_rate(gyro_instance[i], gyro_sample_hz[i]*8);
}
}
hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_InertialSensor_SITL::timer_update, void));
@ -109,8 +115,11 @@ void AP_InertialSensor_SITL::generate_accel(uint8_t instance)
Vector3f accel = Vector3f(xAccel, yAccel, zAccel);
_rotate_and_correct_accel(accel_instance[instance], accel);
_notify_new_accel_raw_sample(accel_instance[instance], accel, AP_HAL::micros64());
uint8_t nsamples = enable_fast_sampling(accel_instance[instance])?4:1;
for (uint8_t i=0; i<nsamples; i++) {
_notify_new_accel_raw_sample(accel_instance[instance], accel);
}
}
/*
@ -152,7 +161,10 @@ void AP_InertialSensor_SITL::generate_gyro(uint8_t instance)
_rotate_and_correct_gyro(gyro_instance[instance], gyro);
_notify_new_gyro_raw_sample(gyro_instance[instance], gyro, AP_HAL::micros64());
uint8_t nsamples = enable_fast_sampling(gyro_instance[instance])?8:1;
for (uint8_t i=0; i<nsamples; i++) {
_notify_new_gyro_raw_sample(gyro_instance[instance], gyro);
}
}
void AP_InertialSensor_SITL::timer_update(void)