Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 :
16 : /*
17 : IMU driver backend class. Each supported gyro/accel sensor type
18 : needs to have an object derived from this class.
19 :
20 : Note that drivers can implement just gyros or just accels, and can
21 : also provide multiple gyro/accel instances.
22 : */
23 : #pragma once
24 :
25 : #include <inttypes.h>
26 :
27 : #include <AP_Math/AP_Math.h>
28 : #include <AP_ExternalAHRS/AP_ExternalAHRS.h>
29 :
30 : #include "AP_InertialSensor.h"
31 :
32 : #ifndef HAL_INS_HIGHRES_SAMPLE
33 : #define HAL_INS_HIGHRES_SAMPLE 0
34 : #endif
35 :
36 : class AuxiliaryBus;
37 : class AP_Logger;
38 :
39 : class AP_InertialSensor_Backend
40 : {
41 : public:
42 : AP_InertialSensor_Backend(AP_InertialSensor &imu);
43 : AP_InertialSensor_Backend(const AP_InertialSensor_Backend &that) = delete;
44 :
45 : // we declare a virtual destructor so that drivers can
46 : // override with a custom destructor if need be.
47 0 : virtual ~AP_InertialSensor_Backend(void) {}
48 :
49 : /*
50 : * Update the sensor data. Called by the frontend to transfer
51 : * accumulated sensor readings to the frontend structure via the
52 : * _publish_gyro() and _publish_accel() functions
53 : */
54 : virtual bool update() = 0; /* front end */
55 :
56 : // if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
57 : /*
58 : * Update the filter parameters. Called by the frontend to propagate
59 : * filter parameters to the frontend structure via the
60 : * update_gyro_filters() and update_accel_filters() functions
61 : */
62 : void update_filters() __RAMFUNC__; /* front end */
63 : // endif AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
64 :
65 : /*
66 : * optional function to accumulate more samples. This is needed for drivers that don't use a timer to gather samples
67 : */
68 84784087 : virtual void accumulate() {}
69 :
70 : /*
71 : * Configure and start all sensors. The empty implementation allows
72 : * subclasses to already start the sensors when it's detected
73 : */
74 0 : virtual void start() { }
75 :
76 : /*
77 : * Return an AuxiliaryBus if backend has another bus it is able to export
78 : */
79 0 : virtual AuxiliaryBus *get_auxiliary_bus() { return nullptr; }
80 :
81 : /*
82 : * Return the unique identifier for this backend: it's the same for
83 : * several sensors if the backend registers more gyros/accels
84 : */
85 0 : int16_t get_id() const { return _id; }
86 :
87 : //Returns the Clip Limit
88 827224498 : float get_clip_limit() const { return _clip_limit; }
89 :
90 : // get a startup banner to output to the GCS
91 2008 : virtual bool get_output_banner(char* banner, uint8_t banner_len) { return false; }
92 :
93 : #if AP_EXTERNAL_AHRS_ENABLED
94 267778 : virtual void handle_external(const AP_ExternalAHRS::ins_data_message_t &pkt) {}
95 : #endif
96 :
97 : #if AP_INERTIALSENSOR_KILL_IMU_ENABLED
98 1156239412 : bool has_been_killed(uint8_t instance) const { return ((1U<<instance) & _imu.imu_kill_mask); }
99 : #else
100 : bool has_been_killed(uint8_t instance) const { return false; }
101 : #endif
102 :
103 : // get the backend update rate for the gyro in Hz
104 : // if the backend polling rate is the same as the sample rate or higher, return raw sample rate
105 : // override and return the backend rate in Hz if it is lower than the sample rate
106 91768 : virtual uint16_t get_gyro_backend_rate_hz() const {
107 91768 : if (gyro_instance >= INS_MAX_INSTANCES) {
108 0 : return 0;
109 : }
110 91768 : return _gyro_raw_sample_rate(gyro_instance);
111 : }
112 :
113 : // return the maximum allowed gyro bias for this sensor (rad/s).
114 : // The EKF clamps its gyro bias state to this value.
115 82388166 : virtual float gyro_bias_limit_rads() const {
116 82388166 : return 0.5f;
117 : }
118 :
119 : // return the initial 1-sigma gyro bias uncertainty for this sensor (deg/s)
120 82388166 : virtual float gyro_bias_init_dps() const {
121 82388166 : return 2.5f;
122 : }
123 :
124 411714042 : uint8_t get_gyro_instance() const {
125 411714042 : return gyro_instance;
126 : }
127 :
128 : uint8_t get_accel_instance() const {
129 : return accel_instance;
130 : }
131 :
132 : /*
133 : device driver IDs. These are used to fill in the devtype field
134 : of the device ID, which shows up as INS*ID* parameters to
135 : users. The values are chosen for compatibility with existing PX4
136 : drivers.
137 : If a change is made to a driver that would make existing
138 : calibration values invalid then this number must be changed.
139 : */
140 : enum DevTypes {
141 : DEVTYPE_BMI160 = 0x09,
142 : DEVTYPE_L3G4200D = 0x10,
143 : DEVTYPE_ACC_LSM303D = 0x11,
144 : DEVTYPE_ACC_BMA180 = 0x12,
145 : DEVTYPE_ACC_MPU6000 = 0x13,
146 : DEVTYPE_ACC_MPU9250 = 0x16,
147 : DEVTYPE_ACC_IIS328DQ = 0x17,
148 : DEVTYPE_ACC_LSM9DS1 = 0x18,
149 : DEVTYPE_GYR_MPU6000 = 0x21,
150 : DEVTYPE_GYR_L3GD20 = 0x22,
151 : DEVTYPE_GYR_MPU9250 = 0x24,
152 : DEVTYPE_GYR_I3G4250D = 0x25,
153 : DEVTYPE_GYR_LSM9DS1 = 0x26,
154 : DEVTYPE_INS_ICM20789 = 0x27,
155 : DEVTYPE_INS_ICM20689 = 0x28,
156 : DEVTYPE_INS_BMI055 = 0x29,
157 : DEVTYPE_SITL = 0x2A,
158 : DEVTYPE_INS_BMI088 = 0x2B,
159 : DEVTYPE_INS_ICM20948 = 0x2C,
160 : DEVTYPE_INS_ICM20648 = 0x2D,
161 : DEVTYPE_INS_ICM20649 = 0x2E,
162 : DEVTYPE_INS_ICM20602 = 0x2F,
163 : DEVTYPE_INS_ICM20601 = 0x30,
164 : DEVTYPE_INS_ADIS1647X = 0x31,
165 : DEVTYPE_SERIAL = 0x32,
166 : DEVTYPE_INS_ICM40609 = 0x33,
167 : DEVTYPE_INS_ICM42688 = 0x34,
168 : DEVTYPE_INS_ICM42605 = 0x35,
169 : DEVTYPE_INS_ICM40605 = 0x36,
170 : DEVTYPE_INS_IIM42652 = 0x37,
171 : DEVTYPE_BMI270 = 0x38,
172 : DEVTYPE_INS_BMI085 = 0x39,
173 : DEVTYPE_INS_ICM42670 = 0x3A,
174 : DEVTYPE_INS_ICM45686 = 0x3B,
175 : DEVTYPE_INS_SCHA63T = 0x3C,
176 : DEVTYPE_INS_IIM42653 = 0x3D,
177 : DEVTYPE_INS_LSM6DSV16X = 0x3E,
178 : DEVTYPE_INS_ASM330 = 0x3F,
179 : DEVTYPE_INS_ADIS16607 = 0x40,
180 : DEVTYPE_INS_ZEROONE_FPGA_SCH16T = 0x41,
181 : DEVTYPE_INS_LSM6DSV32X = 0x42,
182 : DEVTYPE_INS_LSM6DSK320X = 0x43,
183 : DEVTYPE_INS_ICM56686 = 0x44,
184 : };
185 :
186 : protected:
187 : // access to frontend
188 : AP_InertialSensor &_imu;
189 :
190 : // semaphore for access to shared frontend data
191 : HAL_Semaphore _sem;
192 :
193 : //Default Clip Limit
194 : float _clip_limit = (16.0f - 0.5f) * GRAVITY_MSS;
195 :
196 : // instance numbers of accel and gyro data. INS_MAX_INSTANCES means
197 : // registration failed and this backend owns no frontend instance
198 : uint8_t gyro_instance = INS_MAX_INSTANCES;
199 : uint8_t accel_instance = INS_MAX_INSTANCES;
200 : bool is_primary = true;
201 : uint32_t last_primary_update_us;
202 :
203 : void _rotate_and_correct_accel(uint8_t instance, Vector3f &accel) __RAMFUNC__;
204 : void _rotate_and_correct_gyro(uint8_t instance, Vector3f &gyro) __RAMFUNC__;
205 :
206 : // rotate gyro vector, offset and publish
207 : void _publish_gyro(uint8_t instance, const Vector3f &gyro) __RAMFUNC__; /* front end */
208 :
209 : // apply notch and lowpass gyro filters and sample for FFT
210 : void apply_gyro_filters(const uint8_t instance, const Vector3f &gyro);
211 : void save_gyro_window(const uint8_t instance, const Vector3f &gyro, uint8_t phase);
212 :
213 : // this should be called every time a new gyro raw sample is
214 : // available - be it published or not the sample is raw in the
215 : // sense that it's not filtered yet, but it must be rotated and
216 : // corrected (_rotate_and_correct_gyro)
217 : // The sample_us value must be provided for non-FIFO based
218 : // sensors, and should be set to zero for FIFO based sensors
219 : void _notify_new_gyro_raw_sample(uint8_t instance, const Vector3f &accel, uint64_t sample_us=0) __RAMFUNC__;
220 :
221 : // alternative interface using delta-angles. Rotation and correction is handled inside this function
222 : void _notify_new_delta_angle(uint8_t instance, const Vector3f &dangle);
223 :
224 : // rotate accel vector, scale, offset and publish
225 : void _publish_accel(uint8_t instance, const Vector3f &accel) __RAMFUNC__; /* front end */
226 :
227 : // this should be called every time a new accel raw sample is available -
228 : // be it published or not
229 : // the sample is raw in the sense that it's not filtered yet, but it must
230 : // be rotated and corrected (_rotate_and_correct_accel)
231 : // The sample_us value must be provided for non-FIFO based
232 : // sensors, and should be set to zero for FIFO based sensors
233 : void _notify_new_accel_raw_sample(uint8_t instance, const Vector3f &accel, uint64_t sample_us=0, bool fsync_set=false) __RAMFUNC__;
234 :
235 : // alternative interface using delta-velocities. Rotation and correction is handled inside this function
236 : void _notify_new_delta_velocity(uint8_t instance, const Vector3f &dvelocity);
237 :
238 : // set the amount of oversamping a accel is doing
239 : void _set_accel_oversampling(uint8_t instance, uint8_t n);
240 :
241 : // set the amount of oversamping a gyro is doing
242 : void _set_gyro_oversampling(uint8_t instance, uint8_t n);
243 :
244 : // indicate the backend is doing sensor-rate sampling for this accel
245 : void _set_accel_sensor_rate_sampling_enabled(uint8_t instance, bool value) {
246 : const uint8_t bit = (1<<instance);
247 : if (value) {
248 : _imu._accel_sensor_rate_sampling_enabled |= bit;
249 : } else {
250 : _imu._accel_sensor_rate_sampling_enabled &= ~bit;
251 : }
252 : }
253 :
254 : void _set_gyro_sensor_rate_sampling_enabled(uint8_t instance, bool value) {
255 : const uint8_t bit = (1<<instance);
256 : if (value) {
257 : _imu._gyro_sensor_rate_sampling_enabled |= bit;
258 : } else {
259 : _imu._gyro_sensor_rate_sampling_enabled &= ~bit;
260 : }
261 : }
262 :
263 : void _set_raw_sample_accel_multiplier(uint8_t instance, uint16_t mul) {
264 : _imu._accel_raw_sampling_multiplier[instance] = mul;
265 : }
266 : void _set_raw_sample_gyro_multiplier(uint8_t instance, uint16_t mul) {
267 : _imu._gyro_raw_sampling_multiplier[instance] = mul;
268 : }
269 :
270 : // update the sensor rate for FIFO sensors
271 : void _update_sensor_rate(uint16_t &count, uint32_t &start_us, float &rate_hz) const __RAMFUNC__;
272 :
273 : // return true if the sensors are still converging and sampling rates could change significantly
274 : bool sensors_converging() const;
275 :
276 : // get accelerometer raw sample rate.
277 4183 : float _accel_raw_sample_rate(uint8_t instance) const {
278 4183 : return _imu._accel_raw_sample_rates[instance];
279 : }
280 :
281 : // set accelerometer raw sample rate; note that the storage type
282 : // is actually float!
283 : void _set_accel_raw_sample_rate(uint8_t instance, uint16_t rate_hz) {
284 : _imu._accel_raw_sample_rates[instance] = rate_hz;
285 : }
286 :
287 : // get gyroscope raw sample rate
288 84146930 : float _gyro_raw_sample_rate(uint8_t instance) const {
289 84146930 : return _imu._gyro_raw_sample_rates[instance];
290 : }
291 :
292 : // set gyro raw sample rate; note that the storage type is
293 : // actually float!
294 : void _set_gyro_raw_sample_rate(uint8_t instance, uint16_t rate_hz) {
295 : _imu._gyro_raw_sample_rates[instance] = rate_hz;
296 : }
297 :
298 : // publish a temperature value
299 : void _publish_temperature(uint8_t instance, float temperature); /* front end */
300 :
301 : // increment accelerometer error_count
302 : void _inc_accel_error_count(uint8_t instance) __RAMFUNC__;
303 :
304 : // increment gyro error_count
305 : void _inc_gyro_error_count(uint8_t instance) __RAMFUNC__;
306 :
307 : // backend unique identifier or -1 if backend doesn't identify itself
308 : int16_t _id = -1;
309 :
310 : // return the default filter frequency in Hz for the sample rate
311 84063510 : uint16_t _accel_filter_cutoff(void) const { return _imu._accel_filter_cutoff; }
312 :
313 : // return the default filter frequency in Hz for the sample rate
314 128536756 : uint16_t _gyro_filter_cutoff(void) const { return _imu._gyro_filter_cutoff; }
315 :
316 : // return the requested loop rate at which samples will be made available in Hz
317 : uint16_t get_loop_rate_hz(void) const {
318 : // enum can be directly cast to Hz
319 : return (uint16_t)_imu._loop_rate;
320 : }
321 :
322 : // common gyro update function for all backends
323 : void update_gyro(uint8_t instance) __RAMFUNC__; /* front end */
324 : void update_gyro_filters(uint8_t instance) __RAMFUNC__; /* front end */
325 :
326 : // common accel update function for all backends
327 : void update_accel(uint8_t instance) __RAMFUNC__; /* front end */
328 : void update_accel_filters(uint8_t instance) __RAMFUNC__; /* front end */
329 :
330 : // catch updates to the primary gyro and accel
331 : void update_primary() __RAMFUNC__; /* backend */
332 1529148 : virtual void set_primary(bool _is_primary) {}
333 :
334 : // support for updating filter at runtime
335 : uint16_t _last_accel_filter_hz;
336 : uint16_t _last_gyro_filter_hz;
337 :
338 : void set_gyro_orientation(uint8_t instance, enum Rotation rotation) {
339 : _imu._gyro_orientation[instance] = rotation;
340 : }
341 :
342 : void set_accel_orientation(uint8_t instance, enum Rotation rotation) {
343 : _imu._accel_orientation[instance] = rotation;
344 : }
345 :
346 : // increment clipping counted. Used by drivers that do decimation before supplying
347 : // samples to the frontend
348 : void increment_clip_count(uint8_t instance) {
349 : _imu._accel_clip_count[instance]++;
350 : }
351 :
352 : // should fast sampling be enabled on this IMU?
353 545281360 : bool enable_fast_sampling(uint8_t instance) const {
354 545281360 : return (_imu._fast_sampling_mask & (1U<<instance)) != 0;
355 : }
356 :
357 : // should highres sampling be enabled on this IMU?
358 : bool enable_highres_sampling(uint8_t instance) const {
359 : return (HAL_INS_HIGHRES_SAMPLE & (1U<<instance)) != 0;
360 : }
361 :
362 : // if fast sampling is enabled, the rate to use in kHz
363 : uint8_t get_fast_sampling_rate() const {
364 : return (1 << uint8_t(_imu._fast_sampling_rate));
365 : }
366 :
367 : // called by subclass when data is received from the sensor, thus
368 : // at the 'sensor rate'
369 : void _notify_new_accel_sensor_rate_sample(uint8_t instance, const Vector3f &accel) __RAMFUNC__;
370 : void _notify_new_gyro_sensor_rate_sample(uint8_t instance, const Vector3f &gyro) __RAMFUNC__;
371 :
372 : /*
373 : notify of a FIFO reset so we don't use bad data to update observed sensor rate
374 : */
375 : void notify_accel_fifo_reset(uint8_t instance) __RAMFUNC__;
376 : void notify_gyro_fifo_reset(uint8_t instance) __RAMFUNC__;
377 :
378 : // log an unexpected change in a register for an IMU
379 : void log_register_change(uint32_t bus_id, const AP_HAL::Device::checkreg ®) __RAMFUNC__;
380 :
381 : // note that each backend is also expected to have a static detect()
382 : // function which instantiates an instance of the backend sensor
383 : // driver if the sensor is available
384 :
385 : private:
386 :
387 : bool should_log_imu_raw() const ;
388 : void log_accel_raw(uint8_t instance, const uint64_t sample_us, const Vector3f &accel) __RAMFUNC__;
389 : void log_gyro_raw(uint8_t instance, const uint64_t sample_us, const Vector3f &raw_gyro, const Vector3f &filtered_gyro) __RAMFUNC__;
390 :
391 : // logging
392 : void Write_ACC(const uint8_t instance, const uint64_t sample_us, const Vector3f &accel) const __RAMFUNC__; // Write ACC data packet: raw accel data
393 :
394 : protected:
395 : void Write_GYR(const uint8_t instance, const uint64_t sample_us, const Vector3f &gyro, bool use_sample_timestamp=false) const __RAMFUNC__; // Write GYR data packet: raw gyro data
396 :
397 : };
|
