diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 3bf2ce3a30..18dd8967df 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -656,6 +656,7 @@ AP_InertialSensor_Backend *AP_InertialSensor::_find_backend(int16_t backend_id, } bool AP_InertialSensor::set_gyro_window_size(uint16_t size) { +#if HAL_WITH_DSP _gyro_window_size = size; // allocate FFT gyro window @@ -676,7 +677,7 @@ bool AP_InertialSensor::set_gyro_window_size(uint16_t size) { } } } - +#endif return true; } @@ -797,7 +798,8 @@ AP_InertialSensor::detect_backends(void) // IMUs defined by IMU lines in hwdef.dat HAL_INS_PROBE_LIST; #elif CONFIG_HAL_BOARD == HAL_BOARD_SITL - ADD_BACKEND(AP_InertialSensor_SITL::detect(*this)); + ADD_BACKEND(AP_InertialSensor_SITL::detect(*this, INS_SITL_SENSOR_A)); + ADD_BACKEND(AP_InertialSensor_SITL::detect(*this, INS_SITL_SENSOR_B)); #elif HAL_INS_DEFAULT == HAL_INS_HIL ADD_BACKEND(AP_InertialSensor_HIL::detect(*this)); #elif AP_FEATURE_BOARD_DETECT diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 4446000b8e..db25f3d988 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -153,6 +153,7 @@ public: uint16_t get_accel_rate_hz(uint8_t instance) const { return uint16_t(_accel_raw_sample_rates[instance] * _accel_over_sampling[instance]); } // FFT support access +#if HAL_WITH_DSP const Vector3f &get_raw_gyro(void) const { return _gyro_raw[_primary_gyro]; } GyroWindow get_raw_gyro_window(uint8_t instance, uint8_t axis) const { return _gyro_window[instance][axis]; } GyroWindow get_raw_gyro_window(uint8_t axis) const { return get_raw_gyro_window(_primary_gyro, axis); } @@ -161,8 +162,8 @@ public: uint16_t get_raw_gyro_window_index(uint8_t instance) const { return _circular_buffer_idx[instance]; } uint16_t get_raw_gyro_rate_hz() const { return get_raw_gyro_rate_hz(_primary_gyro); } uint16_t get_raw_gyro_rate_hz(uint8_t instance) const { return _gyro_raw_sample_rates[_primary_gyro]; } +#endif bool set_gyro_window_size(uint16_t size); - // get accel offsets in m/s/s const Vector3f &get_accel_offsets(uint8_t i) const { return _accel_offset[i]; } const Vector3f &get_accel_offsets(void) const { return get_accel_offsets(_primary_accel); } @@ -441,13 +442,14 @@ private: LowPassFilter2pVector3f _gyro_filter[INS_MAX_INSTANCES]; Vector3f _accel_filtered[INS_MAX_INSTANCES]; Vector3f _gyro_filtered[INS_MAX_INSTANCES]; +#if HAL_WITH_DSP // Thread-safe public version of _last_raw_gyro Vector3f _gyro_raw[INS_MAX_INSTANCES]; // circular buffer of gyro data for frequency analysis uint16_t _circular_buffer_idx[INS_MAX_INSTANCES]; GyroWindow _gyro_window[INS_MAX_INSTANCES][XYZ_AXIS_COUNT]; uint16_t _gyro_window_size; - +#endif bool _new_accel_data[INS_MAX_INSTANCES]; bool _new_gyro_data[INS_MAX_INSTANCES]; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp index 25288015c5..f8edf4b1dd 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp @@ -225,11 +225,11 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance, // save previous delta angle for coning correction _imu._last_delta_angle[instance] = delta_angle; _imu._last_raw_gyro[instance] = gyro; - +#if HAL_WITH_DSP // capture gyro window for FFT analysis _last_gyro_window[_num_gyro_samples++] = gyro * _imu._gyro_raw_sampling_multiplier[instance]; _num_gyro_samples = _num_gyro_samples % INS_MAX_GYRO_WINDOW_SAMPLES; // protect against overrun - +#endif // apply the low pass filter Vector3f gyro_filtered = _imu._gyro_filter[instance].apply(gyro); @@ -514,6 +514,7 @@ void AP_InertialSensor_Backend::update_gyro(uint8_t instance) if (_imu._new_gyro_data[instance]) { _publish_gyro(instance, _imu._gyro_filtered[instance]); // copy the gyro samples from the backend to the frontend window +#if HAL_WITH_DSP if (_imu._gyro_window_size > 0) { uint8_t idx = _imu._circular_buffer_idx[instance]; for (uint8_t i = 0; i < _num_gyro_samples; i++) { @@ -526,6 +527,7 @@ void AP_InertialSensor_Backend::update_gyro(uint8_t instance) _imu._circular_buffer_idx[instance] = idx; } _imu._gyro_raw[instance] = _imu._last_raw_gyro[instance] * _imu._gyro_raw_sampling_multiplier[instance]; +#endif _imu._new_gyro_data[instance] = false; } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp index 4ca1a31a17..782dec087c 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp @@ -7,17 +7,19 @@ const extern AP_HAL::HAL& hal; -AP_InertialSensor_SITL::AP_InertialSensor_SITL(AP_InertialSensor &imu) : - AP_InertialSensor_Backend(imu) +AP_InertialSensor_SITL::AP_InertialSensor_SITL(AP_InertialSensor &imu, const uint16_t sample_rates[]) : + AP_InertialSensor_Backend(imu), + gyro_sample_hz(sample_rates[0]), + accel_sample_hz(sample_rates[1]) { } /* detect the sensor */ -AP_InertialSensor_Backend *AP_InertialSensor_SITL::detect(AP_InertialSensor &_imu) +AP_InertialSensor_Backend *AP_InertialSensor_SITL::detect(AP_InertialSensor &_imu, const uint16_t sample_rates[]) { - AP_InertialSensor_SITL *sensor = new AP_InertialSensor_SITL(_imu); + AP_InertialSensor_SITL *sensor = new AP_InertialSensor_SITL(_imu, sample_rates); if (sensor == nullptr) { return nullptr; } @@ -34,24 +36,6 @@ bool AP_InertialSensor_SITL::init_sensor(void) if (sitl == nullptr) { return false; } - - // grab the used instances - for (uint8_t i=0; iregister_timer_process(FUNCTOR_BIND_MEMBER(&AP_InertialSensor_SITL::timer_update, void)); - return true; } @@ -63,14 +47,14 @@ static float calculate_noise(float noise, float noise_variation) { /* generate an accelerometer sample */ -void AP_InertialSensor_SITL::generate_accel(uint8_t instance) +void AP_InertialSensor_SITL::generate_accel() { Vector3f accel_accum; - uint8_t nsamples = enable_fast_sampling(accel_instance[instance]) ? 4 : 1; + uint8_t nsamples = enable_fast_sampling(accel_instance) ? 4 : 1; for (uint8_t j = 0; j < nsamples; j++) { // add accel bias and noise - Vector3f accel_bias = instance == 0 ? sitl->accel_bias.get() : sitl->accel2_bias.get(); + Vector3f accel_bias = accel_instance == 0 ? sitl->accel_bias.get() : sitl->accel2_bias.get(); float xAccel = sitl->state.xAccel + accel_bias.x; float yAccel = sitl->state.yAccel + accel_bias.y; float zAccel = sitl->state.zAccel + accel_bias.z; @@ -78,9 +62,9 @@ void AP_InertialSensor_SITL::generate_accel(uint8_t instance) // minimum noise levels are 2 bits, but averaged over many // samples, giving around 0.01 m/s/s float accel_noise = 0.01f; - float noise_variation = 0.05; + float noise_variation = 0.05f; // this smears the individual motor peaks somewhat emulating physical motors - float freq_variation = 0.12; + float freq_variation = 0.12f; xAccel += accel_noise * rand_float(); yAccel += accel_noise * rand_float(); @@ -91,24 +75,24 @@ void AP_InertialSensor_SITL::generate_accel(uint8_t instance) // giving a accel noise variation of 5.33 m/s/s over the full throttle range if (motors_on) { // add extra noise when the motors are on - accel_noise = (instance == 0 ? sitl->accel_noise : sitl->accel2_noise) * sitl->throttle; + accel_noise = (accel_instance == 0 ? sitl->accel_noise : sitl->accel2_noise) * sitl->throttle; } // VIB_FREQ is a static vibration applied to each axis const Vector3f &vibe_freq = sitl->vibe_freq; if (!vibe_freq.is_zero() && motors_on) { - float t = AP_HAL::micros() * 1.0e-6f; - xAccel += sinf(t * 2 * M_PI * vibe_freq.x) * calculate_noise(accel_noise, noise_variation); - yAccel += sinf(t * 2 * M_PI * vibe_freq.y) * calculate_noise(accel_noise, noise_variation); - zAccel += sinf(t * 2 * M_PI * vibe_freq.z) * calculate_noise(accel_noise, noise_variation); + xAccel += sinf(accel_time * 2 * M_PI * vibe_freq.x) * calculate_noise(accel_noise, noise_variation); + yAccel += sinf(accel_time * 2 * M_PI * vibe_freq.y) * calculate_noise(accel_noise, noise_variation); + zAccel += sinf(accel_time * 2 * M_PI * vibe_freq.z) * calculate_noise(accel_noise, noise_variation); + accel_time += 1.0f / (accel_sample_hz * nsamples); } // VIB_MOT_MAX is a rpm-scaled vibration applied to each axis if (!is_zero(sitl->vibe_motor) && motors_on) { for (uint8_t i = 0; i < sitl->state.num_motors; i++) { - float &phase = accel_motor_phase[instance][i]; + float &phase = accel_motor_phase[i]; float motor_freq = calculate_noise(sitl->state.rpm[i] / 60.0f, freq_variation); - float phase_incr = motor_freq * 2 * M_PI / (accel_sample_hz[instance] * nsamples); + float phase_incr = motor_freq * 2 * M_PI / (accel_sample_hz * nsamples); phase += phase_incr; if (phase_incr > M_PI) { phase -= 2 * M_PI; @@ -150,25 +134,26 @@ 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_sensor_rate_sample(instance, accel); + _notify_new_accel_sensor_rate_sample(accel_instance, accel); accel_accum += accel; } accel_accum /= nsamples; - _notify_new_accel_raw_sample(accel_instance[instance], accel_accum); + _rotate_and_correct_accel(accel_instance, accel_accum); + _notify_new_accel_raw_sample(accel_instance, accel_accum); - _publish_temperature(instance, 23); + _publish_temperature(accel_instance, 23); } /* generate a gyro sample */ -void AP_InertialSensor_SITL::generate_gyro(uint8_t instance) +void AP_InertialSensor_SITL::generate_gyro() { Vector3f gyro_accum; - uint8_t nsamples = enable_fast_sampling(gyro_instance[instance]) ? 8 : 1; + uint8_t nsamples = enable_fast_sampling(gyro_instance) ? 8 : 1; + for (uint8_t j = 0; j < nsamples; j++) { float p = radians(sitl->state.rollRate) + gyro_drift(); float q = radians(sitl->state.pitchRate) + gyro_drift(); @@ -195,18 +180,18 @@ void AP_InertialSensor_SITL::generate_gyro(uint8_t instance) // VIB_FREQ is a static vibration applied to each axis const Vector3f &vibe_freq = sitl->vibe_freq; if (!vibe_freq.is_zero() && motors_on) { - float t = AP_HAL::micros() * 1.0e-6f; - p += sinf(t * 2 * M_PI * vibe_freq.x) * calculate_noise(gyro_noise, noise_variation); - q += sinf(t * 2 * M_PI * vibe_freq.y) * calculate_noise(gyro_noise, noise_variation); - r += sinf(t * 2 * M_PI * vibe_freq.z) * calculate_noise(gyro_noise, noise_variation); + p += sinf(gyro_time * 2 * M_PI * vibe_freq.x) * calculate_noise(gyro_noise, noise_variation); + q += sinf(gyro_time * 2 * M_PI * vibe_freq.y) * calculate_noise(gyro_noise, noise_variation); + r += sinf(gyro_time * 2 * M_PI * vibe_freq.z) * calculate_noise(gyro_noise, noise_variation); + gyro_time += 1.0f / (gyro_sample_hz * nsamples); } // VIB_MOT_MAX is a rpm-scaled vibration applied to each axis if (!is_zero(sitl->vibe_motor) && motors_on) { for (uint8_t i = 0; i < sitl->state.num_motors; i++) { float motor_freq = calculate_noise(sitl->state.rpm[i] / 60.0f, freq_variation); - float phase_incr = motor_freq * 2 * M_PI / (gyro_sample_hz[instance] * nsamples); - float &phase = gyro_motor_phase[instance][i]; + float phase_incr = motor_freq * 2 * M_PI / (gyro_sample_hz * nsamples); + float &phase = gyro_motor_phase[i]; phase += phase_incr; if (phase_incr > M_PI) { phase -= 2 * M_PI; @@ -228,12 +213,12 @@ void AP_InertialSensor_SITL::generate_gyro(uint8_t instance) gyro.y *= (1 + scale.y * 0.01f); gyro.z *= (1 + scale.z * 0.01f); - _rotate_and_correct_gyro(gyro_instance[instance], gyro); gyro_accum += gyro; - _notify_new_gyro_sensor_rate_sample(instance, gyro); + _notify_new_gyro_sensor_rate_sample(gyro_instance, gyro); } gyro_accum /= nsamples; - _notify_new_gyro_raw_sample(gyro_instance[instance], gyro_accum); + _rotate_and_correct_gyro(gyro_instance, gyro_accum); + _notify_new_gyro_raw_sample(gyro_instance, gyro_accum); } void AP_InertialSensor_SITL::timer_update(void) @@ -246,30 +231,26 @@ void AP_InertialSensor_SITL::timer_update(void) return; } #endif - for (uint8_t i=0; i= next_accel_sample[i]) { - if (((1U<accel_fail_mask) == 0) { - generate_accel(i); - if (next_accel_sample[i] == 0) { - next_accel_sample[i] = now + 1000000UL / accel_sample_hz[i]; - } - else { - while (now >= next_accel_sample[i]) { - next_accel_sample[i] += 1000000UL / accel_sample_hz[i]; - } + if (now >= next_accel_sample) { + if (((1U << accel_instance) & sitl->accel_fail_mask) == 0) { + generate_accel(); + if (next_accel_sample == 0) { + next_accel_sample = now + 1000000UL / accel_sample_hz; + } else { + while (now >= next_accel_sample) { + next_accel_sample += 1000000UL / accel_sample_hz; } } } - if (now >= next_gyro_sample[i]) { - if (((1U<gyro_fail_mask) == 0) { - generate_gyro(i); - if (next_gyro_sample[i] == 0) { - next_gyro_sample[i] = now + 1000000UL / gyro_sample_hz[i]; - } - else { - while (now >= next_gyro_sample[i]) { - next_gyro_sample[i] += 1000000UL / gyro_sample_hz[i]; - } + } + if (now >= next_gyro_sample) { + if (((1U << gyro_instance) & sitl->gyro_fail_mask) == 0) { + generate_gyro(); + if (next_gyro_sample == 0) { + next_gyro_sample = now + 1000000UL / gyro_sample_hz; + } else { + while (now >= next_gyro_sample) { + next_gyro_sample += 1000000UL / gyro_sample_hz; } } } @@ -293,11 +274,21 @@ float AP_InertialSensor_SITL::gyro_drift(void) bool AP_InertialSensor_SITL::update(void) { - for (uint8_t i=0; iregister_timer_process(FUNCTOR_BIND_MEMBER(&AP_InertialSensor_SITL::timer_update, void)); +} + #endif // HAL_BOARD_SITL diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_SITL.h b/libraries/AP_InertialSensor/AP_InertialSensor_SITL.h index 723d505e4d..8f811a8204 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_SITL.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_SITL.h @@ -6,37 +6,43 @@ #include "AP_InertialSensor.h" #include "AP_InertialSensor_Backend.h" -#define INS_SITL_INSTANCES 2 +// simulated sensor rates in Hz. This matches a pixhawk1 +const uint16_t INS_SITL_SENSOR_A[] = { 1000, 1000 }; +const uint16_t INS_SITL_SENSOR_B[] = { 760, 800 }; class AP_InertialSensor_SITL : public AP_InertialSensor_Backend { public: - AP_InertialSensor_SITL(AP_InertialSensor &imu); + AP_InertialSensor_SITL(AP_InertialSensor &imu, const uint16_t sample_rates[]); /* update accel and gyro state */ bool update() override; + void start() override; // detect the sensor - static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu); + static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu, const uint16_t sample_rates[]); private: bool init_sensor(void); void timer_update(); float gyro_drift(void); - void generate_accel(uint8_t instance); - void generate_gyro(uint8_t instance); + void generate_accel(); + void generate_gyro(); SITL::SITL *sitl; - // simulated sensor rates in Hz. This matches a pixhawk1 - const uint16_t gyro_sample_hz[INS_SITL_INSTANCES] { 1000, 760 }; - const uint16_t accel_sample_hz[INS_SITL_INSTANCES] { 1000, 800 }; + const uint16_t gyro_sample_hz; + const uint16_t accel_sample_hz; - uint8_t gyro_instance[INS_SITL_INSTANCES]; - uint8_t accel_instance[INS_SITL_INSTANCES]; - uint64_t next_gyro_sample[INS_SITL_INSTANCES]; - uint64_t next_accel_sample[INS_SITL_INSTANCES]; - float gyro_motor_phase[INS_SITL_INSTANCES][12]; - float accel_motor_phase[INS_SITL_INSTANCES][12]; + uint8_t gyro_instance; + uint8_t accel_instance; + uint64_t next_gyro_sample; + uint64_t next_accel_sample; + float gyro_time; + float accel_time; + float gyro_motor_phase[12]; + float accel_motor_phase[12]; + + static uint8_t bus_id; }; #endif // CONFIG_HAL_BOARD