/* This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see <http://www.gnu.org/licenses/>. */ /* IMU driver backend class. Each supported gyro/accel sensor type needs to have an object derived from this class. Note that drivers can implement just gyros or just accels, and can also provide multiple gyro/accel instances. */ #pragma once #include <inttypes.h> #include <AP_Math/AP_Math.h> #include <AP_ExternalAHRS/AP_ExternalAHRS.h> #include "AP_InertialSensor.h" class AuxiliaryBus; class AP_Logger; class AP_InertialSensor_Backend { public: AP_InertialSensor_Backend(AP_InertialSensor &imu); AP_InertialSensor_Backend(const AP_InertialSensor_Backend &that) = delete; // we declare a virtual destructor so that drivers can // override with a custom destructor if need be. virtual ~AP_InertialSensor_Backend(void) {} /* * Update the sensor data. Called by the frontend to transfer * accumulated sensor readings to the frontend structure via the * _publish_gyro() and _publish_accel() functions */ virtual bool update() = 0; /* * optional function to accumulate more samples. This is needed for drivers that don't use a timer to gather samples */ virtual void accumulate() {} /* * Configure and start all sensors. The empty implementation allows * subclasses to already start the sensors when it's detected */ virtual void start() { } /* * Return an AuxiliaryBus if backend has another bus it is able to export */ virtual AuxiliaryBus *get_auxiliary_bus() { return nullptr; } /* * Return the unique identifier for this backend: it's the same for * several sensors if the backend registers more gyros/accels */ int16_t get_id() const { return _id; } //Returns the Clip Limit float get_clip_limit() const { return _clip_limit; } // notify of a fifo reset void notify_fifo_reset(void); // get a startup banner to output to the GCS virtual bool get_output_banner(char* banner, uint8_t banner_len) { return false; } #if HAL_EXTERNAL_AHRS_ENABLED virtual void handle_external(const AP_ExternalAHRS::ins_data_message_t &pkt) {} #endif /* device driver IDs. These are used to fill in the devtype field of the device ID, which shows up as INS*ID* parameters to users. The values are chosen for compatibility with existing PX4 drivers. If a change is made to a driver that would make existing calibration values invalid then this number must be changed. */ enum DevTypes { DEVTYPE_BMI160 = 0x09, DEVTYPE_L3G4200D = 0x10, DEVTYPE_ACC_LSM303D = 0x11, DEVTYPE_ACC_BMA180 = 0x12, DEVTYPE_ACC_MPU6000 = 0x13, DEVTYPE_ACC_MPU9250 = 0x16, DEVTYPE_ACC_IIS328DQ = 0x17, DEVTYPE_ACC_LSM9DS1 = 0x18, DEVTYPE_GYR_MPU6000 = 0x21, DEVTYPE_GYR_L3GD20 = 0x22, DEVTYPE_GYR_MPU9250 = 0x24, DEVTYPE_GYR_I3G4250D = 0x25, DEVTYPE_GYR_LSM9DS1 = 0x26, DEVTYPE_INS_ICM20789 = 0x27, DEVTYPE_INS_ICM20689 = 0x28, DEVTYPE_INS_BMI055 = 0x29, DEVTYPE_SITL = 0x2A, DEVTYPE_INS_BMI088 = 0x2B, DEVTYPE_INS_ICM20948 = 0x2C, DEVTYPE_INS_ICM20648 = 0x2D, DEVTYPE_INS_ICM20649 = 0x2E, DEVTYPE_INS_ICM20602 = 0x2F, DEVTYPE_INS_ICM20601 = 0x30, DEVTYPE_INS_ADIS1647X = 0x31, DEVTYPE_SERIAL = 0x32, DEVTYPE_INS_ICM40609 = 0x33, DEVTYPE_INS_ICM42688 = 0x34, DEVTYPE_INS_ICM42605 = 0x35, }; protected: // access to frontend AP_InertialSensor &_imu; // semaphore for access to shared frontend data HAL_Semaphore _sem; //Default Clip Limit float _clip_limit = 15.5f * GRAVITY_MSS; void _rotate_and_correct_accel(uint8_t instance, Vector3f &accel); void _rotate_and_correct_gyro(uint8_t instance, Vector3f &gyro); // rotate gyro vector, offset and publish void _publish_gyro(uint8_t instance, const Vector3f &gyro); // this should be called every time a new gyro raw sample is // available - be it published or not the sample is raw in the // sense that it's not filtered yet, but it must be rotated and // corrected (_rotate_and_correct_gyro) // The sample_us value must be provided for non-FIFO based // sensors, and should be set to zero for FIFO based sensors void _notify_new_gyro_raw_sample(uint8_t instance, const Vector3f &accel, uint64_t sample_us=0); // rotate accel vector, scale, offset and publish void _publish_accel(uint8_t instance, const Vector3f &accel); // this should be called every time a new accel raw sample is available - // be it published or not // the sample is raw in the sense that it's not filtered yet, but it must // be rotated and corrected (_rotate_and_correct_accel) // The sample_us value must be provided for non-FIFO based // sensors, and should be set to zero for FIFO based sensors void _notify_new_accel_raw_sample(uint8_t instance, const Vector3f &accel, uint64_t sample_us=0, bool fsync_set=false); // set the amount of oversamping a accel is doing void _set_accel_oversampling(uint8_t instance, uint8_t n); // set the amount of oversamping a gyro is doing void _set_gyro_oversampling(uint8_t instance, uint8_t n); // indicate the backend is doing sensor-rate sampling for this accel void _set_accel_sensor_rate_sampling_enabled(uint8_t instance, bool value) { const uint8_t bit = (1<<instance); if (value) { _imu._accel_sensor_rate_sampling_enabled |= bit; } else { _imu._accel_sensor_rate_sampling_enabled &= ~bit; } } void _set_gyro_sensor_rate_sampling_enabled(uint8_t instance, bool value) { const uint8_t bit = (1<<instance); if (value) { _imu._gyro_sensor_rate_sampling_enabled |= bit; } else { _imu._gyro_sensor_rate_sampling_enabled &= ~bit; } } void _set_raw_sample_accel_multiplier(uint8_t instance, uint16_t mul) { _imu._accel_raw_sampling_multiplier[instance] = mul; } void _set_raw_sample_gyro_multiplier(uint8_t instance, uint16_t mul) { _imu._gyro_raw_sampling_multiplier[instance] = mul; } // update the sensor rate for FIFO sensors void _update_sensor_rate(uint16_t &count, uint32_t &start_us, float &rate_hz) const; // return true if the sensors are still converging and sampling rates could change significantly bool sensors_converging() const { return AP_HAL::millis() < 30000; } // set accelerometer max absolute offset for calibration void _set_accel_max_abs_offset(uint8_t instance, float offset); // get accelerometer raw sample rate. float _accel_raw_sample_rate(uint8_t instance) const { return _imu._accel_raw_sample_rates[instance]; } // set accelerometer raw sample rate; note that the storage type // is actually float! void _set_accel_raw_sample_rate(uint8_t instance, uint16_t rate_hz) { _imu._accel_raw_sample_rates[instance] = rate_hz; } // get gyroscope raw sample rate float _gyro_raw_sample_rate(uint8_t instance) const { return _imu._gyro_raw_sample_rates[instance]; } // set gyro raw sample rate; note that the storage type is // actually float! void _set_gyro_raw_sample_rate(uint8_t instance, uint16_t rate_hz) { _imu._gyro_raw_sample_rates[instance] = rate_hz; } // publish a temperature value void _publish_temperature(uint8_t instance, float temperature); // set accelerometer error_count void _set_accel_error_count(uint8_t instance, uint32_t error_count); // set gyro error_count void _set_gyro_error_count(uint8_t instance, uint32_t error_count); // increment accelerometer error_count void _inc_accel_error_count(uint8_t instance); // increment gyro error_count void _inc_gyro_error_count(uint8_t instance); // backend unique identifier or -1 if backend doesn't identify itself int16_t _id = -1; // return the default filter frequency in Hz for the sample rate uint16_t _accel_filter_cutoff(void) const { return _imu._accel_filter_cutoff; } // return the default filter frequency in Hz for the sample rate uint16_t _gyro_filter_cutoff(void) const { return _imu._gyro_filter_cutoff; } // return the requested sample rate in Hz uint16_t get_loop_rate_hz(void) const; // return the notch filter center in Hz for the sample rate float _gyro_notch_center_freq_hz(void) const { return _imu._notch_filter.center_freq_hz(); } // return the notch filter bandwidth in Hz for the sample rate float _gyro_notch_bandwidth_hz(void) const { return _imu._notch_filter.bandwidth_hz(); } // return the notch filter attenuation in dB for the sample rate float _gyro_notch_attenuation_dB(void) const { return _imu._notch_filter.attenuation_dB(); } bool _gyro_notch_enabled(void) const { return _imu._notch_filter.enabled(); } // return the harmonic notch filter center in Hz for the sample rate float gyro_harmonic_notch_center_freq_hz() const { return _imu.get_gyro_dynamic_notch_center_freq_hz(); } // set of harmonic notch current center frequencies const float* gyro_harmonic_notch_center_frequencies_hz(void) const { return _imu.get_gyro_dynamic_notch_center_frequencies_hz(); } // number of harmonic notch current center frequencies uint8_t num_gyro_harmonic_notch_center_frequencies(void) const { return _imu.get_num_gyro_dynamic_notch_center_frequencies(); } // return the harmonic notch filter bandwidth in Hz for the sample rate float gyro_harmonic_notch_bandwidth_hz(void) const { return _imu._harmonic_notch_filter.bandwidth_hz(); } // return the harmonic notch filter attenuation in dB for the sample rate float gyro_harmonic_notch_attenuation_dB(void) const { return _imu._harmonic_notch_filter.attenuation_dB(); } bool gyro_harmonic_notch_enabled(void) const { return _imu._harmonic_notch_filter.enabled(); } // common gyro update function for all backends void update_gyro(uint8_t instance); // common accel update function for all backends void update_accel(uint8_t instance); // support for updating filter at runtime uint16_t _last_accel_filter_hz; uint16_t _last_gyro_filter_hz; float _last_notch_center_freq_hz; float _last_notch_bandwidth_hz; float _last_notch_attenuation_dB; // support for updating harmonic filter at runtime float _last_harmonic_notch_center_freq_hz; float _last_harmonic_notch_bandwidth_hz; float _last_harmonic_notch_attenuation_dB; void set_gyro_orientation(uint8_t instance, enum Rotation rotation) { _imu._gyro_orientation[instance] = rotation; } void set_accel_orientation(uint8_t instance, enum Rotation rotation) { _imu._accel_orientation[instance] = rotation; } // increment clipping counted. Used by drivers that do decimation before supplying // samples to the frontend void increment_clip_count(uint8_t instance) { _imu._accel_clip_count[instance]++; } // should fast sampling be enabled on this IMU? bool enable_fast_sampling(uint8_t instance) { return (_imu._fast_sampling_mask & (1U<<instance)) != 0; } // if fast sampling is enabled, the rate to use in kHz uint8_t get_fast_sampling_rate() { return (1 << uint8_t(_imu._fast_sampling_rate)); } // called by subclass when data is received from the sensor, thus // at the 'sensor rate' void _notify_new_accel_sensor_rate_sample(uint8_t instance, const Vector3f &accel); void _notify_new_gyro_sensor_rate_sample(uint8_t instance, const Vector3f &gyro); /* notify of a FIFO reset so we don't use bad data to update observed sensor rate */ void notify_accel_fifo_reset(uint8_t instance); void notify_gyro_fifo_reset(uint8_t instance); // log an unexpected change in a register for an IMU void log_register_change(uint32_t bus_id, const AP_HAL::Device::checkreg ®); // note that each backend is also expected to have a static detect() // function which instantiates an instance of the backend sensor // driver if the sensor is available private: bool should_log_imu_raw() const; void log_accel_raw(uint8_t instance, const uint64_t sample_us, const Vector3f &accel); void log_gyro_raw(uint8_t instance, const uint64_t sample_us, const Vector3f &gryo); // logging void Write_ACC(const uint8_t instance, const uint64_t sample_us, const Vector3f &accel) const; // Write ACC data packet: raw accel data void Write_GYR(const uint8_t instance, const uint64_t sample_us, const Vector3f &gyro) const; // Write GYR data packet: raw gyro data };