Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
ardupilot/libraries/AP_InertialSensor/AP_InertialSensor.cpp

Ignoring revisions in .git-blame-ignore-revs. Click here to bypass and see the normal blame view.

2700 lines
97 KiB
C++
Raw Normal View History

AP_InertialSensor: identify backend with ID Identify backend with an id, allowing other libraries to connect to them. This is different from the _product_id member because it identifies the sensor, not the board the sensor is in, which is meaningless for our use case.
2015-08-16 16:16:11 -03:00
#include <assert.h>
AP_InertialSensor: define AP_INERTIALSENSOR_ENABLED in a new AP_InertialSensor_config.h
2023-01-03 01:09:46 -04:00
#include "AP_InertialSensor.h"
#if AP_INERTIALSENSOR_ENABLED
AP_InertialSensor: standardize inclusion of libaries headers This commit changes the way libraries headers are included in source files: - If the header is in the same directory the source belongs to, so the notation '#include ""' is used with the path relative to the directory containing the source. - If the header is outside the directory containing the source, then we use the notation '#include <>' with the path relative to libraries folder. Some of the advantages of such approach: - Only one search path for libraries headers. - OSs like Windows may have a better lookup time.
2015-08-11 03:28:43 -03:00
#include <AP_Common/AP_Common.h>
#include <AP_HAL/AP_HAL.h>
AP_InertialSensor: MPU60x0: use AP_HAL::Device abstraction
2016-01-12 14:22:11 -04:00
#include <AP_HAL/I2CDevice.h>
#include <AP_HAL/SPIDevice.h>
AP_InertialSensor: calculate number of notches based on motor count configure default harmonics if dynamic harmonics set number of notches correctly for FFT operation include static notch in filter calculation and allow harmonic options on static notch
2021-11-13 18:40:04 -04:00
#include <AP_HAL/DSP.h>
AP_InertialSensor: sanitize includes Due to the way the headers are organized changing a single change in an inertial sensor driver would trigger a rebuild for most of the files in the project. Time could be saved by using ccache (since most of the things didn't change) but we can do better, i.e. re-organize the headers so we don't have to re-build everything. With this patch only AP_InertialSensor/AP_InertialSensor.h is exposed to most users. There are some corner cases to integrate with some example code, but most of the places now depend only on this header and this header doesn't depend on the specific backends. Now changing a single header, e.g. AP_InertialSensor_L3G4200D.h triggers a rebuild only of these files: $ waf copter 'copter' finished successfully (0.000s) Waf: Entering directory `/home/lucas/p/dronecode/ardupilot/build/minlure' [ 80/370] Compiling libraries/AP_InertialSensor/AP_InertialSensor.cpp [ 84/370] Compiling libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp [310/370] Linking build/minlure/ArduCopter/libArduCopter_libs.a [370/370] Linking build/minlure/bin/arducopter Waf: Leaving directory `/home/lucas/p/dronecode/ardupilot/build/minlure'
2016-01-19 21:26:31 -04:00
#include <AP_Math/AP_Math.h>
AP_InertialSensor: standardize inclusion of libaries headers This commit changes the way libraries headers are included in source files: - If the header is in the same directory the source belongs to, so the notation '#include ""' is used with the path relative to the directory containing the source. - If the header is outside the directory containing the source, then we use the notation '#include <>' with the path relative to libraries folder. Some of the advantages of such approach: - Only one search path for libraries headers. - OSs like Windows may have a better lookup time.
2015-08-11 03:28:43 -03:00
#include <AP_Notify/AP_Notify.h>
AP_InertialSensor: allow for in-tree drivers for PX4
2016-11-01 07:34:59 -03:00
#include <AP_BoardConfig/AP_BoardConfig.h>
AP_InertialSensor: added simple_accel_cal() this is useful for mass market vehicles where a full 3D accel cal is too complex.
2017-06-05 00:03:28 -03:00
#include <AP_AHRS/AP_AHRS.h>
AP_InertialSensor: use pitch to guess which axis the user is trying to calibrate, warn about Q_TRIM_PITCH on plane
2022-04-23 08:16:08 -03:00
#include <AP_AHRS/AP_AHRS_View.h>
AP_InertialSensor: added ExternalAHRS backend
2020-12-28 22:29:40 -04:00
#include <AP_ExternalAHRS/AP_ExternalAHRS.h>
AP_InertialSensor: add throttle-based notch setup function allocate harmonic notch filter if FFT is enabled
2022-03-06 11:00:30 -04:00
#include <AP_GyroFFT/AP_GyroFFT.h>
AP_InertialSensor: change namespace of MultiCopter and FixedWing params this stops the libraries knowing anything about AP_Vehicle
2022-11-08 06:10:56 -04:00
#include <AP_Vehicle/AP_Vehicle_Type.h>
AP_InertialSensor: calculate number of notches based on motor count configure default harmonics if dynamic harmonics set number of notches correctly for FFT operation include static notch in filter calculation and allow harmonic options on static notch
2021-11-13 18:40:04 -04:00
#if !APM_BUILD_TYPE(APM_BUILD_Rover)
#include <AP_Motors/AP_Motors_Class.h>
#endif
AP_InertialSensor: change namespace of MultiCopter and FixedWing params this stops the libraries knowing anything about AP_Vehicle
2022-11-08 06:10:56 -04:00
#include <GCS_MAVLink/GCS.h>
AP_InertialSensor: sanitize includes Due to the way the headers are organized changing a single change in an inertial sensor driver would trigger a rebuild for most of the files in the project. Time could be saved by using ccache (since most of the things didn't change) but we can do better, i.e. re-organize the headers so we don't have to re-build everything. With this patch only AP_InertialSensor/AP_InertialSensor.h is exposed to most users. There are some corner cases to integrate with some example code, but most of the places now depend only on this header and this header doesn't depend on the specific backends. Now changing a single header, e.g. AP_InertialSensor_L3G4200D.h triggers a rebuild only of these files: $ waf copter 'copter' finished successfully (0.000s) Waf: Entering directory `/home/lucas/p/dronecode/ardupilot/build/minlure' [ 80/370] Compiling libraries/AP_InertialSensor/AP_InertialSensor.cpp [ 84/370] Compiling libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp [310/370] Linking build/minlure/ArduCopter/libArduCopter_libs.a [370/370] Linking build/minlure/bin/arducopter Waf: Leaving directory `/home/lucas/p/dronecode/ardupilot/build/minlure'
2016-01-19 21:26:31 -04:00
AP_InertialSensor: BMI160: add backend The reason of defining BMI160_MAX_FIFO_SAMPLES as 8 can be found on the following histogram of the number of samples in the FIFO on each read while performing the accelerometer calibration process: Samples Count Freq Acc. Freq ------------------------------ 1 3842 0.1201 0.120111 2 13172 0.4118 0.531904 3 9065 0.2834 0.815300 4 2710 0.0847 0.900022 5 2231 0.0697 0.969769 6 816 0.0255 0.995279 7 137 0.0043 0.999562 8 13 0.0004 0.999969 13 1 0.0000 1.000000
2016-06-10 16:17:37 -03:00
#include "AP_InertialSensor_BMI160.h"
AP_InertialSensor: BMI270 driver
2021-12-28 13:43:48 -04:00
#include "AP_InertialSensor_BMI270.h"
AP_InertialSensor: sanitize includes Due to the way the headers are organized changing a single change in an inertial sensor driver would trigger a rebuild for most of the files in the project. Time could be saved by using ccache (since most of the things didn't change) but we can do better, i.e. re-organize the headers so we don't have to re-build everything. With this patch only AP_InertialSensor/AP_InertialSensor.h is exposed to most users. There are some corner cases to integrate with some example code, but most of the places now depend only on this header and this header doesn't depend on the specific backends. Now changing a single header, e.g. AP_InertialSensor_L3G4200D.h triggers a rebuild only of these files: $ waf copter 'copter' finished successfully (0.000s) Waf: Entering directory `/home/lucas/p/dronecode/ardupilot/build/minlure' [ 80/370] Compiling libraries/AP_InertialSensor/AP_InertialSensor.cpp [ 84/370] Compiling libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp [310/370] Linking build/minlure/ArduCopter/libArduCopter_libs.a [370/370] Linking build/minlure/bin/arducopter Waf: Leaving directory `/home/lucas/p/dronecode/ardupilot/build/minlure'
2016-01-19 21:26:31 -04:00
#include "AP_InertialSensor_Backend.h"
#include "AP_InertialSensor_L3G4200D.h"
#include "AP_InertialSensor_LSM9DS0.h"
AP_InertialSensor: add LSM9DS1 entry Enable LSM9DS1 in case it is selected
2017-10-26 11:36:19 -03:00
#include "AP_InertialSensor_LSM9DS1.h"
AP_InertialSensor: unify MPU6000 and MPU9250 drivers make a single AP_InertialSensor_Invensense driver. This avoids a lot of duplication and will save time as new varients are added
2016-12-13 21:47:22 -04:00
#include "AP_InertialSensor_Invensense.h"
AP_InertialSensor: sanitize includes Due to the way the headers are organized changing a single change in an inertial sensor driver would trigger a rebuild for most of the files in the project. Time could be saved by using ccache (since most of the things didn't change) but we can do better, i.e. re-organize the headers so we don't have to re-build everything. With this patch only AP_InertialSensor/AP_InertialSensor.h is exposed to most users. There are some corner cases to integrate with some example code, but most of the places now depend only on this header and this header doesn't depend on the specific backends. Now changing a single header, e.g. AP_InertialSensor_L3G4200D.h triggers a rebuild only of these files: $ waf copter 'copter' finished successfully (0.000s) Waf: Entering directory `/home/lucas/p/dronecode/ardupilot/build/minlure' [ 80/370] Compiling libraries/AP_InertialSensor/AP_InertialSensor.cpp [ 84/370] Compiling libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp [310/370] Linking build/minlure/ArduCopter/libArduCopter_libs.a [370/370] Linking build/minlure/bin/arducopter Waf: Leaving directory `/home/lucas/p/dronecode/ardupilot/build/minlure'
2016-01-19 21:26:31 -04:00
#include "AP_InertialSensor_SITL.h"
AP_InertialSensor: add Robsense PhenixPro Devkit Board support
2017-10-30 06:03:04 -03:00
#include "AP_InertialSensor_RST.h"
AP_InertialSensor: added BMI055 IMU driver
2018-06-07 02:59:18 -03:00
#include "AP_InertialSensor_BMI055.h"
AP_InertialSensor: added driver for BMI088 setup for 1600 Hz accel sample, 2kHz gyro sample, 24g accel range
2019-01-31 03:00:34 -04:00
#include "AP_InertialSensor_BMI088.h"
AP_InertialSensor: add drivers for register bank based Invensense Sensors
2019-02-15 08:12:45 -04:00
#include "AP_InertialSensor_Invensensev2.h"
AP_InertialSensor: added driver for ADIS16470
2020-04-05 04:19:00 -03:00
#include "AP_InertialSensor_ADIS1647x.h"
AP_InertialSensor: added ExternalAHRS backend
2020-12-28 22:29:40 -04:00
#include "AP_InertialSensor_ExternalAHRS.h"
AP_InertialSensor: added support for v3 Invensense sensors starting with ICM-40609, ICM-42688 and ICM-42605
2021-01-06 23:34:38 -04:00
#include "AP_InertialSensor_Invensensev3.h"
AP_InertialSensor: New fallback type = NONE, which successfully does nothing like an INS, but without needing a real one, like esp32 dev boards. Good for boards that u want to boot even when they dont have an IMU attached. Author: Charles Villard <charlesvillard10@gmail.com> Author: Buzz <davidbuzz@gmail.com>
2021-10-27 05:06:41 -03:00
#include "AP_InertialSensor_NONE.h"
AP_InertialSensor: add SCHA63T IMU support
2023-05-08 03:50:37 -03:00
#include "AP_InertialSensor_SCHA63T.h"
AP_InertialSensor: ported to AP_HAL
2012-10-11 21:27:19 -03:00
AP_InertialSensor: fix timing debug macro - Use "INS_" prefix for the name in order to limit the scope for that macro. - Don't define it in the code and check if it is defined instead of checking the value. With that, there's no need to touch the code for enabling debug, only a reconfiguration is necessary (e.g., `CXXFLAGS='-DINS_TIMING_DEBUG' waf configure ...`).
2016-07-21 16:54:37 -03:00
/* Define INS_TIMING_DEBUG to track down scheduling issues with the main loop.
* Output is on the debug console. */
#ifdef INS_TIMING_DEBUG
AP_InertialSensor: added TIMING_DEBUG code for finding loop timing issues
2015-02-15 19:12:10 -04:00
#include <stdio.h>
#define timing_printf(fmt, args...) do { printf("[timing] " fmt, ##args); } while(0)
#else
#define timing_printf(fmt, args...)
#endif
AP_InertialSensor: support new sensor config setup
2019-05-29 08:01:48 -03:00
#ifndef HAL_DEFAULT_INS_FAST_SAMPLE
AP_InertialSensor: default fast sampling on if we have a first IMU capable of fast sampling then we want it enabled by default
2019-12-22 17:38:57 -04:00
#define HAL_DEFAULT_INS_FAST_SAMPLE 1
AP_InertialSensor: support new sensor config setup
2019-05-29 08:01:48 -03:00
#endif
AP_InertialSensor: ported to AP_HAL
2012-10-11 21:27:19 -03:00
extern const AP_HAL::HAL& hal;
AP_InertialSensor: make the backend fast gyro rate configurable raise gyro rate default on F7 and H7 clean up gyro rate docs and output startup banner
2020-05-21 15:29:28 -03:00
AP_InertialSensor: convert APM_BUILD_COPTER_OR_HELI() to APM_BUILD_COPTER_OR_HELI
2021-10-25 14:11:12 -03:00
#if APM_BUILD_COPTER_OR_HELI
AP_InertialSensor: change copter filters to 20Hz with the backend filters disabled 20Hz is closer to the old default of 30Hz
2015-03-11 23:11:17 -03:00
#define DEFAULT_GYRO_FILTER 20
AP_InertialSensor: increase accel filter to 20hz
2017-07-02 21:38:15 -03:00
#define DEFAULT_ACCEL_FILTER 20
AP_InertialSensor: Added is_still() check very strict check that all axis are not vibrating much at all new param: INS_STILL_THRESH used to be a vibration threshold for different platforms // @Description: Threshold to tolerate vibration to determine if vehicle is motionless. This depends on the frame type and if there is a constant vibration due to motors before launch or after landing. Total motionless is about 0.05. Suggested values: Planes/rover use 0.1, multirotors use 1, tradHeli uses 5
2015-07-29 16:34:48 -03:00
#define DEFAULT_STILL_THRESH 2.5f
AP_InertialSensor: rename APMrover2 to Rover
2020-03-26 21:51:15 -03:00
#elif APM_BUILD_TYPE(APM_BUILD_Rover)
AP_InertialSensor: Changing the default gyro filter from 10 to 4 This change is only for Rovers.
2016-06-16 02:17:59 -03:00
#define DEFAULT_GYRO_FILTER 4
AP_InertialSensor: replaced INS_MPU6K_FILTER with INS_ACCEL_FILTER and INS_GYRO_FILTER this allows filtering to be set separately on accels and gyros where possible
2015-03-11 21:58:36 -03:00
#define DEFAULT_ACCEL_FILTER 10
AP_InertialSensor: Added is_still() check very strict check that all axis are not vibrating much at all new param: INS_STILL_THRESH used to be a vibration threshold for different platforms // @Description: Threshold to tolerate vibration to determine if vehicle is motionless. This depends on the frame type and if there is a constant vibration due to motors before launch or after landing. Total motionless is about 0.05. Suggested values: Planes/rover use 0.1, multirotors use 1, tradHeli uses 5
2015-07-29 16:34:48 -03:00
#define DEFAULT_STILL_THRESH 0.1f
AP_InertialSensor: replaced INS_MPU6K_FILTER with INS_ACCEL_FILTER and INS_GYRO_FILTER this allows filtering to be set separately on accels and gyros where possible
2015-03-11 21:58:36 -03:00
#else
#define DEFAULT_GYRO_FILTER 20
#define DEFAULT_ACCEL_FILTER 20
AP_InertialSensor: relax is_still() threshold for SITL.. which is pretty darn still all the time for Plane
2022-02-04 15:32:59 -04:00
#if APM_BUILD_TYPE(APM_BUILD_ArduPlane) && CONFIG_HAL_BOARD == HAL_BOARD_SITL
// In steady-state level flight on SITL Plane, especially while the motor is off, the INS system
// returns ins.is_still()==true. Baseline vibes while airborne are unrealistically low: around 0.07.
// A real aircraft would be experiencing micro turbulence and be rocking around a tiny bit. Therefore,
// for Plane SIM the vibe threshold needs to be a little lower. Since plane.is_flying() uses
// ins.is_still() during gps loss to detect if we're flying, we want to make sure we are not "perfectly"
// still in the air like we are on the ground.
#define DEFAULT_STILL_THRESH 0.05f
#else
#define DEFAULT_STILL_THRESH 0.1f
#endif
AP_InertialSensor: replaced INS_MPU6K_FILTER with INS_ACCEL_FILTER and INS_GYRO_FILTER this allows filtering to be set separately on accels and gyros where possible
2015-03-11 21:58:36 -03:00
#endif
AP_InertialSensor: merge in calibration features from IMU library add gauss-newton method of accelerometer calibration
2012-11-05 00:27:03 -04:00
AP_InertialSensor: make the backend fast gyro rate configurable raise gyro rate default on F7 and H7 clean up gyro rate docs and output startup banner
2020-05-21 15:29:28 -03:00
#if defined(STM32H7) || defined(STM32F7)
#define MPU_FIFO_FASTSAMPLE_DEFAULT 1
#else
#define MPU_FIFO_FASTSAMPLE_DEFAULT 0
#endif
AP_InertialSensor: inform maximum gyro average difference While at it, define GYRO_INIT_MAX_DIFF_DPS.
2016-06-17 18:00:29 -03:00
#define GYRO_INIT_MAX_DIFF_DPS 0.1f
AP_InertialSensor: allow accelcal with a rotation
2021-08-12 19:57:43 -03:00
#ifndef HAL_INS_TRIM_LIMIT_DEG
#define HAL_INS_TRIM_LIMIT_DEG 10
#endif
AP_InertialSensor: merge in calibration features from IMU library add gauss-newton method of accelerometer calibration
2012-11-05 00:27:03 -04:00
// Class level parameters
Remove use of PROGMEM Now variables don't have to be declared with PROGMEM anymore, so remove them. This was automated with: git grep -l -z PROGMEM | xargs -0 sed -i 's/ PROGMEM / /g' git grep -l -z PROGMEM | xargs -0 sed -i 's/PROGMEM//g' The 2 commands were done so we don't leave behind spurious spaces. AVR-specific places were not changed.
2015-10-25 14:03:46 -03:00
const AP_Param::GroupInfo AP_InertialSensor::var_info[] = {
AP_InertialSensor: Remove unused parameter
2018-12-22 21:53:44 -04:00
// 0 was PRODUCT_ID
AP_InertialSensor: Added INS_MPU6K_FILTER option this allows the user to select the MPU6000 filtering frequency
2012-11-29 16:15:12 -04:00
AP_InertialSensor: shift to new parameter indexes for accel calibration this allows for users to switch between development trees and previous stable versions while retaining their accel calibration values.
2015-03-11 20:17:11 -03:00
/*
The following parameter indexes and reserved from previous use
as accel offsets and scaling from before the 16g change in the
PX4 backend:
ACCSCAL : 1
ACCOFFS : 2
AP_InertialSensor: replaced INS_MPU6K_FILTER with INS_ACCEL_FILTER and INS_GYRO_FILTER this allows filtering to be set separately on accels and gyros where possible
2015-03-11 21:58:36 -03:00
MPU6K_FILTER: 4
AP_InertialSensor: shift to new parameter indexes for accel calibration this allows for users to switch between development trees and previous stable versions while retaining their accel calibration values.
2015-03-11 20:17:11 -03:00
ACC2SCAL : 5
ACC2OFFS : 6
ACC3SCAL : 8
ACC3OFFS : 9
AP_InertialSensor: removed INS_CALSENSFRAME it is no longer needed as we have shifted the accel cal indexes
2015-03-11 20:22:52 -03:00
CALSENSFRAME : 11
AP_InertialSensor: shift to new parameter indexes for accel calibration this allows for users to switch between development trees and previous stable versions while retaining their accel calibration values.
2015-03-11 20:17:11 -03:00
*/
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _GYROFFS_X
AP_InertialSensor: shift to new parameter indexes for accel calibration this allows for users to switch between development trees and previous stable versions while retaining their accel calibration values.
2015-03-11 20:17:11 -03:00
// @DisplayName: Gyro offsets of X axis
// @Description: Gyro sensor offsets of X axis. This is setup on each boot during gyro calibrations
// @Units: rad/s
// @User: Advanced
AP_InertialSensor: mark some parameters as Calibration parameters
2020-02-03 22:03:35 -04:00
// @Calibration: 1
AP_InertialSensor: shift to new parameter indexes for accel calibration this allows for users to switch between development trees and previous stable versions while retaining their accel calibration values.
2015-03-11 20:17:11 -03:00
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _GYROFFS_Y
AP_InertialSensor: shift to new parameter indexes for accel calibration this allows for users to switch between development trees and previous stable versions while retaining their accel calibration values.
2015-03-11 20:17:11 -03:00
// @DisplayName: Gyro offsets of Y axis
// @Description: Gyro sensor offsets of Y axis. This is setup on each boot during gyro calibrations
// @Units: rad/s
// @User: Advanced
AP_InertialSensor: mark some parameters as Calibration parameters
2020-02-03 22:03:35 -04:00
// @Calibration: 1
AP_InertialSensor: shift to new parameter indexes for accel calibration this allows for users to switch between development trees and previous stable versions while retaining their accel calibration values.
2015-03-11 20:17:11 -03:00
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _GYROFFS_Z
AP_InertialSensor: shift to new parameter indexes for accel calibration this allows for users to switch between development trees and previous stable versions while retaining their accel calibration values.
2015-03-11 20:17:11 -03:00
// @DisplayName: Gyro offsets of Z axis
// @Description: Gyro sensor offsets of Z axis. This is setup on each boot during gyro calibrations
// @Units: rad/s
// @User: Advanced
AP_InertialSensor: mark some parameters as Calibration parameters
2020-02-03 22:03:35 -04:00
// @Calibration: 1
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
AP_GROUPINFO("_GYROFFS", 3, AP_InertialSensor, _gyro_offset_old_param[0], 0),
AP_InertialSensor: shift to new parameter indexes for accel calibration this allows for users to switch between development trees and previous stable versions while retaining their accel calibration values.
2015-03-11 20:17:11 -03:00
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _GYR2OFFS_X
AP_InertialSensor: shift to new parameter indexes for accel calibration this allows for users to switch between development trees and previous stable versions while retaining their accel calibration values.
2015-03-11 20:17:11 -03:00
// @DisplayName: Gyro2 offsets of X axis
// @Description: Gyro2 sensor offsets of X axis. This is setup on each boot during gyro calibrations
// @Units: rad/s
// @User: Advanced
AP_InertialSensor: mark some parameters as Calibration parameters
2020-02-03 22:03:35 -04:00
// @Calibration: 1
AP_InertialSensor: shift to new parameter indexes for accel calibration this allows for users to switch between development trees and previous stable versions while retaining their accel calibration values.
2015-03-11 20:17:11 -03:00
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _GYR2OFFS_Y
AP_InertialSensor: shift to new parameter indexes for accel calibration this allows for users to switch between development trees and previous stable versions while retaining their accel calibration values.
2015-03-11 20:17:11 -03:00
// @DisplayName: Gyro2 offsets of Y axis
// @Description: Gyro2 sensor offsets of Y axis. This is setup on each boot during gyro calibrations
// @Units: rad/s
// @User: Advanced
AP_InertialSensor: mark some parameters as Calibration parameters
2020-02-03 22:03:35 -04:00
// @Calibration: 1
AP_InertialSensor: shift to new parameter indexes for accel calibration this allows for users to switch between development trees and previous stable versions while retaining their accel calibration values.
2015-03-11 20:17:11 -03:00
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _GYR2OFFS_Z
AP_InertialSensor: shift to new parameter indexes for accel calibration this allows for users to switch between development trees and previous stable versions while retaining their accel calibration values.
2015-03-11 20:17:11 -03:00
// @DisplayName: Gyro2 offsets of Z axis
// @Description: Gyro2 sensor offsets of Z axis. This is setup on each boot during gyro calibrations
// @Units: rad/s
// @User: Advanced
AP_InertialSensor: mark some parameters as Calibration parameters
2020-02-03 22:03:35 -04:00
// @Calibration: 1
AP_InertialSensor: cope with INS_MAX_INSTANCES < 3
2020-11-07 02:43:06 -04:00
#if INS_MAX_INSTANCES > 1
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
AP_GROUPINFO("_GYR2OFFS", 7, AP_InertialSensor, _gyro_offset_old_param[1], 0),
AP_InertialSensor: cope with INS_MAX_INSTANCES < 3
2020-11-07 02:43:06 -04:00
#endif
AP_InertialSensor: shift to new parameter indexes for accel calibration this allows for users to switch between development trees and previous stable versions while retaining their accel calibration values.
2015-03-11 20:17:11 -03:00
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _GYR3OFFS_X
AP_InertialSensor: shift to new parameter indexes for accel calibration this allows for users to switch between development trees and previous stable versions while retaining their accel calibration values.
2015-03-11 20:17:11 -03:00
// @DisplayName: Gyro3 offsets of X axis
// @Description: Gyro3 sensor offsets of X axis. This is setup on each boot during gyro calibrations
// @Units: rad/s
// @User: Advanced
AP_InertialSensor: mark some parameters as Calibration parameters
2020-02-03 22:03:35 -04:00
// @Calibration: 1
AP_InertialSensor: shift to new parameter indexes for accel calibration this allows for users to switch between development trees and previous stable versions while retaining their accel calibration values.
2015-03-11 20:17:11 -03:00
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _GYR3OFFS_Y
AP_InertialSensor: shift to new parameter indexes for accel calibration this allows for users to switch between development trees and previous stable versions while retaining their accel calibration values.
2015-03-11 20:17:11 -03:00
// @DisplayName: Gyro3 offsets of Y axis
// @Description: Gyro3 sensor offsets of Y axis. This is setup on each boot during gyro calibrations
// @Units: rad/s
// @User: Advanced
AP_InertialSensor: mark some parameters as Calibration parameters
2020-02-03 22:03:35 -04:00
// @Calibration: 1
AP_InertialSensor: shift to new parameter indexes for accel calibration this allows for users to switch between development trees and previous stable versions while retaining their accel calibration values.
2015-03-11 20:17:11 -03:00
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _GYR3OFFS_Z
AP_InertialSensor: shift to new parameter indexes for accel calibration this allows for users to switch between development trees and previous stable versions while retaining their accel calibration values.
2015-03-11 20:17:11 -03:00
// @DisplayName: Gyro3 offsets of Z axis
// @Description: Gyro3 sensor offsets of Z axis. This is setup on each boot during gyro calibrations
// @Units: rad/s
// @User: Advanced
AP_InertialSensor: mark some parameters as Calibration parameters
2020-02-03 22:03:35 -04:00
// @Calibration: 1
AP_InertialSensor: cope with INS_MAX_INSTANCES < 3
2020-11-07 02:43:06 -04:00
#if INS_MAX_INSTANCES > 2
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
AP_GROUPINFO("_GYR3OFFS", 10, AP_InertialSensor, _gyro_offset_old_param[2], 0),
AP_InertialSensor: cope with INS_MAX_INSTANCES < 3
2020-11-07 02:43:06 -04:00
#endif
AP_InertialSensor: shift to new parameter indexes for accel calibration this allows for users to switch between development trees and previous stable versions while retaining their accel calibration values.
2015-03-11 20:17:11 -03:00
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _ACCSCAL_X
AP_InertialSensor: added parameter descriptions
2013-01-02 03:31:58 -04:00
// @DisplayName: Accelerometer scaling of X axis
// @Description: Accelerometer scaling of X axis. Calculated during acceleration calibration routine
AP_InertialSensor: fixed parameter markup
2013-05-21 04:00:22 -03:00
// @Range: 0.8 1.2
AP_InertialSensor: added parameter descriptions
2013-01-02 03:31:58 -04:00
// @User: Advanced
AP_InertialSensor: mark some parameters as Calibration parameters
2020-02-03 22:03:35 -04:00
// @Calibration: 1
AP_InertialSensor: added parameter descriptions
2013-01-02 03:31:58 -04:00
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _ACCSCAL_Y
AP_InertialSensor: added parameter descriptions
2013-01-02 03:31:58 -04:00
// @DisplayName: Accelerometer scaling of Y axis
// @Description: Accelerometer scaling of Y axis Calculated during acceleration calibration routine
AP_InertialSensor: fixed parameter markup
2013-05-21 04:00:22 -03:00
// @Range: 0.8 1.2
AP_InertialSensor: added parameter descriptions
2013-01-02 03:31:58 -04:00
// @User: Advanced
AP_InertialSensor: mark some parameters as Calibration parameters
2020-02-03 22:03:35 -04:00
// @Calibration: 1
AP_InertialSensor: added parameter descriptions
2013-01-02 03:31:58 -04:00
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _ACCSCAL_Z
AP_InertialSensor: added parameter descriptions
2013-01-02 03:31:58 -04:00
// @DisplayName: Accelerometer scaling of Z axis
// @Description: Accelerometer scaling of Z axis Calculated during acceleration calibration routine
AP_InertialSensor: fixed parameter markup
2013-05-21 04:00:22 -03:00
// @Range: 0.8 1.2
AP_InertialSensor: Added INS_MPU6K_FILTER option this allows the user to select the MPU6000 filtering frequency
2012-11-29 16:15:12 -04:00
// @User: Advanced
AP_InertialSensor: mark some parameters as Calibration parameters
2020-02-03 22:03:35 -04:00
// @Calibration: 1
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
AP_GROUPINFO("_ACCSCAL", 12, AP_InertialSensor, _accel_scale_old_param[0], 1.0),
AP_InertialSensor: Added INS_MPU6K_FILTER option this allows the user to select the MPU6000 filtering frequency
2012-11-29 16:15:12 -04:00
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _ACCOFFS_X
AP_InertialSensor: added parameter descriptions
2013-01-02 03:31:58 -04:00
// @DisplayName: Accelerometer offsets of X axis
// @Description: Accelerometer offsets of X axis. This is setup using the acceleration calibration or level operations
// @Units: m/s/s
INS: accel offset parameter range desc to 3.5m/s/s
2015-08-10 14:30:34 -03:00
// @Range: -3.5 3.5
AP_InertialSensor: added parameter descriptions
2013-01-02 03:31:58 -04:00
// @User: Advanced
AP_InertialSensor: mark some parameters as Calibration parameters
2020-02-03 22:03:35 -04:00
// @Calibration: 1
AP_InertialSensor: added parameter descriptions
2013-01-02 03:31:58 -04:00
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _ACCOFFS_Y
AP_InertialSensor: added parameter descriptions
2013-01-02 03:31:58 -04:00
// @DisplayName: Accelerometer offsets of Y axis
// @Description: Accelerometer offsets of Y axis. This is setup using the acceleration calibration or level operations
// @Units: m/s/s
INS: accel offset parameter range desc to 3.5m/s/s
2015-08-10 14:30:34 -03:00
// @Range: -3.5 3.5
AP_InertialSensor: added parameter descriptions
2013-01-02 03:31:58 -04:00
// @User: Advanced
AP_InertialSensor: mark some parameters as Calibration parameters
2020-02-03 22:03:35 -04:00
// @Calibration: 1
AP_InertialSensor: added parameter descriptions
2013-01-02 03:31:58 -04:00
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _ACCOFFS_Z
AP_InertialSensor: added parameter descriptions
2013-01-02 03:31:58 -04:00
// @DisplayName: Accelerometer offsets of Z axis
// @Description: Accelerometer offsets of Z axis. This is setup using the acceleration calibration or level operations
AP_InertialSensor: Added INS_MPU6K_FILTER option this allows the user to select the MPU6000 filtering frequency
2012-11-29 16:15:12 -04:00
// @Units: m/s/s
INS: accel offset parameter range desc to 3.5m/s/s
2015-08-10 14:30:34 -03:00
// @Range: -3.5 3.5
AP_InertialSensor: Added INS_MPU6K_FILTER option this allows the user to select the MPU6000 filtering frequency
2012-11-29 16:15:12 -04:00
// @User: Advanced
AP_InertialSensor: mark some parameters as Calibration parameters
2020-02-03 22:03:35 -04:00
// @Calibration: 1
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
AP_GROUPINFO("_ACCOFFS", 13, AP_InertialSensor, _accel_offset_old_param[0], 0),
AP_InertialSensor: Added INS_MPU6K_FILTER option this allows the user to select the MPU6000 filtering frequency
2012-11-29 16:15:12 -04:00
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _ACC2SCAL_X
INS: param descriptions for ACC2, GYR2
2014-09-27 09:05:33 -03:00
// @DisplayName: Accelerometer2 scaling of X axis
// @Description: Accelerometer2 scaling of X axis. Calculated during acceleration calibration routine
// @Range: 0.8 1.2
// @User: Advanced
AP_InertialSensor: mark some parameters as Calibration parameters
2020-02-03 22:03:35 -04:00
// @Calibration: 1
INS: param descriptions for ACC2, GYR2
2014-09-27 09:05:33 -03:00
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _ACC2SCAL_Y
INS: param descriptions for ACC2, GYR2
2014-09-27 09:05:33 -03:00
// @DisplayName: Accelerometer2 scaling of Y axis
// @Description: Accelerometer2 scaling of Y axis Calculated during acceleration calibration routine
// @Range: 0.8 1.2
// @User: Advanced
AP_InertialSensor: mark some parameters as Calibration parameters
2020-02-03 22:03:35 -04:00
// @Calibration: 1
INS: param descriptions for ACC2, GYR2
2014-09-27 09:05:33 -03:00
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _ACC2SCAL_Z
INS: param descriptions for ACC2, GYR2
2014-09-27 09:05:33 -03:00
// @DisplayName: Accelerometer2 scaling of Z axis
// @Description: Accelerometer2 scaling of Z axis Calculated during acceleration calibration routine
// @Range: 0.8 1.2
// @User: Advanced
AP_InertialSensor: mark some parameters as Calibration parameters
2020-02-03 22:03:35 -04:00
// @Calibration: 1
AP_InertialSensor: cope with INS_MAX_INSTANCES < 3
2020-11-07 02:43:06 -04:00
#if INS_MAX_INSTANCES > 1
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
AP_GROUPINFO("_ACC2SCAL", 14, AP_InertialSensor, _accel_scale_old_param[1], 1.0),
AP_InertialSensor: cope with INS_MAX_INSTANCES < 3
2020-11-07 02:43:06 -04:00
#endif
INS: param descriptions for ACC2, GYR2
2014-09-27 09:05:33 -03:00
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _ACC2OFFS_X
INS: param descriptions for ACC2, GYR2
2014-09-27 09:05:33 -03:00
// @DisplayName: Accelerometer2 offsets of X axis
// @Description: Accelerometer2 offsets of X axis. This is setup using the acceleration calibration or level operations
// @Units: m/s/s
INS: accel offset parameter range desc to 3.5m/s/s
2015-08-10 14:30:34 -03:00
// @Range: -3.5 3.5
INS: param descriptions for ACC2, GYR2
2014-09-27 09:05:33 -03:00
// @User: Advanced
AP_InertialSensor: mark some parameters as Calibration parameters
2020-02-03 22:03:35 -04:00
// @Calibration: 1
INS: param descriptions for ACC2, GYR2
2014-09-27 09:05:33 -03:00
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _ACC2OFFS_Y
INS: param descriptions for ACC2, GYR2
2014-09-27 09:05:33 -03:00
// @DisplayName: Accelerometer2 offsets of Y axis
// @Description: Accelerometer2 offsets of Y axis. This is setup using the acceleration calibration or level operations
// @Units: m/s/s
INS: accel offset parameter range desc to 3.5m/s/s
2015-08-10 14:30:34 -03:00
// @Range: -3.5 3.5
INS: param descriptions for ACC2, GYR2
2014-09-27 09:05:33 -03:00
// @User: Advanced
AP_InertialSensor: mark some parameters as Calibration parameters
2020-02-03 22:03:35 -04:00
// @Calibration: 1
INS: param descriptions for ACC2, GYR2
2014-09-27 09:05:33 -03:00
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _ACC2OFFS_Z
INS: param descriptions for ACC2, GYR2
2014-09-27 09:05:33 -03:00
// @DisplayName: Accelerometer2 offsets of Z axis
// @Description: Accelerometer2 offsets of Z axis. This is setup using the acceleration calibration or level operations
// @Units: m/s/s
INS: accel offset parameter range desc to 3.5m/s/s
2015-08-10 14:30:34 -03:00
// @Range: -3.5 3.5
INS: param descriptions for ACC2, GYR2
2014-09-27 09:05:33 -03:00
// @User: Advanced
AP_InertialSensor: mark some parameters as Calibration parameters
2020-02-03 22:03:35 -04:00
// @Calibration: 1
AP_InertialSensor: cope with INS_MAX_INSTANCES < 3
2020-11-07 02:43:06 -04:00
#if INS_MAX_INSTANCES > 1
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
AP_GROUPINFO("_ACC2OFFS", 15, AP_InertialSensor, _accel_offset_old_param[1], 0),
AP_InertialSensor: cope with INS_MAX_INSTANCES < 3
2020-11-07 02:43:06 -04:00
#endif
AP_InertialSensor: generalise the accel/gyro calibration for N sensors
2013-12-08 18:50:12 -04:00
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _ACC3SCAL_X
INS: param descriptions for ACC2, GYR2
2014-09-27 09:05:33 -03:00
// @DisplayName: Accelerometer3 scaling of X axis
// @Description: Accelerometer3 scaling of X axis. Calculated during acceleration calibration routine
// @Range: 0.8 1.2
// @User: Advanced
AP_InertialSensor: mark some parameters as Calibration parameters
2020-02-03 22:03:35 -04:00
// @Calibration: 1
INS: param descriptions for ACC2, GYR2
2014-09-27 09:05:33 -03:00
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _ACC3SCAL_Y
INS: param descriptions for ACC2, GYR2
2014-09-27 09:05:33 -03:00
// @DisplayName: Accelerometer3 scaling of Y axis
// @Description: Accelerometer3 scaling of Y axis Calculated during acceleration calibration routine
// @Range: 0.8 1.2
// @User: Advanced
AP_InertialSensor: mark some parameters as Calibration parameters
2020-02-03 22:03:35 -04:00
// @Calibration: 1
INS: param descriptions for ACC2, GYR2
2014-09-27 09:05:33 -03:00
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _ACC3SCAL_Z
INS: param descriptions for ACC2, GYR2
2014-09-27 09:05:33 -03:00
// @DisplayName: Accelerometer3 scaling of Z axis
// @Description: Accelerometer3 scaling of Z axis Calculated during acceleration calibration routine
// @Range: 0.8 1.2
// @User: Advanced
AP_InertialSensor: mark some parameters as Calibration parameters
2020-02-03 22:03:35 -04:00
// @Calibration: 1
AP_InertialSensor: cope with INS_MAX_INSTANCES < 3
2020-11-07 02:43:06 -04:00
#if INS_MAX_INSTANCES > 2
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
AP_GROUPINFO("_ACC3SCAL", 16, AP_InertialSensor, _accel_scale_old_param[2], 1.0),
AP_InertialSensor: cope with INS_MAX_INSTANCES < 3
2020-11-07 02:43:06 -04:00
#endif
INS: param descriptions for ACC2, GYR2
2014-09-27 09:05:33 -03:00
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _ACC3OFFS_X
INS: param descriptions for ACC2, GYR2
2014-09-27 09:05:33 -03:00
// @DisplayName: Accelerometer3 offsets of X axis
// @Description: Accelerometer3 offsets of X axis. This is setup using the acceleration calibration or level operations
// @Units: m/s/s
INS: accel offset parameter range desc to 3.5m/s/s
2015-08-10 14:30:34 -03:00
// @Range: -3.5 3.5
INS: param descriptions for ACC2, GYR2
2014-09-27 09:05:33 -03:00
// @User: Advanced
AP_InertialSensor: mark some parameters as Calibration parameters
2020-02-03 22:03:35 -04:00
// @Calibration: 1
INS: param descriptions for ACC2, GYR2
2014-09-27 09:05:33 -03:00
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _ACC3OFFS_Y
INS: param descriptions for ACC2, GYR2
2014-09-27 09:05:33 -03:00
// @DisplayName: Accelerometer3 offsets of Y axis
// @Description: Accelerometer3 offsets of Y axis. This is setup using the acceleration calibration or level operations
// @Units: m/s/s
INS: accel offset parameter range desc to 3.5m/s/s
2015-08-10 14:30:34 -03:00
// @Range: -3.5 3.5
INS: param descriptions for ACC2, GYR2
2014-09-27 09:05:33 -03:00
// @User: Advanced
AP_InertialSensor: mark some parameters as Calibration parameters
2020-02-03 22:03:35 -04:00
// @Calibration: 1
INS: param descriptions for ACC2, GYR2
2014-09-27 09:05:33 -03:00
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _ACC3OFFS_Z
INS: param descriptions for ACC2, GYR2
2014-09-27 09:05:33 -03:00
// @DisplayName: Accelerometer3 offsets of Z axis
// @Description: Accelerometer3 offsets of Z axis. This is setup using the acceleration calibration or level operations
// @Units: m/s/s
INS: accel offset parameter range desc to 3.5m/s/s
2015-08-10 14:30:34 -03:00
// @Range: -3.5 3.5
INS: param descriptions for ACC2, GYR2
2014-09-27 09:05:33 -03:00
// @User: Advanced
AP_InertialSensor: mark some parameters as Calibration parameters
2020-02-03 22:03:35 -04:00
// @Calibration: 1
AP_InertialSensor: cope with INS_MAX_INSTANCES < 3
2020-11-07 02:43:06 -04:00
#if INS_MAX_INSTANCES > 2
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
AP_GROUPINFO("_ACC3OFFS", 17, AP_InertialSensor, _accel_offset_old_param[2], 0),
AP_InertialSensor: cope with INS_MAX_INSTANCES < 3
2020-11-07 02:43:06 -04:00
#endif
AP_InertialSensor: allow for 3 accels and 3 gyros on Pixhawk this supports FMUv3
2014-06-26 01:04:33 -03:00
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _GYRO_FILTER
AP_InertialSensor: replaced INS_MPU6K_FILTER with INS_ACCEL_FILTER and INS_GYRO_FILTER this allows filtering to be set separately on accels and gyros where possible
2015-03-11 21:58:36 -03:00
// @DisplayName: Gyro filter cutoff frequency
AP_InertialSensor: correct reboot documentation for batch logger and filters
2019-09-09 05:29:34 -03:00
// @Description: Filter cutoff frequency for gyroscopes. This can be set to a lower value to try to cope with very high vibration levels in aircraft. A value of zero means no filtering (not recommended!)
AP_InertialSensor: replaced INS_MPU6K_FILTER with INS_ACCEL_FILTER and INS_GYRO_FILTER this allows filtering to be set separately on accels and gyros where possible
2015-03-11 21:58:36 -03:00
// @Units: Hz
AP_InertialSensor: Allow all filter frequencies to be 16bit.
2019-05-25 07:59:57 -03:00
// @Range: 0 256
AP_InertialSensor: replaced INS_MPU6K_FILTER with INS_ACCEL_FILTER and INS_GYRO_FILTER this allows filtering to be set separately on accels and gyros where possible
2015-03-11 21:58:36 -03:00
// @User: Advanced
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
AP_GROUPINFO("_GYRO_FILTER", 18, AP_InertialSensor, _gyro_filter_cutoff, DEFAULT_GYRO_FILTER),
AP_InertialSensor: replaced INS_MPU6K_FILTER with INS_ACCEL_FILTER and INS_GYRO_FILTER this allows filtering to be set separately on accels and gyros where possible
2015-03-11 21:58:36 -03:00
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _ACCEL_FILTER
AP_InertialSensor: replaced INS_MPU6K_FILTER with INS_ACCEL_FILTER and INS_GYRO_FILTER this allows filtering to be set separately on accels and gyros where possible
2015-03-11 21:58:36 -03:00
// @DisplayName: Accel filter cutoff frequency
AP_InertialSensor: correct reboot documentation for batch logger and filters
2019-09-09 05:29:34 -03:00
// @Description: Filter cutoff frequency for accelerometers. This can be set to a lower value to try to cope with very high vibration levels in aircraft. A value of zero means no filtering (not recommended!)
AP_InertialSensor: replaced INS_MPU6K_FILTER with INS_ACCEL_FILTER and INS_GYRO_FILTER this allows filtering to be set separately on accels and gyros where possible
2015-03-11 21:58:36 -03:00
// @Units: Hz
AP_InertialSensor: Allow all filter frequencies to be 16bit.
2019-05-25 07:59:57 -03:00
// @Range: 0 256
AP_InertialSensor: replaced INS_MPU6K_FILTER with INS_ACCEL_FILTER and INS_GYRO_FILTER this allows filtering to be set separately on accels and gyros where possible
2015-03-11 21:58:36 -03:00
// @User: Advanced
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
AP_GROUPINFO("_ACCEL_FILTER", 19, AP_InertialSensor, _accel_filter_cutoff, DEFAULT_ACCEL_FILTER),
AP_InertialSensor: replaced INS_MPU6K_FILTER with INS_ACCEL_FILTER and INS_GYRO_FILTER this allows filtering to be set separately on accels and gyros where possible
2015-03-11 21:58:36 -03:00
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _USE
INS: add USE parameters
2015-08-01 03:40:40 -03:00
// @DisplayName: Use first IMU for attitude, velocity and position estimates
// @Description: Use first IMU for attitude, velocity and position estimates
// @Values: 0:Disabled,1:Enabled
// @User: Advanced
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
AP_GROUPINFO("_USE", 20, AP_InertialSensor, _use_old_param[0], 1),
INS: add USE parameters
2015-08-01 03:40:40 -03:00
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _USE2
INS: add USE parameters
2015-08-01 03:40:40 -03:00
// @DisplayName: Use second IMU for attitude, velocity and position estimates
// @Description: Use second IMU for attitude, velocity and position estimates
// @Values: 0:Disabled,1:Enabled
// @User: Advanced
AP_InertialSensor: cope with INS_MAX_INSTANCES < 3
2020-11-07 02:43:06 -04:00
#if INS_MAX_INSTANCES > 1
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
AP_GROUPINFO("_USE2", 21, AP_InertialSensor, _use_old_param[1], 1),
AP_InertialSensor: cope with INS_MAX_INSTANCES < 3
2020-11-07 02:43:06 -04:00
#endif
AP_InertialSensor: remove check for max INS instances For all supported boards the maximum number of instances is 3.
2015-10-14 12:47:20 -03:00
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _USE3
INS: add USE parameters
2015-08-01 03:40:40 -03:00
// @DisplayName: Use third IMU for attitude, velocity and position estimates
// @Description: Use third IMU for attitude, velocity and position estimates
// @Values: 0:Disabled,1:Enabled
// @User: Advanced
AP_InertialSensor: cope with INS_MAX_INSTANCES < 3
2020-11-07 02:43:06 -04:00
#if INS_MAX_INSTANCES > 2
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
AP_GROUPINFO("_USE3", 22, AP_InertialSensor, _use_old_param[2], 1),
AP_InertialSensor: cope with INS_MAX_INSTANCES < 3
2020-11-07 02:43:06 -04:00
#endif
INS: add USE parameters
2015-08-01 03:40:40 -03:00
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _STILL_THRESH
AP_InertialSensor: Added is_still() check very strict check that all axis are not vibrating much at all new param: INS_STILL_THRESH used to be a vibration threshold for different platforms // @Description: Threshold to tolerate vibration to determine if vehicle is motionless. This depends on the frame type and if there is a constant vibration due to motors before launch or after landing. Total motionless is about 0.05. Suggested values: Planes/rover use 0.1, multirotors use 1, tradHeli uses 5
2015-07-29 16:34:48 -03:00
// @DisplayName: Stillness threshold for detecting if we are moving
// @Description: Threshold to tolerate vibration to determine if vehicle is motionless. This depends on the frame type and if there is a constant vibration due to motors before launch or after landing. Total motionless is about 0.05. Suggested values: Planes/rover use 0.1, multirotors use 1, tradHeli uses 5
AP_InertialSensor: fix invalid Range meta data
2015-12-27 01:08:32 -04:00
// @Range: 0.05 50
AP_InertialSensor: Added is_still() check very strict check that all axis are not vibrating much at all new param: INS_STILL_THRESH used to be a vibration threshold for different platforms // @Description: Threshold to tolerate vibration to determine if vehicle is motionless. This depends on the frame type and if there is a constant vibration due to motors before launch or after landing. Total motionless is about 0.05. Suggested values: Planes/rover use 0.1, multirotors use 1, tradHeli uses 5
2015-07-29 16:34:48 -03:00
// @User: Advanced
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
AP_GROUPINFO("_STILL_THRESH", 23, AP_InertialSensor, _still_threshold, DEFAULT_STILL_THRESH),
AP_InertialSensor: Added is_still() check very strict check that all axis are not vibrating much at all new param: INS_STILL_THRESH used to be a vibration threshold for different platforms // @Description: Threshold to tolerate vibration to determine if vehicle is motionless. This depends on the frame type and if there is a constant vibration due to motors before launch or after landing. Total motionless is about 0.05. Suggested values: Planes/rover use 0.1, multirotors use 1, tradHeli uses 5
2015-07-29 16:34:48 -03:00
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _GYR_CAL
InertialSensor: add GYR_CAL to control when gyro calibration occurs
2015-09-17 03:42:41 -03:00
// @DisplayName: Gyro Calibration scheme
// @Description: Conrols when automatic gyro calibration is performed
InertialSensor: remove gyro cal on first arming
2015-10-15 03:06:34 -03:00
// @Values: 0:Never, 1:Start-up only
InertialSensor: add GYR_CAL to control when gyro calibration occurs
2015-09-17 03:42:41 -03:00
// @User: Advanced
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
AP_GROUPINFO("_GYR_CAL", 24, AP_InertialSensor, _gyro_cal_timing, 1),
InertialSensor: add GYR_CAL to control when gyro calibration occurs
2015-09-17 03:42:41 -03:00
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _TRIM_OPTION
AP_InertialSensor: fixes in response to review
2015-12-30 02:51:27 -04:00
// @DisplayName: Accel cal trim option
// @Description: Specifies how the accel cal routine determines the trims
AP_InertialSensor: support AP_AccelCal
2015-07-20 17:25:40 -03:00
// @User: Advanced
AP_InertialSensor: fixes in response to review
2015-12-30 02:51:27 -04:00
// @Values: 0:Don't adjust the trims,1:Assume first orientation was level,2:Assume ACC_BODYFIX is perfectly aligned to the vehicle
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
AP_GROUPINFO("_TRIM_OPTION", 25, AP_InertialSensor, _trim_option, 1),
AP_InertialSensor: support AP_AccelCal
2015-07-20 17:25:40 -03:00
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _ACC_BODYFIX
AP_InertialSensor: fixes in response to review
2015-12-30 02:51:27 -04:00
// @DisplayName: Body-fixed accelerometer
// @Description: The body-fixed accelerometer to be used for trim calculation
AP_InertialSensor: support AP_AccelCal
2015-07-20 17:25:40 -03:00
// @User: Advanced
AP_InertialSensor: fixes in response to review
2015-12-30 02:51:27 -04:00
// @Values: 1:IMU 1,2:IMU 2,3:IMU 3
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
AP_GROUPINFO("_ACC_BODYFIX", 26, AP_InertialSensor, _acc_body_aligned, 2),
AP_InertialSensor: Add parameters defining accelerometer position offset
2016-10-07 19:11:10 -03:00
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _POS1_X
AP_InertialSensor: Add parameters defining accelerometer position offset
2016-10-07 19:11:10 -03:00
// @DisplayName: IMU accelerometer X position
AP_InertialSensor: Update parameter documentation Add brief description of body frame conventions.
2016-10-18 19:24:35 -03:00
// @Description: X position of the first IMU Accelerometer in body frame. Positive X is forward of the origin. Attention: The IMU should be located as close to the vehicle c.g. as practical so that the value of this parameter is minimised. Failure to do so can result in noisy navigation velocity measurements due to vibration and IMU gyro noise. If the IMU cannot be moved and velocity noise is a problem, a location closer to the IMU can be used as the body frame origin.
AP_InertialSensor: Add parameters defining accelerometer position offset
2016-10-07 19:11:10 -03:00
// @Units: m
AP_InertialSensor: reduce POS param range to 5m and add increment
2020-01-30 00:31:26 -04:00
// @Range: -5 5
// @Increment: 0.01
AP_InertialSensor: Add parameters defining accelerometer position offset
2016-10-07 19:11:10 -03:00
// @User: Advanced
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _POS1_Y
AP_InertialSensor: Add parameters defining accelerometer position offset
2016-10-07 19:11:10 -03:00
// @DisplayName: IMU accelerometer Y position
AP_InertialSensor: Update parameter documentation Add brief description of body frame conventions.
2016-10-18 19:24:35 -03:00
// @Description: Y position of the first IMU accelerometer in body frame. Positive Y is to the right of the origin. Attention: The IMU should be located as close to the vehicle c.g. as practical so that the value of this parameter is minimised. Failure to do so can result in noisy navigation velocity measurements due to vibration and IMU gyro noise. If the IMU cannot be moved and velocity noise is a problem, a location closer to the IMU can be used as the body frame origin.
AP_InertialSensor: Add parameters defining accelerometer position offset
2016-10-07 19:11:10 -03:00
// @Units: m
AP_InertialSensor: reduce POS param range to 5m and add increment
2020-01-30 00:31:26 -04:00
// @Range: -5 5
// @Increment: 0.01
AP_InertialSensor: Add parameters defining accelerometer position offset
2016-10-07 19:11:10 -03:00
// @User: Advanced
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _POS1_Z
AP_InertialSensor: Add parameters defining accelerometer position offset
2016-10-07 19:11:10 -03:00
// @DisplayName: IMU accelerometer Z position
AP_InertialSensor: Update parameter documentation Add brief description of body frame conventions.
2016-10-18 19:24:35 -03:00
// @Description: Z position of the first IMU accelerometer in body frame. Positive Z is down from the origin. Attention: The IMU should be located as close to the vehicle c.g. as practical so that the value of this parameter is minimised. Failure to do so can result in noisy navigation velocity measurements due to vibration and IMU gyro noise. If the IMU cannot be moved and velocity noise is a problem, a location closer to the IMU can be used as the body frame origin.
AP_InertialSensor: Add parameters defining accelerometer position offset
2016-10-07 19:11:10 -03:00
// @Units: m
AP_InertialSensor: reduce POS param range to 5m and add increment
2020-01-30 00:31:26 -04:00
// @Range: -5 5
// @Increment: 0.01
AP_InertialSensor: Add parameters defining accelerometer position offset
2016-10-07 19:11:10 -03:00
// @User: Advanced
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
AP_GROUPINFO("_POS1", 27, AP_InertialSensor, _accel_pos_old_param[0], 0.0f),
AP_InertialSensor: Add parameters defining accelerometer position offset
2016-10-07 19:11:10 -03:00
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _POS2_X
AP_InertialSensor: Add parameters defining accelerometer position offset
2016-10-07 19:11:10 -03:00
// @DisplayName: IMU accelerometer X position
AP_InertialSensor: Update parameter documentation Add brief description of body frame conventions.
2016-10-18 19:24:35 -03:00
// @Description: X position of the second IMU accelerometer in body frame. Positive X is forward of the origin. Attention: The IMU should be located as close to the vehicle c.g. as practical so that the value of this parameter is minimised. Failure to do so can result in noisy navigation velocity measurements due to vibration and IMU gyro noise. If the IMU cannot be moved and velocity noise is a problem, a location closer to the IMU can be used as the body frame origin.
AP_InertialSensor: Add parameters defining accelerometer position offset
2016-10-07 19:11:10 -03:00
// @Units: m
AP_InertialSensor: reduce POS param range to 5m and add increment
2020-01-30 00:31:26 -04:00
// @Range: -5 5
// @Increment: 0.01
AP_InertialSensor: Add parameters defining accelerometer position offset
2016-10-07 19:11:10 -03:00
// @User: Advanced
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _POS2_Y
AP_InertialSensor: Add parameters defining accelerometer position offset
2016-10-07 19:11:10 -03:00
// @DisplayName: IMU accelerometer Y position
AP_InertialSensor: Update parameter documentation Add brief description of body frame conventions.
2016-10-18 19:24:35 -03:00
// @Description: Y position of the second IMU accelerometer in body frame. Positive Y is to the right of the origin. Attention: The IMU should be located as close to the vehicle c.g. as practical so that the value of this parameter is minimised. Failure to do so can result in noisy navigation velocity measurements due to vibration and IMU gyro noise. If the IMU cannot be moved and velocity noise is a problem, a location closer to the IMU can be used as the body frame origin.
AP_InertialSensor: Add parameters defining accelerometer position offset
2016-10-07 19:11:10 -03:00
// @Units: m
AP_InertialSensor: reduce POS param range to 5m and add increment
2020-01-30 00:31:26 -04:00
// @Range: -5 5
// @Increment: 0.01
AP_InertialSensor: Add parameters defining accelerometer position offset
2016-10-07 19:11:10 -03:00
// @User: Advanced
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _POS2_Z
AP_InertialSensor: Add parameters defining accelerometer position offset
2016-10-07 19:11:10 -03:00
// @DisplayName: IMU accelerometer Z position
AP_InertialSensor: Update parameter documentation Add brief description of body frame conventions.
2016-10-18 19:24:35 -03:00
// @Description: Z position of the second IMU accelerometer in body frame. Positive Z is down from the origin. Attention: The IMU should be located as close to the vehicle c.g. as practical so that the value of this parameter is minimised. Failure to do so can result in noisy navigation velocity measurements due to vibration and IMU gyro noise. If the IMU cannot be moved and velocity noise is a problem, a location closer to the IMU can be used as the body frame origin.
AP_InertialSensor: Add parameters defining accelerometer position offset
2016-10-07 19:11:10 -03:00
// @Units: m
AP_InertialSensor: reduce POS param range to 5m and add increment
2020-01-30 00:31:26 -04:00
// @Range: -5 5
// @Increment: 0.01
AP_InertialSensor: Add parameters defining accelerometer position offset
2016-10-07 19:11:10 -03:00
// @User: Advanced
AP_InertialSensor: cope with INS_MAX_INSTANCES < 3
2020-11-07 02:43:06 -04:00
#if INS_MAX_INSTANCES > 1
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
AP_GROUPINFO("_POS2", 28, AP_InertialSensor, _accel_pos_old_param[1], 0.0f),
AP_InertialSensor: cope with INS_MAX_INSTANCES < 3
2020-11-07 02:43:06 -04:00
#endif
AP_InertialSensor: Add parameters defining accelerometer position offset
2016-10-07 19:11:10 -03:00
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _POS3_X
AP_InertialSensor: Add parameters defining accelerometer position offset
2016-10-07 19:11:10 -03:00
// @DisplayName: IMU accelerometer X position
AP_InertialSensor: Update parameter documentation Add brief description of body frame conventions.
2016-10-18 19:24:35 -03:00
// @Description: X position of the third IMU accelerometer in body frame. Positive X is forward of the origin. Attention: The IMU should be located as close to the vehicle c.g. as practical so that the value of this parameter is minimised. Failure to do so can result in noisy navigation velocity measurements due to vibration and IMU gyro noise. If the IMU cannot be moved and velocity noise is a problem, a location closer to the IMU can be used as the body frame origin.
AP_InertialSensor: Add parameters defining accelerometer position offset
2016-10-07 19:11:10 -03:00
// @Units: m
AP_InertialSensor: add range to POS param description
2018-12-13 06:44:08 -04:00
// @Range: -10 10
AP_InertialSensor: Add parameters defining accelerometer position offset
2016-10-07 19:11:10 -03:00
// @User: Advanced
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _POS3_Y
AP_InertialSensor: Add parameters defining accelerometer position offset
2016-10-07 19:11:10 -03:00
// @DisplayName: IMU accelerometer Y position
AP_InertialSensor: Update parameter documentation Add brief description of body frame conventions.
2016-10-18 19:24:35 -03:00
// @Description: Y position of the third IMU accelerometer in body frame. Positive Y is to the right of the origin. Attention: The IMU should be located as close to the vehicle c.g. as practical so that the value of this parameter is minimised. Failure to do so can result in noisy navigation velocity measurements due to vibration and IMU gyro noise. If the IMU cannot be moved and velocity noise is a problem, a location closer to the IMU can be used as the body frame origin.
AP_InertialSensor: Add parameters defining accelerometer position offset
2016-10-07 19:11:10 -03:00
// @Units: m
AP_InertialSensor: reduce POS param range to 5m and add increment
2020-01-30 00:31:26 -04:00
// @Range: -5 5
// @Increment: 0.01
AP_InertialSensor: Add parameters defining accelerometer position offset
2016-10-07 19:11:10 -03:00
// @User: Advanced
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _POS3_Z
AP_InertialSensor: Add parameters defining accelerometer position offset
2016-10-07 19:11:10 -03:00
// @DisplayName: IMU accelerometer Z position
AP_InertialSensor: Update parameter documentation Add brief description of body frame conventions.
2016-10-18 19:24:35 -03:00
// @Description: Z position of the third IMU accelerometer in body frame. Positive Z is down from the origin. Attention: The IMU should be located as close to the vehicle c.g. as practical so that the value of this parameter is minimised. Failure to do so can result in noisy navigation velocity measurements due to vibration and IMU gyro noise. If the IMU cannot be moved and velocity noise is a problem, a location closer to the IMU can be used as the body frame origin.
AP_InertialSensor: Add parameters defining accelerometer position offset
2016-10-07 19:11:10 -03:00
// @Units: m
AP_InertialSensor: reduce POS param range to 5m and add increment
2020-01-30 00:31:26 -04:00
// @Range: -5 5
// @Increment: 0.01
AP_InertialSensor: Add parameters defining accelerometer position offset
2016-10-07 19:11:10 -03:00
// @User: Advanced
AP_InertialSensor: cope with INS_MAX_INSTANCES < 3
2020-11-07 02:43:06 -04:00
#if INS_MAX_INSTANCES > 2
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
AP_GROUPINFO("_POS3", 29, AP_InertialSensor, _accel_pos_old_param[2], 0.0f),
AP_InertialSensor: cope with INS_MAX_INSTANCES < 3
2020-11-07 02:43:06 -04:00
#endif
AP_InertialSensor: Add parameters defining accelerometer position offset
2016-10-07 19:11:10 -03:00
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _GYR_ID
AP_InertialSensor: save id for gyro and accel instances This allows each sensor to be uniquely identified in the system by using either the index inside the backend or for those that use the Device interface, to use the bus type, location, and device id. We leave 16-bit for each sensor to be able to change its own identification in future, which allows them to be changed in an incompatible manner forcing a re-calibration.
2016-09-03 21:51:37 -03:00
// @DisplayName: Gyro ID
// @Description: Gyro sensor ID, taking into account its type, bus and instance
// @ReadOnly: True
// @User: Advanced
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
AP_GROUPINFO("_GYR_ID", 30, AP_InertialSensor, _gyro_id_old_param[0], 0),
AP_InertialSensor: save id for gyro and accel instances This allows each sensor to be uniquely identified in the system by using either the index inside the backend or for those that use the Device interface, to use the bus type, location, and device id. We leave 16-bit for each sensor to be able to change its own identification in future, which allows them to be changed in an incompatible manner forcing a re-calibration.
2016-09-03 21:51:37 -03:00
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _GYR2_ID
AP_InertialSensor: save id for gyro and accel instances This allows each sensor to be uniquely identified in the system by using either the index inside the backend or for those that use the Device interface, to use the bus type, location, and device id. We leave 16-bit for each sensor to be able to change its own identification in future, which allows them to be changed in an incompatible manner forcing a re-calibration.
2016-09-03 21:51:37 -03:00
// @DisplayName: Gyro2 ID
// @Description: Gyro2 sensor ID, taking into account its type, bus and instance
// @ReadOnly: True
// @User: Advanced
AP_InertialSensor: cope with INS_MAX_INSTANCES < 3
2020-11-07 02:43:06 -04:00
#if INS_MAX_INSTANCES > 1
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
AP_GROUPINFO("_GYR2_ID", 31, AP_InertialSensor, _gyro_id_old_param[1], 0),
AP_InertialSensor: cope with INS_MAX_INSTANCES < 3
2020-11-07 02:43:06 -04:00
#endif
AP_InertialSensor: save id for gyro and accel instances This allows each sensor to be uniquely identified in the system by using either the index inside the backend or for those that use the Device interface, to use the bus type, location, and device id. We leave 16-bit for each sensor to be able to change its own identification in future, which allows them to be changed in an incompatible manner forcing a re-calibration.
2016-09-03 21:51:37 -03:00
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _GYR3_ID
AP_InertialSensor: save id for gyro and accel instances This allows each sensor to be uniquely identified in the system by using either the index inside the backend or for those that use the Device interface, to use the bus type, location, and device id. We leave 16-bit for each sensor to be able to change its own identification in future, which allows them to be changed in an incompatible manner forcing a re-calibration.
2016-09-03 21:51:37 -03:00
// @DisplayName: Gyro3 ID
// @Description: Gyro3 sensor ID, taking into account its type, bus and instance
// @ReadOnly: True
// @User: Advanced
AP_InertialSensor: cope with INS_MAX_INSTANCES < 3
2020-11-07 02:43:06 -04:00
#if INS_MAX_INSTANCES > 2
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
AP_GROUPINFO("_GYR3_ID", 32, AP_InertialSensor, _gyro_id_old_param[2], 0),
AP_InertialSensor: cope with INS_MAX_INSTANCES < 3
2020-11-07 02:43:06 -04:00
#endif
AP_InertialSensor: save id for gyro and accel instances This allows each sensor to be uniquely identified in the system by using either the index inside the backend or for those that use the Device interface, to use the bus type, location, and device id. We leave 16-bit for each sensor to be able to change its own identification in future, which allows them to be changed in an incompatible manner forcing a re-calibration.
2016-09-03 21:51:37 -03:00
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _ACC_ID
AP_InertialSensor: save id for gyro and accel instances This allows each sensor to be uniquely identified in the system by using either the index inside the backend or for those that use the Device interface, to use the bus type, location, and device id. We leave 16-bit for each sensor to be able to change its own identification in future, which allows them to be changed in an incompatible manner forcing a re-calibration.
2016-09-03 21:51:37 -03:00
// @DisplayName: Accelerometer ID
// @Description: Accelerometer sensor ID, taking into account its type, bus and instance
// @ReadOnly: True
// @User: Advanced
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
AP_GROUPINFO("_ACC_ID", 33, AP_InertialSensor, _accel_id_old_param[0], 0),
AP_InertialSensor: save id for gyro and accel instances This allows each sensor to be uniquely identified in the system by using either the index inside the backend or for those that use the Device interface, to use the bus type, location, and device id. We leave 16-bit for each sensor to be able to change its own identification in future, which allows them to be changed in an incompatible manner forcing a re-calibration.
2016-09-03 21:51:37 -03:00
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _ACC2_ID
AP_InertialSensor: save id for gyro and accel instances This allows each sensor to be uniquely identified in the system by using either the index inside the backend or for those that use the Device interface, to use the bus type, location, and device id. We leave 16-bit for each sensor to be able to change its own identification in future, which allows them to be changed in an incompatible manner forcing a re-calibration.
2016-09-03 21:51:37 -03:00
// @DisplayName: Accelerometer2 ID
// @Description: Accelerometer2 sensor ID, taking into account its type, bus and instance
// @ReadOnly: True
// @User: Advanced
AP_InertialSensor: cope with INS_MAX_INSTANCES < 3
2020-11-07 02:43:06 -04:00
#if INS_MAX_INSTANCES > 1
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
AP_GROUPINFO("_ACC2_ID", 34, AP_InertialSensor, _accel_id_old_param[1], 0),
AP_InertialSensor: cope with INS_MAX_INSTANCES < 3
2020-11-07 02:43:06 -04:00
#endif
AP_InertialSensor: save id for gyro and accel instances This allows each sensor to be uniquely identified in the system by using either the index inside the backend or for those that use the Device interface, to use the bus type, location, and device id. We leave 16-bit for each sensor to be able to change its own identification in future, which allows them to be changed in an incompatible manner forcing a re-calibration.
2016-09-03 21:51:37 -03:00
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _ACC3_ID
AP_InertialSensor: save id for gyro and accel instances This allows each sensor to be uniquely identified in the system by using either the index inside the backend or for those that use the Device interface, to use the bus type, location, and device id. We leave 16-bit for each sensor to be able to change its own identification in future, which allows them to be changed in an incompatible manner forcing a re-calibration.
2016-09-03 21:51:37 -03:00
// @DisplayName: Accelerometer3 ID
// @Description: Accelerometer3 sensor ID, taking into account its type, bus and instance
// @ReadOnly: True
// @User: Advanced
AP_InertialSensor: cope with INS_MAX_INSTANCES < 3
2020-11-07 02:43:06 -04:00
#if INS_MAX_INSTANCES > 2
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
AP_GROUPINFO("_ACC3_ID", 35, AP_InertialSensor, _accel_id_old_param[2], 0),
AP_InertialSensor: cope with INS_MAX_INSTANCES < 3
2020-11-07 02:43:06 -04:00
#endif
AP_InertialSensor: save id for gyro and accel instances This allows each sensor to be uniquely identified in the system by using either the index inside the backend or for those that use the Device interface, to use the bus type, location, and device id. We leave 16-bit for each sensor to be able to change its own identification in future, which allows them to be changed in an incompatible manner forcing a re-calibration.
2016-09-03 21:51:37 -03:00
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _FAST_SAMPLE
AP_InertialSensor: added INS_FAST_SAMPLE parameter this allows enable/disable of fast sampling per IMU, making experimentation easier. It also fixes the fast sampling to always average over 8 samples, and fixes the 9250 to use the correct accumulator when not doing fast sampling
2016-11-15 01:51:18 -04:00
// @DisplayName: Fast sampling mask
// @Description: Mask of IMUs to enable fast sampling on, if available
// @User: Advanced
AP_InertialSensor: added INS_ENABLE_MASK this allows for only a specified subset of IMUs to be probed, so you can disable IMUs that aren't needed. The back corresponds to bits in the order the IMUs are normally probed on the board
2018-01-17 03:16:48 -04:00
// @Bitmask: 0:FirstIMU,1:SecondIMU,2:ThirdIMU
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
AP_GROUPINFO("_FAST_SAMPLE", 36, AP_InertialSensor, _fast_sampling_mask, HAL_DEFAULT_INS_FAST_SAMPLE),
AP_InertialSensor: added INS_FAST_SAMPLE parameter this allows enable/disable of fast sampling per IMU, making experimentation easier. It also fixes the fast sampling to always average over 8 samples, and fixes the 9250 to use the correct accumulator when not doing fast sampling
2016-11-15 01:51:18 -04:00
AP_InertialSensor: move INS_HNTC2 to a new parameter table ID
2022-04-15 02:03:31 -03:00
// index 37 was NOTCH_
AP_InertialSensor: add IMU batch sampling
2017-10-03 20:44:07 -03:00
AP_InertialSensor: create explicit define for AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED this allows the batch sampler to be removed for size reasons, or because you don't have logging enabled
2023-01-03 01:51:51 -04:00
#if AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Group: _LOG_
AP_InertialSensor: fix param docs for notch and log settings
2017-12-11 04:10:38 -04:00
// @Path: ../AP_InertialSensor/BatchSampler.cpp
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
AP_SUBGROUPINFO(batchsampler, "_LOG_", 39, AP_InertialSensor, AP_InertialSensor::BatchSampler),
AP_InertialSensor: create explicit define for AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED this allows the batch sampler to be removed for size reasons, or because you don't have logging enabled
2023-01-03 01:51:51 -04:00
#endif
AP_InertialSensor: add IMU batch sampling
2017-10-03 20:44:07 -03:00
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _ENABLE_MASK
AP_InertialSensor: added INS_ENABLE_MASK this allows for only a specified subset of IMUs to be probed, so you can disable IMUs that aren't needed. The back corresponds to bits in the order the IMUs are normally probed on the board
2018-01-17 03:16:48 -04:00
// @DisplayName: IMU enable mask
AP_InetialSensor: fixup ENABLE_MASK parameter desc
2019-03-26 23:46:44 -03:00
// @Description: Bitmask of IMUs to enable. It can be used to prevent startup of specific detected IMUs
AP_InertialSensor: added INS_ENABLE_MASK this allows for only a specified subset of IMUs to be probed, so you can disable IMUs that aren't needed. The back corresponds to bits in the order the IMUs are normally probed on the board
2018-01-17 03:16:48 -04:00
// @User: Advanced
AP_InertialSensor: extend ENABLE_MASK param desc IMUs 4 to 7
2022-05-18 03:28:11 -03:00
// @Bitmask: 0:FirstIMU,1:SecondIMU,2:ThirdIMU,3:FourthIMU,4:FifthIMU,5:SixthIMU,6:SeventhIMU
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
AP_GROUPINFO("_ENABLE_MASK", 40, AP_InertialSensor, _enable_mask, 0x7F),
AP_InetialSensor: fixup ENABLE_MASK parameter desc
2019-03-26 23:46:44 -03:00
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Group: _HNTCH_
AP_InertialSensor: add harmonic notch filter to gyro filter chain Allow dynamic updates to the calculated frequency. Convert bandwidth and frequency to floats. backend variables do not need to be indexed per-backed
2019-06-17 05:44:12 -03:00
// @Path: ../Filter/HarmonicNotchFilter.cpp
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
AP_SUBGROUPINFO(harmonic_notches[0].params, "_HNTCH_", 41, AP_InertialSensor, HarmonicNotchFilterParams),
AP_InertialSensor: add harmonic notch filter to gyro filter chain Allow dynamic updates to the calculated frequency. Convert bandwidth and frequency to floats. backend variables do not need to be indexed per-backed
2019-06-17 05:44:12 -03:00
AP_InertialSensor: support two full harmonic notch filters
2022-04-13 00:36:18 -03:00
#if HAL_INS_NUM_HARMONIC_NOTCH_FILTERS > 1
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Group: _HNTC2_
AP_InertialSensor: move INS_HNTC2 to a new parameter table ID
2022-04-15 02:03:31 -03:00
// @Path: ../Filter/HarmonicNotchFilter.cpp
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
AP_SUBGROUPINFO(harmonic_notches[1].params, "_HNTC2_", 53, AP_InertialSensor, HarmonicNotchFilterParams),
AP_InertialSensor: support two full harmonic notch filters
2022-04-13 00:36:18 -03:00
#endif
AP_InertialSensor: move INS_HNTC2 to a new parameter table ID
2022-04-15 02:03:31 -03:00
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _GYRO_RATE
AP_InertialSensor: make the backend fast gyro rate configurable raise gyro rate default on F7 and H7 clean up gyro rate docs and output startup banner
2020-05-21 15:29:28 -03:00
// @DisplayName: Gyro rate for IMUs with Fast Sampling enabled
// @Description: Gyro rate for IMUs with fast sampling enabled. The gyro rate is the sample rate at which the IMU filters operate and needs to be at least double the maximum filter frequency. If the sensor does not support the selected rate the next highest supported rate will be used. For IMUs which do not support fast sampling this setting is ignored and the default gyro rate of 1Khz is used.
// @User: Advanced
AP_InertialSensor: make the backend fast gyro rate configurable and independent from the accel rate allow fast sampling of gyros on MPU6000 and MPU6500
2020-05-21 15:29:28 -03:00
// @Values: 0:1kHz,1:2kHz,2:4kHz,3:8kHz
AP_InertialSensor: make the backend fast gyro rate configurable raise gyro rate default on F7 and H7 clean up gyro rate docs and output startup banner
2020-05-21 15:29:28 -03:00
// @RebootRequired: True
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
AP_GROUPINFO("_GYRO_RATE", 42, AP_InertialSensor, _fast_sampling_rate, MPU_FIFO_FASTSAMPLE_DEFAULT),
AP_InertialSensor: make the backend fast gyro rate configurable raise gyro rate default on F7 and H7 clean up gyro rate docs and output startup banner
2020-05-21 15:29:28 -03:00
AP_InertialSensor: added IMU temperature calibration support this adds parameters that can be setup by an external script for compensating for temperature variation in gyros and accels using a 3rd order polynomial
2021-01-07 20:46:35 -04:00
#if HAL_INS_TEMPERATURE_CAL_ENABLE
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Group: _TCAL1_
AP_InertialSensor: added IMU temperature calibration support this adds parameters that can be setup by an external script for compensating for temperature variation in gyros and accels using a 3rd order polynomial
2021-01-07 20:46:35 -04:00
// @Path: AP_InertialSensor_tempcal.cpp
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
AP_SUBGROUPINFO(tcal_old_param[0], "_TCAL1_", 43, AP_InertialSensor, AP_InertialSensor_TCal),
AP_InertialSensor: added IMU temperature calibration support this adds parameters that can be setup by an external script for compensating for temperature variation in gyros and accels using a 3rd order polynomial
2021-01-07 20:46:35 -04:00
#if INS_MAX_INSTANCES > 1
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Group: _TCAL2_
AP_InertialSensor: added IMU temperature calibration support this adds parameters that can be setup by an external script for compensating for temperature variation in gyros and accels using a 3rd order polynomial
2021-01-07 20:46:35 -04:00
// @Path: AP_InertialSensor_tempcal.cpp
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
AP_SUBGROUPINFO(tcal_old_param[1], "_TCAL2_", 44, AP_InertialSensor, AP_InertialSensor_TCal),
AP_InertialSensor: added IMU temperature calibration support this adds parameters that can be setup by an external script for compensating for temperature variation in gyros and accels using a 3rd order polynomial
2021-01-07 20:46:35 -04:00
#endif
#if INS_MAX_INSTANCES > 2
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Group: _TCAL3_
AP_InertialSensor: added IMU temperature calibration support this adds parameters that can be setup by an external script for compensating for temperature variation in gyros and accels using a 3rd order polynomial
2021-01-07 20:46:35 -04:00
// @Path: AP_InertialSensor_tempcal.cpp
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
AP_SUBGROUPINFO(tcal_old_param[2], "_TCAL3_", 45, AP_InertialSensor, AP_InertialSensor_TCal),
AP_InertialSensor: added IMU temperature calibration support this adds parameters that can be setup by an external script for compensating for temperature variation in gyros and accels using a 3rd order polynomial
2021-01-07 20:46:35 -04:00
#endif
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _ACC1_CALTEMP
AP_InertialSensor: added IMU temperature calibration support this adds parameters that can be setup by an external script for compensating for temperature variation in gyros and accels using a 3rd order polynomial
2021-01-07 20:46:35 -04:00
// @DisplayName: Calibration temperature for 1st accelerometer
// @Description: Temperature that the 1st accelerometer was calibrated at
// @User: Advanced
// @Units: degC
// @Calibration: 1
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
AP_GROUPINFO("_ACC1_CALTEMP", 46, AP_InertialSensor, caltemp_accel_old_param[0], -300),
AP_InertialSensor: added IMU temperature calibration support this adds parameters that can be setup by an external script for compensating for temperature variation in gyros and accels using a 3rd order polynomial
2021-01-07 20:46:35 -04:00
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _GYR1_CALTEMP
AP_InertialSensor: added IMU temperature calibration support this adds parameters that can be setup by an external script for compensating for temperature variation in gyros and accels using a 3rd order polynomial
2021-01-07 20:46:35 -04:00
// @DisplayName: Calibration temperature for 1st gyroscope
// @Description: Temperature that the 1st gyroscope was calibrated at
// @User: Advanced
// @Units: degC
// @Calibration: 1
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
AP_GROUPINFO("_GYR1_CALTEMP", 47, AP_InertialSensor, caltemp_gyro_old_param[0], -300),
AP_InertialSensor: added IMU temperature calibration support this adds parameters that can be setup by an external script for compensating for temperature variation in gyros and accels using a 3rd order polynomial
2021-01-07 20:46:35 -04:00
#if INS_MAX_INSTANCES > 1
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _ACC2_CALTEMP
AP_InertialSensor: added IMU temperature calibration support this adds parameters that can be setup by an external script for compensating for temperature variation in gyros and accels using a 3rd order polynomial
2021-01-07 20:46:35 -04:00
// @DisplayName: Calibration temperature for 2nd accelerometer
// @Description: Temperature that the 2nd accelerometer was calibrated at
// @User: Advanced
// @Units: degC
// @Calibration: 1
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
AP_GROUPINFO("_ACC2_CALTEMP", 48, AP_InertialSensor, caltemp_accel_old_param[1], -300),
AP_InertialSensor: added IMU temperature calibration support this adds parameters that can be setup by an external script for compensating for temperature variation in gyros and accels using a 3rd order polynomial
2021-01-07 20:46:35 -04:00
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _GYR2_CALTEMP
AP_InertialSensor: added IMU temperature calibration support this adds parameters that can be setup by an external script for compensating for temperature variation in gyros and accels using a 3rd order polynomial
2021-01-07 20:46:35 -04:00
// @DisplayName: Calibration temperature for 2nd gyroscope
// @Description: Temperature that the 2nd gyroscope was calibrated at
// @User: Advanced
// @Units: degC
// @Calibration: 1
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
AP_GROUPINFO("_GYR2_CALTEMP", 49, AP_InertialSensor, caltemp_gyro_old_param[1], -300),
AP_InertialSensor: added IMU temperature calibration support this adds parameters that can be setup by an external script for compensating for temperature variation in gyros and accels using a 3rd order polynomial
2021-01-07 20:46:35 -04:00
#endif
#if INS_MAX_INSTANCES > 2
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _ACC3_CALTEMP
AP_InertialSensor: added IMU temperature calibration support this adds parameters that can be setup by an external script for compensating for temperature variation in gyros and accels using a 3rd order polynomial
2021-01-07 20:46:35 -04:00
// @DisplayName: Calibration temperature for 3rd accelerometer
// @Description: Temperature that the 3rd accelerometer was calibrated at
// @User: Advanced
// @Units: degC
// @Calibration: 1
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
AP_GROUPINFO("_ACC3_CALTEMP", 50, AP_InertialSensor, caltemp_accel_old_param[2], -300),
AP_InertialSensor: added IMU temperature calibration support this adds parameters that can be setup by an external script for compensating for temperature variation in gyros and accels using a 3rd order polynomial
2021-01-07 20:46:35 -04:00
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _GYR3_CALTEMP
AP_InertialSensor: added IMU temperature calibration support this adds parameters that can be setup by an external script for compensating for temperature variation in gyros and accels using a 3rd order polynomial
2021-01-07 20:46:35 -04:00
// @DisplayName: Calibration temperature for 3rd gyroscope
// @Description: Temperature that the 3rd gyroscope was calibrated at
// @User: Advanced
// @Units: degC
// @Calibration: 1
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
AP_GROUPINFO("_GYR3_CALTEMP", 51, AP_InertialSensor, caltemp_gyro_old_param[2], -300),
AP_InertialSensor: added IMU temperature calibration support this adds parameters that can be setup by an external script for compensating for temperature variation in gyros and accels using a 3rd order polynomial
2021-01-07 20:46:35 -04:00
#endif
AP_InertialSensor: added INS_TCAL_OPTIONS make persistent params optional, and allow save of accelcal
2021-01-16 01:40:00 -04:00
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
// @Param: _TCAL_OPTIONS
AP_InertialSensor: added INS_TCAL_OPTIONS make persistent params optional, and allow save of accelcal
2021-01-16 01:40:00 -04:00
// @DisplayName: Options for temperature calibration
// @Description: This enables optional temperature calibration features. Setting PersistParams will save the accelerometer and temperature calibration parameters in the bootloader sector on the next update of the bootloader.
// @Bitmask: 0:PersistParams
// @User: Advanced
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
AP_GROUPINFO("_TCAL_OPTIONS", 52, AP_InertialSensor, tcal_options, 0),
AP_InertialSensor: added INS_TCAL_OPTIONS make persistent params optional, and allow save of accelcal
2021-01-16 01:40:00 -04:00
AP_InertialSensor: added IMU temperature calibration support this adds parameters that can be setup by an external script for compensating for temperature variation in gyros and accels using a 3rd order polynomial
2021-01-07 20:46:35 -04:00
#endif // HAL_INS_TEMPERATURE_CAL_ENABLE
AP_InertialSensor: add support for extra Aux IMUs
2023-02-17 00:26:27 -04:00
#if INS_MAX_INSTANCES > 3
// @Group: 4_
// @Path: AP_InertialSensor_Params.cpp
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
AP_SUBGROUPINFO(params[0], "4_", 54, AP_InertialSensor, AP_InertialSensor_Params),
AP_InertialSensor: add support for extra Aux IMUs
2023-02-17 00:26:27 -04:00
#endif
#if INS_MAX_INSTANCES > 4
// @Group: 5_
// @Path: AP_InertialSensor_Params.cpp
AP_InertialSensor: move from INS_ top level parameters to INS
2023-03-08 06:37:24 -04:00
AP_SUBGROUPINFO(params[1], "5_", 55, AP_InertialSensor, AP_InertialSensor_Params),
AP_InertialSensor: add support for extra Aux IMUs
2023-02-17 00:26:27 -04:00
#endif
AP_InertialSensor: add INS_RAW_LOG_OPT to allow raw logging of post, and pre+post on primary or all gyros
2023-09-29 18:26:31 -03:00
// @Param: _RAW_LOG_OPT
// @DisplayName: Raw logging options
// @Description: Raw logging options bitmask
// @Bitmask: 0:Log primary gyro only, 1:Log all gyros, 2:Post filter, 3: Pre and post filter
// @User: Advanced
AP_GROUPINFO("_RAW_LOG_OPT", 56, AP_InertialSensor, raw_logging_options, 0),
AP_InertialSensor: shift to new parameter indexes for accel calibration this allows for users to switch between development trees and previous stable versions while retaining their accel calibration values.
2015-03-11 20:17:11 -03:00
/*
NOTE: parameter indexes have gaps above. When adding new
parameters check for conflicts carefully
*/
AP_InertialSensor: merge in calibration features from IMU library add gauss-newton method of accelerometer calibration
2012-11-05 00:27:03 -04:00
AP_GROUPEND
};
AP_InertialSensor: unify singleton naming to _singleton and get_singleton()
2019-02-10 14:29:24 -04:00
AP_InertialSensor *AP_InertialSensor::_singleton = nullptr;
AP_InertialSensor: add singleton interface In order to allow other libraries to use the InertialSensor we need a way to let them to get the only instance of InertialSensor. The conventional way to do a singleton would be to let the constructor private and force it to be instantiated from the get_instance() method. Here however we just call panic() on the constructor if there's already an instance alive. This allows us to let the vehicles as is. Later we can change it so they call the get_instance() method instead.
2015-08-12 15:04:29 -03:00
AP_InertialSensor: ensure parent class is initialised in instance classes
2013-04-12 01:30:35 -03:00
AP_InertialSensor::AP_InertialSensor() :
AP_InertialSensor: first steps in frontend/backend split This converts the MPU6000 driver to a frontend/backend structure, and disables all other drivers. They will be progressively re-enabled as each is converted
2014-10-14 01:48:33 -03:00
_board_orientation(ROTATION_NONE),
AP_InertialSensor: remove pointless initialisations AP_InertialSensor: avoid pointless zeroing in constructor AP_InertialSensor is either in bss or is created via new (which zeroes memory)
2018-02-22 02:46:28 -04:00
_log_raw_bit(-1)
AP_InertialSensor: ensure parent class is initialised in instance classes
2013-04-12 01:30:35 -03:00
{
AP_InertialSensor: unify singleton naming to _singleton and get_singleton()
2019-02-10 14:29:24 -04:00
if (_singleton) {
AP_InertialSensor: use millis/micros/panic functions
2015-11-19 23:11:52 -04:00
AP_HAL::panic("Too many inertial sensors");
AP_InertialSensor: add singleton interface In order to allow other libraries to use the InertialSensor we need a way to let them to get the only instance of InertialSensor. The conventional way to do a singleton would be to let the constructor private and force it to be instantiated from the get_instance() method. Here however we just call panic() on the constructor if there's already an instance alive. This allows us to let the vehicles as is. Later we can change it so they call the get_instance() method instead.
2015-08-12 15:04:29 -03:00
}
AP_InertialSensor: unify singleton naming to _singleton and get_singleton()
2019-02-10 14:29:24 -04:00
_singleton = this;
AP_InertialSensor: sanitize includes Due to the way the headers are organized changing a single change in an inertial sensor driver would trigger a rebuild for most of the files in the project. Time could be saved by using ccache (since most of the things didn't change) but we can do better, i.e. re-organize the headers so we don't have to re-build everything. With this patch only AP_InertialSensor/AP_InertialSensor.h is exposed to most users. There are some corner cases to integrate with some example code, but most of the places now depend only on this header and this header doesn't depend on the specific backends. Now changing a single header, e.g. AP_InertialSensor_L3G4200D.h triggers a rebuild only of these files: $ waf copter 'copter' finished successfully (0.000s) Waf: Entering directory `/home/lucas/p/dronecode/ardupilot/build/minlure' [ 80/370] Compiling libraries/AP_InertialSensor/AP_InertialSensor.cpp [ 84/370] Compiling libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp [310/370] Linking build/minlure/ArduCopter/libArduCopter_libs.a [370/370] Linking build/minlure/bin/arducopter Waf: Leaving directory `/home/lucas/p/dronecode/ardupilot/build/minlure'
2016-01-19 21:26:31 -04:00
AP_Param::setup_object_defaults(this, var_info);
AP_InertialSensor: add harmonic notch filter to gyro filter chain Allow dynamic updates to the calculated frequency. Convert bandwidth and frequency to floats. backend variables do not need to be indexed per-backed
2019-06-17 05:44:12 -03:00
AP_InertialSensor: prefer sensors that have zero error counts if a sensor has failed in flight then try not to use it if another error-free sensor is available
2014-12-29 06:19:35 -04:00
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
InertialSensor: add GYR_CAL to control when gyro calibration occurs
2015-09-17 03:42:41 -03:00
_gyro_cal_ok[i] = true;
AP_InertialSensor: make max_abs_offsets a single float, not a vector
2015-06-29 21:51:43 -03:00
_accel_max_abs_offsets[i] = 3.5f;
AP_InertialSensor: prefer sensors that have zero error counts if a sensor has failed in flight then try not to use it if another error-free sensor is available
2014-12-29 06:19:35 -04:00
}
InertialSensor: add vibration monitoring of 2nd IMU
2015-07-30 04:58:06 -03:00
for (uint8_t i=0; i<INS_VIBRATION_CHECK_INSTANCES; i++) {
_accel_vibe_floor_filter[i].set_cutoff_frequency(AP_INERTIAL_SENSOR_ACCEL_VIBE_FLOOR_FILT_HZ);
_accel_vibe_filter[i].set_cutoff_frequency(AP_INERTIAL_SENSOR_ACCEL_VIBE_FILT_HZ);
}
AP_InertialSensor: remove dependence on gcs for AP_Periph builds
2021-05-17 14:44:53 -03:00
#if HAL_INS_ACCELCAL_ENABLED
AP_InertialSensor: statically register with AP_AccelCal
2016-01-04 18:42:22 -04:00
AP_AccelCal::register_client(this);
AP_InertialSensor: remove dependence on gcs for AP_Periph builds
2021-05-17 14:44:53 -03:00
#endif
AP_InertialSensor: first steps in frontend/backend split This converts the MPU6000 driver to a frontend/backend structure, and disables all other drivers. They will be progressively re-enabled as each is converted
2014-10-14 01:48:33 -03:00
}
AP_InertialSensor: add singleton interface In order to allow other libraries to use the InertialSensor we need a way to let them to get the only instance of InertialSensor. The conventional way to do a singleton would be to let the constructor private and force it to be instantiated from the get_instance() method. Here however we just call panic() on the constructor if there's already an instance alive. This allows us to let the vehicles as is. Later we can change it so they call the get_instance() method instead.
2015-08-12 15:04:29 -03:00
/*
* Get the AP_InertialSensor singleton
*/
AP_InertialSensor: unify singleton naming to _singleton and get_singleton()
2019-02-10 14:29:24 -04:00
AP_InertialSensor *AP_InertialSensor::get_singleton()
AP_InertialSensor: add singleton interface In order to allow other libraries to use the InertialSensor we need a way to let them to get the only instance of InertialSensor. The conventional way to do a singleton would be to let the constructor private and force it to be instantiated from the get_instance() method. Here however we just call panic() on the constructor if there's already an instance alive. This allows us to let the vehicles as is. Later we can change it so they call the get_instance() method instead.
2015-08-12 15:04:29 -03:00
{
AP_InertialSensor: unify singleton naming to _singleton and get_singleton()
2019-02-10 14:29:24 -04:00
if (!_singleton) {
_singleton = new AP_InertialSensor();
AP_InertialSensor: minor formatting fixes No functional change
2016-12-23 23:21:29 -04:00
}
AP_InertialSensor: unify singleton naming to _singleton and get_singleton()
2019-02-10 14:29:24 -04:00
return _singleton;
AP_InertialSensor: add singleton interface In order to allow other libraries to use the InertialSensor we need a way to let them to get the only instance of InertialSensor. The conventional way to do a singleton would be to let the constructor private and force it to be instantiated from the get_instance() method. Here however we just call panic() on the constructor if there's already an instance alive. This allows us to let the vehicles as is. Later we can change it so they call the get_instance() method instead.
2015-08-12 15:04:29 -03:00
}
AP_InertialSensor: first steps in frontend/backend split This converts the MPU6000 driver to a frontend/backend structure, and disables all other drivers. They will be progressively re-enabled as each is converted
2014-10-14 01:48:33 -03:00
/*
register a new gyro instance
*/
AP_InertialSensor: fixed handling of more than 3 accel/gyro instances allows for first 3 to work without a panic
2021-03-17 22:36:41 -03:00
bool AP_InertialSensor::register_gyro(uint8_t &instance, uint16_t raw_sample_rate_hz, uint32_t id)
AP_InertialSensor: first steps in frontend/backend split This converts the MPU6000 driver to a frontend/backend structure, and disables all other drivers. They will be progressively re-enabled as each is converted
2014-10-14 01:48:33 -03:00
{
if (_gyro_count == INS_MAX_INSTANCES) {
AP_InertialSensor: remove dependence on gcs for AP_Periph builds
2021-05-17 14:44:53 -03:00
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Failed to register gyro id %u", unsigned(id));
AP_InertialSensor: fixed handling of more than 3 accel/gyro instances allows for first 3 to work without a panic
2021-03-17 22:36:41 -03:00
return false;
AP_InertialSensor: first steps in frontend/backend split This converts the MPU6000 driver to a frontend/backend structure, and disables all other drivers. They will be progressively re-enabled as each is converted
2014-10-14 01:48:33 -03:00
}
AP_InertialSensor: save id for gyro and accel instances This allows each sensor to be uniquely identified in the system by using either the index inside the backend or for those that use the Device interface, to use the bus type, location, and device id. We leave 16-bit for each sensor to be able to change its own identification in future, which allows them to be changed in an incompatible manner forcing a re-calibration.
2016-09-03 21:51:37 -03:00
AP_InertialSensor: simplify sensor backends use common code for filtering and update, allowing each sensor driver to be simpler and more consistent
2015-11-15 20:05:20 -04:00
_gyro_raw_sample_rates[_gyro_count] = raw_sample_rate_hz;
AP_InertialSensor: estimate and log sensor rates for all IMUs this adds IMU.GHz and IMU.AHz log fields so we can see the actual observed sensor rates of each IMU
2017-05-01 00:01:43 -03:00
_gyro_over_sampling[_gyro_count] = 1;
AP_InertialSensor: parameterise sensor-rate logging, generalise it AP_InertialSensor: add parameters for push-to-log interval and count AP_InertialSensor: rename BAT_RAW to BAT_OPT This becomes a bitmask of options for the BatchSampler AP_InertialSensor: rename 'fast sample' to 'sensorrate sample' AP_InertialSensor: const sensor-rate filter method AP_InertialSampler: remove hard-coding of sample rate multiplier AP_InertialSensor: use parameter to enable/disable sensor-rate logging AP_InertialSensor: use a parameter to control sensor-rate logging AP_InertialSensor: allow backends to override sensor data multiplier e.g. some accelerometers are sensitive over wider ranges than the default 16G AP_Inertialsensor: correct sample rate multiplier
2018-03-18 20:28:33 -03:00
_gyro_raw_sampling_multiplier[_gyro_count] = INT16_MAX/radians(2000);
AP_InertialSensor: save id for gyro and accel instances This allows each sensor to be uniquely identified in the system by using either the index inside the backend or for those that use the Device interface, to use the bus type, location, and device id. We leave 16-bit for each sensor to be able to change its own identification in future, which allows them to be changed in an incompatible manner forcing a re-calibration.
2016-09-03 21:51:37 -03:00
AP_InertialSensor: add support for extra Aux IMUs
2023-02-17 00:26:27 -04:00
bool saved = _gyro_id(_gyro_count).load();
AP_InertialSensor: save id for gyro and accel instances This allows each sensor to be uniquely identified in the system by using either the index inside the backend or for those that use the Device interface, to use the bus type, location, and device id. We leave 16-bit for each sensor to be able to change its own identification in future, which allows them to be changed in an incompatible manner forcing a re-calibration.
2016-09-03 21:51:37 -03:00
AP_InertialSensor: add support for extra Aux IMUs
2023-02-17 00:26:27 -04:00
if (saved && (uint32_t)_gyro_id(_gyro_count) != id) {
AP_InertialSensor: save id for gyro and accel instances This allows each sensor to be uniquely identified in the system by using either the index inside the backend or for those that use the Device interface, to use the bus type, location, and device id. We leave 16-bit for each sensor to be able to change its own identification in future, which allows them to be changed in an incompatible manner forcing a re-calibration.
2016-09-03 21:51:37 -03:00
// inconsistent gyro id - mark it as needing calibration
_gyro_cal_ok[_gyro_count] = false;
}
AP_InertialSensor: add support for extra Aux IMUs
2023-02-17 00:26:27 -04:00
_gyro_id(_gyro_count).set((int32_t) id);
AP_InertialSensor: save id for gyro and accel instances This allows each sensor to be uniquely identified in the system by using either the index inside the backend or for those that use the Device interface, to use the bus type, location, and device id. We leave 16-bit for each sensor to be able to change its own identification in future, which allows them to be changed in an incompatible manner forcing a re-calibration.
2016-09-03 21:51:37 -03:00
AP_InertialSensor: remove use of never-set AP_HAL_PX4 and AP_HAL_VRBRAIN
2019-01-16 20:29:58 -04:00
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
AP_InertialSensor: save id for gyro and accel instances This allows each sensor to be uniquely identified in the system by using either the index inside the backend or for those that use the Device interface, to use the bus type, location, and device id. We leave 16-bit for each sensor to be able to change its own identification in future, which allows them to be changed in an incompatible manner forcing a re-calibration.
2016-09-03 21:51:37 -03:00
if (!saved) {
// assume this is the same sensor and save its ID to allow seamless
// transition from when we didn't have the IDs.
AP_InertialSensor: add support for extra Aux IMUs
2023-02-17 00:26:27 -04:00
_gyro_id(_gyro_count).save();
AP_InertialSensor: save id for gyro and accel instances This allows each sensor to be uniquely identified in the system by using either the index inside the backend or for those that use the Device interface, to use the bus type, location, and device id. We leave 16-bit for each sensor to be able to change its own identification in future, which allows them to be changed in an incompatible manner forcing a re-calibration.
2016-09-03 21:51:37 -03:00
}
#endif
AP_InertialSensor: fixed handling of more than 3 accel/gyro instances allows for first 3 to work without a panic
2021-03-17 22:36:41 -03:00
instance = _gyro_count++;
return true;
AP_InertialSensor: first steps in frontend/backend split This converts the MPU6000 driver to a frontend/backend structure, and disables all other drivers. They will be progressively re-enabled as each is converted
2014-10-14 01:48:33 -03:00
}
AP_InertialSensor: pre-fetch instances for use in filter setup we use the instance numbers during filter configuration to check if fast sampling is enabled. We need to ensure these instance numbers have been setup before the filtering functions get called
2023-08-27 22:33:40 -03:00
/*
get the accel instance number we will get from register_accel()
*/
bool AP_InertialSensor::get_accel_instance(uint8_t &instance) const
{
if (_accel_count == INS_MAX_INSTANCES) {
return false;
}
instance = _accel_count;
return true;
}
/*
get the gyro instance number we will get from register_accel()
*/
bool AP_InertialSensor::get_gyro_instance(uint8_t &instance) const
{
if (_gyro_count == INS_MAX_INSTANCES) {
return false;
}
instance = _gyro_count;
return true;
}
AP_InertialSensor: first steps in frontend/backend split This converts the MPU6000 driver to a frontend/backend structure, and disables all other drivers. They will be progressively re-enabled as each is converted
2014-10-14 01:48:33 -03:00
/*
register a new accel instance
*/
AP_InertialSensor: fixed handling of more than 3 accel/gyro instances allows for first 3 to work without a panic
2021-03-17 22:36:41 -03:00
bool AP_InertialSensor::register_accel(uint8_t &instance, uint16_t raw_sample_rate_hz, uint32_t id)
AP_InertialSensor: first steps in frontend/backend split This converts the MPU6000 driver to a frontend/backend structure, and disables all other drivers. They will be progressively re-enabled as each is converted
2014-10-14 01:48:33 -03:00
{
if (_accel_count == INS_MAX_INSTANCES) {
AP_InertialSensor: remove dependence on gcs for AP_Periph builds
2021-05-17 14:44:53 -03:00
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Failed to register accel id %u", unsigned(id));
AP_InertialSensor: fixed handling of more than 3 accel/gyro instances allows for first 3 to work without a panic
2021-03-17 22:36:41 -03:00
return false;
AP_InertialSensor: first steps in frontend/backend split This converts the MPU6000 driver to a frontend/backend structure, and disables all other drivers. They will be progressively re-enabled as each is converted
2014-10-14 01:48:33 -03:00
}
AP_InertialSensor: save id for gyro and accel instances This allows each sensor to be uniquely identified in the system by using either the index inside the backend or for those that use the Device interface, to use the bus type, location, and device id. We leave 16-bit for each sensor to be able to change its own identification in future, which allows them to be changed in an incompatible manner forcing a re-calibration.
2016-09-03 21:51:37 -03:00
AP_InertialSensor: simplify sensor backends use common code for filtering and update, allowing each sensor driver to be simpler and more consistent
2015-11-15 20:05:20 -04:00
_accel_raw_sample_rates[_accel_count] = raw_sample_rate_hz;
AP_InertialSensor: estimate and log sensor rates for all IMUs this adds IMU.GHz and IMU.AHz log fields so we can see the actual observed sensor rates of each IMU
2017-05-01 00:01:43 -03:00
_accel_over_sampling[_accel_count] = 1;
AP_InertialSensor: parameterise sensor-rate logging, generalise it AP_InertialSensor: add parameters for push-to-log interval and count AP_InertialSensor: rename BAT_RAW to BAT_OPT This becomes a bitmask of options for the BatchSampler AP_InertialSensor: rename 'fast sample' to 'sensorrate sample' AP_InertialSensor: const sensor-rate filter method AP_InertialSampler: remove hard-coding of sample rate multiplier AP_InertialSensor: use parameter to enable/disable sensor-rate logging AP_InertialSensor: use a parameter to control sensor-rate logging AP_InertialSensor: allow backends to override sensor data multiplier e.g. some accelerometers are sensitive over wider ranges than the default 16G AP_Inertialsensor: correct sample rate multiplier
2018-03-18 20:28:33 -03:00
_accel_raw_sampling_multiplier[_accel_count] = INT16_MAX/(16*GRAVITY_MSS);
AP_InertialSensor: estimate and log sensor rates for all IMUs this adds IMU.GHz and IMU.AHz log fields so we can see the actual observed sensor rates of each IMU
2017-05-01 00:01:43 -03:00
AP_InertialSensor: add support for extra Aux IMUs
2023-02-17 00:26:27 -04:00
bool saved = _accel_id(_accel_count).load();
AP_InertialSensor: save id for gyro and accel instances This allows each sensor to be uniquely identified in the system by using either the index inside the backend or for those that use the Device interface, to use the bus type, location, and device id. We leave 16-bit for each sensor to be able to change its own identification in future, which allows them to be changed in an incompatible manner forcing a re-calibration.
2016-09-03 21:51:37 -03:00
if (!saved) {
// inconsistent accel id
_accel_id_ok[_accel_count] = false;
AP_InertialSensor: add support for extra Aux IMUs
2023-02-17 00:26:27 -04:00
} else if ((uint32_t)_accel_id(_accel_count) != id) {
AP_InertialSensor: save id for gyro and accel instances This allows each sensor to be uniquely identified in the system by using either the index inside the backend or for those that use the Device interface, to use the bus type, location, and device id. We leave 16-bit for each sensor to be able to change its own identification in future, which allows them to be changed in an incompatible manner forcing a re-calibration.
2016-09-03 21:51:37 -03:00
// inconsistent accel id
_accel_id_ok[_accel_count] = false;
} else {
_accel_id_ok[_accel_count] = true;
}
AP_InertialSensor: add support for extra Aux IMUs
2023-02-17 00:26:27 -04:00
_accel_id(_accel_count).set((int32_t) id);
AP_InertialSensor: save id for gyro and accel instances This allows each sensor to be uniquely identified in the system by using either the index inside the backend or for those that use the Device interface, to use the bus type, location, and device id. We leave 16-bit for each sensor to be able to change its own identification in future, which allows them to be changed in an incompatible manner forcing a re-calibration.
2016-09-03 21:51:37 -03:00
AP_InertialSensor: assume accel cals are stored for sitl-on-hw Same as for SITL - they don't need to actually be in storage to be valid
2022-03-10 20:12:29 -04:00
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL || (CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS && AP_SIM_ENABLED)
AP_InertialSensor: save id for gyro and accel instances This allows each sensor to be uniquely identified in the system by using either the index inside the backend or for those that use the Device interface, to use the bus type, location, and device id. We leave 16-bit for each sensor to be able to change its own identification in future, which allows them to be changed in an incompatible manner forcing a re-calibration.
2016-09-03 21:51:37 -03:00
// assume this is the same sensor and save its ID to allow seamless
// transition from when we didn't have the IDs.
_accel_id_ok[_accel_count] = true;
AP_InertialSensor: add support for extra Aux IMUs
2023-02-17 00:26:27 -04:00
_accel_id(_accel_count).save();
AP_InertialSensor: save id for gyro and accel instances This allows each sensor to be uniquely identified in the system by using either the index inside the backend or for those that use the Device interface, to use the bus type, location, and device id. We leave 16-bit for each sensor to be able to change its own identification in future, which allows them to be changed in an incompatible manner forcing a re-calibration.
2016-09-03 21:51:37 -03:00
#endif
AP_InertialSensor: fixed handling of more than 3 accel/gyro instances allows for first 3 to work without a panic
2021-03-17 22:36:41 -03:00
instance = _accel_count++;
return true;
AP_InertialSensor: move constructor into cpp
2012-12-19 00:55:38 -04:00
}
AP_InertialSensor: allow to split detection and initialization This allows backends to have a separate detection and initialization logic. It doesn't change any backend yet and with the current code there's no change in behavior either. This only allows AP_InertialSensor::_detect_backend() to be called earlier so AP_InertialSensor object can be used by other libraries. If it's not called, later on AP_InertialSensor::init() will detect and start all backends.
2015-08-05 10:01:52 -03:00
/*
* Start all backends for gyro and accel measurements. It automatically calls
AP_InertialSensor: Make detect_backends() public This method will be used to initialize and configure I2C backends that have an auxiliary I2C bus that can be connected to the main I2C bus, like MPU6000 and MPU9250.
2015-10-13 15:33:20 -03:00
* detect_backends() if it has not been called already.
AP_InertialSensor: allow to split detection and initialization This allows backends to have a separate detection and initialization logic. It doesn't change any backend yet and with the current code there's no change in behavior either. This only allows AP_InertialSensor::_detect_backend() to be called earlier so AP_InertialSensor object can be used by other libraries. If it's not called, later on AP_InertialSensor::init() will detect and start all backends.
2015-08-05 10:01:52 -03:00
*/
void AP_InertialSensor::_start_backends()
{
AP_InertialSensor: Make detect_backends() public This method will be used to initialize and configure I2C backends that have an auxiliary I2C bus that can be connected to the main I2C bus, like MPU6000 and MPU9250.
2015-10-13 15:33:20 -03:00
detect_backends();
AP_InertialSensor: allow to split detection and initialization This allows backends to have a separate detection and initialization logic. It doesn't change any backend yet and with the current code there's no change in behavior either. This only allows AP_InertialSensor::_detect_backend() to be called earlier so AP_InertialSensor object can be used by other libraries. If it's not called, later on AP_InertialSensor::init() will detect and start all backends.
2015-08-05 10:01:52 -03:00
for (uint8_t i = 0; i < _backend_count; i++) {
_backends[i]->start();
}
if (_gyro_count == 0 || _accel_count == 0) {
AP_InertialSensor: use millis/micros/panic functions
2015-11-19 23:11:52 -04:00
AP_HAL::panic("INS needs at least 1 gyro and 1 accel");
AP_InertialSensor: allow to split detection and initialization This allows backends to have a separate detection and initialization logic. It doesn't change any backend yet and with the current code there's no change in behavior either. This only allows AP_InertialSensor::_detect_backend() to be called earlier so AP_InertialSensor object can be used by other libraries. If it's not called, later on AP_InertialSensor::init() will detect and start all backends.
2015-08-05 10:01:52 -03:00
}
AP_InertialSensor: added INS_ENABLE_MASK this allows for only a specified subset of IMUs to be probed, so you can disable IMUs that aren't needed. The back corresponds to bits in the order the IMUs are normally probed on the board
2018-01-17 03:16:48 -04:00
// clear IDs for unused sensor instances
for (uint8_t i=get_accel_count(); i<INS_MAX_INSTANCES; i++) {
AP_InertialSensor: add support for extra Aux IMUs
2023-02-17 00:26:27 -04:00
_accel_id(i).set(0);
AP_InertialSensor: added INS_ENABLE_MASK this allows for only a specified subset of IMUs to be probed, so you can disable IMUs that aren't needed. The back corresponds to bits in the order the IMUs are normally probed on the board
2018-01-17 03:16:48 -04:00
}
for (uint8_t i=get_gyro_count(); i<INS_MAX_INSTANCES; i++) {
AP_InertialSensor: add support for extra Aux IMUs
2023-02-17 00:26:27 -04:00
_gyro_id(i).set(0);
AP_InertialSensor: added INS_ENABLE_MASK this allows for only a specified subset of IMUs to be probed, so you can disable IMUs that aren't needed. The back corresponds to bits in the order the IMUs are normally probed on the board
2018-01-17 03:16:48 -04:00
}
AP_InertialSensor: allow to split detection and initialization This allows backends to have a separate detection and initialization logic. It doesn't change any backend yet and with the current code there's no change in behavior either. This only allows AP_InertialSensor::_detect_backend() to be called earlier so AP_InertialSensor object can be used by other libraries. If it's not called, later on AP_InertialSensor::init() will detect and start all backends.
2015-08-05 10:01:52 -03:00
}
AP_InertialSensor: Add support to get auxiliary bus of multiple instances of the same sensor
2015-10-02 15:45:21 -03:00
/* Find the N instance of the backend that has already been successfully detected */
AP_InertialSensor_Backend *AP_InertialSensor::_find_backend(int16_t backend_id, uint8_t instance)
AP_InertialSensor: identify backend with ID Identify backend with an id, allowing other libraries to connect to them. This is different from the _product_id member because it identifies the sensor, not the board the sensor is in, which is meaningless for our use case.
2015-08-16 16:16:11 -03:00
{
AP_InertialSensor: Add support to get auxiliary bus of multiple instances of the same sensor
2015-10-02 15:45:21 -03:00
uint8_t found = 0;
AP_InertialSensor: identify backend with ID Identify backend with an id, allowing other libraries to connect to them. This is different from the _product_id member because it identifies the sensor, not the board the sensor is in, which is meaningless for our use case.
2015-08-16 16:16:11 -03:00
for (uint8_t i = 0; i < _backend_count; i++) {
int16_t id = _backends[i]->get_id();
AP_InertialSensor: minor formatting fixes No functional change
2016-12-23 23:21:29 -04:00
if (id < 0 || id != backend_id) {
AP_InertialSensor: identify backend with ID Identify backend with an id, allowing other libraries to connect to them. This is different from the _product_id member because it identifies the sensor, not the board the sensor is in, which is meaningless for our use case.
2015-08-16 16:16:11 -03:00
continue;
AP_InertialSensor: minor formatting fixes No functional change
2016-12-23 23:21:29 -04:00
}
AP_InertialSensor: identify backend with ID Identify backend with an id, allowing other libraries to connect to them. This is different from the _product_id member because it identifies the sensor, not the board the sensor is in, which is meaningless for our use case.
2015-08-16 16:16:11 -03:00
AP_InertialSensor: Add support to get auxiliary bus of multiple instances of the same sensor
2015-10-02 15:45:21 -03:00
if (instance == found) {
return _backends[i];
} else {
found++;
}
AP_InertialSensor: identify backend with ID Identify backend with an id, allowing other libraries to connect to them. This is different from the _product_id member because it identifies the sensor, not the board the sensor is in, which is meaningless for our use case.
2015-08-16 16:16:11 -03:00
}
return nullptr;
}
AP_InertialSensor: expose statically filtered gyro values for FFT analysis and allow sampling to gyro window for FFT analysis. FFT windows can be dynamically allocated add harmonic notch dynamic tracking mode unwind gyro window allocation in the case of failure allow access to harmonic notch harmonics
2019-07-23 05:43:18 -03:00
bool AP_InertialSensor::set_gyro_window_size(uint16_t size) {
AP_InertialSensor: HAL_WITH_DSP -> HAL_GYROFFT_ENABLED
2022-11-28 05:33:50 -04:00
#if HAL_GYROFFT_ENABLED
AP_InertialSensor: expose statically filtered gyro values for FFT analysis and allow sampling to gyro window for FFT analysis. FFT windows can be dynamically allocated add harmonic notch dynamic tracking mode unwind gyro window allocation in the case of failure allow access to harmonic notch harmonics
2019-07-23 05:43:18 -03:00
_gyro_window_size = size;
// allocate FFT gyro window
for (uint8_t i = 0; i < INS_MAX_INSTANCES; i++) {
for (uint8_t j = 0; j < XYZ_AXIS_COUNT; j++) {
AP_InertialSensor: FFT gyro window would overflow on windows >= 256 convert FFT buffers to ObjectBuffer<float> for lock-free access push gyro samples directly into the FFT ring buffer from the gyro thread
2020-03-27 19:08:30 -03:00
if (!_gyro_window[i][j].set_size(size)) {
AP_InertialSensor: remove dependence on gcs for AP_Periph builds
2021-05-17 14:44:53 -03:00
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Failed to allocate window for INS");
AP_InertialSensor: expose statically filtered gyro values for FFT analysis and allow sampling to gyro window for FFT analysis. FFT windows can be dynamically allocated add harmonic notch dynamic tracking mode unwind gyro window allocation in the case of failure allow access to harmonic notch harmonics
2019-07-23 05:43:18 -03:00
// clean up whatever we have currently allocated
for (uint8_t ii = 0; ii <= i; ii++) {
for (uint8_t jj = 0; jj < j; jj++) {
AP_InertialSensor: FFT gyro window would overflow on windows >= 256 convert FFT buffers to ObjectBuffer<float> for lock-free access push gyro samples directly into the FFT ring buffer from the gyro thread
2020-03-27 19:08:30 -03:00
_gyro_window[ii][jj].set_size(0);
AP_InertialSensor: expose statically filtered gyro values for FFT analysis and allow sampling to gyro window for FFT analysis. FFT windows can be dynamically allocated add harmonic notch dynamic tracking mode unwind gyro window allocation in the case of failure allow access to harmonic notch harmonics
2019-07-23 05:43:18 -03:00
_gyro_window_size = 0;
}
}
return false;
}
}
}
AP_InertialSensor: SITL the raw sample rate is not the same as the sensor rate use regulated time for frequency noise to avoid spurious harmonics SITL sensors must be true separate instances don't compile in FFT structures if DSP disabled
2020-01-03 15:52:33 -04:00
#endif
AP_InertialSensor: expose statically filtered gyro values for FFT analysis and allow sampling to gyro window for FFT analysis. FFT windows can be dynamically allocated add harmonic notch dynamic tracking mode unwind gyro window allocation in the case of failure allow access to harmonic notch harmonics
2019-07-23 05:43:18 -03:00
return true;
}
AP_InertialSensor: allow FFT window to be captured post-filter selectively apply filters twice to make sure that FFT is not tracking itself clean-up notch disablement for non-DSP builds use filter gyro data for FFT when analysing post-filter always apply LPF after all of the notches move to a phased application of window information for FFT tap FFT gyro window at the appropriate place for downsampled FFTs add accessor for FFT notch document notch uses cases and flow
2022-09-22 11:35:48 -03:00
#if HAL_WITH_DSP
bool AP_InertialSensor::has_fft_notch() const
{
for (auto &notch : harmonic_notches) {
if (notch.params.enabled() && notch.params.tracking_mode() == HarmonicNotchDynamicMode::UpdateGyroFFT) {
return true;
}
}
return false;
}
#endif
AP_InertialSensor: merge in calibration features from IMU library add gauss-newton method of accelerometer calibration
2012-11-05 00:27:03 -04:00
void
AP_InertialSensor: make the backend fast gyro rate configurable raise gyro rate default on F7 and H7 clean up gyro rate docs and output startup banner
2020-05-21 15:29:28 -03:00
AP_InertialSensor::init(uint16_t loop_rate)
AP_InertialSensor: merge in calibration features from IMU library add gauss-newton method of accelerometer calibration
2012-11-05 00:27:03 -04:00
{
AP_InertialSensor: moved default filter and sample_rate to frontend this simplifies the backends and prevents code repitition
2014-10-16 17:52:21 -03:00
// remember the sample rate
AP_InertialSensor: make the backend fast gyro rate configurable raise gyro rate default on F7 and H7 clean up gyro rate docs and output startup banner
2020-05-21 15:29:28 -03:00
_loop_rate = loop_rate;
_loop_delta_t = 1.0f / loop_rate;
AP_InertialSensor: moved default filter and sample_rate to frontend this simplifies the backends and prevents code repitition
2014-10-16 17:52:21 -03:00
AP_InertialSensor: limit delta-time from INS limit delta-time to 2x average time DEBUG CODE - DO NOT MERGE
2017-12-04 01:33:36 -04:00
// we don't allow deltat values greater than 10x the normal loop
// time to be exposed outside of INS. Large deltat values can
// cause divergence of state estimators
_loop_delta_t_max = 10 * _loop_delta_t;
AP_InertialSensor: call init for harmonic notch params
2023-07-14 14:35:25 -03:00
// Initialize notch params
for (auto &notch : harmonic_notches) {
notch.params.init();
}
AP_InertialSensor: first steps in frontend/backend split This converts the MPU6000 driver to a frontend/backend structure, and disables all other drivers. They will be progressively re-enabled as each is converted
2014-10-14 01:48:33 -03:00
if (_gyro_count == 0 && _accel_count == 0) {
AP_InertialSensor: allow to split detection and initialization This allows backends to have a separate detection and initialization logic. It doesn't change any backend yet and with the current code there's no change in behavior either. This only allows AP_InertialSensor::_detect_backend() to be called earlier so AP_InertialSensor object can be used by other libraries. If it's not called, later on AP_InertialSensor::init() will detect and start all backends.
2015-08-05 10:01:52 -03:00
_start_backends();
AP_InertialSensor: first steps in frontend/backend split This converts the MPU6000 driver to a frontend/backend structure, and disables all other drivers. They will be progressively re-enabled as each is converted
2014-10-14 01:48:33 -03:00
}
AP_InertialSensor: merge in calibration features from IMU library add gauss-newton method of accelerometer calibration
2012-11-05 00:27:03 -04:00
InertialSensor: add GYR_CAL to control when gyro calibration occurs
2015-09-17 03:42:41 -03:00
// calibrate gyros unless gyro calibration has been disabled
if (gyro_calibration_timing() != GYRO_CAL_NEVER) {
AP_InertialSensor: use init_gyro() on startup this saves the calibration parameters which should be done on calibration
2016-11-05 06:19:29 -03:00
init_gyro();
AP_InertialSensor: merge in calibration features from IMU library add gauss-newton method of accelerometer calibration
2012-11-05 00:27:03 -04:00
}
AP_InertialSensor: first steps in frontend/backend split This converts the MPU6000 driver to a frontend/backend structure, and disables all other drivers. They will be progressively re-enabled as each is converted
2014-10-14 01:48:33 -03:00
AP_InertialSensor: make the backend fast gyro rate configurable raise gyro rate default on F7 and H7 clean up gyro rate docs and output startup banner
2020-05-21 15:29:28 -03:00
_sample_period_usec = 1000*1000UL / _loop_rate;
AP_InertialSensor: improved timing in all drivers
2014-10-15 20:32:40 -03:00
// establish the baseline time between samples
_delta_time = 0;
AP_InertialSensor: simplify next sample time calculations this makes the calculation much clearer
2014-10-19 20:46:02 -03:00
_next_sample_usec = 0;
_last_sample_usec = 0;
AP_InertialSensor: improved timing in all drivers
2014-10-15 20:32:40 -03:00
_have_sample = false;
AP_InertialSensor: add IMU batch sampling
2017-10-03 20:44:07 -03:00
AP_InertialSensor: create explicit define for AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED this allows the batch sampler to be removed for size reasons, or because you don't have logging enabled
2023-01-03 01:51:51 -04:00
#if AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED
AP_InertialSensor: add IMU batch sampling
2017-10-03 20:44:07 -03:00
// initialise IMU batch logging
batchsampler.init();
AP_InertialSensor: create explicit define for AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED this allows the batch sampler to be removed for size reasons, or because you don't have logging enabled
2023-01-03 01:51:51 -04:00
#endif
AP_InertialSensor: add harmonic notch filter to gyro filter chain Allow dynamic updates to the calculated frequency. Convert bandwidth and frequency to floats. backend variables do not need to be indexed per-backed
2019-06-17 05:44:12 -03:00
AP_InertialSensor: HAL_WITH_DSP -> HAL_GYROFFT_ENABLED
2022-11-28 05:33:50 -04:00
#if HAL_GYROFFT_ENABLED
AP_InertialSensor: add throttle-based notch setup function allocate harmonic notch filter if FFT is enabled
2022-03-06 11:00:30 -04:00
AP_GyroFFT* fft = AP::fft();
bool fft_enabled = fft != nullptr && fft->enabled();
AP_InertialSensor: allow FFT window to be captured post-filter selectively apply filters twice to make sure that FFT is not tracking itself clean-up notch disablement for non-DSP builds use filter gyro data for FFT when analysing post-filter always apply LPF after all of the notches move to a phased application of window information for FFT tap FFT gyro window at the appropriate place for downsampled FFTs add accessor for FFT notch document notch uses cases and flow
2022-09-22 11:35:48 -03:00
if (fft_enabled) {
_post_filter_fft = fft->using_post_filter_samples();
}
// calculate the position that the FFT window needs to be applied
// Use cases:
// Gyro -> FFT window -> FFT Notch1/2 -> Non-FFT Notch2/1 -> LPF -> Filtered Gyro -- Phase 0
// Gyro -> FFT window -> Non-FFT Notch1/2 -> LPF -> Filtered Gyro -- Phase 0
// Gyro -> Non-FFT Notch1 -> Filtered FFT Window -> FFT Notch2 -> LPF -> Filtered Gyro -- Phase 1
// Gyro -> Non-FFT Notch1/2 -> Non-FFT Notch1/2 -> Filtered FFT Window -> LPF -> Filtered Gyro -- Phase 2
// Gyro -> Non-FFT Notch1/2 -> Filtered FFT Window -> LPF -> Filtered Gyro -- Phase 1
// Gyro -> Filtered FFT Window -> LPF -> Filtered Gyro -- Phase 0
// Gyro -> FFT window -> LPF -> Filtered Gyro -- Phase 0
// Gyro -> Notch1/2 -> LPF -> Filtered Gyro
if (_post_filter_fft) {
for (auto &notch : harmonic_notches) {
if (!notch.params.enabled()) {
continue;
}
// window must always come before any FFT notch
if (notch.params.tracking_mode() == HarmonicNotchDynamicMode::UpdateGyroFFT) {
break;
}
_fft_window_phase++;
}
}
AP_InertialSensor: add throttle-based notch setup function allocate harmonic notch filter if FFT is enabled
2022-03-06 11:00:30 -04:00
#else
bool fft_enabled = false;
#endif
AP_InertialSensor: add harmonic notch filter to gyro filter chain Allow dynamic updates to the calculated frequency. Convert bandwidth and frequency to floats. backend variables do not need to be indexed per-backed
2019-06-17 05:44:12 -03:00
// the center frequency of the harmonic notch is always taken from the calculated value so that it can be updated
// dynamically, the calculated value is always some multiple of the configured center frequency, so start with the
// configured value
AP_InertialSensor: switch to HarmonicNotch class this makes the logic much easier to follow, without indexes into arrays
2022-04-15 04:38:56 -03:00
for (auto &notch : harmonic_notches) {
AP_InertialSensor: add throttle-based notch setup function allocate harmonic notch filter if FFT is enabled
2022-03-06 11:00:30 -04:00
if (!notch.params.enabled() && !fft_enabled) {
AP_InertialSensor: don't update disabled notches
2022-04-15 23:08:09 -03:00
continue;
}
AP_InertialSensor: init all notch center frequencies
2023-10-24 09:25:53 -03:00
for (uint8_t i = 0; i < ARRAY_SIZE(notch.calculated_notch_freq_hz); i++) {
notch.calculated_notch_freq_hz[i] = notch.params.center_freq_hz();
}
AP_InertialSensor: switch to HarmonicNotch class this makes the logic much easier to follow, without indexes into arrays
2022-04-15 04:38:56 -03:00
notch.num_calculated_notch_frequencies = 1;
notch.num_dynamic_notches = 1;
AP_InertialSensor: calculate number of notches based on motor count configure default harmonics if dynamic harmonics set number of notches correctly for FFT operation include static notch in filter calculation and allow harmonic options on static notch
2021-11-13 18:40:04 -04:00
#if APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane)
AP_InertialSensor: switch to HarmonicNotch class this makes the logic much easier to follow, without indexes into arrays
2022-04-15 04:38:56 -03:00
if (notch.params.hasOption(HarmonicNotchFilterParams::Options::DynamicHarmonic)) {
AP_InertialSensor: HAL_WITH_DSP -> HAL_GYROFFT_ENABLED
2022-11-28 05:33:50 -04:00
#if HAL_GYROFFT_ENABLED
AP_InertialSensor: add throttle-based notch setup function allocate harmonic notch filter if FFT is enabled
2022-03-06 11:00:30 -04:00
if (notch.params.tracking_mode() == HarmonicNotchDynamicMode::UpdateGyroFFT) {
notch.num_dynamic_notches = AP_HAL::DSP::MAX_TRACKED_PEAKS; // only 3 peaks supported currently
} else
AP_InertialSensor: calculate number of notches based on motor count configure default harmonics if dynamic harmonics set number of notches correctly for FFT operation include static notch in filter calculation and allow harmonic options on static notch
2021-11-13 18:40:04 -04:00
#endif
AP_InertialSensor: add throttle-based notch setup function allocate harmonic notch filter if FFT is enabled
2022-03-06 11:00:30 -04:00
{
AP_Motors *motors = AP::motors();
if (motors != nullptr) {
notch.num_dynamic_notches = __builtin_popcount(motors->get_motor_mask());
}
AP_InertialSensor: support two full harmonic notch filters
2022-04-13 00:36:18 -03:00
}
AP_InertialSensor: add throttle-based notch setup function allocate harmonic notch filter if FFT is enabled
2022-03-06 11:00:30 -04:00
// avoid harmonics unless actually configured by the user
notch.params.set_default_harmonics(1);
AP_InertialSensor: support two full harmonic notch filters
2022-04-13 00:36:18 -03:00
}
AP_InertialSensor: calculate number of notches based on motor count configure default harmonics if dynamic harmonics set number of notches correctly for FFT operation include static notch in filter calculation and allow harmonic options on static notch
2021-11-13 18:40:04 -04:00
#endif
AP_InertialSensor: support two full harmonic notch filters
2022-04-13 00:36:18 -03:00
}
AP_InertialSensor: calculate number of notches based on motor count configure default harmonics if dynamic harmonics set number of notches correctly for FFT operation include static notch in filter calculation and allow harmonic options on static notch
2021-11-13 18:40:04 -04:00
// count number of used sensors
uint8_t sensors_used = 0;
for (uint8_t i = 0; i < INS_MAX_INSTANCES; i++) {
AP_InertialSensor: add support for extra Aux IMUs
2023-02-17 00:26:27 -04:00
sensors_used += _use(i);
AP_InertialSensor: calculate number of notches based on motor count configure default harmonics if dynamic harmonics set number of notches correctly for FFT operation include static notch in filter calculation and allow harmonic options on static notch
2021-11-13 18:40:04 -04:00
}
AP_InertialSensor: allow up to four distinct notch center frequencies to be updated apply the LPF after the notch filters to reduced notch-induced noise
2020-05-29 13:28:06 -03:00
AP_InertialSensor: switch to HarmonicNotch class this makes the logic much easier to follow, without indexes into arrays
2022-04-15 04:38:56 -03:00
uint8_t num_filters = 0;
for (auto &notch : harmonic_notches) {
AP_InertialSensor: support two full harmonic notch filters
2022-04-13 00:36:18 -03:00
// calculate number of notches we might want to use for harmonic notch
AP_InertialSensor: add throttle-based notch setup function allocate harmonic notch filter if FFT is enabled
2022-03-06 11:00:30 -04:00
if (notch.params.enabled() || fft_enabled) {
AP_InertialSensor: switch to HarmonicNotch class this makes the logic much easier to follow, without indexes into arrays
2022-04-15 04:38:56 -03:00
const bool double_notch = notch.params.hasOption(HarmonicNotchFilterParams::Options::DoubleNotch);
AP_InertialSensor: implement triple notches
2022-06-23 13:53:39 -03:00
const bool triple_notch = notch.params.hasOption(HarmonicNotchFilterParams::Options::TripleNotch);
AP_InertialSensor: count filters to match notch options this allows for much more complex filter setups as long as not filtering on all IMUs
2022-06-07 06:39:50 -03:00
const bool all_sensors = notch.params.hasOption(HarmonicNotchFilterParams::Options::EnableOnAllIMUs);
AP_InertialSensor: switch to HarmonicNotch class this makes the logic much easier to follow, without indexes into arrays
2022-04-15 04:38:56 -03:00
num_filters += __builtin_popcount(notch.params.harmonics())
AP_InertialSensor: implement triple notches
2022-06-23 13:53:39 -03:00
* notch.num_dynamic_notches * (double_notch ? 2 : triple_notch ? 3 : 1)
AP_InertialSensor: count filters to match notch options this allows for much more complex filter setups as long as not filtering on all IMUs
2022-06-07 06:39:50 -03:00
* (all_sensors?sensors_used:1);
AP_InertialSensor: support two full harmonic notch filters
2022-04-13 00:36:18 -03:00
}
AP_InertialSensor: calculate number of notches based on motor count configure default harmonics if dynamic harmonics set number of notches correctly for FFT operation include static notch in filter calculation and allow harmonic options on static notch
2021-11-13 18:40:04 -04:00
}
if (num_filters > HAL_HNF_MAX_FILTERS) {
AP_BoardConfig::config_error("Too many notches: %u > %u", num_filters, HAL_HNF_MAX_FILTERS);
}
// allocate notches
AP_InertialSensor: add harmonic notch filter to gyro filter chain Allow dynamic updates to the calculated frequency. Convert bandwidth and frequency to floats. backend variables do not need to be indexed per-backed
2019-06-17 05:44:12 -03:00
for (uint8_t i=0; i<get_gyro_count(); i++) {
AP_InertialSensor: calculate number of notches based on motor count configure default harmonics if dynamic harmonics set number of notches correctly for FFT operation include static notch in filter calculation and allow harmonic options on static notch
2021-11-13 18:40:04 -04:00
// only allocate notches for IMUs in use
AP_InertialSensor: add support for extra Aux IMUs
2023-02-17 00:26:27 -04:00
if (_use(i)) {
AP_InertialSensor: switch to HarmonicNotch class this makes the logic much easier to follow, without indexes into arrays
2022-04-15 04:38:56 -03:00
for (auto &notch : harmonic_notches) {
AP_InertialSensor: add throttle-based notch setup function allocate harmonic notch filter if FFT is enabled
2022-03-06 11:00:30 -04:00
if (notch.params.enabled() || fft_enabled) {
AP_InertialSensor: switch to HarmonicNotch class this makes the logic much easier to follow, without indexes into arrays
2022-04-15 04:38:56 -03:00
const bool double_notch = notch.params.hasOption(HarmonicNotchFilterParams::Options::DoubleNotch);
AP_InertialSensor: implement triple notches
2022-06-23 13:53:39 -03:00
const bool triple_notch = notch.params.hasOption(HarmonicNotchFilterParams::Options::TripleNotch);
AP_InertialSensor: switch to HarmonicNotch class this makes the logic much easier to follow, without indexes into arrays
2022-04-15 04:38:56 -03:00
notch.filter[i].allocate_filters(notch.num_dynamic_notches,
AP_InertialSensor: implement triple notches
2022-06-23 13:53:39 -03:00
notch.params.harmonics(), double_notch ? 2 : triple_notch ? 3 : 1);
AP_InertialSensor: support two full harmonic notch filters
2022-04-13 00:36:18 -03:00
// initialise default settings, these will be subsequently changed in AP_InertialSensor_Backend::update_gyro()
AP_InertialSensor: switch to HarmonicNotch class this makes the logic much easier to follow, without indexes into arrays
2022-04-15 04:38:56 -03:00
notch.filter[i].init(_gyro_raw_sample_rates[i], notch.calculated_notch_freq_hz[0],
notch.params.bandwidth_hz(), notch.params.attenuation_dB());
AP_InertialSensor: support two full harmonic notch filters
2022-04-13 00:36:18 -03:00
}
AP_InertialSensor: calculate number of notches based on motor count configure default harmonics if dynamic harmonics set number of notches correctly for FFT operation include static notch in filter calculation and allow harmonic options on static notch
2021-11-13 18:40:04 -04:00
}
}
AP_InertialSensor: add harmonic notch filter to gyro filter chain Allow dynamic updates to the calculated frequency. Convert bandwidth and frequency to floats. backend variables do not need to be indexed per-backed
2019-06-17 05:44:12 -03:00
}
AP_InertialSensor: start auto-learning at boot This makes it easier to setup the params. The ENABLE=2 value only takes effect at boot
2021-01-15 21:23:17 -04:00
#if HAL_INS_TEMPERATURE_CAL_ENABLE
/*
see if user has setup for on-boot enable of temperature learning
*/
if (temperature_cal_running()) {
tcal_learning = true;
}
#endif
AP_InertialSensor: first steps in frontend/backend split This converts the MPU6000 driver to a frontend/backend structure, and disables all other drivers. They will be progressively re-enabled as each is converted
2014-10-14 01:48:33 -03:00
}
AP_InertialSensor: auto-handle invensense sensor changes some boards swap out MPU6000 for MPU9250 or ICM20608
2016-11-10 00:30:48 -04:00
bool AP_InertialSensor::_add_backend(AP_InertialSensor_Backend *backend)
AP_InertialSensor: make it easier to add multiple backends use _add_backend() to add new backends on startup
2014-10-16 17:27:01 -03:00
{
AP_InertialSensor: add LSM9DS1 entry Enable LSM9DS1 in case it is selected
2017-10-26 11:36:19 -03:00
AP_InertialSensor: minor formatting fixes No functional change
2016-12-23 23:21:29 -04:00
if (!backend) {
AP_InertialSensor: auto-handle invensense sensor changes some boards swap out MPU6000 for MPU9250 or ICM20608
2016-11-10 00:30:48 -04:00
return false;
AP_InertialSensor: minor formatting fixes No functional change
2016-12-23 23:21:29 -04:00
}
if (_backend_count == INS_MAX_BACKENDS) {
AP_InertialSensor: use millis/micros/panic functions
2015-11-19 23:11:52 -04:00
AP_HAL::panic("Too many INS backends");
AP_InertialSensor: minor formatting fixes No functional change
2016-12-23 23:21:29 -04:00
}
AP_InertialSensor: pass backend instead of pointer to function Different detect() function might need different arguments and passing a pointer to function here is cumbersome. For example, it forces to have a method like "detect_i2c2" rather than allowing hal.i2c2 to be passed as parameter.
2015-07-28 11:02:09 -03:00
_backends[_backend_count++] = backend;
AP_InertialSensor: auto-handle invensense sensor changes some boards swap out MPU6000 for MPU9250 or ICM20608
2016-11-10 00:30:48 -04:00
return true;
AP_InertialSensor: make it easier to add multiple backends use _add_backend() to add new backends on startup
2014-10-16 17:27:01 -03:00
}
AP_InertialSensor: first steps in frontend/backend split This converts the MPU6000 driver to a frontend/backend structure, and disables all other drivers. They will be progressively re-enabled as each is converted
2014-10-14 01:48:33 -03:00
/*
detect available backends for this board
*/
AP_InertialSensor: Make detect_backends() public This method will be used to initialize and configure I2C backends that have an auxiliary I2C bus that can be connected to the main I2C bus, like MPU6000 and MPU9250.
2015-10-13 15:33:20 -03:00
void
AP_InertialSensor::detect_backends(void)
AP_InertialSensor: first steps in frontend/backend split This converts the MPU6000 driver to a frontend/backend structure, and disables all other drivers. They will be progressively re-enabled as each is converted
2014-10-14 01:48:33 -03:00
{
AP_InertialSensor: minor formatting fixes No functional change
2016-12-23 23:21:29 -04:00
if (_backends_detected) {
AP_InertialSensor: allow to split detection and initialization This allows backends to have a separate detection and initialization logic. It doesn't change any backend yet and with the current code there's no change in behavior either. This only allows AP_InertialSensor::_detect_backend() to be called earlier so AP_InertialSensor object can be used by other libraries. If it's not called, later on AP_InertialSensor::init() will detect and start all backends.
2015-08-05 10:01:52 -03:00
return;
AP_InertialSensor: minor formatting fixes No functional change
2016-12-23 23:21:29 -04:00
}
AP_InertialSensor: allow to split detection and initialization This allows backends to have a separate detection and initialization logic. It doesn't change any backend yet and with the current code there's no change in behavior either. This only allows AP_InertialSensor::_detect_backend() to be called earlier so AP_InertialSensor object can be used by other libraries. If it's not called, later on AP_InertialSensor::init() will detect and start all backends.
2015-08-05 10:01:52 -03:00
_backends_detected = true;
AP_InertialSensor: calculate number of notches based on motor count configure default harmonics if dynamic harmonics set number of notches correctly for FFT operation include static notch in filter calculation and allow harmonic options on static notch
2021-11-13 18:40:04 -04:00
#if defined(HAL_CHIBIOS_ARCH_CUBE) && INS_MAX_INSTANCES > 2
AP_InertialSensor: for all Cubes ensure use of non-isolated IMU
2021-11-29 01:17:14 -04:00
// special case for Cubes, where the IMUs on the isolated
AP_InertialSensor: special check for CubeBlack to force INS_USE3=1 this will cope with users loading old parameter files
2019-07-30 21:47:50 -03:00
// board could fail on some boards. If the user has INS_USE=1,
// INS_USE2=1 and INS_USE3=0 then force INS_USE3 to 1. This is
// done as users loading past parameter files may end up with
// INS_USE3=0 unintentionally, which is unsafe on these
// boards. For users who really want limited IMUs they will need
// to either use the INS_ENABLE_MASK or set INS_USE2=0 which will
// enable the first IMU without triggering this check
AP_InertialSensor: add support for extra Aux IMUs
2023-02-17 00:26:27 -04:00
if (_use(0) == 1 && _use(1) == 1 && _use(2) == 0) {
_use(2).set(1);
AP_InertialSensor: special check for CubeBlack to force INS_USE3=1 this will cope with users loading old parameter files
2019-07-30 21:47:50 -03:00
}
#endif
AP_InertialSensor: put some functions in fast ram precisely split backend/frontend on Invensense for RAMFUNC allocation allow building with no IMUs
2021-09-14 17:28:20 -03:00
uint8_t probe_count __attribute__((unused)) = 0;
uint8_t enable_mask __attribute__((unused)) = uint8_t(_enable_mask.get());
uint8_t found_mask __attribute__((unused)) = 0;
AP_InertialSensor: added INS_ENABLE_MASK this allows for only a specified subset of IMUs to be probed, so you can disable IMUs that aren't needed. The back corresponds to bits in the order the IMUs are normally probed on the board
2018-01-17 03:16:48 -04:00
/*
use ADD_BACKEND() macro to allow for INS_ENABLE_MASK for enabling/disabling INS backends
*/
#define ADD_BACKEND(x) do { \
if (((1U<<probe_count)&enable_mask) && _add_backend(x)) { \
found_mask |= (1U<<probe_count); \
} \
probe_count++; \
} while (0)
AP_InertialSensor: define GET_I2C_DEVICE()
2019-09-01 19:13:27 -03:00
AP_InertialSensor: fix duplicate sensor detection for AUX sensors
2023-03-22 02:46:41 -03:00
// Can be used by adding INSTANCE:<num> keyword in hwdef.
// This keyword is used to denote the instance number of the sensor
// while probing. Probing is skipped if the instance number doesn't match the
// backend count. Its important the IMUs are listed in order of precedence globally
// (i.e. INSTANCE:0 IMUs are listed before INSTANCE:1 IMUs) and locally (i.e. IMUs
// on the same bus are listed in order of detection precedence)
AP_InertialSensor: add support for ICM45686 IMU
2022-10-25 21:35:55 -03:00
#define ADD_BACKEND_INSTANCE(x, instance) if (instance == _backend_count) { ADD_BACKEND(x); }
AP_InertialSensor: fix duplicate sensor detection for AUX sensors
2023-03-22 02:46:41 -03:00
// Can be used by adding AUX:<devid> keyword in hwdef.
// AUX:<devid> keyword is used to check for the presence of the sensor
// in the detected IMUs list. If the IMU with the given devid is found
// then we skip the probe for the sensor the second time. This is useful
// if you have multiple choices for IMU over same instance number, and still
// want to instantiate the sensor after main IMUs are detected.
#define ADD_BACKEND_AUX(x, devid) do { \
bool init_aux = true; \
for (uint8_t i=0; i<_backend_count; i++) { \
if (((uint32_t)_accel_id(i) == devid) || ((uint32_t)_gyro_id(i) == devid)) { \
init_aux = false; \
} \
} \
if (init_aux) { \
ADD_BACKEND(x); \
} \
} while (0)
AP_InertialSensor: support BOARD_MATCH() for IMUs
2022-07-18 19:32:27 -03:00
// support for adding IMUs conditioned on board type
#define BOARD_MATCH(board_type) AP_BoardConfig::get_board_type()==AP_BoardConfig::board_type
#define ADD_BACKEND_BOARD_MATCH(board_match, x) do { if (board_match) { ADD_BACKEND(x); } } while(0)
AP_InertialSensor: define GET_I2C_DEVICE()
2019-09-01 19:13:27 -03:00
// macro for use by HAL_INS_PROBE_LIST
#define GET_I2C_DEVICE(bus, address) hal.i2c_mgr->get_device(bus, address)
AP_InertialSensor: added ExternalAHRS backend
2020-12-28 22:29:40 -04:00
#if HAL_EXTERNAL_AHRS_ENABLED
// if enabled, make the first IMU the external AHRS
AP_InertialSensor: allow enabling of only some ExternalAHRS sensors
2023-01-24 00:40:17 -04:00
const int8_t serial_port = AP::externalAHRS().get_port(AP_ExternalAHRS::AvailableSensor::IMU);
AP_InertialSensor: fix externalAHRS detection 'A = B >= C' kind. The expression is calculated as following: 'A = (B >= C)'
2021-08-10 19:33:49 -03:00
if (serial_port >= 0) {
AP_InertialSensor: added ExternalAHRS backend
2020-12-28 22:29:40 -04:00
ADD_BACKEND(new AP_InertialSensor_ExternalAHRS(*this, serial_port));
}
#endif
AP_InertialSensor: add and use AP_SIM_INS_ENABLED
2021-10-29 22:15:49 -03:00
#if AP_SIM_INS_ENABLED
AP_InertialSensor: instantiate SIM_IMU_COUNT sensors in SITL
2020-08-27 05:32:58 -03:00
for (uint8_t i=0; i<AP::sitl()->imu_count; i++) {
ADD_BACKEND(AP_InertialSensor_SITL::detect(*this, i==1?INS_SITL_SENSOR_B:INS_SITL_SENSOR_A));
}
AP_InertialSensor: add and use AP_SIM_INS_ENABLED
2021-10-29 22:15:49 -03:00
return;
#endif
#if defined(HAL_INS_PROBE_LIST)
// IMUs defined by IMU lines in hwdef.dat
HAL_INS_PROBE_LIST;
AP_InertialSensor: add simulator for ICM40609
2021-01-10 20:10:00 -04:00
#if defined(HAL_SITL_INVENSENSEV3)
ADD_BACKEND(AP_InertialSensor_Invensensev3::probe(*this, hal.i2c_mgr->get_device(1, 1), ROTATION_NONE));
#endif
AP_InertialSensor: support ChibiOS FMUv4
2018-01-10 06:34:50 -04:00
#elif AP_FEATURE_BOARD_DETECT
AP_InertialSensor: change if statements to switch statement No functional change
2016-12-23 23:22:08 -04:00
switch (AP_BoardConfig::get_board_type()) {
case AP_BoardConfig::PX4_BOARD_PX4V1:
AP_InertialSensor: removed old method of specifying most IMUs and removed dangerous default values for rotations
2019-08-27 06:28:37 -03:00
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME), ROTATION_NONE));
AP_InertialSensor: change if statements to switch statement No functional change
2016-12-23 23:22:08 -04:00
break;
AP_InertialSensor: auto-handle invensense sensor changes some boards swap out MPU6000 for MPU9250 or ICM20608
2016-11-10 00:30:48 -04:00
AP_InertialSensor: change if statements to switch statement No functional change
2016-12-23 23:22:08 -04:00
case AP_BoardConfig::PX4_BOARD_PIXHAWK:
AP_InertialSensor: added INS_ENABLE_MASK this allows for only a specified subset of IMUs to be probed, so you can disable IMUs that aren't needed. The back corresponds to bits in the order the IMUs are normally probed on the board
2018-01-17 03:16:48 -04:00
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME), ROTATION_ROLL_180));
ADD_BACKEND(AP_InertialSensor_LSM9DS0::probe(*this,
AP_InertialSensor: allow for in-tree drivers for PX4
2016-11-01 07:34:59 -03:00
hal.spi->get_device(HAL_INS_LSM9DS0_G_NAME),
AP_InertialSensor: fixed gyro orientation on l3gd20H on pixhawk
2016-11-16 00:16:15 -04:00
hal.spi->get_device(HAL_INS_LSM9DS0_A_NAME),
AP_InertialSensor: support the L3GD20H gyro this is used by the MRo Pixhawk1
2017-01-29 23:49:03 -04:00
ROTATION_ROLL_180,
ROTATION_ROLL_180_YAW_270,
ROTATION_PITCH_180));
AP_InertialSensor: change if statements to switch statement No functional change
2016-12-23 23:22:08 -04:00
break;
case AP_BoardConfig::PX4_BOARD_PIXHAWK2:
AP_InertialSensor: auto-handle invensense sensor changes some boards swap out MPU6000 for MPU9250 or ICM20608
2016-11-10 00:30:48 -04:00
// older Pixhawk2 boards have the MPU6000 instead of MPU9250
AP_InertialSensor: enable fast sampling by default on some boards
2016-11-25 02:59:54 -04:00
_fast_sampling_mask.set_default(1);
AP_InertialSensor: added INS_ENABLE_MASK this allows for only a specified subset of IMUs to be probed, so you can disable IMUs that aren't needed. The back corresponds to bits in the order the IMUs are normally probed on the board
2018-01-17 03:16:48 -04:00
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_EXT_NAME), ROTATION_PITCH_180));
ADD_BACKEND(AP_InertialSensor_LSM9DS0::probe(*this,
AP_InertialSensor: enable all 3 IMUs on PH2
2016-11-04 03:58:37 -03:00
hal.spi->get_device(HAL_INS_LSM9DS0_EXT_G_NAME),
AP_InertialSensor: fixed gyro orientation on l3gd20H on pixhawk
2016-11-16 00:16:15 -04:00
hal.spi->get_device(HAL_INS_LSM9DS0_EXT_A_NAME),
AP_InertialSensor: support the L3GD20H gyro this is used by the MRo Pixhawk1
2017-01-29 23:49:03 -04:00
ROTATION_ROLL_180_YAW_270,
ROTATION_ROLL_180_YAW_90,
ROTATION_ROLL_180_YAW_90));
AP_InertialSensor: added INS_ENABLE_MASK this allows for only a specified subset of IMUs to be probed, so you can disable IMUs that aren't needed. The back corresponds to bits in the order the IMUs are normally probed on the board
2018-01-17 03:16:48 -04:00
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_YAW_270));
INS: AP_InertialSensor comments for new cubes Correct comments ICM20648 should have been ICM20948
2019-03-14 22:39:51 -03:00
// new cubes have ICM20602, ICM20948, ICM20649
AP_InertialSensor: look for icm20602 for Pixhawk 2 series flight boards
2019-01-22 22:46:05 -04:00
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device("icm20602_ext"), ROTATION_ROLL_180_YAW_270));
AP_IneertialSensor: fixed orientations and gyro scale factors for new Invensense IMUs
2019-03-14 02:47:45 -03:00
ADD_BACKEND(AP_InertialSensor_Invensensev2::probe(*this, hal.spi->get_device("icm20948_ext"), ROTATION_PITCH_180));
ADD_BACKEND(AP_InertialSensor_Invensensev2::probe(*this, hal.spi->get_device("icm20948"), ROTATION_YAW_270));
AP_InertialSensor: change if statements to switch statement No functional change
2016-12-23 23:22:08 -04:00
break;
AP_InertialSensor: detect two IMUs on FMUv5
2018-05-29 08:44:04 -03:00
case AP_BoardConfig::PX4_BOARD_FMUV5:
AP_InertialSensor: setup for fmuv6
2019-02-06 17:10:08 -04:00
case AP_BoardConfig::PX4_BOARD_FMUV6:
AP_InertialSensor: detect two IMUs on FMUv5
2018-05-29 08:44:04 -03:00
_fast_sampling_mask.set_default(1);
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device("icm20689"), ROTATION_NONE));
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device("icm20602"), ROTATION_NONE));
AP_InertialSensor: added driver for BMI088 setup for 1600 Hz accel sample, 2kHz gyro sample, 24g accel range
2019-01-31 03:00:34 -04:00
// allow for either BMI055 or BMI088
AP_InertialSensor: added BMI055 IMU driver
2018-06-07 02:59:18 -03:00
ADD_BACKEND(AP_InertialSensor_BMI055::probe(*this,
hal.spi->get_device("bmi055_a"),
hal.spi->get_device("bmi055_g"),
ROTATION_ROLL_180_YAW_90));
AP_InertialSensor: added driver for BMI088 setup for 1600 Hz accel sample, 2kHz gyro sample, 24g accel range
2019-01-31 03:00:34 -04:00
ADD_BACKEND(AP_InertialSensor_BMI088::probe(*this,
hal.spi->get_device("bmi055_a"),
hal.spi->get_device("bmi055_g"),
ROTATION_ROLL_180_YAW_90));
AP_InertialSensor: detect two IMUs on FMUv5
2018-05-29 08:44:04 -03:00
break;
AP_InertialSensor: added SP01 board type
2018-02-14 01:36:14 -04:00
case AP_BoardConfig::PX4_BOARD_SP01:
_fast_sampling_mask.set_default(1);
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_EXT_NAME), ROTATION_NONE));
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_NONE));
break;
AP_InertialSensor: add ppro support
2017-07-15 02:13:44 -03:00
case AP_BoardConfig::PX4_BOARD_PIXHAWK_PRO:
_fast_sampling_mask.set_default(3);
AP_InertialSensor: added INS_ENABLE_MASK this allows for only a specified subset of IMUs to be probed, so you can disable IMUs that aren't needed. The back corresponds to bits in the order the IMUs are normally probed on the board
2018-01-17 03:16:48 -04:00
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_ICM20608_NAME), ROTATION_ROLL_180_YAW_90));
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_ROLL_180_YAW_90));
AP_InertialSensor: add ppro support
2017-07-15 02:13:44 -03:00
break;
AP_InertialSensor: change if statements to switch statement No functional change
2016-12-23 23:22:08 -04:00
case AP_BoardConfig::PX4_BOARD_PHMINI:
AP_InertialSensor: use internal drivers for PHMINI this gets us fast sampling
2016-11-08 20:33:22 -04:00
// PHMINI uses ICM20608 on the ACCEL_MAG device and a MPU9250 on the old MPU6000 CS line
AP_InertialSensor: enable fast sampling by default on some boards
2016-11-25 02:59:54 -04:00
_fast_sampling_mask.set_default(3);
AP_InertialSensor: added INS_ENABLE_MASK this allows for only a specified subset of IMUs to be probed, so you can disable IMUs that aren't needed. The back corresponds to bits in the order the IMUs are normally probed on the board
2018-01-17 03:16:48 -04:00
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_ICM20608_AM_NAME), ROTATION_ROLL_180));
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_ROLL_180));
AP_InertialSensor: change if statements to switch statement No functional change
2016-12-23 23:22:08 -04:00
break;
AP_InertialSensor: auto-handle invensense sensor changes some boards swap out MPU6000 for MPU9250 or ICM20608
2016-11-10 00:30:48 -04:00
AP_InertialSensor: support AUAV21 board
2017-02-28 21:42:29 -04:00
case AP_BoardConfig::PX4_BOARD_AUAV21:
// AUAV2.1 uses ICM20608 on the ACCEL_MAG device and a MPU9250 on the old MPU6000 CS line
_fast_sampling_mask.set_default(3);
AP_InertialSensor: added INS_ENABLE_MASK this allows for only a specified subset of IMUs to be probed, so you can disable IMUs that aren't needed. The back corresponds to bits in the order the IMUs are normally probed on the board
2018-01-17 03:16:48 -04:00
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_ICM20608_AM_NAME), ROTATION_ROLL_180_YAW_90));
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_ROLL_180_YAW_90));
AP_InertialSensor: support AUAV21 board
2017-02-28 21:42:29 -04:00
break;
AP_InertialSensor: change if statements to switch statement No functional change
2016-12-23 23:22:08 -04:00
case AP_BoardConfig::PX4_BOARD_PH2SLIM:
AP_InertialSensor: enable fast sampling by default on some boards
2016-11-25 02:59:54 -04:00
_fast_sampling_mask.set_default(1);
AP_InertialSensor: added INS_ENABLE_MASK this allows for only a specified subset of IMUs to be probed, so you can disable IMUs that aren't needed. The back corresponds to bits in the order the IMUs are normally probed on the board
2018-01-17 03:16:48 -04:00
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_YAW_270));
AP_InertialSensor: change if statements to switch statement No functional change
2016-12-23 23:22:08 -04:00
break;
AP_InertialSensor: use MPU6500 on aerofc
2017-02-06 19:50:48 -04:00
case AP_BoardConfig::PX4_BOARD_AEROFC:
AP_InertialSensor: enable fast sampling for aerofc
2017-03-10 17:01:40 -04:00
_fast_sampling_mask.set_default(1);
AP_InertialSensor: added INS_ENABLE_MASK this allows for only a specified subset of IMUs to be probed, so you can disable IMUs that aren't needed. The back corresponds to bits in the order the IMUs are normally probed on the board
2018-01-17 03:16:48 -04:00
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU6500_NAME), ROTATION_YAW_270));
AP_InertialSensor: use MPU6500 on aerofc
2017-02-06 19:50:48 -04:00
break;
AP_InertialSensor: support MindPX-v2
2018-01-11 17:27:02 -04:00
case AP_BoardConfig::PX4_BOARD_MINDPXV2:
AP_InertialSensor: added INS_ENABLE_MASK this allows for only a specified subset of IMUs to be probed, so you can disable IMUs that aren't needed. The back corresponds to bits in the order the IMUs are normally probed on the board
2018-01-17 03:16:48 -04:00
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU6500_NAME), ROTATION_NONE));
ADD_BACKEND(AP_InertialSensor_LSM9DS0::probe(*this,
AP_InertialSensor: support MindPX-v2
2018-01-11 17:27:02 -04:00
hal.spi->get_device(HAL_INS_LSM9DS0_G_NAME),
hal.spi->get_device(HAL_INS_LSM9DS0_A_NAME),
AP_InertialSensor: fixed MindPX-v2 orientation of IMU2
2018-01-11 21:01:22 -04:00
ROTATION_YAW_90,
ROTATION_YAW_90,
ROTATION_YAW_90));
AP_InertialSensor: support MindPX-v2
2018-01-11 17:27:02 -04:00
break;
VRBRAIN / AP_InertialSensor: added inertial sensors init for VR Brain boards
2018-02-03 09:56:40 -04:00
case AP_BoardConfig::VRX_BOARD_BRAIN54:
_fast_sampling_mask.set_default(7);
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME), ROTATION_YAW_180));
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_EXT_NAME), ROTATION_YAW_180));
#ifdef HAL_INS_MPU60x0_IMU_NAME
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_IMU_NAME), ROTATION_YAW_180));
#endif
break;
case AP_BoardConfig::VRX_BOARD_BRAIN51:
case AP_BoardConfig::VRX_BOARD_BRAIN52:
case AP_BoardConfig::VRX_BOARD_BRAIN52E:
case AP_BoardConfig::VRX_BOARD_CORE10:
case AP_BoardConfig::VRX_BOARD_UBRAIN51:
case AP_BoardConfig::VRX_BOARD_UBRAIN52:
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME), ROTATION_YAW_180));
break;
AP_InertialSensor: support PCNC1
2017-06-21 04:03:20 -03:00
case AP_BoardConfig::PX4_BOARD_PCNC1:
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME), ROTATION_ROLL_180));
break;
VRBRAIN / AP_InertialSensor: added inertial sensors init for VR Brain boards
2018-02-03 09:56:40 -04:00
AP_InertialSensor: change if statements to switch statement No functional change
2016-12-23 23:22:08 -04:00
default:
break;
AP_InertialSensor: allow for in-tree drivers for PX4
2016-11-01 07:34:59 -03:00
}
AP_InertialSensor: allow for HAL_INS_NONE
2018-03-01 20:45:15 -04:00
#elif HAL_INS_DEFAULT == HAL_INS_NONE
// no INS device
AP_InertialSensor: converted HIL backend, which gets SITL working
2014-10-15 02:37:59 -03:00
#else
#error Unrecognised HAL_INS_TYPE setting
#endif
AP_InertialSensor: make it easier to add multiple backends use _add_backend() to add new backends on startup
2014-10-16 17:27:01 -03:00
AP_InertialSensor: allow to split detection and initialization This allows backends to have a separate detection and initialization logic. It doesn't change any backend yet and with the current code there's no change in behavior either. This only allows AP_InertialSensor::_detect_backend() to be called earlier so AP_InertialSensor object can be used by other libraries. If it's not called, later on AP_InertialSensor::init() will detect and start all backends.
2015-08-05 10:01:52 -03:00
if (_backend_count == 0) {
AP_InertialSensor: New fallback type = NONE, which successfully does nothing like an INS, but without needing a real one, like esp32 dev boards. Good for boards that u want to boot even when they dont have an IMU attached. Author: Charles Villard <charlesvillard10@gmail.com> Author: Buzz <davidbuzz@gmail.com>
2021-10-27 05:06:41 -03:00
// no real INS backends avail, lets use an empty substitute to boot ok and get to mavlink
#if CONFIG_HAL_BOARD == HAL_BOARD_ESP32
ADD_BACKEND(AP_InertialSensor_NONE::detect(*this, INS_NONE_SENSOR_A));
#else
AP_InertialSensor: Console output can be disabled
2022-03-21 06:25:18 -03:00
DEV_PRINTF("INS: unable to initialise driver\n");
AP_InertialSensor: New fallback type = NONE, which successfully does nothing like an INS, but without needing a real one, like esp32 dev boards. Good for boards that u want to boot even when they dont have an IMU attached. Author: Charles Villard <charlesvillard10@gmail.com> Author: Buzz <davidbuzz@gmail.com>
2021-10-27 05:06:41 -03:00
GCS_SEND_TEXT(MAV_SEVERITY_DEBUG, "INS: unable to initialise driver");
AP_InertialSensor: rename sensor_config_error to config_error
2019-11-06 15:45:30 -04:00
AP_BoardConfig::config_error("INS: unable to initialise driver");
AP_InertialSensor: New fallback type = NONE, which successfully does nothing like an INS, but without needing a real one, like esp32 dev boards. Good for boards that u want to boot even when they dont have an IMU attached. Author: Charles Villard <charlesvillard10@gmail.com> Author: Buzz <davidbuzz@gmail.com>
2021-10-27 05:06:41 -03:00
#endif
AP_InertialSensor: first steps in frontend/backend split This converts the MPU6000 driver to a frontend/backend structure, and disables all other drivers. They will be progressively re-enabled as each is converted
2014-10-14 01:48:33 -03:00
}
AP_InertialSensor: merge in calibration features from IMU library add gauss-newton method of accelerometer calibration
2012-11-05 00:27:03 -04:00
}
AP_InertialSensor: add IMU batch sampling
2017-10-03 20:44:07 -03:00
// Armed, Copter, PixHawk:
// ins_periodic: 57500 events, 0 overruns, 208754us elapsed, 3us avg, min 1us max 218us 40.662us rms
void AP_InertialSensor::periodic()
{
AP_InertialSensor: create explicit define for AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED this allows the batch sampler to be removed for size reasons, or because you don't have logging enabled
2023-01-03 01:51:51 -04:00
#if AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED
AP_InertialSensor: add IMU batch sampling
2017-10-03 20:44:07 -03:00
batchsampler.periodic();
AP_InertialSensor: create explicit define for AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED this allows the batch sampler to be removed for size reasons, or because you don't have logging enabled
2023-01-03 01:51:51 -04:00
#endif
AP_InertialSensor: add IMU batch sampling
2017-10-03 20:44:07 -03:00
}
AP_InertialSensor: fixed AHRS_TRIM calculation again
2015-05-15 18:18:09 -03:00
/*
_calculate_trim - calculates the x and y trim angles. The
accel_sample must be correctly scaled, offset and oriented for the
AP_InertialSensor: sanitize includes Due to the way the headers are organized changing a single change in an inertial sensor driver would trigger a rebuild for most of the files in the project. Time could be saved by using ccache (since most of the things didn't change) but we can do better, i.e. re-organize the headers so we don't have to re-build everything. With this patch only AP_InertialSensor/AP_InertialSensor.h is exposed to most users. There are some corner cases to integrate with some example code, but most of the places now depend only on this header and this header doesn't depend on the specific backends. Now changing a single header, e.g. AP_InertialSensor_L3G4200D.h triggers a rebuild only of these files: $ waf copter 'copter' finished successfully (0.000s) Waf: Entering directory `/home/lucas/p/dronecode/ardupilot/build/minlure' [ 80/370] Compiling libraries/AP_InertialSensor/AP_InertialSensor.cpp [ 84/370] Compiling libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp [310/370] Linking build/minlure/ArduCopter/libArduCopter_libs.a [370/370] Linking build/minlure/bin/arducopter Waf: Leaving directory `/home/lucas/p/dronecode/ardupilot/build/minlure'
2016-01-19 21:26:31 -04:00
board
AP_InertialSensor: allow accelcal with a rotation
2021-08-12 19:57:43 -03:00
Note that this only changes 2 axes of the trim vector. When in
ROTATION_NONE view we can calculate the x and y trim. When in
ROTATION_PITCH_90 for tailsitters we can calculate y and z. This
allows users to trim for both flight orientations by doing two trim
operations, one at each orientation
When doing a full accel cal we pass in a trim vector that has been
zeroed so the 3rd non-observable axis is reset
AP_InertialSensor: fixed AHRS_TRIM calculation again
2015-05-15 18:18:09 -03:00
*/
AP_InertialSensor: allow accelcal with a rotation
2021-08-12 19:57:43 -03:00
bool AP_InertialSensor::_calculate_trim(const Vector3f &accel_sample, Vector3f &trim)
AP_InertialSensor: fixed AHRS_TRIM calculation again
2015-05-15 18:18:09 -03:00
{
AP_InertialSensor: use pitch to guess which axis the user is trying to calibrate, warn about Q_TRIM_PITCH on plane
2022-04-23 08:16:08 -03:00
Rotation rotation = ROTATION_NONE;
#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
AP_AHRS_View *view = AP::ahrs().get_view();
if (view != nullptr) {
// Use pitch to guess which axis the user is trying to trim
// 5 deg buffer to favor normal AHRS and avoid floating point funny business
if (fabsf(view->pitch) < (fabsf(AP::ahrs().pitch)+radians(5)) ) {
// user is trying to calibrate view
rotation = view->get_rotation();
if (!is_zero(view->get_pitch_trim())) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Cannot calibrate with Q_TRIM_PITCH set");
return false;
}
AP_InertialSensor: allow accelcal with a rotation
2021-08-12 19:57:43 -03:00
}
}
AP_InertialSensor: use pitch to guess which axis the user is trying to calibrate, warn about Q_TRIM_PITCH on plane
2022-04-23 08:16:08 -03:00
#endif
Vector3f newtrim = trim;
switch (rotation) {
case ROTATION_NONE:
newtrim.y = atan2f(accel_sample.x, norm(accel_sample.y, accel_sample.z));
newtrim.x = atan2f(-accel_sample.y, -accel_sample.z);
break;
case ROTATION_PITCH_90: {
newtrim.y = atan2f(accel_sample.z, norm(accel_sample.y, -accel_sample.x));
newtrim.z = atan2f(-accel_sample.y, accel_sample.x);
break;
}
default:
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "unsupported trim rotation");
AP_InertialSensor: fixed AHRS_TRIM calculation again
2015-05-15 18:18:09 -03:00
return false;
}
AP_InertialSensor: use pitch to guess which axis the user is trying to calibrate, warn about Q_TRIM_PITCH on plane
2022-04-23 08:16:08 -03:00
if (fabsf(newtrim.x) <= radians(HAL_INS_TRIM_LIMIT_DEG) &&
fabsf(newtrim.y) <= radians(HAL_INS_TRIM_LIMIT_DEG) &&
fabsf(newtrim.z) <= radians(HAL_INS_TRIM_LIMIT_DEG)) {
trim = newtrim;
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Trim OK: roll=%.2f pitch=%.2f yaw=%.2f",
(double)degrees(trim.x),
(double)degrees(trim.y),
(double)degrees(trim.z));
return true;
}
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "trim over maximum of 10 degrees");
return false;
AP_InertialSensor: fixed AHRS_TRIM calculation again
2015-05-15 18:18:09 -03:00
}
AP_InertialSensor: merge in calibration features from IMU library add gauss-newton method of accelerometer calibration
2012-11-05 00:27:03 -04:00
void
InertialSensor: reorder .cpp file to match .h No functional changes
2014-09-01 03:28:07 -03:00
AP_InertialSensor::init_gyro()
AP_InertialSensor: merge in calibration features from IMU library add gauss-newton method of accelerometer calibration
2012-11-05 00:27:03 -04:00
{
InertialSensor: reorder .cpp file to match .h No functional changes
2014-09-01 03:28:07 -03:00
_init_gyro();
AP_InertialSensor: merge in calibration features from IMU library add gauss-newton method of accelerometer calibration
2012-11-05 00:27:03 -04:00
// save calibration
AP_InertialSensor: save only gyro calibration When we are initializing the gyro and then saving the calibration we are also saving the calibration values for the accelerometers. Right now this is non-problematic, but we want to check that the ID of the accelerometer corresponds to the ID of the sensor detected. If we also save accel calibrations we would actually override the ID of the accelerometer. Rename the method to _save_gyro_calibration() and save only on gyro values.
2016-09-03 21:18:09 -03:00
_save_gyro_calibration();
AP_InertialSensor: merge in calibration features from IMU library add gauss-newton method of accelerometer calibration
2012-11-05 00:27:03 -04:00
}
AP_InertialSensor: make the backend fast gyro rate configurable raise gyro rate default on F7 and H7 clean up gyro rate docs and output startup banner
2020-05-21 15:29:28 -03:00
// output GCS startup messages
bool AP_InertialSensor::get_output_banner(uint8_t backend_id, char* banner, uint8_t banner_len)
{
if (backend_id >= INS_MAX_BACKENDS || _backends[backend_id] == nullptr) {
return false;
}
return _backends[backend_id]->get_output_banner(banner, banner_len);
}
InertialSensor: calc vibration and accel clipping
2015-06-11 04:15:57 -03:00
// accelerometer clipping reporting
uint32_t AP_InertialSensor::get_accel_clip_count(uint8_t instance) const
{
InertialSensor: disable vibration checks on APM2 Also bug fix get_accel_clip_count's instance check
2015-06-12 06:23:38 -03:00
if (instance >= get_accel_count()) {
InertialSensor: calc vibration and accel clipping
2015-06-11 04:15:57 -03:00
return 0;
}
return _accel_clip_count[instance];
}
INS: add get_accel_health_all and get_gyro_health_all Returns true only if all available accels or gyros are healthy
2014-09-01 08:20:27 -03:00
// get_gyro_health_all - return true if all gyros are healthy
bool AP_InertialSensor::get_gyro_health_all(void) const
{
for (uint8_t i=0; i<get_gyro_count(); i++) {
if (!get_gyro_health(i)) {
return false;
}
}
// return true if we have at least one gyro
return (get_gyro_count() > 0);
}
INS: add gyro_calibrated_ok_all method This returns true if the gyros have been calibrated successfully
2014-10-08 08:17:31 -03:00
// gyro_calibration_ok_all - returns true if all gyros were calibrated successfully
bool AP_InertialSensor::gyro_calibrated_ok_all() const
{
for (uint8_t i=0; i<get_gyro_count(); i++) {
if (!gyro_calibrated_ok(i)) {
return false;
}
}
AP_InertialSensor: detect missing accels or gyros
2016-11-05 06:20:43 -03:00
for (uint8_t i=get_gyro_count(); i<INS_MAX_INSTANCES; i++) {
AP_InertialSensor: add support for extra Aux IMUs
2023-02-17 00:26:27 -04:00
if (_gyro_id(i) != 0) {
AP_InertialSensor: detect missing accels or gyros
2016-11-05 06:20:43 -03:00
// missing gyro
return false;
}
}
INS: add gyro_calibrated_ok_all method This returns true if the gyros have been calibrated successfully
2014-10-08 08:17:31 -03:00
return (get_gyro_count() > 0);
}
INS: add USE parameters
2015-08-01 03:40:40 -03:00
// return true if gyro instance should be used (must be healthy and have it's use parameter set to 1)
bool AP_InertialSensor::use_gyro(uint8_t instance) const
{
if (instance >= INS_MAX_INSTANCES) {
return false;
}
AP_InertialSensor: add support for extra Aux IMUs
2023-02-17 00:26:27 -04:00
return (get_gyro_health(instance) && _use(instance));
INS: add USE parameters
2015-08-01 03:40:40 -03:00
}
INS: add get_accel_health_all and get_gyro_health_all Returns true only if all available accels or gyros are healthy
2014-09-01 08:20:27 -03:00
// get_accel_health_all - return true if all accels are healthy
bool AP_InertialSensor::get_accel_health_all(void) const
{
for (uint8_t i=0; i<get_accel_count(); i++) {
if (!get_accel_health(i)) {
return false;
}
}
// return true if we have at least one accel
return (get_accel_count() > 0);
}
AP_InertialSensor: Factor accel cal in from GCS_MAVLink
2022-10-04 17:42:03 -03:00
#if HAL_GCS_ENABLED
AP_InertialSensor: added calibrate_trim() function used for redoing trim calculation
2015-05-15 18:22:25 -03:00
/*
calculate the trim_roll and trim_pitch. This is used for redoing the
trim without needing a full accel cal
*/
AP_InertialSensor: Factor accel cal in from GCS_MAVLink
2022-10-04 17:42:03 -03:00
MAV_RESULT AP_InertialSensor::calibrate_trim()
AP_InertialSensor: added calibrate_trim() function used for redoing trim calculation
2015-05-15 18:22:25 -03:00
{
// exit immediately if calibration is already in progress
AP_InertialSensor: added IMU temperature calibration support this adds parameters that can be setup by an external script for compensating for temperature variation in gyros and accels using a 3rd order polynomial
2021-01-07 20:46:35 -04:00
if (calibrating()) {
AP_InertialSensor: Factor accel cal in from GCS_MAVLink
2022-10-04 17:42:03 -03:00
return MAV_RESULT_TEMPORARILY_REJECTED;
}
// reject any time we've done a calibration recently
const uint32_t now = AP_HAL::millis();
if ((now - last_accel_cal_ms) < 5000) {
return MAV_RESULT_TEMPORARILY_REJECTED;
}
if (!calibrate_gyros()) {
return MAV_RESULT_FAILED;
AP_InertialSensor: added calibrate_trim() function used for redoing trim calculation
2015-05-15 18:22:25 -03:00
}
AP_InertialSensor: Factor accel cal in from GCS_MAVLink
2022-10-04 17:42:03 -03:00
AP_AHRS &ahrs = AP::ahrs();
Vector3f trim_rad = ahrs.get_trim();
AP_InertialSensor: make the backend fast gyro rate configurable raise gyro rate default on F7 and H7 clean up gyro rate docs and output startup banner
2020-05-21 15:29:28 -03:00
const uint8_t update_dt_milliseconds = (uint8_t)(1000.0f/get_loop_rate_hz()+0.5f);
AP_InertialSensor: Factor accel cal in from GCS_MAVLink
2022-10-04 17:42:03 -03:00
Vector3f level_sample;
uint32_t num_samples = 0;
_trimming_accel = true;
AP_InertialSensor: added calibrate_trim() function used for redoing trim calculation
2015-05-15 18:22:25 -03:00
// wait 100ms for ins filter to rise
for (uint8_t k=0; k<100/update_dt_milliseconds; k++) {
wait_for_sample();
update();
hal.scheduler->delay(update_dt_milliseconds);
}
while (num_samples < 400/update_dt_milliseconds) {
wait_for_sample();
// read samples from ins
update();
// capture sample
Vector3f samp;
samp = get_accel(0);
level_sample += samp;
if (!get_accel_health(0)) {
AP_InertialSensor: Factor accel cal in from GCS_MAVLink
2022-10-04 17:42:03 -03:00
goto failed;
AP_InertialSensor: added calibrate_trim() function used for redoing trim calculation
2015-05-15 18:22:25 -03:00
}
hal.scheduler->delay(update_dt_milliseconds);
num_samples++;
}
level_sample /= num_samples;
AP_InertialSensor: allow accelcal with a rotation
2021-08-12 19:57:43 -03:00
if (!_calculate_trim(level_sample, trim_rad)) {
AP_InertialSensor: Factor accel cal in from GCS_MAVLink
2022-10-04 17:42:03 -03:00
goto failed;
AP_InertialSensor: added calibrate_trim() function used for redoing trim calculation
2015-05-15 18:22:25 -03:00
}
AP_InertialSensor: Factor accel cal in from GCS_MAVLink
2022-10-04 17:42:03 -03:00
// reset ahrs's trim to suggested values from calibration routine
ahrs.set_trim(trim_rad);
last_accel_cal_ms = AP_HAL::millis();
_trimming_accel = false;
return MAV_RESULT_ACCEPTED;
failed:
last_accel_cal_ms = AP_HAL::millis();
_trimming_accel = false;
return MAV_RESULT_FAILED;
AP_InertialSensor: added calibrate_trim() function used for redoing trim calculation
2015-05-15 18:22:25 -03:00
}
AP_InertialSensor: Factor accel cal in from GCS_MAVLink
2022-10-04 17:42:03 -03:00
#endif // HAL_GCS_ENABLED
AP_InertialSensor: added calibrate_trim() function used for redoing trim calculation
2015-05-15 18:22:25 -03:00
InertialSensor: accel_calibrated_all_ok replaces calibrated This checks that the current number of accelerometers matches the number of calibrated accels in order to catch accel failures at boot
2015-05-12 03:18:25 -03:00
/*
check if the accelerometers are calibrated in 3D and that current number of accels matched number when calibrated
*/
bool AP_InertialSensor::accel_calibrated_ok_all() const
{
// check each accelerometer has offsets saved
for (uint8_t i=0; i<get_accel_count(); i++) {
AP_InertialSensor: save id for gyro and accel instances This allows each sensor to be uniquely identified in the system by using either the index inside the backend or for those that use the Device interface, to use the bus type, location, and device id. We leave 16-bit for each sensor to be able to change its own identification in future, which allows them to be changed in an incompatible manner forcing a re-calibration.
2016-09-03 21:51:37 -03:00
if (!_accel_id_ok[i]) {
return false;
}
InertialSensor: accel_calibrated_all_ok replaces calibrated This checks that the current number of accelerometers matches the number of calibrated accels in order to catch accel failures at boot
2015-05-12 03:18:25 -03:00
// exactly 0.0 offset is extremely unlikely
AP_InertialSensor: add support for extra Aux IMUs
2023-02-17 00:26:27 -04:00
if (_accel_offset(i).get().is_zero()) {
InertialSensor: accel_calibrated_all_ok replaces calibrated This checks that the current number of accelerometers matches the number of calibrated accels in order to catch accel failures at boot
2015-05-12 03:18:25 -03:00
return false;
}
// zero scaling also indicates not calibrated
AP_InertialSensor: add support for extra Aux IMUs
2023-02-17 00:26:27 -04:00
if (_accel_scale(i).get().is_zero()) {
InertialSensor: accel_calibrated_all_ok replaces calibrated This checks that the current number of accelerometers matches the number of calibrated accels in order to catch accel failures at boot
2015-05-12 03:18:25 -03:00
return false;
}
}
AP_InertialSensor: detect missing accels or gyros
2016-11-05 06:20:43 -03:00
for (uint8_t i=get_accel_count(); i<INS_MAX_INSTANCES; i++) {
AP_InertialSensor: add support for extra Aux IMUs
2023-02-17 00:26:27 -04:00
if (_accel_id(i) != 0) {
AP_InertialSensor: detect missing accels or gyros
2016-11-05 06:20:43 -03:00
// missing accel
return false;
}
}
InertialSensor: accel_calibrated_all_ok replaces calibrated This checks that the current number of accelerometers matches the number of calibrated accels in order to catch accel failures at boot
2015-05-12 03:18:25 -03:00
// check calibrated accels matches number of accels (no unused accels should have offsets or scaling)
if (get_accel_count() < INS_MAX_INSTANCES) {
for (uint8_t i=get_accel_count(); i<INS_MAX_INSTANCES; i++) {
AP_InertialSensor: add support for extra Aux IMUs
2023-02-17 00:26:27 -04:00
const Vector3f &scaling = _accel_scale(i).get();
InertialSensor: accel_calibrated_all_ok replaces calibrated This checks that the current number of accelerometers matches the number of calibrated accels in order to catch accel failures at boot
2015-05-12 03:18:25 -03:00
bool have_scaling = (!is_zero(scaling.x) && !is_equal(scaling.x,1.0f)) || (!is_zero(scaling.y) && !is_equal(scaling.y,1.0f)) || (!is_zero(scaling.z) && !is_equal(scaling.z,1.0f));
AP_InertialSensor: add support for extra Aux IMUs
2023-02-17 00:26:27 -04:00
bool have_offsets = !_accel_offset(i).get().is_zero();
InertialSensor: accel_calibrated_all_ok replaces calibrated This checks that the current number of accelerometers matches the number of calibrated accels in order to catch accel failures at boot
2015-05-12 03:18:25 -03:00
if (have_scaling || have_offsets) {
return false;
}
}
}
// if we got this far the accelerometers must have been calibrated
return true;
}
INS: add USE parameters
2015-08-01 03:40:40 -03:00
// return true if accel instance should be used (must be healthy and have it's use parameter set to 1)
bool AP_InertialSensor::use_accel(uint8_t instance) const
{
if (instance >= INS_MAX_INSTANCES) {
return false;
}
AP_InertialSensor: add support for extra Aux IMUs
2023-02-17 00:26:27 -04:00
return (get_accel_health(instance) && _use(instance));
INS: add USE parameters
2015-08-01 03:40:40 -03:00
}
InertialSensor: reorder .cpp file to match .h No functional changes
2014-09-01 03:28:07 -03:00
void
AP_InertialSensor::_init_gyro()
AP_InertialSensor: merge in calibration features from IMU library add gauss-newton method of accelerometer calibration
2012-11-05 00:27:03 -04:00
{
Global: rename min and max macros to uppercase The problem with using min() and max() is that they conflict with some C++ headers. Name the macros in uppercase instead. We may go case by case later converting them to be typesafe. Changes generated with: git ls-files '*.cpp' '*.h' -z | xargs -0 sed -i 's/\([^_[:alnum:]]\)max(/\1MAX(/g' git ls-files '*.cpp' '*.h' -z | xargs -0 sed -i 's/\([^_[:alnum:]]\)min(/\1MIN(/g'
2015-11-27 13:11:58 -04:00
uint8_t num_gyros = MIN(get_gyro_count(), INS_MAX_INSTANCES);
InertialSensor: reorder .cpp file to match .h No functional changes
2014-09-01 03:28:07 -03:00
Vector3f last_average[INS_MAX_INSTANCES], best_avg[INS_MAX_INSTANCES];
AP_InertialSensor: fixed gyro calibration bug we must not update _gyro_offset[] until we have completed calibration of that gyro, or we will end up using the new offsets when asking for the raw gyro vector
2015-04-03 10:57:30 -03:00
Vector3f new_gyro_offset[INS_MAX_INSTANCES];
InertialSensor: reorder .cpp file to match .h No functional changes
2014-09-01 03:28:07 -03:00
float best_diff[INS_MAX_INSTANCES];
bool converged[INS_MAX_INSTANCES];
AP_InertialSensor: added IMU temperature calibration support this adds parameters that can be setup by an external script for compensating for temperature variation in gyros and accels using a 3rd order polynomial
2021-01-07 20:46:35 -04:00
#if HAL_INS_TEMPERATURE_CAL_ENABLE
AP_InertialSensor: fixed build error
2021-01-13 19:31:47 -04:00
float start_temperature[INS_MAX_INSTANCES] {};
AP_InertialSensor: added IMU temperature calibration support this adds parameters that can be setup by an external script for compensating for temperature variation in gyros and accels using a 3rd order polynomial
2021-01-07 20:46:35 -04:00
#endif
AP_InertialSensor: generalise the accel/gyro calibration for N sensors
2013-12-08 18:50:12 -04:00
INS: protect against two calibrations running at the same time
2015-03-09 03:37:11 -03:00
// exit immediately if calibration is already in progress
AP_InertialSensor: added IMU temperature calibration support this adds parameters that can be setup by an external script for compensating for temperature variation in gyros and accels using a 3rd order polynomial
2021-01-07 20:46:35 -04:00
if (calibrating()) {
INS: protect against two calibrations running at the same time
2015-03-09 03:37:11 -03:00
return;
}
AP_InertialSensor: generalise the accel/gyro calibration for N sensors
2013-12-08 18:50:12 -04:00
INS: add calibrating method
2015-03-09 03:31:55 -03:00
// record we are calibrating
AP_InertialSensor: added IMU temperature calibration support this adds parameters that can be setup by an external script for compensating for temperature variation in gyros and accels using a 3rd order polynomial
2021-01-07 20:46:35 -04:00
_calibrating_gyro = true;
INS: add calibrating method
2015-03-09 03:31:55 -03:00
InertialSensor: reorder .cpp file to match .h No functional changes
2014-09-01 03:28:07 -03:00
// flash leds to tell user to keep the IMU still
AP_Notify::flags.initialising = true;
INS: protect against two calibrations running at the same time
2015-03-09 03:37:11 -03:00
// cold start
AP_InertialSensor: Console output can be disabled
2022-03-21 06:25:18 -03:00
DEV_PRINTF("Init Gyro");
INS: protect against two calibrations running at the same time
2015-03-09 03:37:11 -03:00
AP_InertialSensor: added INS_CALSENSFRAME parameter this allows us to detect if accel calibration was done in sensor frame or not. If it was done in sensor frame then the accel calibration is independent of AHRS_ORIENTATION, which makes it easier to move a board to a new airframe without having to recalibrate.
2015-03-10 04:05:41 -03:00
/*
we do the gyro calibration with no board rotation. This avoids
having to rotate readings during the calibration
*/
enum Rotation saved_orientation = _board_orientation;
_board_orientation = ROTATION_NONE;
InertialSensor: reorder .cpp file to match .h No functional changes
2014-09-01 03:28:07 -03:00
// remove existing gyro offsets
for (uint8_t k=0; k<num_gyros; k++) {
AP_InertialSensor: add support for extra Aux IMUs
2023-02-17 00:26:27 -04:00
_gyro_offset(k).set(Vector3f());
AP_InertialSensor: fixed gyro calibration bug we must not update _gyro_offset[] until we have completed calibration of that gyro, or we will end up using the new offsets when asking for the raw gyro vector
2015-04-03 10:57:30 -03:00
new_gyro_offset[k].zero();
AP_InertialSensor: fix best_diff on gyro initialization Without this patch, if accel_diff.length() > 0.2f and j == 0, then best_diff[k] would be zero forever since diff_norm[k] >= 0 for any j.
2016-06-17 17:44:48 -03:00
best_diff[k] = -1.f;
InertialSensor: reorder .cpp file to match .h No functional changes
2014-09-01 03:28:07 -03:00
last_average[k].zero();
converged[k] = false;
AP_InertialSensor: generalise the accel/gyro calibration for N sensors
2013-12-08 18:50:12 -04:00
}
AP_InertialSensor: merge in calibration features from IMU library add gauss-newton method of accelerometer calibration
2012-11-05 00:27:03 -04:00
InertialSensor: reorder .cpp file to match .h No functional changes
2014-09-01 03:28:07 -03:00
for(int8_t c = 0; c < 5; c++) {
hal.scheduler->delay(5);
update();
}
AP_InertialSensor: merge in calibration features from IMU library add gauss-newton method of accelerometer calibration
2012-11-05 00:27:03 -04:00
AP_InertialSensor: added IMU temperature calibration support this adds parameters that can be setup by an external script for compensating for temperature variation in gyros and accels using a 3rd order polynomial
2021-01-07 20:46:35 -04:00
#if HAL_INS_TEMPERATURE_CAL_ENABLE
// get start temperature. gyro cal usually happens when the board
// has just been powered on, so the temperature may be changing
// rapidly. We use the average between start and end temperature
// as the calibration temperature to minimise errors
for (uint8_t k=0; k<num_gyros; k++) {
start_temperature[k] = get_temperature(k);
}
#endif
InertialSensor: reorder .cpp file to match .h No functional changes
2014-09-01 03:28:07 -03:00
// the strategy is to average 50 points over 0.5 seconds, then do it
// again and see if the 2nd average is within a small margin of
// the first
AP_InertialSensor: merge in calibration features from IMU library add gauss-newton method of accelerometer calibration
2012-11-05 00:27:03 -04:00
InertialSensor: reorder .cpp file to match .h No functional changes
2014-09-01 03:28:07 -03:00
uint8_t num_converged = 0;
AP_InertialSensor: merge in calibration features from IMU library add gauss-newton method of accelerometer calibration
2012-11-05 00:27:03 -04:00
AP_InertialSensor: allow gyro calibration to take up to 30 seconds this gives time for the user time to get their battery hatches on and hold the model steady. 5 seconds was just far too short
2014-11-21 22:25:23 -04:00
// we try to get a good calibration estimate for up to 30 seconds
InertialSensor: reorder .cpp file to match .h No functional changes
2014-09-01 03:28:07 -03:00
// if the gyros are stable, we should get it in 1 second
AP_InertialSensor: allow gyro calibration to take up to 30 seconds this gives time for the user time to get their battery hatches on and hold the model steady. 5 seconds was just far too short
2014-11-21 22:25:23 -04:00
for (int16_t j = 0; j <= 30*4 && num_converged < num_gyros; j++) {
InertialSensor: reorder .cpp file to match .h No functional changes
2014-09-01 03:28:07 -03:00
Vector3f gyro_sum[INS_MAX_INSTANCES], gyro_avg[INS_MAX_INSTANCES], gyro_diff[INS_MAX_INSTANCES];
AP_InertialSensor: use accelerometers to prevent bad gyro cal if the board is rotating at a steady rate we can end up with a bad gyro calibration. This can happen on a steadily moving platform such as a ship. This uses the accelerometers to detect the steady movement and not accept the gyro calibration
2015-03-07 16:48:16 -04:00
Vector3f accel_start;
InertialSensor: reorder .cpp file to match .h No functional changes
2014-09-01 03:28:07 -03:00
float diff_norm[INS_MAX_INSTANCES];
uint8_t i;
AP_InertialSensor: merge in calibration features from IMU library add gauss-newton method of accelerometer calibration
2012-11-05 00:27:03 -04:00
AP_InertialSensor: fixed watchdog on AHRS trim gyro wait
2019-08-17 18:05:12 -03:00
EXPECT_DELAY_MS(1000);
InertialSensor: reorder .cpp file to match .h No functional changes
2014-09-01 03:28:07 -03:00
memset(diff_norm, 0, sizeof(diff_norm));
AP_InertialSensor: Console output can be disabled
2022-03-21 06:25:18 -03:00
DEV_PRINTF("*");
InertialSensor: reorder .cpp file to match .h No functional changes
2014-09-01 03:28:07 -03:00
for (uint8_t k=0; k<num_gyros; k++) {
gyro_sum[k].zero();
AP_InertialSensor: generalise the accel/gyro calibration for N sensors
2013-12-08 18:50:12 -04:00
}
AP_InertialSensor: use accelerometers to prevent bad gyro cal if the board is rotating at a steady rate we can end up with a bad gyro calibration. This can happen on a steadily moving platform such as a ship. This uses the accelerometers to detect the steady movement and not accept the gyro calibration
2015-03-07 16:48:16 -04:00
accel_start = get_accel(0);
InertialSensor: reorder .cpp file to match .h No functional changes
2014-09-01 03:28:07 -03:00
for (i=0; i<50; i++) {
AP_InertialSensor: added wait_for_sample() API call this waits for a new INS sample to arrive, using whatever method is most efficient on each INS type
2013-10-08 03:28:39 -03:00
update();
InertialSensor: reorder .cpp file to match .h No functional changes
2014-09-01 03:28:07 -03:00
for (uint8_t k=0; k<num_gyros; k++) {
gyro_sum[k] += get_gyro(k);
AP_InertialSensor: generalise the accel/gyro calibration for N sensors
2013-12-08 18:50:12 -04:00
}
InertialSensor: reorder .cpp file to match .h No functional changes
2014-09-01 03:28:07 -03:00
hal.scheduler->delay(5);
AP_InertialSensor: merge in calibration features from IMU library add gauss-newton method of accelerometer calibration
2012-11-05 00:27:03 -04:00
}
AP_InertialSensor: use accelerometers to prevent bad gyro cal if the board is rotating at a steady rate we can end up with a bad gyro calibration. This can happen on a steadily moving platform such as a ship. This uses the accelerometers to detect the steady movement and not accept the gyro calibration
2015-03-07 16:48:16 -04:00
Vector3f accel_diff = get_accel(0) - accel_start;
if (accel_diff.length() > 0.2f) {
// the accelerometers changed during the gyro sum. Skip
// this sample. This copes with doing gyro cal on a
AP_InertialSensor: updated comment on accel check in gyro cal
2015-03-08 18:25:08 -03:00
// steadily moving platform. The value 0.2 corresponds
// with around 5 degrees/second of rotation.
AP_InertialSensor: use accelerometers to prevent bad gyro cal if the board is rotating at a steady rate we can end up with a bad gyro calibration. This can happen on a steadily moving platform such as a ship. This uses the accelerometers to detect the steady movement and not accept the gyro calibration
2015-03-07 16:48:16 -04:00
continue;
}
InertialSensor: reorder .cpp file to match .h No functional changes
2014-09-01 03:28:07 -03:00
for (uint8_t k=0; k<num_gyros; k++) {
gyro_avg[k] = gyro_sum[k] / i;
gyro_diff[k] = last_average[k] - gyro_avg[k];
diff_norm[k] = gyro_diff[k].length();
AP_InertialSensor: generalise the accel/gyro calibration for N sensors
2013-12-08 18:50:12 -04:00
}
AP_InertialSensor: changes after review with Tridge. sanity checking added to accelerometer calibration routine. user feedback is sent using gcs_send_text_fmt instead of Serial.printf. moved ins parameters to new eeprom number to avoid conflicts with older parameters. other small changes including renaming of functions and parameters.
2012-11-07 02:20:22 -04:00
InertialSensor: reorder .cpp file to match .h No functional changes
2014-09-01 03:28:07 -03:00
for (uint8_t k=0; k<num_gyros; k++) {
AP_InertialSensor: fix best_diff on gyro initialization Without this patch, if accel_diff.length() > 0.2f and j == 0, then best_diff[k] would be zero forever since diff_norm[k] >= 0 for any j.
2016-06-17 17:44:48 -03:00
if (best_diff[k] < 0) {
InertialSensor: reorder .cpp file to match .h No functional changes
2014-09-01 03:28:07 -03:00
best_diff[k] = diff_norm[k];
best_avg[k] = gyro_avg[k];
AP_InertialSensor: inform maximum gyro average difference While at it, define GYRO_INIT_MAX_DIFF_DPS.
2016-06-17 18:00:29 -03:00
} else if (gyro_diff[k].length() < ToRad(GYRO_INIT_MAX_DIFF_DPS)) {
InertialSensor: reorder .cpp file to match .h No functional changes
2014-09-01 03:28:07 -03:00
// we want the average to be within 0.1 bit, which is 0.04 degrees/s
last_average[k] = (gyro_avg[k] * 0.5f) + (last_average[k] * 0.5f);
AP_InertialSensor: fixed gyro calibration bug we must not update _gyro_offset[] until we have completed calibration of that gyro, or we will end up using the new offsets when asking for the raw gyro vector
2015-04-03 10:57:30 -03:00
if (!converged[k] || last_average[k].length() < new_gyro_offset[k].length()) {
new_gyro_offset[k] = last_average[k];
AP_InertialSensor: continue finding best gyro after convergence with multiple gyros if we are still calibrating one of the gyros then keep looking for better values for the already converged gyros.
2015-03-07 16:49:38 -04:00
}
if (!converged[k]) {
converged[k] = true;
num_converged++;
}
InertialSensor: reorder .cpp file to match .h No functional changes
2014-09-01 03:28:07 -03:00
} else if (diff_norm[k] < best_diff[k]) {
best_diff[k] = diff_norm[k];
best_avg[k] = (gyro_avg[k] * 0.5f) + (last_average[k] * 0.5f);
}
last_average[k] = gyro_avg[k];
AP_InertialSensor: generalise the accel/gyro calibration for N sensors
2013-12-08 18:50:12 -04:00
}
INS: make calibrate_accel() return a bool this will allow APM to auto set MANUAL_LEVEL to 1
2012-11-20 03:26:51 -04:00
}
AP_InertialSensor: changes after review with Tridge. sanity checking added to accelerometer calibration routine. user feedback is sent using gcs_send_text_fmt instead of Serial.printf. moved ins parameters to new eeprom number to avoid conflicts with older parameters. other small changes including renaming of functions and parameters.
2012-11-07 02:20:22 -04:00
InertialSensor: reorder .cpp file to match .h No functional changes
2014-09-01 03:28:07 -03:00
// we've kept the user waiting long enough - use the best pair we
// found so far
AP_InertialSensor: Console output can be disabled
2022-03-21 06:25:18 -03:00
DEV_PRINTF("\n");
InertialSensor: reorder .cpp file to match .h No functional changes
2014-09-01 03:28:07 -03:00
for (uint8_t k=0; k<num_gyros; k++) {
if (!converged[k]) {
AP_InertialSensor: Console output can be disabled
2022-03-21 06:25:18 -03:00
DEV_PRINTF("gyro[%u] did not converge: diff=%f dps (expected < %f)\n",
AP_InertialSensor: inform maximum gyro average difference While at it, define GYRO_INIT_MAX_DIFF_DPS.
2016-06-17 18:00:29 -03:00
(unsigned)k,
(double)ToDeg(best_diff[k]),
(double)GYRO_INIT_MAX_DIFF_DPS);
AP_InertialSensor: add support for extra Aux IMUs
2023-02-17 00:26:27 -04:00
_gyro_offset(k).set(best_avg[k]);
INS: add gyro_calibrated_ok_all method This returns true if the gyros have been calibrated successfully
2014-10-08 08:17:31 -03:00
// flag calibration as failed for this gyro
_gyro_cal_ok[k] = false;
INS: set gyro_cal_ok only after completing calibration
2015-03-09 01:21:43 -03:00
} else {
_gyro_cal_ok[k] = true;
AP_InertialSensor: add support for extra Aux IMUs
2023-02-17 00:26:27 -04:00
_gyro_offset(k).set(new_gyro_offset[k]);
AP_InertialSensor: added IMU temperature calibration support this adds parameters that can be setup by an external script for compensating for temperature variation in gyros and accels using a 3rd order polynomial
2021-01-07 20:46:35 -04:00
#if HAL_INS_TEMPERATURE_CAL_ENABLE
AP_InertialSensor: add support for extra Aux IMUs
2023-02-17 00:26:27 -04:00
caltemp_gyro(k).set(0.5 * (get_temperature(k) + start_temperature[k]));
AP_InertialSensor: added IMU temperature calibration support this adds parameters that can be setup by an external script for compensating for temperature variation in gyros and accels using a 3rd order polynomial
2021-01-07 20:46:35 -04:00
#endif
INS: calibrated method checks all accelerometers
2014-07-13 10:05:21 -03:00
}
}
INS: add calibrating method
2015-03-09 03:31:55 -03:00
AP_InertialSensor: added INS_CALSENSFRAME parameter this allows us to detect if accel calibration was done in sensor frame or not. If it was done in sensor frame then the accel calibration is independent of AHRS_ORIENTATION, which makes it easier to move a board to a new airframe without having to recalibrate.
2015-03-10 04:05:41 -03:00
// restore orientation
_board_orientation = saved_orientation;
INS: add calibrating method
2015-03-09 03:31:55 -03:00
// record calibration complete
AP_InertialSensor: added IMU temperature calibration support this adds parameters that can be setup by an external script for compensating for temperature variation in gyros and accels using a 3rd order polynomial
2021-01-07 20:46:35 -04:00
_calibrating_gyro = false;
INS: add calibrating method
2015-03-09 03:31:55 -03:00
// stop flashing leds
AP_Notify::flags.initialising = false;
AP_InertialSensor: add buzzer noises
2023-04-03 09:42:53 -03:00
AP_Notify::flags.gyro_calibrated = true;
INS: add calibrated check Checks is the scaling vector has been updated. Required for pre-arm checks for ArduCopter
2013-04-22 11:55:53 -03:00
}
InertialSensor: reorder .cpp file to match .h No functional changes
2014-09-01 03:28:07 -03:00
// save parameters to eeprom
AP_InertialSensor: save only gyro calibration When we are initializing the gyro and then saving the calibration we are also saving the calibration values for the accelerometers. Right now this is non-problematic, but we want to check that the ID of the accelerometer corresponds to the ID of the sensor detected. If we also save accel calibrations we would actually override the ID of the accelerometer. Rename the method to _save_gyro_calibration() and save only on gyro values.
2016-09-03 21:18:09 -03:00
void AP_InertialSensor::_save_gyro_calibration()
InertialSensor: reorder .cpp file to match .h No functional changes
2014-09-01 03:28:07 -03:00
{
AP_InertialSensor: detect missing accels or gyros
2016-11-05 06:20:43 -03:00
for (uint8_t i=0; i<_gyro_count; i++) {
AP_InertialSensor: add support for extra Aux IMUs
2023-02-17 00:26:27 -04:00
_gyro_offset(i).save();
_gyro_id(i).save();
AP_InertialSensor: added IMU temperature calibration support this adds parameters that can be setup by an external script for compensating for temperature variation in gyros and accels using a 3rd order polynomial
2021-01-07 20:46:35 -04:00
#if HAL_INS_TEMPERATURE_CAL_ENABLE
AP_InertialSensor: add support for extra Aux IMUs
2023-02-17 00:26:27 -04:00
caltemp_gyro(i).save();
AP_InertialSensor: added IMU temperature calibration support this adds parameters that can be setup by an external script for compensating for temperature variation in gyros and accels using a 3rd order polynomial
2021-01-07 20:46:35 -04:00
#endif
InertialSensor: reorder .cpp file to match .h No functional changes
2014-09-01 03:28:07 -03:00
}
AP_InertialSensor: detect missing accels or gyros
2016-11-05 06:20:43 -03:00
for (uint8_t i=_gyro_count; i<INS_MAX_INSTANCES; i++) {
AP_InertialSensor: add support for extra Aux IMUs
2023-02-17 00:26:27 -04:00
_gyro_offset(i).set_and_save(Vector3f());
_gyro_id(i).set_and_save(0);
AP_InertialSensor: added IMU temperature calibration support this adds parameters that can be setup by an external script for compensating for temperature variation in gyros and accels using a 3rd order polynomial
2021-01-07 20:46:35 -04:00
#if HAL_INS_TEMPERATURE_CAL_ENABLE
AP_InertialSensor: add support for extra Aux IMUs
2023-02-17 00:26:27 -04:00
caltemp_gyro(i).set_and_save_ifchanged(-300);
AP_InertialSensor: added IMU temperature calibration support this adds parameters that can be setup by an external script for compensating for temperature variation in gyros and accels using a 3rd order polynomial
2021-01-07 20:46:35 -04:00
#endif
AP_InertialSensor: detect missing accels or gyros
2016-11-05 06:20:43 -03:00
}
InertialSensor: reorder .cpp file to match .h No functional changes
2014-09-01 03:28:07 -03:00
}
AP_InertialSensor: first steps in frontend/backend split This converts the MPU6000 driver to a frontend/backend structure, and disables all other drivers. They will be progressively re-enabled as each is converted
2014-10-14 01:48:33 -03:00
AP_InertialSensor: switch to HarmonicNotch class this makes the logic much easier to follow, without indexes into arrays
2022-04-15 04:38:56 -03:00
/*
update harmonic notch parameters
*/
void AP_InertialSensor::HarmonicNotch::update_params(uint8_t instance, bool converging, float gyro_rate)
{
const float center_freq = calculated_notch_freq_hz[0];
AP_InertialSensor: fixed the last notch values to be per-instance thanks to Andy for noticing this
2022-04-17 04:03:46 -03:00
if (!is_equal(last_bandwidth_hz[instance], params.bandwidth_hz()) ||
!is_equal(last_attenuation_dB[instance], params.attenuation_dB()) ||
AP_InertialSensor: make sure dynamic notches always get updates so that slew limiting is not too aggressive
2022-09-20 04:08:26 -03:00
(params.tracking_mode() == HarmonicNotchDynamicMode::Fixed && !is_equal(last_center_freq_hz[instance], center_freq)) ||
AP_InertialSensor: switch to HarmonicNotch class this makes the logic much easier to follow, without indexes into arrays
2022-04-15 04:38:56 -03:00
converging) {
filter[instance].init(gyro_rate,
center_freq,
params.bandwidth_hz(),
params.attenuation_dB());
AP_InertialSensor: fixed the last notch values to be per-instance thanks to Andy for noticing this
2022-04-17 04:03:46 -03:00
last_center_freq_hz[instance] = center_freq;
last_bandwidth_hz[instance] = params.bandwidth_hz();
last_attenuation_dB[instance] = params.attenuation_dB();
AP_InertialSensor: make sure dynamic notches always get updates so that slew limiting is not too aggressive
2022-09-20 04:08:26 -03:00
} else if (params.tracking_mode() != HarmonicNotchDynamicMode::Fixed) {
AP_InertialSensor: switch to HarmonicNotch class this makes the logic much easier to follow, without indexes into arrays
2022-04-15 04:38:56 -03:00
if (num_calculated_notch_frequencies > 1) {
filter[instance].update(num_calculated_notch_frequencies, calculated_notch_freq_hz);
} else {
filter[instance].update(center_freq);
}
AP_InertialSensor: fixed the last notch values to be per-instance thanks to Andy for noticing this
2022-04-17 04:03:46 -03:00
last_center_freq_hz[instance] = center_freq;
AP_InertialSensor: switch to HarmonicNotch class this makes the logic much easier to follow, without indexes into arrays
2022-04-15 04:38:56 -03:00
}
}
AP_InertialSensor: first steps in frontend/backend split This converts the MPU6000 driver to a frontend/backend structure, and disables all other drivers. They will be progressively re-enabled as each is converted
2014-10-14 01:48:33 -03:00
/*
update gyro and accel values from backends
*/
void AP_InertialSensor::update(void)
{
AP_InertialSensor: improved timing in all drivers
2014-10-15 20:32:40 -03:00
// during initialisation update() may be called without
// wait_for_sample(), and a wait is implied
wait_for_sample();
AP_InertialSensor: prefer sensors that have zero error counts if a sensor has failed in flight then try not to use it if another error-free sensor is available
2014-12-29 06:19:35 -04:00
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
AP_InertialSensor: implement gyro and accel health monitoring sensor is healthy if it gave a sample on the last update()
2014-10-15 23:27:22 -03:00
// mark sensors unhealthy and let update() in each backend
AP_InertialSensor: rename _rotate_and_offset to _publish
2015-02-17 02:50:59 -04:00
// mark them healthy via _publish_gyro() and
// _publish_accel()
AP_InertialSensor: implement gyro and accel health monitoring sensor is healthy if it gave a sample on the last update()
2014-10-15 23:27:22 -03:00
_gyro_healthy[i] = false;
_accel_healthy[i] = false;
AP_InertialSensor: allow backends to publish delta velocities and angles
2015-02-17 02:54:17 -04:00
_delta_velocity_valid[i] = false;
_delta_angle_valid[i] = false;
AP_InertialSensor: implement gyro and accel health monitoring sensor is healthy if it gave a sample on the last update()
2014-10-15 23:27:22 -03:00
}
AP_InertialSensor: make it easier to add multiple backends use _add_backend() to add new backends on startup
2014-10-16 17:27:01 -03:00
for (uint8_t i=0; i<_backend_count; i++) {
_backends[i]->update();
AP_InertialSensor: implement gyro and accel health monitoring sensor is healthy if it gave a sample on the last update()
2014-10-15 23:27:22 -03:00
}
AP_InertialSensor: prefer sensors that have zero error counts if a sensor has failed in flight then try not to use it if another error-free sensor is available
2014-12-29 06:19:35 -04:00
AP_InertialSensor: work around gyro and accel errors on startup
2015-04-30 06:15:36 -03:00
if (!_startup_error_counts_set) {
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
_accel_startup_error_count[i] = _accel_error_count[i];
_gyro_startup_error_count[i] = _gyro_error_count[i];
}
if (_startup_ms == 0) {
_startup_ms = AP_HAL::millis();
} else if (AP_HAL::millis()-_startup_ms > 2000) {
_startup_error_counts_set = true;
}
}
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
if (_accel_error_count[i] < _accel_startup_error_count[i]) {
_accel_startup_error_count[i] = _accel_error_count[i];
}
if (_gyro_error_count[i] < _gyro_startup_error_count[i]) {
_gyro_startup_error_count[i] = _gyro_error_count[i];
}
}
AP_InertialSensor: prefer sensors that have zero error counts if a sensor has failed in flight then try not to use it if another error-free sensor is available
2014-12-29 06:19:35 -04:00
// adjust health status if a sensor has a non-zero error count
AP_InertialSensor: sanitize includes Due to the way the headers are organized changing a single change in an inertial sensor driver would trigger a rebuild for most of the files in the project. Time could be saved by using ccache (since most of the things didn't change) but we can do better, i.e. re-organize the headers so we don't have to re-build everything. With this patch only AP_InertialSensor/AP_InertialSensor.h is exposed to most users. There are some corner cases to integrate with some example code, but most of the places now depend only on this header and this header doesn't depend on the specific backends. Now changing a single header, e.g. AP_InertialSensor_L3G4200D.h triggers a rebuild only of these files: $ waf copter 'copter' finished successfully (0.000s) Waf: Entering directory `/home/lucas/p/dronecode/ardupilot/build/minlure' [ 80/370] Compiling libraries/AP_InertialSensor/AP_InertialSensor.cpp [ 84/370] Compiling libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp [310/370] Linking build/minlure/ArduCopter/libArduCopter_libs.a [370/370] Linking build/minlure/bin/arducopter Waf: Leaving directory `/home/lucas/p/dronecode/ardupilot/build/minlure'
2016-01-19 21:26:31 -04:00
// but another sensor doesn't.
AP_InertialSensor: prefer sensors that have zero error counts if a sensor has failed in flight then try not to use it if another error-free sensor is available
2014-12-29 06:19:35 -04:00
bool have_zero_accel_error_count = false;
bool have_zero_gyro_error_count = false;
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
AP_InertialSensor: work around gyro and accel errors on startup
2015-04-30 06:15:36 -03:00
if (_accel_healthy[i] && _accel_error_count[i] <= _accel_startup_error_count[i]) {
AP_InertialSensor: prefer sensors that have zero error counts if a sensor has failed in flight then try not to use it if another error-free sensor is available
2014-12-29 06:19:35 -04:00
have_zero_accel_error_count = true;
}
AP_InertialSensor: work around gyro and accel errors on startup
2015-04-30 06:15:36 -03:00
if (_gyro_healthy[i] && _gyro_error_count[i] <= _gyro_startup_error_count[i]) {
AP_InertialSensor: prefer sensors that have zero error counts if a sensor has failed in flight then try not to use it if another error-free sensor is available
2014-12-29 06:19:35 -04:00
have_zero_gyro_error_count = true;
}
}
AP_InertialSensor: fixed error detection on secondary IMUs the break; was preventing error detection on lsm303d/l3gd20
2014-12-31 17:55:34 -04:00
AP_InertialSensor: prefer sensors that have zero error counts if a sensor has failed in flight then try not to use it if another error-free sensor is available
2014-12-29 06:19:35 -04:00
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
AP_InertialSensor: work around gyro and accel errors on startup
2015-04-30 06:15:36 -03:00
if (_gyro_healthy[i] && _gyro_error_count[i] > _gyro_startup_error_count[i] && have_zero_gyro_error_count) {
AP_InertialSensor: prefer sensors that have zero error counts if a sensor has failed in flight then try not to use it if another error-free sensor is available
2014-12-29 06:19:35 -04:00
// we prefer not to use a gyro that has had errors
_gyro_healthy[i] = false;
}
AP_InertialSensor: work around gyro and accel errors on startup
2015-04-30 06:15:36 -03:00
if (_accel_healthy[i] && _accel_error_count[i] > _accel_startup_error_count[i] && have_zero_accel_error_count) {
AP_InertialSensor: fixed error detection on secondary IMUs the break; was preventing error detection on lsm303d/l3gd20
2014-12-31 17:55:34 -04:00
// we prefer not to use a accel that has had errors
_accel_healthy[i] = false;
}
}
// set primary to first healthy accel and gyro
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
AP_InertialSensor: add support for extra Aux IMUs
2023-02-17 00:26:27 -04:00
if (_gyro_healthy[i] && _use(i)) {
AP_InertialSensor: implement gyro and accel health monitoring sensor is healthy if it gave a sample on the last update()
2014-10-15 23:27:22 -03:00
_primary_gyro = i;
break;
}
}
AP_InertialSensor: prefer sensors that have zero error counts if a sensor has failed in flight then try not to use it if another error-free sensor is available
2014-12-29 06:19:35 -04:00
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
AP_InertialSensor: add support for extra Aux IMUs
2023-02-17 00:26:27 -04:00
if (_accel_healthy[i] && _use(i)) {
AP_InertialSensor: implement gyro and accel health monitoring sensor is healthy if it gave a sample on the last update()
2014-10-15 23:27:22 -03:00
_primary_accel = i;
break;
}
}
AP_InertialSensor: improved timing in all drivers
2014-10-15 20:32:40 -03:00
AP_InertialSensor: added get_last_update_usec() this will be used by the inter-EKF scheduling changes
2017-04-28 21:26:58 -03:00
_last_update_usec = AP_HAL::micros();
AP_InertialSensor: improved timing in all drivers
2014-10-15 20:32:40 -03:00
_have_sample = false;
AP_InertialSensor: start auto-learning at boot This makes it easier to setup the params. The ENABLE=2 value only takes effect at boot
2021-01-15 21:23:17 -04:00
#if HAL_INS_TEMPERATURE_CAL_ENABLE
if (tcal_learning && !temperature_cal_running()) {
AP_Notify::flags.temp_cal_running = false;
AP_Notify::events.temp_cal_saved = 1;
AP_InertialSensor: use a fixed reference temperature of 35C this allows us to timeout the calibration when the temperature stops rising as the polynomial no longer depends on the maximum temperature
2021-01-16 23:27:51 -04:00
tcal_learning = false;
AP_InertialSensor: start auto-learning at boot This makes it easier to setup the params. The ENABLE=2 value only takes effect at boot
2021-01-15 21:23:17 -04:00
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "TCAL finished all IMUs");
}
#endif
AP_InertialSensor: added support for writing raw IMU data to a UART this is for supporting external visual odomotry systems which need the IMU data to correctly process image data # Conflicts: # libraries/AP_InertialSensor/AP_InertialSensor.cpp
2023-11-27 23:20:19 -04:00
#if AP_SERIALMANAGER_IMUOUT_ENABLED
if (uart.imu_out_uart) {
send_uart_data();
}
#endif
AP_InertialSensor: first steps in frontend/backend split This converts the MPU6000 driver to a frontend/backend structure, and disables all other drivers. They will be progressively re-enabled as each is converted
2014-10-14 01:48:33 -03:00
}
/*
AP_InertialSensor: improved timing in all drivers
2014-10-15 20:32:40 -03:00
wait for a sample to be available. This is the function that
AP_InertialSensor: sanitize includes Due to the way the headers are organized changing a single change in an inertial sensor driver would trigger a rebuild for most of the files in the project. Time could be saved by using ccache (since most of the things didn't change) but we can do better, i.e. re-organize the headers so we don't have to re-build everything. With this patch only AP_InertialSensor/AP_InertialSensor.h is exposed to most users. There are some corner cases to integrate with some example code, but most of the places now depend only on this header and this header doesn't depend on the specific backends. Now changing a single header, e.g. AP_InertialSensor_L3G4200D.h triggers a rebuild only of these files: $ waf copter 'copter' finished successfully (0.000s) Waf: Entering directory `/home/lucas/p/dronecode/ardupilot/build/minlure' [ 80/370] Compiling libraries/AP_InertialSensor/AP_InertialSensor.cpp [ 84/370] Compiling libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp [310/370] Linking build/minlure/ArduCopter/libArduCopter_libs.a [370/370] Linking build/minlure/bin/arducopter Waf: Leaving directory `/home/lucas/p/dronecode/ardupilot/build/minlure'
2016-01-19 21:26:31 -04:00
determines the timing of the main loop in ardupilot.
AP_InertialSensor: improved timing in all drivers
2014-10-15 20:32:40 -03:00
Ideally this function would return at exactly the rate given by the
AP_InertialSensor: sanitize includes Due to the way the headers are organized changing a single change in an inertial sensor driver would trigger a rebuild for most of the files in the project. Time could be saved by using ccache (since most of the things didn't change) but we can do better, i.e. re-organize the headers so we don't have to re-build everything. With this patch only AP_InertialSensor/AP_InertialSensor.h is exposed to most users. There are some corner cases to integrate with some example code, but most of the places now depend only on this header and this header doesn't depend on the specific backends. Now changing a single header, e.g. AP_InertialSensor_L3G4200D.h triggers a rebuild only of these files: $ waf copter 'copter' finished successfully (0.000s) Waf: Entering directory `/home/lucas/p/dronecode/ardupilot/build/minlure' [ 80/370] Compiling libraries/AP_InertialSensor/AP_InertialSensor.cpp [ 84/370] Compiling libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp [310/370] Linking build/minlure/ArduCopter/libArduCopter_libs.a [370/370] Linking build/minlure/bin/arducopter Waf: Leaving directory `/home/lucas/p/dronecode/ardupilot/build/minlure'
2016-01-19 21:26:31 -04:00
sample_rate argument given to AP_InertialSensor::init().
AP_InertialSensor: improved timing in all drivers
2014-10-15 20:32:40 -03:00
The key output of this function is _delta_time, which is the time
over which the gyro and accel integration will happen for this
sample. We want that to be a constant time if possible, but if
delays occur we need to cope with them. The long term sum of
_delta_time should be exactly equal to the wall clock elapsed time
AP_InertialSensor: first steps in frontend/backend split This converts the MPU6000 driver to a frontend/backend structure, and disables all other drivers. They will be progressively re-enabled as each is converted
2014-10-14 01:48:33 -03:00
*/
void AP_InertialSensor::wait_for_sample(void)
{
AP_InertialSensor: improved timing in all drivers
2014-10-15 20:32:40 -03:00
if (_have_sample) {
// the user has called wait_for_sample() again without
// consuming the sample with update()
return;
}
AP_InertialSensor: converted PX4 driver to new API
2014-10-15 05:54:30 -03:00
AP_InertialSensor: use millis/micros/panic functions
2015-11-19 23:11:52 -04:00
uint32_t now = AP_HAL::micros();
AP_InertialSensor: improved timing in all drivers
2014-10-15 20:32:40 -03:00
AP_InertialSensor: simplify next sample time calculations this makes the calculation much clearer
2014-10-19 20:46:02 -03:00
if (_next_sample_usec == 0 && _delta_time <= 0) {
AP_InertialSensor: improved timing in all drivers
2014-10-15 20:32:40 -03:00
// this is the first call to wait_for_sample()
AP_InertialSensor: simplify next sample time calculations this makes the calculation much clearer
2014-10-19 20:46:02 -03:00
_last_sample_usec = now - _sample_period_usec;
_next_sample_usec = now + _sample_period_usec;
goto check_sample;
AP_InertialSensor: improved timing in all drivers
2014-10-15 20:32:40 -03:00
}
AP_InertialSensor: simplify next sample time calculations this makes the calculation much clearer
2014-10-19 20:46:02 -03:00
// see how long it is till the next sample is due
if (_next_sample_usec - now <=_sample_period_usec) {
// we're ahead on time, schedule next sample at expected period
uint32_t wait_usec = _next_sample_usec - now;
AP_InertialSensor: use delay_microseconds_boost() this gives much more consistent timing for PX4
2015-02-15 20:54:01 -04:00
hal.scheduler->delay_microseconds_boost(wait_usec);
AP_InertialSensor: use millis/micros/panic functions
2015-11-19 23:11:52 -04:00
uint32_t now2 = AP_HAL::micros();
AP_InertialSensor: added TIMING_DEBUG code for finding loop timing issues
2015-02-15 19:12:10 -04:00
if (now2+100 < _next_sample_usec) {
timing_printf("shortsleep %u\n", (unsigned)(_next_sample_usec-now2));
}
if (now2 > _next_sample_usec+400) {
AP_InertialSensor: sanitize includes Due to the way the headers are organized changing a single change in an inertial sensor driver would trigger a rebuild for most of the files in the project. Time could be saved by using ccache (since most of the things didn't change) but we can do better, i.e. re-organize the headers so we don't have to re-build everything. With this patch only AP_InertialSensor/AP_InertialSensor.h is exposed to most users. There are some corner cases to integrate with some example code, but most of the places now depend only on this header and this header doesn't depend on the specific backends. Now changing a single header, e.g. AP_InertialSensor_L3G4200D.h triggers a rebuild only of these files: $ waf copter 'copter' finished successfully (0.000s) Waf: Entering directory `/home/lucas/p/dronecode/ardupilot/build/minlure' [ 80/370] Compiling libraries/AP_InertialSensor/AP_InertialSensor.cpp [ 84/370] Compiling libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp [310/370] Linking build/minlure/ArduCopter/libArduCopter_libs.a [370/370] Linking build/minlure/bin/arducopter Waf: Leaving directory `/home/lucas/p/dronecode/ardupilot/build/minlure'
2016-01-19 21:26:31 -04:00
timing_printf("longsleep %u wait_usec=%u\n",
AP_InertialSensor: added TIMING_DEBUG code for finding loop timing issues
2015-02-15 19:12:10 -04:00
(unsigned)(now2-_next_sample_usec),
(unsigned)wait_usec);
}
AP_InertialSensor: simplify next sample time calculations this makes the calculation much clearer
2014-10-19 20:46:02 -03:00
_next_sample_usec += _sample_period_usec;
} else if (now - _next_sample_usec < _sample_period_usec/8) {
// we've overshot, but only by a small amount, keep on
// schedule with no delay
AP_InertialSensor: added TIMING_DEBUG code for finding loop timing issues
2015-02-15 19:12:10 -04:00
timing_printf("overshoot1 %u\n", (unsigned)(now-_next_sample_usec));
AP_InertialSensor: simplify next sample time calculations this makes the calculation much clearer
2014-10-19 20:46:02 -03:00
_next_sample_usec += _sample_period_usec;
AP_InertialSensor: improved timing in all drivers
2014-10-15 20:32:40 -03:00
} else {
AP_InertialSensor: simplify next sample time calculations this makes the calculation much clearer
2014-10-19 20:46:02 -03:00
// we've overshot by a larger amount, re-zero scheduling with
// no delay
AP_InertialSensor: added TIMING_DEBUG code for finding loop timing issues
2015-02-15 19:12:10 -04:00
timing_printf("overshoot2 %u\n", (unsigned)(now-_next_sample_usec));
AP_InertialSensor: simplify next sample time calculations this makes the calculation much clearer
2014-10-19 20:46:02 -03:00
_next_sample_usec = now + _sample_period_usec;
AP_InertialSensor: converted PX4 driver to new API
2014-10-15 05:54:30 -03:00
}
AP_InertialSensor: improved timing in all drivers
2014-10-15 20:32:40 -03:00
check_sample:
AP_InertialSensor: try much harder to get all IMU samples this we ensures we get new data for all active IMUs on each loop, rather than sometimes returning with some IMUs not having data. This matters as not having a sample on an IMU for a single loop can cause an EKF IMU failover, which will degrade the learned bias variances The issue is usually only seen under high load, such as requesting a loop rate beyond what the hardware is capable of
2019-07-04 07:18:14 -03:00
// now we wait until we have the gyro and accel samples we need
uint8_t gyro_available_mask = 0;
uint8_t accel_available_mask = 0;
uint32_t wait_counter = 0;
AP_InertialSensor: relax IMU wait time for low loop rates allow for waiting for all IMUs for up to 1/3 of the loop time
2020-12-29 15:39:44 -04:00
// allow to wait for up to 1/3 of the loop time for samples from all
// IMUs to come in
const uint8_t wait_per_loop = 100;
const uint8_t wait_counter_limit = uint32_t(_loop_delta_t * 1.0e6) / (3*wait_per_loop);
AP_InertialSensor: try much harder to get all IMU samples this we ensures we get new data for all active IMUs on each loop, rather than sometimes returning with some IMUs not having data. This matters as not having a sample on an IMU for a single loop can cause an EKF IMU failover, which will degrade the learned bias variances The issue is usually only seen under high load, such as requesting a loop rate beyond what the hardware is capable of
2019-07-04 07:18:14 -03:00
AP_InertialSensor: don't check {gyro,accel}_available twice That gives a slightly simpler code.
2016-07-21 17:27:26 -03:00
while (true) {
AP_InertialSensor: make it easier to add multiple backends use _add_backend() to add new backends on startup
2014-10-16 17:27:01 -03:00
for (uint8_t i=0; i<_backend_count; i++) {
AP_InertialSensor: try much harder to get all IMU samples this we ensures we get new data for all active IMUs on each loop, rather than sometimes returning with some IMUs not having data. This matters as not having a sample on an IMU for a single loop can cause an EKF IMU failover, which will degrade the learned bias variances The issue is usually only seen under high load, such as requesting a loop rate beyond what the hardware is capable of
2019-07-04 07:18:14 -03:00
// this is normally a nop, but can be used by backends
// that don't accumulate samples on a timer
AP_InertialSensor: removed "have sample" logic from drivers the frontend has all the information it needs, so we can simplify the drivers some more
2015-11-15 20:38:43 -04:00
_backends[i]->accumulate();
}
AP_InertialSensor: don't check {gyro,accel}_available twice That gives a slightly simpler code.
2016-07-21 17:27:26 -03:00
AP_InertialSensor: try much harder to get all IMU samples this we ensures we get new data for all active IMUs on each loop, rather than sometimes returning with some IMUs not having data. This matters as not having a sample on an IMU for a single loop can cause an EKF IMU failover, which will degrade the learned bias variances The issue is usually only seen under high load, such as requesting a loop rate beyond what the hardware is capable of
2019-07-04 07:18:14 -03:00
for (uint8_t i=0; i<_gyro_count; i++) {
if (_new_gyro_data[i]) {
const uint8_t imask = (1U<<i);
gyro_available_mask |= imask;
AP_InertialSensor: add support for extra Aux IMUs
2023-02-17 00:26:27 -04:00
if (_use(i)) {
AP_InertialSensor: try much harder to get all IMU samples this we ensures we get new data for all active IMUs on each loop, rather than sometimes returning with some IMUs not having data. This matters as not having a sample on an IMU for a single loop can cause an EKF IMU failover, which will degrade the learned bias variances The issue is usually only seen under high load, such as requesting a loop rate beyond what the hardware is capable of
2019-07-04 07:18:14 -03:00
_gyro_wait_mask |= imask;
} else {
_gyro_wait_mask &= ~imask;
}
}
}
for (uint8_t i=0; i<_accel_count; i++) {
if (_new_accel_data[i]) {
const uint8_t imask = (1U<<i);
accel_available_mask |= imask;
AP_InertialSensor: add support for extra Aux IMUs
2023-02-17 00:26:27 -04:00
if (_use(i)) {
AP_InertialSensor: try much harder to get all IMU samples this we ensures we get new data for all active IMUs on each loop, rather than sometimes returning with some IMUs not having data. This matters as not having a sample on an IMU for a single loop can cause an EKF IMU failover, which will degrade the learned bias variances The issue is usually only seen under high load, such as requesting a loop rate beyond what the hardware is capable of
2019-07-04 07:18:14 -03:00
_accel_wait_mask |= imask;
} else {
_accel_wait_mask &= ~imask;
}
}
AP_InertialSensor: improved timing in all drivers
2014-10-15 20:32:40 -03:00
}
AP_InertialSensor: don't check {gyro,accel}_available twice That gives a slightly simpler code.
2016-07-21 17:27:26 -03:00
AP_InertialSensor: relax IMU wait time for low loop rates allow for waiting for all IMUs for up to 1/3 of the loop time
2020-12-29 15:39:44 -04:00
// we wait for up to 1/3 of the loop time to get all of the required
AP_InertialSensor: try much harder to get all IMU samples this we ensures we get new data for all active IMUs on each loop, rather than sometimes returning with some IMUs not having data. This matters as not having a sample on an IMU for a single loop can cause an EKF IMU failover, which will degrade the learned bias variances The issue is usually only seen under high load, such as requesting a loop rate beyond what the hardware is capable of
2019-07-04 07:18:14 -03:00
// accel and gyro samples. After that we accept at least
// one of each
AP_InertialSensor: relax IMU wait time for low loop rates allow for waiting for all IMUs for up to 1/3 of the loop time
2020-12-29 15:39:44 -04:00
if (wait_counter < wait_counter_limit) {
AP_InertialSensor: try much harder to get all IMU samples this we ensures we get new data for all active IMUs on each loop, rather than sometimes returning with some IMUs not having data. This matters as not having a sample on an IMU for a single loop can cause an EKF IMU failover, which will degrade the learned bias variances The issue is usually only seen under high load, such as requesting a loop rate beyond what the hardware is capable of
2019-07-04 07:18:14 -03:00
if (gyro_available_mask &&
((gyro_available_mask & _gyro_wait_mask) == _gyro_wait_mask) &&
accel_available_mask &&
((accel_available_mask & _accel_wait_mask) == _accel_wait_mask)) {
break;
}
} else {
if (gyro_available_mask && accel_available_mask) {
// reset the wait mask so we don't keep delaying
// for a dead IMU on the next loop. As soon as it
// comes back we will start waiting on it again
_gyro_wait_mask &= gyro_available_mask;
_accel_wait_mask &= accel_available_mask;
break;
}
AP_InertialSensor: first steps in frontend/backend split This converts the MPU6000 driver to a frontend/backend structure, and disables all other drivers. They will be progressively re-enabled as each is converted
2014-10-14 01:48:33 -03:00
}
AP_InertialSensor: don't check {gyro,accel}_available twice That gives a slightly simpler code.
2016-07-21 17:27:26 -03:00
AP_InertialSensor: relax IMU wait time for low loop rates allow for waiting for all IMUs for up to 1/3 of the loop time
2020-12-29 15:39:44 -04:00
hal.scheduler->delay_microseconds_boost(wait_per_loop);
AP_InertialSensor: try much harder to get all IMU samples this we ensures we get new data for all active IMUs on each loop, rather than sometimes returning with some IMUs not having data. This matters as not having a sample on an IMU for a single loop can cause an EKF IMU failover, which will degrade the learned bias variances The issue is usually only seen under high load, such as requesting a loop rate beyond what the hardware is capable of
2019-07-04 07:18:14 -03:00
wait_counter++;
AP_InertialSensor: converted PX4 driver to new API
2014-10-15 05:54:30 -03:00
}
AP_InertialSensor: improved timing in all drivers
2014-10-15 20:32:40 -03:00
AP_InertialSensor: use millis/micros/panic functions
2015-11-19 23:11:52 -04:00
now = AP_HAL::micros();
AP_InertialSensor: remove HIL support
2021-06-09 08:31:35 -03:00
_delta_time = (now - _last_sample_usec) * 1.0e-6f;
AP_InertialSensor: simplify next sample time calculations this makes the calculation much clearer
2014-10-19 20:46:02 -03:00
_last_sample_usec = now;
#if 0
{
static uint64_t delta_time_sum;
static uint16_t counter;
if (delta_time_sum == 0) {
delta_time_sum = _sample_period_usec;
}
delta_time_sum += _delta_time * 1.0e6f;
if (counter++ == 400) {
counter = 0;
hal.console->printf("now=%lu _delta_time_sum=%lu diff=%ld\n",
AP_InertialSensor: sanitize includes Due to the way the headers are organized changing a single change in an inertial sensor driver would trigger a rebuild for most of the files in the project. Time could be saved by using ccache (since most of the things didn't change) but we can do better, i.e. re-organize the headers so we don't have to re-build everything. With this patch only AP_InertialSensor/AP_InertialSensor.h is exposed to most users. There are some corner cases to integrate with some example code, but most of the places now depend only on this header and this header doesn't depend on the specific backends. Now changing a single header, e.g. AP_InertialSensor_L3G4200D.h triggers a rebuild only of these files: $ waf copter 'copter' finished successfully (0.000s) Waf: Entering directory `/home/lucas/p/dronecode/ardupilot/build/minlure' [ 80/370] Compiling libraries/AP_InertialSensor/AP_InertialSensor.cpp [ 84/370] Compiling libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp [310/370] Linking build/minlure/ArduCopter/libArduCopter_libs.a [370/370] Linking build/minlure/bin/arducopter Waf: Leaving directory `/home/lucas/p/dronecode/ardupilot/build/minlure'
2016-01-19 21:26:31 -04:00
(unsigned long)now,
AP_InertialSensor: simplify next sample time calculations this makes the calculation much clearer
2014-10-19 20:46:02 -03:00
(unsigned long)delta_time_sum,
(long)(now - delta_time_sum));
}
}
#endif
AP_InertialSensor: improved timing in all drivers
2014-10-15 20:32:40 -03:00
_have_sample = true;
AP_InertialSensor: converted HIL backend, which gets SITL working
2014-10-15 02:37:59 -03:00
}
AP_InertialSensor: always provide delta_velocity and delta_angles this makes the NavEKF code simpler
2015-06-17 00:01:51 -03:00
/*
get delta angles
*/
AP_InertialSensor: remove separate calls to get delta-times for vel and ang
2021-02-11 17:34:37 -04:00
bool AP_InertialSensor::get_delta_angle(uint8_t i, Vector3f &delta_angle, float &delta_angle_dt) const
AP_InertialSensor: always provide delta_velocity and delta_angles this makes the NavEKF code simpler
2015-06-17 00:01:51 -03:00
{
AP_InertialSensor: remove separate calls to get delta-times for vel and ang
2021-02-11 17:34:37 -04:00
if (_delta_angle_valid[i] && _delta_angle_dt[i] > 0) {
delta_angle_dt = _delta_angle_dt[i];
} else {
delta_angle_dt = get_delta_time();
}
delta_angle_dt = MIN(delta_angle_dt, _loop_delta_t_max);
AP_InertialSensor: always provide delta_velocity and delta_angles this makes the NavEKF code simpler
2015-06-17 00:01:51 -03:00
if (_delta_angle_valid[i]) {
delta_angle = _delta_angle[i];
return true;
} else if (get_gyro_health(i)) {
// provide delta angle from raw gyro, so we use the same code
// at higher level
delta_angle = get_gyro(i) * get_delta_time();
return true;
}
return false;
}
/*
get delta velocity if available
*/
AP_InertialSensor: remove separate calls to get delta-times for vel and ang
2021-02-11 17:34:37 -04:00
bool AP_InertialSensor::get_delta_velocity(uint8_t i, Vector3f &delta_velocity, float &delta_velocity_dt) const
AP_InertialSensor: always provide delta_velocity and delta_angles this makes the NavEKF code simpler
2015-06-17 00:01:51 -03:00
{
AP_InertialSensor: remove separate calls to get delta-times for vel and ang
2021-02-11 17:34:37 -04:00
if (_delta_velocity_valid[i]) {
delta_velocity_dt = _delta_velocity_dt[i];
} else {
delta_velocity_dt = get_delta_time();
}
delta_velocity_dt = MIN(delta_velocity_dt, _loop_delta_t_max);
AP_InertialSensor: always provide delta_velocity and delta_angles this makes the NavEKF code simpler
2015-06-17 00:01:51 -03:00
if (_delta_velocity_valid[i]) {
delta_velocity = _delta_velocity[i];
return true;
} else if (get_accel_health(i)) {
delta_velocity = get_accel(i) * get_delta_time();
return true;
}
return false;
}
AP_InertialSensor: Add support for auxiliary buses Add an AuxiliaryBus class that can be derived for specific implementations in inertial sensor backends. It's an abstract implementation so other libraries can use the auxiliary bus exported. In order for this to succeed the backend implementation must split the initialization of the sensor from the actual sample collecting, like is done in MPU6000. When AP_InertialSensor::get_auxiliary_bus() is called it will execute following steps: a) Force the backends to be detected if it's the first time it's being called b) Find the backend identified by the id c) call get_auxiliary_bus() on the backend so other libraries can that AuxiliaryBus to initialize a slave device Slave devices can be used by calling AuxiliaryBus::request_next_slave() and are owned by the caller until AuxiliaryBus::register_periodic_read() is called. From that time on the AuxiliaryBus object takes its ownership. This way it's possible to do the necessary cleanup later without introducing refcounts, that we don't have support to. Between these 2 functions the caller can configure the slave device by doing its specific initializations by calling the passthrough_* functions. After the initial configuration and register_periodic_read() is called only read() can be called.
2015-08-05 13:36:14 -03:00
/*
AP_InertialSensor: Add support to get auxiliary bus of multiple instances of the same sensor
2015-10-02 15:45:21 -03:00
* Get an AuxiliaryBus of N @instance of backend identified by @backend_id
AP_InertialSensor: Add support for auxiliary buses Add an AuxiliaryBus class that can be derived for specific implementations in inertial sensor backends. It's an abstract implementation so other libraries can use the auxiliary bus exported. In order for this to succeed the backend implementation must split the initialization of the sensor from the actual sample collecting, like is done in MPU6000. When AP_InertialSensor::get_auxiliary_bus() is called it will execute following steps: a) Force the backends to be detected if it's the first time it's being called b) Find the backend identified by the id c) call get_auxiliary_bus() on the backend so other libraries can that AuxiliaryBus to initialize a slave device Slave devices can be used by calling AuxiliaryBus::request_next_slave() and are owned by the caller until AuxiliaryBus::register_periodic_read() is called. From that time on the AuxiliaryBus object takes its ownership. This way it's possible to do the necessary cleanup later without introducing refcounts, that we don't have support to. Between these 2 functions the caller can configure the slave device by doing its specific initializations by calling the passthrough_* functions. After the initial configuration and register_periodic_read() is called only read() can be called.
2015-08-05 13:36:14 -03:00
*/
AP_InertialSensor: Add support to get auxiliary bus of multiple instances of the same sensor
2015-10-02 15:45:21 -03:00
AuxiliaryBus *AP_InertialSensor::get_auxiliary_bus(int16_t backend_id, uint8_t instance)
AP_InertialSensor: Add support for auxiliary buses Add an AuxiliaryBus class that can be derived for specific implementations in inertial sensor backends. It's an abstract implementation so other libraries can use the auxiliary bus exported. In order for this to succeed the backend implementation must split the initialization of the sensor from the actual sample collecting, like is done in MPU6000. When AP_InertialSensor::get_auxiliary_bus() is called it will execute following steps: a) Force the backends to be detected if it's the first time it's being called b) Find the backend identified by the id c) call get_auxiliary_bus() on the backend so other libraries can that AuxiliaryBus to initialize a slave device Slave devices can be used by calling AuxiliaryBus::request_next_slave() and are owned by the caller until AuxiliaryBus::register_periodic_read() is called. From that time on the AuxiliaryBus object takes its ownership. This way it's possible to do the necessary cleanup later without introducing refcounts, that we don't have support to. Between these 2 functions the caller can configure the slave device by doing its specific initializations by calling the passthrough_* functions. After the initial configuration and register_periodic_read() is called only read() can be called.
2015-08-05 13:36:14 -03:00
{
AP_InertialSensor: Make detect_backends() public This method will be used to initialize and configure I2C backends that have an auxiliary I2C bus that can be connected to the main I2C bus, like MPU6000 and MPU9250.
2015-10-13 15:33:20 -03:00
detect_backends();
AP_InertialSensor: Add support for auxiliary buses Add an AuxiliaryBus class that can be derived for specific implementations in inertial sensor backends. It's an abstract implementation so other libraries can use the auxiliary bus exported. In order for this to succeed the backend implementation must split the initialization of the sensor from the actual sample collecting, like is done in MPU6000. When AP_InertialSensor::get_auxiliary_bus() is called it will execute following steps: a) Force the backends to be detected if it's the first time it's being called b) Find the backend identified by the id c) call get_auxiliary_bus() on the backend so other libraries can that AuxiliaryBus to initialize a slave device Slave devices can be used by calling AuxiliaryBus::request_next_slave() and are owned by the caller until AuxiliaryBus::register_periodic_read() is called. From that time on the AuxiliaryBus object takes its ownership. This way it's possible to do the necessary cleanup later without introducing refcounts, that we don't have support to. Between these 2 functions the caller can configure the slave device by doing its specific initializations by calling the passthrough_* functions. After the initial configuration and register_periodic_read() is called only read() can be called.
2015-08-05 13:36:14 -03:00
AP_InertialSensor: Add support to get auxiliary bus of multiple instances of the same sensor
2015-10-02 15:45:21 -03:00
AP_InertialSensor_Backend *backend = _find_backend(backend_id, instance);
AP_InertialSensor: minor formatting fixes No functional change
2016-12-23 23:21:29 -04:00
if (backend == nullptr) {
Global: To nullptr from NULL. RC_Channel: To nullptr from NULL. AC_Fence: To nullptr from NULL. AC_Avoidance: To nullptr from NULL. AC_PrecLand: To nullptr from NULL. DataFlash: To nullptr from NULL. SITL: To nullptr from NULL. GCS_MAVLink: To nullptr from NULL. DataFlash: To nullptr from NULL. AP_Compass: To nullptr from NULL. Global: To nullptr from NULL. Global: To nullptr from NULL.
2016-10-30 02:24:21 -03:00
return nullptr;
AP_InertialSensor: minor formatting fixes No functional change
2016-12-23 23:21:29 -04:00
}
AP_InertialSensor: Add support for auxiliary buses Add an AuxiliaryBus class that can be derived for specific implementations in inertial sensor backends. It's an abstract implementation so other libraries can use the auxiliary bus exported. In order for this to succeed the backend implementation must split the initialization of the sensor from the actual sample collecting, like is done in MPU6000. When AP_InertialSensor::get_auxiliary_bus() is called it will execute following steps: a) Force the backends to be detected if it's the first time it's being called b) Find the backend identified by the id c) call get_auxiliary_bus() on the backend so other libraries can that AuxiliaryBus to initialize a slave device Slave devices can be used by calling AuxiliaryBus::request_next_slave() and are owned by the caller until AuxiliaryBus::register_periodic_read() is called. From that time on the AuxiliaryBus object takes its ownership. This way it's possible to do the necessary cleanup later without introducing refcounts, that we don't have support to. Between these 2 functions the caller can configure the slave device by doing its specific initializations by calling the passthrough_* functions. After the initial configuration and register_periodic_read() is called only read() can be called.
2015-08-05 13:36:14 -03:00
AP_InertialSensor: Fix typo: auxiliar to auxiliary
2015-10-02 15:02:16 -03:00
return backend->get_auxiliary_bus();
AP_InertialSensor: Add support for auxiliary buses Add an AuxiliaryBus class that can be derived for specific implementations in inertial sensor backends. It's an abstract implementation so other libraries can use the auxiliary bus exported. In order for this to succeed the backend implementation must split the initialization of the sensor from the actual sample collecting, like is done in MPU6000. When AP_InertialSensor::get_auxiliary_bus() is called it will execute following steps: a) Force the backends to be detected if it's the first time it's being called b) Find the backend identified by the id c) call get_auxiliary_bus() on the backend so other libraries can that AuxiliaryBus to initialize a slave device Slave devices can be used by calling AuxiliaryBus::request_next_slave() and are owned by the caller until AuxiliaryBus::register_periodic_read() is called. From that time on the AuxiliaryBus object takes its ownership. This way it's possible to do the necessary cleanup later without introducing refcounts, that we don't have support to. Between these 2 functions the caller can configure the slave device by doing its specific initializations by calling the passthrough_* functions. After the initial configuration and register_periodic_read() is called only read() can be called.
2015-08-05 13:36:14 -03:00
}
InertialSensor: calc vibration and accel clipping
2015-06-11 04:15:57 -03:00
// calculate vibration levels and check for accelerometer clipping (called by a backends)
void AP_InertialSensor::calc_vibration_and_clipping(uint8_t instance, const Vector3f &accel, float dt)
{
// check for clipping
AP_IneertialSensor: fixed startup race in SITL
2019-03-21 03:48:40 -03:00
if (_backends[instance] == nullptr) {
return;
}
AP_IneertialSensor: add support for custom accel scale adds support for 30g measurement on ICM20649
2019-03-11 09:56:40 -03:00
if (fabsf(accel.x) > _backends[instance]->get_clip_limit() ||
fabsf(accel.y) > _backends[instance]->get_clip_limit() ||
fabsf(accel.z) > _backends[instance]->get_clip_limit()) {
InertialSensor: calc vibration and accel clipping
2015-06-11 04:15:57 -03:00
_accel_clip_count[instance]++;
}
InertialSensor: add vibration monitoring of 2nd IMU
2015-07-30 04:58:06 -03:00
// calculate vibration levels
if (instance < INS_VIBRATION_CHECK_INSTANCES) {
// filter accel at 5hz
Vector3f accel_filt = _accel_vibe_floor_filter[instance].apply(accel, dt);
// calc difference from this sample and 5hz filtered value, square and filter at 2hz
Vector3f accel_diff = (accel - accel_filt);
accel_diff.x *= accel_diff.x;
accel_diff.y *= accel_diff.y;
accel_diff.z *= accel_diff.z;
_accel_vibe_filter[instance].apply(accel_diff, dt);
InertialSensor: calc vibration and accel clipping
2015-06-11 04:15:57 -03:00
}
}
AP_InertialSensor: add pos/neg peak detector new functions that get a filtered min/max accel peaks on each axis with fixed 500ms timeout: Vector3f get_accel_peak_hold_pos() Vector3f get_accel_peak_hold_neg() This allows slower mechanisms, such as is_flying, to detect accel spikes which would indicate ground or object impacts. Vibe is too filtered. Independent positive and negative peaks are available
2015-12-29 13:18:07 -04:00
// peak hold detector for slower mechanisms to detect spikes
void AP_InertialSensor::set_accel_peak_hold(uint8_t instance, const Vector3f &accel)
{
AP_InertialSensor: we only need peak hold for negative X for now
2015-12-30 23:23:04 -04:00
if (instance != _primary_accel) {
// we only record for primary accel
return;
AP_InertialSensor: add pos/neg peak detector new functions that get a filtered min/max accel peaks on each axis with fixed 500ms timeout: Vector3f get_accel_peak_hold_pos() Vector3f get_accel_peak_hold_neg() This allows slower mechanisms, such as is_flying, to detect accel spikes which would indicate ground or object impacts. Vibe is too filtered. Independent positive and negative peaks are available
2015-12-29 13:18:07 -04:00
}
AP_InertialSensor: we only need peak hold for negative X for now
2015-12-30 23:23:04 -04:00
uint32_t now = AP_HAL::millis();
AP_InertialSensor: add pos/neg peak detector new functions that get a filtered min/max accel peaks on each axis with fixed 500ms timeout: Vector3f get_accel_peak_hold_pos() Vector3f get_accel_peak_hold_neg() This allows slower mechanisms, such as is_flying, to detect accel spikes which would indicate ground or object impacts. Vibe is too filtered. Independent positive and negative peaks are available
2015-12-29 13:18:07 -04:00
AP_InertialSensor: we only need peak hold for negative X for now
2015-12-30 23:23:04 -04:00
// negative x peak(min) hold detector
if (accel.x < _peak_hold_state.accel_peak_hold_neg_x ||
_peak_hold_state.accel_peak_hold_neg_x_age <= now) {
_peak_hold_state.accel_peak_hold_neg_x = accel.x;
_peak_hold_state.accel_peak_hold_neg_x_age = now + AP_INERTIAL_SENSOR_ACCEL_PEAK_DETECT_TIMEOUT_MS;
AP_InertialSensor: add pos/neg peak detector new functions that get a filtered min/max accel peaks on each axis with fixed 500ms timeout: Vector3f get_accel_peak_hold_pos() Vector3f get_accel_peak_hold_neg() This allows slower mechanisms, such as is_flying, to detect accel spikes which would indicate ground or object impacts. Vibe is too filtered. Independent positive and negative peaks are available
2015-12-29 13:18:07 -04:00
}
}
InertialSensor: calc vibration and accel clipping
2015-06-11 04:15:57 -03:00
// retrieve latest calculated vibration levels
InertialSensor: add vibration monitoring of 2nd IMU
2015-07-30 04:58:06 -03:00
Vector3f AP_InertialSensor::get_vibration_levels(uint8_t instance) const
InertialSensor: calc vibration and accel clipping
2015-06-11 04:15:57 -03:00
{
InertialSensor: add vibration monitoring of 2nd IMU
2015-07-30 04:58:06 -03:00
Vector3f vibe;
if (instance < INS_VIBRATION_CHECK_INSTANCES) {
vibe = _accel_vibe_filter[instance].get();
vibe.x = safe_sqrt(vibe.x);
vibe.y = safe_sqrt(vibe.y);
vibe.z = safe_sqrt(vibe.z);
}
InertialSensor: calc vibration and accel clipping
2015-06-11 04:15:57 -03:00
return vibe;
}
AP_InertialSensor: Added is_still() check very strict check that all axis are not vibrating much at all new param: INS_STILL_THRESH used to be a vibration threshold for different platforms // @Description: Threshold to tolerate vibration to determine if vehicle is motionless. This depends on the frame type and if there is a constant vibration due to motors before launch or after landing. Total motionless is about 0.05. Suggested values: Planes/rover use 0.1, multirotors use 1, tradHeli uses 5
2015-07-29 16:34:48 -03:00
// check for vibration movement. Return true if all axis show nearly zero movement
bool AP_InertialSensor::is_still()
{
Vector3f vibe = get_vibration_levels();
return (vibe.x < _still_threshold) &&
(vibe.y < _still_threshold) &&
(vibe.z < _still_threshold);
}
AP_InertialSensor: support AP_AccelCal
2015-07-20 17:25:40 -03:00
AP_InertialSensor: added IMU temperature calibration support this adds parameters that can be setup by an external script for compensating for temperature variation in gyros and accels using a 3rd order polynomial
2021-01-07 20:46:35 -04:00
// return true if we are in a calibration
bool AP_InertialSensor::calibrating() const
{
AP_InertialSensor: Factor accel cal in from GCS_MAVLink
2022-10-04 17:42:03 -03:00
if (_calibrating_accel || _calibrating_gyro || _trimming_accel) {
AP_InertialSensor: correct compilation with HAL_INS_ACCELCAL_ENABLED false
2022-03-18 04:02:30 -03:00
return true;
}
#if HAL_INS_ACCELCAL_ENABLED
if (_acal && _acal->running()) {
return true;
}
#endif
return false;
AP_InertialSensor: added IMU temperature calibration support this adds parameters that can be setup by an external script for compensating for temperature variation in gyros and accels using a 3rd order polynomial
2021-01-07 20:46:35 -04:00
}
AP_InertialSensor: added notify events for temperature calibration
2021-01-10 16:09:44 -04:00
/// calibrating - returns true if a temperature calibration is running
bool AP_InertialSensor::temperature_cal_running() const
{
#if HAL_INS_TEMPERATURE_CAL_ENABLE
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
AP_InertialSensor: add support for extra Aux IMUs
2023-02-17 00:26:27 -04:00
if (tcal(i).enable == AP_InertialSensor_TCal::Enable::LearnCalibration) {
AP_InertialSensor: added notify events for temperature calibration
2021-01-10 16:09:44 -04:00
return true;
}
}
#endif
return false;
}
AP_InertialSensor: correct compilation with HAL_INS_ACCELCAL_ENABLED false
2022-03-18 04:02:30 -03:00
#if HAL_INS_ACCELCAL_ENABLED
AP_InertialSensor: support AP_AccelCal
2015-07-20 17:25:40 -03:00
// initialise and register accel calibrator
// called during the startup of accel cal
void AP_InertialSensor::acal_init()
{
AP_InertialSensor: fixes in response to review
2015-12-30 02:51:27 -04:00
// NOTE: these objects are never deallocated because the pre-arm checks force a reboot
Global: To nullptr from NULL. RC_Channel: To nullptr from NULL. AC_Fence: To nullptr from NULL. AC_Avoidance: To nullptr from NULL. AC_PrecLand: To nullptr from NULL. DataFlash: To nullptr from NULL. SITL: To nullptr from NULL. GCS_MAVLink: To nullptr from NULL. DataFlash: To nullptr from NULL. AP_Compass: To nullptr from NULL. Global: To nullptr from NULL. Global: To nullptr from NULL.
2016-10-30 02:24:21 -03:00
if (_acal == nullptr) {
AP_InertialSensor: ensure that accel calibration object isn't allocated more than once
2015-12-29 00:16:16 -04:00
_acal = new AP_AccelCal;
}
Global: To nullptr from NULL. RC_Channel: To nullptr from NULL. AC_Fence: To nullptr from NULL. AC_Avoidance: To nullptr from NULL. AC_PrecLand: To nullptr from NULL. DataFlash: To nullptr from NULL. SITL: To nullptr from NULL. GCS_MAVLink: To nullptr from NULL. DataFlash: To nullptr from NULL. AP_Compass: To nullptr from NULL. Global: To nullptr from NULL. Global: To nullptr from NULL.
2016-10-30 02:24:21 -03:00
if (_accel_calibrator == nullptr) {
AP_InertialSensor: ensure that accel calibration object isn't allocated more than once
2015-12-29 00:16:16 -04:00
_accel_calibrator = new AccelCalibrator[INS_MAX_INSTANCES];
}
AP_InertialSensor: support AP_AccelCal
2015-07-20 17:25:40 -03:00
}
// update accel calibrator
AP_InertialSensor: sanitize includes Due to the way the headers are organized changing a single change in an inertial sensor driver would trigger a rebuild for most of the files in the project. Time could be saved by using ccache (since most of the things didn't change) but we can do better, i.e. re-organize the headers so we don't have to re-build everything. With this patch only AP_InertialSensor/AP_InertialSensor.h is exposed to most users. There are some corner cases to integrate with some example code, but most of the places now depend only on this header and this header doesn't depend on the specific backends. Now changing a single header, e.g. AP_InertialSensor_L3G4200D.h triggers a rebuild only of these files: $ waf copter 'copter' finished successfully (0.000s) Waf: Entering directory `/home/lucas/p/dronecode/ardupilot/build/minlure' [ 80/370] Compiling libraries/AP_InertialSensor/AP_InertialSensor.cpp [ 84/370] Compiling libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp [310/370] Linking build/minlure/ArduCopter/libArduCopter_libs.a [370/370] Linking build/minlure/bin/arducopter Waf: Leaving directory `/home/lucas/p/dronecode/ardupilot/build/minlure'
2016-01-19 21:26:31 -04:00
void AP_InertialSensor::acal_update()
AP_InertialSensor: support AP_AccelCal
2015-07-20 17:25:40 -03:00
{
Global: To nullptr from NULL. RC_Channel: To nullptr from NULL. AC_Fence: To nullptr from NULL. AC_Avoidance: To nullptr from NULL. AC_PrecLand: To nullptr from NULL. DataFlash: To nullptr from NULL. SITL: To nullptr from NULL. GCS_MAVLink: To nullptr from NULL. DataFlash: To nullptr from NULL. AP_Compass: To nullptr from NULL. Global: To nullptr from NULL. Global: To nullptr from NULL.
2016-10-30 02:24:21 -03:00
if(_acal == nullptr) {
AP_InertialSensor: support AP_AccelCal
2015-07-20 17:25:40 -03:00
return;
}
AP_InertialSensor: rename to EXPECT_DELAY_MS()
2019-05-15 01:09:40 -03:00
EXPECT_DELAY_MS(20000);
AP_InertialSensor: support AP_AccelCal
2015-07-20 17:25:40 -03:00
_acal->update();
if (hal.util->get_soft_armed() && _acal->get_status() != ACCEL_CAL_NOT_STARTED) {
_acal->cancel();
}
}
AP_InertialSensor: correct compilation with HAL_INS_ACCELCAL_ENABLED false
2022-03-18 04:02:30 -03:00
#endif
AP_InertialSensor: support AP_AccelCal
2015-07-20 17:25:40 -03:00
AP_InertialSensor: add harmonic notch filter to gyro filter chain Allow dynamic updates to the calculated frequency. Convert bandwidth and frequency to floats. backend variables do not need to be indexed per-backed
2019-06-17 05:44:12 -03:00
// Update the harmonic notch frequency
AP_InertialSensor: switch to HarmonicNotch class this makes the logic much easier to follow, without indexes into arrays
2022-04-15 04:38:56 -03:00
void AP_InertialSensor::HarmonicNotch::update_freq_hz(float scaled_freq)
{
AP_InertialSensor: remove _ prefix from methods. use changed filter methods.
2019-08-28 17:34:12 -03:00
// protect against zero as the scaled frequency
AP_InertialSensor: add harmonic notch filter to gyro filter chain Allow dynamic updates to the calculated frequency. Convert bandwidth and frequency to floats. backend variables do not need to be indexed per-backed
2019-06-17 05:44:12 -03:00
if (is_positive(scaled_freq)) {
AP_InertialSensor: switch to HarmonicNotch class this makes the logic much easier to follow, without indexes into arrays
2022-04-15 04:38:56 -03:00
calculated_notch_freq_hz[0] = scaled_freq;
AP_InertialSensor: allow up to four distinct notch center frequencies to be updated apply the LPF after the notch filters to reduced notch-induced noise
2020-05-29 13:28:06 -03:00
}
AP_InertialSensor: switch to HarmonicNotch class this makes the logic much easier to follow, without indexes into arrays
2022-04-15 04:38:56 -03:00
num_calculated_notch_frequencies = 1;
AP_InertialSensor: allow up to four distinct notch center frequencies to be updated apply the LPF after the notch filters to reduced notch-induced noise
2020-05-29 13:28:06 -03:00
}
// Update the harmonic notch frequency
AP_InertialSensor: switch to HarmonicNotch class this makes the logic much easier to follow, without indexes into arrays
2022-04-15 04:38:56 -03:00
void AP_InertialSensor::HarmonicNotch::update_frequencies_hz(uint8_t num_freqs, const float scaled_freq[]) {
AP_InertialSensor: allow up to four distinct notch center frequencies to be updated apply the LPF after the notch filters to reduced notch-induced noise
2020-05-29 13:28:06 -03:00
// protect against zero as the scaled frequency
for (uint8_t i = 0; i < num_freqs; i++) {
if (is_positive(scaled_freq[i])) {
AP_InertialSensor: switch to HarmonicNotch class this makes the logic much easier to follow, without indexes into arrays
2022-04-15 04:38:56 -03:00
calculated_notch_freq_hz[i] = scaled_freq[i];
AP_InertialSensor: allow up to four distinct notch center frequencies to be updated apply the LPF after the notch filters to reduced notch-induced noise
2020-05-29 13:28:06 -03:00
}
AP_InertialSensor: add harmonic notch filter to gyro filter chain Allow dynamic updates to the calculated frequency. Convert bandwidth and frequency to floats. backend variables do not need to be indexed per-backed
2019-06-17 05:44:12 -03:00
}
AP_InertialSensor: allow up to four distinct notch center frequencies to be updated apply the LPF after the notch filters to reduced notch-induced noise
2020-05-29 13:28:06 -03:00
// any uncalculated frequencies will float at the previous value or the initialized freq if none
AP_InertialSensor: switch to HarmonicNotch class this makes the logic much easier to follow, without indexes into arrays
2022-04-15 04:38:56 -03:00
num_calculated_notch_frequencies = num_freqs;
AP_InertialSensor: add harmonic notch filter to gyro filter chain Allow dynamic updates to the calculated frequency. Convert bandwidth and frequency to floats. backend variables do not need to be indexed per-backed
2019-06-17 05:44:12 -03:00
}
AP_InertialSensor: add throttle-based notch setup function allocate harmonic notch filter if FFT is enabled
2022-03-06 11:00:30 -04:00
// setup the notch for throttle based tracking, called from FFT based tuning
AP_InertialSensor: use freq_min_ratio on notch tune setup set harmonics in notch setup
2023-01-18 18:58:44 -04:00
bool AP_InertialSensor::setup_throttle_gyro_harmonic_notch(float center_freq_hz, float lower_freq_hz, float ref, uint8_t harmonics)
AP_InertialSensor: add throttle-based notch setup function allocate harmonic notch filter if FFT is enabled
2022-03-06 11:00:30 -04:00
{
for (auto &notch : harmonic_notches) {
if (notch.params.tracking_mode() != HarmonicNotchDynamicMode::UpdateThrottle) {
continue;
}
notch.params.enable();
notch.params.set_center_freq_hz(center_freq_hz);
notch.params.set_reference(ref);
notch.params.set_bandwidth_hz(center_freq_hz / 2.0f);
AP_InertialSensor: use freq_min_ratio on notch tune setup set harmonics in notch setup
2023-01-18 18:58:44 -04:00
notch.params.set_freq_min_ratio(lower_freq_hz / center_freq_hz);
notch.params.set_harmonics(harmonics);
AP_InertialSensor: add throttle-based notch setup function allocate harmonic notch filter if FFT is enabled
2022-03-06 11:00:30 -04:00
notch.params.save_params();
// only enable the first notch
return true;
}
return false;
}
AP_InertialSensor: support AP_AccelCal
2015-07-20 17:25:40 -03:00
/*
set and save accelerometer bias along with trim calculation
*/
void AP_InertialSensor::_acal_save_calibrations()
{
Vector3f bias, gain;
for (uint8_t i=0; i<_accel_count; i++) {
if (_accel_calibrator[i].get_status() == ACCEL_CAL_SUCCESS) {
_accel_calibrator[i].get_calibration(bias, gain);
AP_InertialSensor: add support for extra Aux IMUs
2023-02-17 00:26:27 -04:00
_accel_offset(i).set_and_save(bias);
_accel_scale(i).set_and_save(gain);
_accel_id(i).save();
AP_InertialSensor: save id for gyro and accel instances This allows each sensor to be uniquely identified in the system by using either the index inside the backend or for those that use the Device interface, to use the bus type, location, and device id. We leave 16-bit for each sensor to be able to change its own identification in future, which allows them to be changed in an incompatible manner forcing a re-calibration.
2016-09-03 21:51:37 -03:00
_accel_id_ok[i] = true;
AP_InertialSensor: added IMU temperature calibration support this adds parameters that can be setup by an external script for compensating for temperature variation in gyros and accels using a 3rd order polynomial
2021-01-07 20:46:35 -04:00
#if HAL_INS_TEMPERATURE_CAL_ENABLE
AP_InertialSensor: add support for extra Aux IMUs
2023-02-17 00:26:27 -04:00
caltemp_accel(i).set_and_save(get_temperature(i));
AP_InertialSensor: added IMU temperature calibration support this adds parameters that can be setup by an external script for compensating for temperature variation in gyros and accels using a 3rd order polynomial
2021-01-07 20:46:35 -04:00
#endif
AP_InertialSensor: support AP_AccelCal
2015-07-20 17:25:40 -03:00
} else {
AP_InertialSensor: add support for extra Aux IMUs
2023-02-17 00:26:27 -04:00
_accel_offset(i).set_and_save(Vector3f());
_accel_scale(i).set_and_save(Vector3f());
AP_InertialSensor: added IMU temperature calibration support this adds parameters that can be setup by an external script for compensating for temperature variation in gyros and accels using a 3rd order polynomial
2021-01-07 20:46:35 -04:00
#if HAL_INS_TEMPERATURE_CAL_ENABLE
AP_InertialSensor: add support for extra Aux IMUs
2023-02-17 00:26:27 -04:00
caltemp_accel(i).set_and_save(-300);
AP_InertialSensor: added IMU temperature calibration support this adds parameters that can be setup by an external script for compensating for temperature variation in gyros and accels using a 3rd order polynomial
2021-01-07 20:46:35 -04:00
#endif
AP_InertialSensor: support AP_AccelCal
2015-07-20 17:25:40 -03:00
}
}
AP_InertialSensor: detect missing accels or gyros
2016-11-05 06:20:43 -03:00
// clear any unused accels
for (uint8_t i=_accel_count; i<INS_MAX_INSTANCES; i++) {
AP_InertialSensor: add support for extra Aux IMUs
2023-02-17 00:26:27 -04:00
_accel_id(i).set_and_save(0);
_accel_offset(i).set_and_save(Vector3f());
_accel_scale(i).set_and_save(Vector3f());
AP_InertialSensor: added IMU temperature calibration support this adds parameters that can be setup by an external script for compensating for temperature variation in gyros and accels using a 3rd order polynomial
2021-01-07 20:46:35 -04:00
#if HAL_INS_TEMPERATURE_CAL_ENABLE
AP_InertialSensor: add support for extra Aux IMUs
2023-02-17 00:26:27 -04:00
caltemp_accel(i).set_and_save_ifchanged(-300);
AP_InertialSensor: added IMU temperature calibration support this adds parameters that can be setup by an external script for compensating for temperature variation in gyros and accels using a 3rd order polynomial
2021-01-07 20:46:35 -04:00
#endif
AP_InertialSensor: detect missing accels or gyros
2016-11-05 06:20:43 -03:00
}
AP_InertialSensor: support AP_AccelCal
2015-07-20 17:25:40 -03:00
Vector3f aligned_sample;
Vector3f misaligned_sample;
switch(_trim_option) {
case 0:
AP_InertialSensor: fixes in response to review
2015-12-30 02:51:27 -04:00
break;
case 1:
AP_InertialSensor: support AP_AccelCal
2015-07-20 17:25:40 -03:00
// The first level step of accel cal will be taken as gnd truth,
// i.e. trim will be set as per the output of primary accel from the level step
get_primary_accel_cal_sample_avg(0,aligned_sample);
AP_InertialSensor: allow accelcal with a rotation
2021-08-12 19:57:43 -03:00
_trim_rad.zero();
_calculate_trim(aligned_sample, _trim_rad);
AP_InertialSensor: support AP_AccelCal
2015-07-20 17:25:40 -03:00
_new_trim = true;
break;
AP_InertialSensor: fixes in response to review
2015-12-30 02:51:27 -04:00
case 2:
AP_InertialSensor: support AP_AccelCal
2015-07-20 17:25:40 -03:00
// Reference accel is truth, in this scenario there is a reference accel
// as mentioned in ACC_BODY_ALIGNED
if (get_primary_accel_cal_sample_avg(0,misaligned_sample) && get_fixed_mount_accel_cal_sample(0,aligned_sample)) {
// determine trim from aligned sample vs misaligned sample
Vector3f cross = (misaligned_sample%aligned_sample);
float dot = (misaligned_sample*aligned_sample);
AP_InertialSensor: replace sqrt with safe_sqrt to resolve compiler warning Also add suppressing comment for missing break at end of switch
2016-01-22 21:10:17 -04:00
Quaternion q(safe_sqrt(sq(misaligned_sample.length())*sq(aligned_sample.length()))+dot, cross.x, cross.y, cross.z);
AP_InertialSensor: support AP_AccelCal
2015-07-20 17:25:40 -03:00
q.normalize();
AP_InertialSensor: allow accelcal with a rotation
2021-08-12 19:57:43 -03:00
_trim_rad.x = q.get_euler_roll();
_trim_rad.y = q.get_euler_pitch();
_trim_rad.z = 0;
AP_InertialSensor: support AP_AccelCal
2015-07-20 17:25:40 -03:00
_new_trim = true;
}
break;
default:
_new_trim = false;
AP_InertialSensor: replace sqrt with safe_sqrt to resolve compiler warning Also add suppressing comment for missing break at end of switch
2016-01-22 21:10:17 -04:00
/* no break */
AP_InertialSensor: support AP_AccelCal
2015-07-20 17:25:40 -03:00
}
AP_InertialSensor: allow accelcal with a rotation
2021-08-12 19:57:43 -03:00
if (fabsf(_trim_rad.x) > radians(HAL_INS_TRIM_LIMIT_DEG) ||
fabsf(_trim_rad.y) > radians(HAL_INS_TRIM_LIMIT_DEG) ||
fabsf(_trim_rad.z) > radians(HAL_INS_TRIM_LIMIT_DEG)) {
AP_InertialSensor: Console output can be disabled
2022-03-21 06:25:18 -03:00
DEV_PRINTF("ERR: Trim over maximum of %.1f degrees!!", float(HAL_INS_TRIM_LIMIT_DEG));
AP_InertialSensor: sanitize includes Due to the way the headers are organized changing a single change in an inertial sensor driver would trigger a rebuild for most of the files in the project. Time could be saved by using ccache (since most of the things didn't change) but we can do better, i.e. re-organize the headers so we don't have to re-build everything. With this patch only AP_InertialSensor/AP_InertialSensor.h is exposed to most users. There are some corner cases to integrate with some example code, but most of the places now depend only on this header and this header doesn't depend on the specific backends. Now changing a single header, e.g. AP_InertialSensor_L3G4200D.h triggers a rebuild only of these files: $ waf copter 'copter' finished successfully (0.000s) Waf: Entering directory `/home/lucas/p/dronecode/ardupilot/build/minlure' [ 80/370] Compiling libraries/AP_InertialSensor/AP_InertialSensor.cpp [ 84/370] Compiling libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp [310/370] Linking build/minlure/ArduCopter/libArduCopter_libs.a [370/370] Linking build/minlure/bin/arducopter Waf: Leaving directory `/home/lucas/p/dronecode/ardupilot/build/minlure'
2016-01-19 21:26:31 -04:00
_new_trim = false; //we have either got faulty level during acal or highly misaligned accelerometers
AP_InertialSensor: support AP_AccelCal
2015-07-20 17:25:40 -03:00
}
AP_InertialSensor: add accel_cal_requires_reboot
2015-12-29 00:32:06 -04:00
_accel_cal_requires_reboot = true;
AP_InertialSensor: support AP_AccelCal
2015-07-20 17:25:40 -03:00
}
void AP_InertialSensor::_acal_event_failure()
{
for (uint8_t i=0; i<_accel_count; i++) {
AP_InertialSensor: add support for extra Aux IMUs
2023-02-17 00:26:27 -04:00
_accel_offset(i).set_and_notify(Vector3f(0,0,0));
_accel_scale(i).set_and_notify(Vector3f(1,1,1));
AP_InertialSensor: support AP_AccelCal
2015-07-20 17:25:40 -03:00
}
}
/*
AP_InertialSensor: sanitize includes Due to the way the headers are organized changing a single change in an inertial sensor driver would trigger a rebuild for most of the files in the project. Time could be saved by using ccache (since most of the things didn't change) but we can do better, i.e. re-organize the headers so we don't have to re-build everything. With this patch only AP_InertialSensor/AP_InertialSensor.h is exposed to most users. There are some corner cases to integrate with some example code, but most of the places now depend only on this header and this header doesn't depend on the specific backends. Now changing a single header, e.g. AP_InertialSensor_L3G4200D.h triggers a rebuild only of these files: $ waf copter 'copter' finished successfully (0.000s) Waf: Entering directory `/home/lucas/p/dronecode/ardupilot/build/minlure' [ 80/370] Compiling libraries/AP_InertialSensor/AP_InertialSensor.cpp [ 84/370] Compiling libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp [310/370] Linking build/minlure/ArduCopter/libArduCopter_libs.a [370/370] Linking build/minlure/bin/arducopter Waf: Leaving directory `/home/lucas/p/dronecode/ardupilot/build/minlure'
2016-01-19 21:26:31 -04:00
Returns true if new valid trim values are available and passes them to reference vars
AP_InertialSensor: support AP_AccelCal
2015-07-20 17:25:40 -03:00
*/
AP_InertialSensor: allow accelcal with a rotation
2021-08-12 19:57:43 -03:00
bool AP_InertialSensor::get_new_trim(Vector3f &trim_rad)
AP_InertialSensor: support AP_AccelCal
2015-07-20 17:25:40 -03:00
{
if (_new_trim) {
AP_InertialSensor: allow accelcal with a rotation
2021-08-12 19:57:43 -03:00
trim_rad = _trim_rad;
AP_InertialSensor: support AP_AccelCal
2015-07-20 17:25:40 -03:00
_new_trim = false;
return true;
}
return false;
}
AP_InertialSensor: sanitize includes Due to the way the headers are organized changing a single change in an inertial sensor driver would trigger a rebuild for most of the files in the project. Time could be saved by using ccache (since most of the things didn't change) but we can do better, i.e. re-organize the headers so we don't have to re-build everything. With this patch only AP_InertialSensor/AP_InertialSensor.h is exposed to most users. There are some corner cases to integrate with some example code, but most of the places now depend only on this header and this header doesn't depend on the specific backends. Now changing a single header, e.g. AP_InertialSensor_L3G4200D.h triggers a rebuild only of these files: $ waf copter 'copter' finished successfully (0.000s) Waf: Entering directory `/home/lucas/p/dronecode/ardupilot/build/minlure' [ 80/370] Compiling libraries/AP_InertialSensor/AP_InertialSensor.cpp [ 84/370] Compiling libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp [310/370] Linking build/minlure/ArduCopter/libArduCopter_libs.a [370/370] Linking build/minlure/bin/arducopter Waf: Leaving directory `/home/lucas/p/dronecode/ardupilot/build/minlure'
2016-01-19 21:26:31 -04:00
/*
AP_InertialSensor: support AP_AccelCal
2015-07-20 17:25:40 -03:00
Returns body fixed accelerometer level data averaged during accel calibration's first step
*/
bool AP_InertialSensor::get_fixed_mount_accel_cal_sample(uint8_t sample_num, Vector3f& ret) const
{
AP_InertialSensor: fixes in response to review
2015-12-30 02:51:27 -04:00
if (_accel_count <= (_acc_body_aligned-1) || _accel_calibrator[2].get_status() != ACCEL_CAL_SUCCESS || sample_num>=_accel_calibrator[2].get_num_samples_collected()) {
AP_InertialSensor: support AP_AccelCal
2015-07-20 17:25:40 -03:00
return false;
}
AP_InertialSensor: fixes in response to review
2015-12-30 02:51:27 -04:00
_accel_calibrator[_acc_body_aligned-1].get_sample_corrected(sample_num, ret);
AP_InertialSensor: remove custom orentations
2021-11-05 13:11:46 -03:00
ret.rotate(_board_orientation);
AP_InertialSensor: support AP_AccelCal
2015-07-20 17:25:40 -03:00
return true;
}
AP_InertialSensor: sanitize includes Due to the way the headers are organized changing a single change in an inertial sensor driver would trigger a rebuild for most of the files in the project. Time could be saved by using ccache (since most of the things didn't change) but we can do better, i.e. re-organize the headers so we don't have to re-build everything. With this patch only AP_InertialSensor/AP_InertialSensor.h is exposed to most users. There are some corner cases to integrate with some example code, but most of the places now depend only on this header and this header doesn't depend on the specific backends. Now changing a single header, e.g. AP_InertialSensor_L3G4200D.h triggers a rebuild only of these files: $ waf copter 'copter' finished successfully (0.000s) Waf: Entering directory `/home/lucas/p/dronecode/ardupilot/build/minlure' [ 80/370] Compiling libraries/AP_InertialSensor/AP_InertialSensor.cpp [ 84/370] Compiling libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp [310/370] Linking build/minlure/ArduCopter/libArduCopter_libs.a [370/370] Linking build/minlure/bin/arducopter Waf: Leaving directory `/home/lucas/p/dronecode/ardupilot/build/minlure'
2016-01-19 21:26:31 -04:00
/*
AP_InertialSensor: support AP_AccelCal
2015-07-20 17:25:40 -03:00
Returns Primary accelerometer level data averaged during accel calibration's first step
*/
bool AP_InertialSensor::get_primary_accel_cal_sample_avg(uint8_t sample_num, Vector3f& ret) const
{
uint8_t count = 0;
Vector3f avg = Vector3f(0,0,0);
AP_InertialSensor: minor formatting fixes No functional change
2016-12-23 23:21:29 -04:00
for (uint8_t i=0; i<MIN(_accel_count,2); i++) {
AP_InertialSensor: support AP_AccelCal
2015-07-20 17:25:40 -03:00
if (_accel_calibrator[i].get_status() != ACCEL_CAL_SUCCESS || sample_num>=_accel_calibrator[i].get_num_samples_collected()) {
continue;
}
Vector3f sample;
_accel_calibrator[i].get_sample_corrected(sample_num, sample);
avg += sample;
count++;
}
AP_InertialSensor: minor formatting fixes No functional change
2016-12-23 23:21:29 -04:00
if (count == 0) {
AP_InertialSensor: support AP_AccelCal
2015-07-20 17:25:40 -03:00
return false;
}
avg /= count;
ret = avg;
AP_InertialSensor: remove custom orentations
2021-11-05 13:11:46 -03:00
ret.rotate(_board_orientation);
AP_InertialSensor: support AP_AccelCal
2015-07-20 17:25:40 -03:00
return true;
}
AP_InertialSensor: added simple_accel_cal() this is useful for mass market vehicles where a full 3D accel cal is too complex.
2017-06-05 00:03:28 -03:00
AP_InertialSensor: Factor accel cal in from GCS_MAVLink
2022-10-04 17:42:03 -03:00
#if HAL_GCS_ENABLED
bool AP_InertialSensor::calibrate_gyros()
{
init_gyro();
if (!gyro_calibrated_ok_all()) {
return false;
}
AP::ahrs().reset_gyro_drift();
return true;
}
AP_InertialSensor: added simple_accel_cal() this is useful for mass market vehicles where a full 3D accel cal is too complex.
2017-06-05 00:03:28 -03:00
/*
perform a simple 1D accel calibration, returning mavlink result code
*/
AP_InertialSensor: use ahrs singleton for simple accelcal
2018-03-17 09:03:13 -03:00
MAV_RESULT AP_InertialSensor::simple_accel_cal()
AP_InertialSensor: added simple_accel_cal() this is useful for mass market vehicles where a full 3D accel cal is too complex.
2017-06-05 00:03:28 -03:00
{
AP_InertialSensor: Factor accel cal in from GCS_MAVLink
2022-10-04 17:42:03 -03:00
const uint32_t now = AP_HAL::millis();
if ((now - last_accel_cal_ms) < 5000) {
return MAV_RESULT_TEMPORARILY_REJECTED;
}
last_accel_cal_ms = now;
AP_InertialSensor: added simple_accel_cal() this is useful for mass market vehicles where a full 3D accel cal is too complex.
2017-06-05 00:03:28 -03:00
uint8_t num_accels = MIN(get_accel_count(), INS_MAX_INSTANCES);
Vector3f last_average[INS_MAX_INSTANCES];
Vector3f new_accel_offset[INS_MAX_INSTANCES];
Vector3f saved_offsets[INS_MAX_INSTANCES];
Vector3f saved_scaling[INS_MAX_INSTANCES];
bool converged[INS_MAX_INSTANCES];
const float accel_convergence_limit = 0.05;
Vector3f rotated_gravity(0, 0, -GRAVITY_MSS);
// exit immediately if calibration is already in progress
AP_InertialSensor: added IMU temperature calibration support this adds parameters that can be setup by an external script for compensating for temperature variation in gyros and accels using a 3rd order polynomial
2021-01-07 20:46:35 -04:00
if (calibrating()) {
AP_InertialSensor: added simple_accel_cal() this is useful for mass market vehicles where a full 3D accel cal is too complex.
2017-06-05 00:03:28 -03:00
return MAV_RESULT_TEMPORARILY_REJECTED;
}
AP_InertialSensor: rename to EXPECT_DELAY_MS()
2019-05-15 01:09:40 -03:00
EXPECT_DELAY_MS(20000);
AP_InertialSensor: added simple_accel_cal() this is useful for mass market vehicles where a full 3D accel cal is too complex.
2017-06-05 00:03:28 -03:00
// record we are calibrating
AP_InertialSensor: added IMU temperature calibration support this adds parameters that can be setup by an external script for compensating for temperature variation in gyros and accels using a 3rd order polynomial
2021-01-07 20:46:35 -04:00
_calibrating_accel = true;
AP_InertialSensor: added simple_accel_cal() this is useful for mass market vehicles where a full 3D accel cal is too complex.
2017-06-05 00:03:28 -03:00
// flash leds to tell user to keep the IMU still
AP_Notify::flags.initialising = true;
/*
we do the accel calibration with no board rotation. This avoids
having to rotate readings during the calibration
*/
enum Rotation saved_orientation = _board_orientation;
_board_orientation = ROTATION_NONE;
// get the rotated gravity vector which will need to be applied to the offsets
rotated_gravity.rotate_inverse(saved_orientation);
// save existing accel offsets
for (uint8_t k=0; k<num_accels; k++) {
AP_InertialSensor: add support for extra Aux IMUs
2023-02-17 00:26:27 -04:00
saved_offsets[k] = _accel_offset(k);
saved_scaling[k] = _accel_scale(k);
AP_InertialSensor: added simple_accel_cal() this is useful for mass market vehicles where a full 3D accel cal is too complex.
2017-06-05 00:03:28 -03:00
}
// remove existing accel offsets and scaling
for (uint8_t k=0; k<num_accels; k++) {
AP_InertialSensor: add support for extra Aux IMUs
2023-02-17 00:26:27 -04:00
_accel_offset(k).set(Vector3f());
_accel_scale(k).set(Vector3f(1,1,1));
AP_InertialSensor: added simple_accel_cal() this is useful for mass market vehicles where a full 3D accel cal is too complex.
2017-06-05 00:03:28 -03:00
new_accel_offset[k].zero();
last_average[k].zero();
converged[k] = false;
}
for (uint8_t c = 0; c < 5; c++) {
hal.scheduler->delay(5);
update();
}
// the strategy is to average 50 points over 0.5 seconds, then do it
// again and see if the 2nd average is within a small margin of
// the first
uint8_t num_converged = 0;
// we try to get a good calibration estimate for up to 10 seconds
// if the accels are stable, we should get it in 1 second
for (int16_t j = 0; j <= 10*4 && num_converged < num_accels; j++) {
Vector3f accel_sum[INS_MAX_INSTANCES], accel_avg[INS_MAX_INSTANCES], accel_diff[INS_MAX_INSTANCES];
float diff_norm[INS_MAX_INSTANCES];
uint8_t i;
memset(diff_norm, 0, sizeof(diff_norm));
AP_InertialSensor: Console output can be disabled
2022-03-21 06:25:18 -03:00
DEV_PRINTF("*");
AP_InertialSensor: added simple_accel_cal() this is useful for mass market vehicles where a full 3D accel cal is too complex.
2017-06-05 00:03:28 -03:00
for (uint8_t k=0; k<num_accels; k++) {
accel_sum[k].zero();
}
for (i=0; i<50; i++) {
update();
for (uint8_t k=0; k<num_accels; k++) {
accel_sum[k] += get_accel(k);
}
hal.scheduler->delay(5);
}
for (uint8_t k=0; k<num_accels; k++) {
accel_avg[k] = accel_sum[k] / i;
accel_diff[k] = last_average[k] - accel_avg[k];
diff_norm[k] = accel_diff[k].length();
}
for (uint8_t k=0; k<num_accels; k++) {
if (j > 0 && diff_norm[k] < accel_convergence_limit) {
last_average[k] = (accel_avg[k] * 0.5f) + (last_average[k] * 0.5f);
if (!converged[k] || last_average[k].length() < new_accel_offset[k].length()) {
new_accel_offset[k] = last_average[k];
}
if (!converged[k]) {
converged[k] = true;
num_converged++;
}
} else {
last_average[k] = accel_avg[k];
}
}
}
AP_InertialSensor: tighten type of simple_accel_cal result
2017-11-27 01:54:55 -04:00
MAV_RESULT result = MAV_RESULT_ACCEPTED;
AP_InertialSensor: added simple_accel_cal() this is useful for mass market vehicles where a full 3D accel cal is too complex.
2017-06-05 00:03:28 -03:00
// see if we've passed
for (uint8_t k=0; k<num_accels; k++) {
if (!converged[k]) {
result = MAV_RESULT_FAILED;
}
}
// restore orientation
_board_orientation = saved_orientation;
if (result == MAV_RESULT_ACCEPTED) {
AP_InertialSensor: Console output can be disabled
2022-03-21 06:25:18 -03:00
DEV_PRINTF("\nPASSED\n");
AP_InertialSensor: added simple_accel_cal() this is useful for mass market vehicles where a full 3D accel cal is too complex.
2017-06-05 00:03:28 -03:00
for (uint8_t k=0; k<num_accels; k++) {
// remove rotated gravity
new_accel_offset[k] -= rotated_gravity;
AP_InertialSensor: add support for extra Aux IMUs
2023-02-17 00:26:27 -04:00
_accel_offset(k).set_and_save(new_accel_offset[k]);
_accel_scale(k).save();
_accel_id(k).save();
AP_InertialSensor: added simple_accel_cal() this is useful for mass market vehicles where a full 3D accel cal is too complex.
2017-06-05 00:03:28 -03:00
_accel_id_ok[k] = true;
AP_InertialSensor: added IMU temperature calibration support this adds parameters that can be setup by an external script for compensating for temperature variation in gyros and accels using a 3rd order polynomial
2021-01-07 20:46:35 -04:00
#if HAL_INS_TEMPERATURE_CAL_ENABLE
AP_InertialSensor: add support for extra Aux IMUs
2023-02-17 00:26:27 -04:00
caltemp_accel(k).set_and_save(get_temperature(k));
AP_InertialSensor: added IMU temperature calibration support this adds parameters that can be setup by an external script for compensating for temperature variation in gyros and accels using a 3rd order polynomial
2021-01-07 20:46:35 -04:00
#endif
AP_InertialSensor: added simple_accel_cal() this is useful for mass market vehicles where a full 3D accel cal is too complex.
2017-06-05 00:03:28 -03:00
}
// force trim to zero
AP_InertialSensor: use ahrs singleton for simple accelcal
2018-03-17 09:03:13 -03:00
AP::ahrs().set_trim(Vector3f(0, 0, 0));
AP_InertialSensor: added simple_accel_cal() this is useful for mass market vehicles where a full 3D accel cal is too complex.
2017-06-05 00:03:28 -03:00
} else {
AP_InertialSensor: Console output can be disabled
2022-03-21 06:25:18 -03:00
DEV_PRINTF("\nFAILED\n");
AP_InertialSensor: added simple_accel_cal() this is useful for mass market vehicles where a full 3D accel cal is too complex.
2017-06-05 00:03:28 -03:00
// restore old values
for (uint8_t k=0; k<num_accels; k++) {
AP_InertialSensor: add support for extra Aux IMUs
2023-02-17 00:26:27 -04:00
_accel_offset(k).set(saved_offsets[k]);
_accel_scale(k).set(saved_scaling[k]);
AP_InertialSensor: added simple_accel_cal() this is useful for mass market vehicles where a full 3D accel cal is too complex.
2017-06-05 00:03:28 -03:00
}
}
// record calibration complete
AP_InertialSensor: added IMU temperature calibration support this adds parameters that can be setup by an external script for compensating for temperature variation in gyros and accels using a 3rd order polynomial
2021-01-07 20:46:35 -04:00
_calibrating_accel = false;
AP_InertialSensor: added simple_accel_cal() this is useful for mass market vehicles where a full 3D accel cal is too complex.
2017-06-05 00:03:28 -03:00
// throw away any existing samples that may have the wrong
// orientation. We do this by throwing samples away for 0.5s,
// which is enough time for the filters to settle
uint32_t start_ms = AP_HAL::millis();
while (AP_HAL::millis() - start_ms < 500) {
update();
}
// and reset state estimators
AP_InertialSensor: use ahrs singleton for simple accelcal
2018-03-17 09:03:13 -03:00
AP::ahrs().reset();
AP_InertialSensor: added simple_accel_cal() this is useful for mass market vehicles where a full 3D accel cal is too complex.
2017-06-05 00:03:28 -03:00
// stop flashing leds
AP_Notify::flags.initialising = false;
AP_InertialSensor: Factor accel cal in from GCS_MAVLink
2022-10-04 17:42:03 -03:00
last_accel_cal_ms = AP_HAL::millis ();
AP_InertialSensor: added simple_accel_cal() this is useful for mass market vehicles where a full 3D accel cal is too complex.
2017-06-05 00:03:28 -03:00
return result;
}
AP_InertialSensor: move from HAL_NO_GCS to HAL_GCS_ENABLED
2021-09-17 10:06:16 -03:00
#endif
AP_InertialSensor: add ins() to AP:: namespace
2017-11-13 03:09:43 -04:00
AP_InertialSensor: skip gyro cal on watchdog reset
2019-04-11 06:51:25 -03:00
/*
see if gyro calibration should be performed
*/
AP_InertialSensor::Gyro_Calibration_Timing AP_InertialSensor::gyro_calibration_timing()
{
if (hal.util->was_watchdog_reset()) {
return GYRO_CAL_NEVER;
}
return (Gyro_Calibration_Timing)_gyro_cal_timing.get();
}
AP_InertialSensor: create define specifically for the developer feature for killing IMUs
2023-06-05 23:12:12 -03:00
#if AP_INERTIALSENSOR_KILL_IMU_ENABLED
AP_InertialSensor: added RC switch for killing IMUs
2019-04-18 01:24:01 -03:00
/*
update IMU kill mask, used for testing IMU failover
*/
void AP_InertialSensor::kill_imu(uint8_t imu_idx, bool kill_it)
{
if (kill_it) {
uint8_t new_kill_mask = imu_kill_mask | (1U<<imu_idx);
// don't allow the last IMU to be killed
bool all_dead = true;
for (uint8_t i=0; i<MIN(_gyro_count, _accel_count); i++) {
if (use_gyro(i) && use_accel(i) && !(new_kill_mask & (1U<<i))) {
// we have at least one healthy IMU left
all_dead = false;
}
}
if (!all_dead) {
imu_kill_mask = new_kill_mask;
}
} else {
imu_kill_mask &= ~(1U<<imu_idx);
}
}
AP_InertialSensor: create define specifically for the developer feature for killing IMUs
2023-06-05 23:12:12 -03:00
#endif // AP_INERTIALSENSOR_KILL_IMU_ENABLED
AP_InertialSensor: added RC switch for killing IMUs
2019-04-18 01:24:01 -03:00
AP_InertialSensor: added support for writing raw IMU data to a UART this is for supporting external visual odomotry systems which need the IMU data to correctly process image data # Conflicts: # libraries/AP_InertialSensor/AP_InertialSensor.cpp
2023-11-27 23:20:19 -04:00
#if AP_SERIALMANAGER_IMUOUT_ENABLED
/*
setup a UART for sending external data
*/
void AP_InertialSensor::set_imu_out_uart(AP_HAL::UARTDriver *_uart)
{
uart.imu_out_uart = _uart;
uart.counter = 0;
}
/*
send IMU delta-angle and delta-velocity to a UART
*/
void AP_InertialSensor::send_uart_data(void)
{
struct {
uint16_t magic = 0x29c4;
uint16_t length;
uint32_t timestamp_us;
Vector3f delta_velocity;
Vector3f delta_angle;
float delta_velocity_dt;
float delta_angle_dt;
uint16_t counter;
uint16_t crc;
} data;
if (uart.imu_out_uart->txspace() < sizeof(data)) {
// not enough space
return;
}
data.length = sizeof(data);
data.timestamp_us = AP_HAL::micros();
get_delta_angle(get_primary_gyro(), data.delta_angle, data.delta_angle_dt);
get_delta_velocity(get_primary_accel(), data.delta_velocity, data.delta_velocity_dt);
data.counter = uart.counter++;
data.crc = crc_xmodem((const uint8_t *)&data, sizeof(data)-sizeof(uint16_t));
uart.imu_out_uart->write((const uint8_t *)&data, sizeof(data));
}
#endif // AP_SERIALMANAGER_IMUOUT_ENABLED
AP_InertialSensor: add ins() to AP:: namespace
2017-11-13 03:09:43 -04:00
AP_InertialSensor: added ExternalAHRS backend
2020-12-28 22:29:40 -04:00
#if HAL_EXTERNAL_AHRS_ENABLED
void AP_InertialSensor::handle_external(const AP_ExternalAHRS::ins_data_message_t &pkt)
{
for (uint8_t i = 0; i < _backend_count; i++) {
_backends[i]->handle_external(pkt);
}
}
#endif // HAL_EXTERNAL_AHRS_ENABLED
AP_InertialSensor: added force_save_calibration()
2021-02-10 20:49:08 -04:00
// force save of current calibration as valid
void AP_InertialSensor::force_save_calibration(void)
{
for (uint8_t i=0; i<_accel_count; i++) {
AP_InertialSensor: add support for extra Aux IMUs
2023-02-17 00:26:27 -04:00
if (_accel_id(i) != 0) {
_accel_id(i).save();
AP_InertialSensor: added force_save_calibration()
2021-02-10 20:49:08 -04:00
// we also save the scale as the default of 1.0 may be
// over a stored value of 0.0
AP_InertialSensor: add support for extra Aux IMUs
2023-02-17 00:26:27 -04:00
_accel_scale(i).save();
AP_InertialSensor: added force_save_calibration()
2021-02-10 20:49:08 -04:00
_accel_id_ok[i] = true;
}
}
}
AP_InertialSensor: add ins() to AP:: namespace
2017-11-13 03:09:43 -04:00
namespace AP {
AP_InertialSensor &ins()
{
AP_InertialSensor: unify singleton naming to _singleton and get_singleton()
2019-02-10 14:29:24 -04:00
return *AP_InertialSensor::get_singleton();
AP_InertialSensor: add ins() to AP:: namespace
2017-11-13 03:09:43 -04:00
}
};
AP_InertialSensor: add HAL_INS_ENABLED
2021-06-23 02:22:56 -03:00
AP_InertialSensor: rename HAL_INS_ENABLED to AP_INERTIALSENSOR_ENABLED
2022-12-22 21:27:14 -04:00
#endif // AP_INERTIALSENSOR_ENABLED