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

1967 lines
70 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: 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: 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>
#include <AP_Vehicle/AP_Vehicle.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: 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.h"
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: 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_HIL.h"
#include "AP_InertialSensor_L3G4200D.h"
#include "AP_InertialSensor_LSM9DS0.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_PX4.h"
#include "AP_InertialSensor_QURT.h"
#include "AP_InertialSensor_SITL.h"
#include "AP_InertialSensor_qflight.h"
AP_InertialSensor: add Robsense PhenixPro Devkit Board support
2017-10-30 06:03:04 -03:00
#include "AP_InertialSensor_RST.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: ported to AP_HAL
2012-10-11 21:27:19 -03:00
extern const AP_HAL::HAL& hal;
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
#if APM_BUILD_TYPE(APM_BUILD_ArduCopter)
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: 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
#elif APM_BUILD_TYPE(APM_BUILD_APMrover2)
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: 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
#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
#define SAMPLE_UNIT 1
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: 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: Added INS_MPU6K_FILTER option this allows the user to select the MPU6000 filtering frequency
2012-11-29 16:15:12 -04:00
// @Param: PRODUCT_ID
// @DisplayName: IMU Product ID
AP_InertialSensor: remove product_id We only leave the parameter there for backward-compatibility. However product id on the inertial sensor is not much useful since it's only kept for the first instance. A better implementation per-gyro and per-accel is needed in order to avoid problems with sensors taking the offsets configured for another sensor.
2016-09-03 12:37:47 -03:00
// @Description: unused
INS: move Product ID to @User Advanced
2013-11-26 09:18:28 -04:00
// @User: Advanced
AP_InertialSensor: remove product_id We only leave the parameter there for backward-compatibility. However product id on the inertial sensor is not much useful since it's only kept for the first instance. A better implementation per-gyro and per-accel is needed in order to avoid problems with sensors taking the offsets configured for another sensor.
2016-09-03 12:37:47 -03:00
AP_GROUPINFO("PRODUCT_ID", 0, AP_InertialSensor, _old_product_id, 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: 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
*/
// @Param: GYROFFS_X
// @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
// @Param: GYROFFS_Y
// @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
// @Param: GYROFFS_Z
// @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_GROUPINFO("GYROFFS", 3, AP_InertialSensor, _gyro_offset[0], 0),
// @Param: GYR2OFFS_X
// @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
// @Param: GYR2OFFS_Y
// @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
// @Param: GYR2OFFS_Z
// @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_GROUPINFO("GYR2OFFS", 7, AP_InertialSensor, _gyro_offset[1], 0),
// @Param: GYR3OFFS_X
// @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
// @Param: GYR3OFFS_Y
// @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
// @Param: GYR3OFFS_Z
// @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_GROUPINFO("GYR3OFFS", 10, AP_InertialSensor, _gyro_offset[2], 0),
AP_InertialSensor: added parameter descriptions
2013-01-02 03:31:58 -04:00
// @Param: ACCSCAL_X
// @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
// @Param: ACCSCAL_Y
// @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
// @Param: ACCSCAL_Z
// @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: 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_GROUPINFO("ACCSCAL", 12, AP_InertialSensor, _accel_scale[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: added parameter descriptions
2013-01-02 03:31:58 -04:00
// @Param: ACCOFFS_X
// @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
// @Param: ACCOFFS_Y
// @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
// @Param: ACCOFFS_Z
// @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: 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_GROUPINFO("ACCOFFS", 13, AP_InertialSensor, _accel_offset[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
INS: param descriptions for ACC2, GYR2
2014-09-27 09:05:33 -03:00
// @Param: ACC2SCAL_X
// @DisplayName: Accelerometer2 scaling of X axis
// @Description: Accelerometer2 scaling of X axis. Calculated during acceleration calibration routine
// @Range: 0.8 1.2
// @User: Advanced
// @Param: ACC2SCAL_Y
// @DisplayName: Accelerometer2 scaling of Y axis
// @Description: Accelerometer2 scaling of Y axis Calculated during acceleration calibration routine
// @Range: 0.8 1.2
// @User: Advanced
// @Param: ACC2SCAL_Z
// @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: 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_GROUPINFO("ACC2SCAL", 14, AP_InertialSensor, _accel_scale[1], 0),
INS: param descriptions for ACC2, GYR2
2014-09-27 09:05:33 -03:00
// @Param: ACC2OFFS_X
// @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
// @Param: ACC2OFFS_Y
// @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
// @Param: ACC2OFFS_Z
// @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: 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_GROUPINFO("ACC2OFFS", 15, AP_InertialSensor, _accel_offset[1], 0),
AP_InertialSensor: generalise the accel/gyro calibration for N sensors
2013-12-08 18:50:12 -04:00
INS: param descriptions for ACC2, GYR2
2014-09-27 09:05:33 -03:00
// @Param: ACC3SCAL_X
// @DisplayName: Accelerometer3 scaling of X axis
// @Description: Accelerometer3 scaling of X axis. Calculated during acceleration calibration routine
// @Range: 0.8 1.2
// @User: Advanced
// @Param: ACC3SCAL_Y
// @DisplayName: Accelerometer3 scaling of Y axis
// @Description: Accelerometer3 scaling of Y axis Calculated during acceleration calibration routine
// @Range: 0.8 1.2
// @User: Advanced
// @Param: ACC3SCAL_Z
// @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: 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_GROUPINFO("ACC3SCAL", 16, AP_InertialSensor, _accel_scale[2], 0),
INS: param descriptions for ACC2, GYR2
2014-09-27 09:05:33 -03:00
// @Param: ACC3OFFS_X
// @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
// @Param: ACC3OFFS_Y
// @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
// @Param: ACC3OFFS_Z
// @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: 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_GROUPINFO("ACC3OFFS", 17, AP_InertialSensor, _accel_offset[2], 0),
AP_InertialSensor: allow for 3 accels and 3 gyros on Pixhawk this supports FMUv3
2014-06-26 01:04:33 -03:00
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
// @Param: GYRO_FILTER
// @DisplayName: Gyro filter cutoff frequency
// @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. This option takes effect on the next reboot. A value of zero means no filtering (not recommended!)
// @Units: Hz
// @Range: 0 127
// @User: Advanced
AP_GROUPINFO("GYRO_FILTER", 18, AP_InertialSensor, _gyro_filter_cutoff, DEFAULT_GYRO_FILTER),
// @Param: ACCEL_FILTER
// @DisplayName: Accel filter cutoff frequency
// @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. This option takes effect on the next reboot. A value of zero means no filtering (not recommended!)
// @Units: Hz
// @Range: 0 127
// @User: Advanced
AP_GROUPINFO("ACCEL_FILTER", 19, AP_InertialSensor, _accel_filter_cutoff, DEFAULT_ACCEL_FILTER),
INS: add USE parameters
2015-08-01 03:40:40 -03:00
// @Param: USE
// @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_GROUPINFO("USE", 20, AP_InertialSensor, _use[0], 1),
// @Param: USE2
// @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_GROUPINFO("USE2", 21, AP_InertialSensor, _use[1], 1),
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
INS: add USE parameters
2015-08-01 03:40:40 -03:00
// @Param: USE3
// @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: correct USE param storage index USE, USE2, USE3 have 20, 21, 21 but should be 20, 21, 22
2015-08-20 20:09:18 -03:00
AP_GROUPINFO("USE3", 22, AP_InertialSensor, _use[2], 0),
INS: add USE parameters
2015-08-01 03:40:40 -03:00
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
// @Param: STILL_THRESH
// @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_GROUPINFO("STILL_THRESH", 23, AP_InertialSensor, _still_threshold, DEFAULT_STILL_THRESH),
InertialSensor: add GYR_CAL to control when gyro calibration occurs
2015-09-17 03:42:41 -03:00
// @Param: GYR_CAL
// @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_GROUPINFO("GYR_CAL", 24, AP_InertialSensor, _gyro_cal_timing, 1),
AP_InertialSensor: support AP_AccelCal
2015-07-20 17:25:40 -03: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_GROUPINFO("TRIM_OPTION", 25, AP_InertialSensor, _trim_option, 1),
AP_InertialSensor: support AP_AccelCal
2015-07-20 17:25:40 -03:00
AP_InertialSensor: fixes in response to review
2015-12-30 02:51:27 -04:00
// @Param: ACC_BODYFIX
// @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: support AP_AccelCal
2015-07-20 17:25:40 -03: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
// @Param: POS1_X
// @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
// @User: Advanced
// @Param: POS1_Y
// @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
// @User: Advanced
// @Param: POS1_Z
// @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
// @User: Advanced
AP_GROUPINFO("POS1", 27, AP_InertialSensor, _accel_pos[0], 0.0f),
// @Param: POS2_X
// @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
// @User: Advanced
// @Param: POS2_Y
// @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
// @User: Advanced
// @Param: POS2_Z
// @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
// @User: Advanced
AP_GROUPINFO("POS2", 28, AP_InertialSensor, _accel_pos[1], 0.0f),
// @Param: POS3_X
// @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
// @User: Advanced
// @Param: POS3_Y
// @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
// @User: Advanced
// @Param: POS3_Z
// @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
// @User: Advanced
AP_GROUPINFO("POS3", 29, AP_InertialSensor, _accel_pos[2], 0.0f),
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
// @Param: GYR_ID
// @DisplayName: Gyro ID
// @Description: Gyro sensor ID, taking into account its type, bus and instance
// @ReadOnly: True
// @User: Advanced
AP_GROUPINFO("GYR_ID", 30, AP_InertialSensor, _gyro_id[0], 0),
// @Param: GYR2_ID
// @DisplayName: Gyro2 ID
// @Description: Gyro2 sensor ID, taking into account its type, bus and instance
// @ReadOnly: True
// @User: Advanced
AP_GROUPINFO("GYR2_ID", 31, AP_InertialSensor, _gyro_id[1], 0),
// @Param: GYR3_ID
// @DisplayName: Gyro3 ID
// @Description: Gyro3 sensor ID, taking into account its type, bus and instance
// @ReadOnly: True
// @User: Advanced
AP_GROUPINFO("GYR3_ID", 32, AP_InertialSensor, _gyro_id[2], 0),
// @Param: ACC_ID
// @DisplayName: Accelerometer ID
// @Description: Accelerometer sensor ID, taking into account its type, bus and instance
// @ReadOnly: True
// @User: Advanced
AP_GROUPINFO("ACC_ID", 33, AP_InertialSensor, _accel_id[0], 0),
// @Param: ACC2_ID
// @DisplayName: Accelerometer2 ID
// @Description: Accelerometer2 sensor ID, taking into account its type, bus and instance
// @ReadOnly: True
// @User: Advanced
AP_GROUPINFO("ACC2_ID", 34, AP_InertialSensor, _accel_id[1], 0),
// @Param: ACC3_ID
// @DisplayName: Accelerometer3 ID
// @Description: Accelerometer3 sensor ID, taking into account its type, bus and instance
// @ReadOnly: True
// @User: Advanced
AP_GROUPINFO("ACC3_ID", 35, AP_InertialSensor, _accel_id[2], 0),
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
// @Param: FAST_SAMPLE
// @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
// @Values: 1:FirstIMUOnly,3:FirstAndSecondIMU
// @Bitmask: 0:FirstIMU,1:SecondIMU,2:ThirdIMU
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_GROUPINFO("FAST_SAMPLE", 36, AP_InertialSensor, _fast_sampling_mask, 0),
AP_InertialSensor: fix param docs for notch and log settings
2017-12-11 04:10:38 -04:00
// @Group: NOTCH_
// @Path: ../Filter/NotchFilter.cpp
AP_InertialSensor: implement notch filter on gyro
2017-08-16 02:27:58 -03:00
AP_SUBGROUPINFO(_notch_filter, "NOTCH_", 37, AP_InertialSensor, NotchFilterVector3fParam),
AP_InertialSensor: add IMU batch sampling
2017-10-03 20:44:07 -03:00
AP_InertialSensor: fix param docs for notch and log settings
2017-12-11 04:10:38 -04:00
// @Group: LOG_
// @Path: ../AP_InertialSensor/BatchSampler.cpp
AP_InertialSensor: add IMU batch sampling
2017-10-03 20:44:07 -03:00
AP_SUBGROUPINFO(batchsampler, "LOG_", 39, AP_InertialSensor, AP_InertialSensor::BatchSampler),
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
// @Group: ENABLE_MASK
// @DisplayName: IMU enable mask
// @Description: This is a bitmask of IMUs to enable. It can be used to prevent startup of specific detected IMUs
// @User: Advanced
// @Values: 1:FirstIMUOnly,3:FirstAndSecondIMU,7:FirstSecondAndThirdIMU,127:AllIMUs
// @Bitmask: 0:FirstIMU,1:SecondIMU,2:ThirdIMU
AP_GROUPINFO("ENABLE_MASK", 40, AP_InertialSensor, _enable_mask, 0x7F),
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: 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 *AP_InertialSensor::_s_instance = nullptr;
AP_InertialSensor: ensure parent class is initialised in instance classes
2013-04-12 01:30:35 -03:00
AP_InertialSensor::AP_InertialSensor() :
AP_InertialSensor: ported L3G4200D driver to new API
2014-10-15 20:55:18 -03:00
_gyro_count(0),
_accel_count(0),
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
_backend_count(0),
AP_InertialSensor: ensure parent class is initialised in instance classes
2013-04-12 01:30:35 -03:00
_accel(),
AP_InertialSensor: init members to reduce compiler warnings
2014-07-15 00:43:32 -03:00
_gyro(),
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: default primary accel and gyro
2015-05-01 03:29:01 -03:00
_primary_gyro(0),
_primary_accel(0),
AP_InertialSensor: resolve member init order compiler warning
2017-07-07 05:18:34 -03:00
_log_raw_bit(-1),
AP_InertialSensor: make calibrated() function fast enough to call in flight this will be used in plane to make AHRS SYS_STATUS unhealthy if a user tries to fly with EKF enabled without a full 3D accel cal. Note that it doesn't rely on using AP_Param load() to detect that a value has been set, as some users are first doing a 3D cal then later doing a 1D cal. In that case load() was returning true and would give a false positive
2015-01-02 02:38:28 -04:00
_hil_mode(false),
AP_InertialSensor: support raw accel and gyro dataflash logging
2015-05-06 23:08:30 -03:00
_calibrating(false),
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(false),
AP_InertialSensor: work around gyro and accel errors on startup
2015-04-30 06:15:36 -03:00
_accel_cal_requires_reboot(false),
_startup_error_counts_set(false),
_startup_ms(0)
AP_InertialSensor: ensure parent class is initialised in instance classes
2013-04-12 01:30:35 -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
if (_s_instance) {
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
}
_s_instance = 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: improved timing in all drivers
2014-10-15 20:32:40 -03:00
for (uint8_t i=0; i<INS_MAX_BACKENDS; i++) {
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
_backends[i] = nullptr;
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: 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++) {
_accel_error_count[i] = 0;
_gyro_error_count[i] = 0;
InertialSensor: add GYR_CAL to control when gyro calibration occurs
2015-09-17 03:42:41 -03:00
_gyro_cal_ok[i] = true;
InertialSensor: calc vibration and accel clipping
2015-06-11 04:15:57 -03:00
_accel_clip_count[i] = 0;
AP_InertialSensor: use accel instance max abs offset instead of a constant value The calibration on LSM9DS0 was giving offsets between 4.0 and 4.2 on x-axis and around 3.6 on y-axis. It turned out that those offsets were actually right. The maximum absolute values of calibration offset should be a sensor characteristic rather than a constant value for all sensors. The constant value previously used (3.5 m/s/s for all axes) is set here as a default maximum absolute calibration offset for every instance to keep it working.
2015-06-09 17:47:16 -03:00
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: allow publishing gyro raw sample rate to frontend That information will be used for a unified delta angle calculation.
2015-09-10 08:56:34 -03:00
AP_InertialSensor: use raw sample rate terminology In order to avoid confusion between sample rate from sensor and sample rate from the frontend class (AP_InertialSensor), use "raw sample rate" to refer to the former. The changes in the code were basically done with the following commands: git grep -wl _accel_sample_rates | xargs sed -i "s,\<_accel_sample_rates\>,_accel_raw_sample_rates,g" git grep -wl _set_accel_sample_rate | xargs sed -i "s,\<_set_accel_sample_rate\>,_set_accel_raw_sample_rate,g" git grep -wl _accel_sample_rate | xargs sed -i "s,\<_accel_sample_rate\>,_accel_raw_sample_rate,g" git grep -wl _gyro_sample_rates | xargs sed -i "s,\<_gyro_sample_rates\>,_gyro_raw_sample_rates,g" git grep -wl _set_gyro_sample_rate | xargs sed -i "s,\<_set_gyro_sample_rate\>,_set_gyro_raw_sample_rate,g" git grep -wl _gyro_sample_rate | xargs sed -i "s,\<_gyro_sample_rate\>,_gyro_raw_sample_rate,g" And also with minor changes on indentation and comments.
2015-09-10 09:56:28 -03:00
_accel_raw_sample_rates[i] = 0;
_gyro_raw_sample_rates[i] = 0;
AP_InertialSensor: unify delta velocity calculation This commit basically moves delta velocity calculation that was previously done in AP_InertialSensor_PX4 to a common place. Instances must publish their accel raw sample rate to enable delta velocity calculation.
2015-09-08 11:42:28 -03:00
_delta_velocity_acc[i].zero();
_delta_velocity_acc_dt[i] = 0;
AP_InertialSensor: unify delta angle calculation This commit basically moves delta angle calculation that was previously done in AP_InertialSensor_PX4 to a common place. Instances must publish their gyro raw sample rate to enable delta angle calculation.
2015-09-10 09:34:01 -03:00
_delta_angle_acc[i].zero();
AP_InertialSensor: added get_delta_angle_dt() API
2016-01-16 00:41:19 -04:00
_delta_angle_acc_dt[i] = 0;
AP_InertialSensor: unify delta angle calculation This commit basically moves delta angle calculation that was previously done in AP_InertialSensor_PX4 to a common place. Instances must publish their gyro raw sample rate to enable delta angle calculation.
2015-09-10 09:34:01 -03:00
_last_delta_angle[i].zero();
_last_raw_gyro[i].zero();
AP_InertialSensor: work around gyro and accel errors on startup
2015-04-30 06:15:36 -03:00
_accel_startup_error_count[i] = 0;
_gyro_startup_error_count[i] = 0;
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: allow backends to publish delta velocities and angles
2015-02-17 02:54:17 -04:00
memset(_delta_velocity_valid,0,sizeof(_delta_velocity_valid));
memset(_delta_angle_valid,0,sizeof(_delta_angle_valid));
AP_InertialSensor: statically register with AP_AccelCal
2016-01-04 18:42:22 -04:00
AP_AccelCal::register_client(this);
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 *AP_InertialSensor::get_instance()
{
AP_InertialSensor: minor formatting fixes No functional change
2016-12-23 23:21:29 -04:00
if (!_s_instance) {
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
_s_instance = new AP_InertialSensor();
AP_InertialSensor: minor formatting fixes No functional change
2016-12-23 23:21:29 -04: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
return _s_instance;
}
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: 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
uint8_t AP_InertialSensor::register_gyro(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: use millis/micros/panic functions
2015-11-19 23:11:52 -04:00
AP_HAL::panic("Too many gyros");
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: 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
bool saved = _gyro_id[_gyro_count].load();
if (saved && (uint32_t)_gyro_id[_gyro_count] != id) {
// inconsistent gyro id - mark it as needing calibration
_gyro_cal_ok[_gyro_count] = false;
}
_gyro_id[_gyro_count].set((int32_t) id);
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN || CONFIG_HAL_BOARD == HAL_BOARD_SITL
if (!saved) {
// assume this is the same sensor and save its ID to allow seamless
// transition from when we didn't have the IDs.
_gyro_id[_gyro_count].save();
}
#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
return _gyro_count++;
}
/*
register a new accel instance
*/
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
uint8_t AP_InertialSensor::register_accel(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: use millis/micros/panic functions
2015-11-19 23:11:52 -04:00
AP_HAL::panic("Too many accels");
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: 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
bool saved = _accel_id[_accel_count].load();
if (!saved) {
// inconsistent accel id
_accel_id_ok[_accel_count] = false;
} else if ((uint32_t)_accel_id[_accel_count] != id) {
// inconsistent accel id
_accel_id_ok[_accel_count] = false;
} else {
_accel_id_ok[_accel_count] = true;
}
_accel_id[_accel_count].set((int32_t) id);
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN || CONFIG_HAL_BOARD == HAL_BOARD_SITL
// 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;
_accel_id[_accel_count].save();
#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
return _accel_count++;
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++) {
_accel_id[i].set(0);
}
for (uint8_t i=get_gyro_count(); i<INS_MAX_INSTANCES; i++) {
_gyro_id[i].set(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
}
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
{
assert(_backends_detected);
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: 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: re-work for more flexible main loop rates
2015-12-25 23:11:33 -04:00
AP_InertialSensor::init(uint16_t sample_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
_sample_rate = sample_rate;
AP_InertialSensor: added get_loop_delta_t() API
2015-12-25 22:34:34 -04:00
_loop_delta_t = 1.0f / sample_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: 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
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
// initialise accel scale if need be. This is needed as we can't
// give non-zero default values for vectors in AP_Param
AP_InertialSensor: generalise the accel/gyro calibration for N sensors
2013-12-08 18:50:12 -04:00
for (uint8_t i=0; i<get_accel_count(); i++) {
if (_accel_scale[i].get().is_zero()) {
_accel_scale[i].set(Vector3f(1,1,1));
}
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: re-work for more flexible main loop rates
2015-12-25 23:11:33 -04:00
_sample_period_usec = 1000*1000UL / _sample_rate;
AP_InertialSensor: improved timing in all drivers
2014-10-15 20:32:40 -03:00
AP_InertialSensor: implement notch filter on gyro
2017-08-16 02:27:58 -03:00
_notch_filter.init(sample_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
// initialise IMU batch logging
batchsampler.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
}
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: 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: 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
uint8_t probe_count = 0;
uint8_t enable_mask = uint8_t(_enable_mask.get());
uint8_t found_mask = 0;
/*
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: load only HIL backend for hil_mode
2015-03-13 08:32:52 -03:00
if (_hil_mode) {
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_HIL::detect(*this));
AP_InertialSensor: load only HIL backend for hil_mode
2015-03-13 08:32:52 -03:00
return;
}
AP_InertialSensor: created a SITL specific backend
2015-11-16 00:09:37 -04:00
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
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_SITL::detect(*this));
AP_InertialSensor: created a SITL specific backend
2015-11-16 00:09:37 -04:00
#elif HAL_INS_DEFAULT == HAL_INS_HIL
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_HIL::detect(*this));
AP_InertialSensor: use AP_HAL rotations for MPU6000
2016-11-08 21:36:47 -04:00
#elif HAL_INS_DEFAULT == HAL_INS_MPU60XX_SPI && defined(HAL_INS_DEFAULT_ROTATION)
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),
AP_InertialSensor: fixed typo
2016-11-09 01:23:27 -04:00
HAL_INS_DEFAULT_ROTATION));
AP_InertialSensor: add i2c bus driver for MPU6050
2015-06-15 09:58:30 -03:00
#elif HAL_INS_DEFAULT == HAL_INS_MPU60XX_SPI
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)));
AP_InertialSensor: use AP_HAL rotations for MPU6000
2016-11-08 21:36:47 -04:00
#elif HAL_INS_DEFAULT == HAL_INS_MPU60XX_I2C && defined(HAL_INS_DEFAULT_ROTATION)
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.i2c_mgr->get_device(HAL_INS_MPU60x0_I2C_BUS, HAL_INS_MPU60x0_I2C_ADDR),
AP_InertialSensor: fixed typo
2016-11-09 01:23:27 -04:00
HAL_INS_DEFAULT_ROTATION));
AP_InertialSensor: MPU60x0: use AP_HAL::Device abstraction
2016-01-12 14:22:11 -04:00
#elif HAL_INS_DEFAULT == HAL_INS_MPU60XX_I2C
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.i2c_mgr->get_device(HAL_INS_MPU60x0_I2C_BUS, HAL_INS_MPU60x0_I2C_ADDR)));
AP_InertialSensor: add support to BH hat - MPU6050 I2C for BH v0.1 - MPU9250 SPI for BH v0.2
2015-11-28 05:36:37 -04:00
#elif HAL_INS_DEFAULT == HAL_INS_BH
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.i2c_mgr->get_device(HAL_INS_MPU60x0_I2C_BUS, HAL_INS_MPU60x0_I2C_ADDR)));
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME)));
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: 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)));
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));
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_PIXRACER:
AP_InertialSensor: disable fast sampling on 2nd IMU on Pixracer it also hosts a mag, and gets lots of FIFO resets if we try to run it at 8kHz
2018-01-11 00:59:46 -04:00
// only do fast samplng on ICM-20608. The MPU9250 doesn't handle high rate well when it has a mag enabled
_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_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: 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: 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;
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: support ChibiOS
2018-01-05 03:08:10 -04:00
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
AP_InertialSensor: allow for in-tree drivers for PX4
2016-11-01 07:34:59 -03:00
// also add any PX4 backends (eg. canbus sensors)
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_PX4::detect(*this));
AP_InertialSensor: support ChibiOS
2018-01-05 03:08:10 -04:00
#endif
AP_InertialSensor: simplify rotations for MPU9250
2016-11-05 02:03:31 -03:00
#elif HAL_INS_DEFAULT == HAL_INS_MPU9250_SPI && defined(HAL_INS_DEFAULT_ROTATION)
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), HAL_INS_DEFAULT_ROTATION));
AP_InertialSensor: MPU9250: use AP_HAL::Device abstraction This makes MPU9250 be almost the same as MPU6000 driver. Work has been done here to make than similar so it's easier to spot the differences.
2016-01-20 18:20:35 -04:00
#elif HAL_INS_DEFAULT == HAL_INS_MPU9250_SPI
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)));
AP_InertialSensor: use ICM backend for Edge
2017-10-18 14:07:59 -03:00
#elif HAL_INS_DEFAULT == HAL_INS_EDGE
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_YAW_90));
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME_EXT), ROTATION_YAW_90));
AP_InertialSensor: add LSM9DS0 backend This adds the backend driver for LSM9DS0. This implementation is based on the legacy driver coded by Víctor Mayoral Vilches (under folder LSM9DS0) and makes some necessary adaptations and fixes in order to work properly. The legacy driver folder was removed.
2015-05-13 19:39:03 -03:00
#elif HAL_INS_DEFAULT == HAL_INS_LSM9DS0
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_LSM9DS0::probe(*this,
Corrects a bracket error for the LSM9DS0 IMU Switches the probe of the accel and gyro so they boot correctly (was failing the WHOAMI with a switched result)
2016-02-25 02:52:10 -04:00
hal.spi->get_device(HAL_INS_LSM9DS0_G_NAME),
hal.spi->get_device(HAL_INS_LSM9DS0_A_NAME)));
AP_InertialSensor: L3G4200D: add probe code This driver works properly but had the initialization logic missing. Add the support to probe it.
2015-02-23 12:46:16 -04:00
#elif HAL_INS_DEFAULT == HAL_INS_L3G4200D
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_L3G4200D::probe(*this, hal.i2c_mgr->get_device(HAL_INS_L3G4200D_I2C_BUS, HAL_INS_L3G4200D_I2C_ADDR)));
AP_InertialSensor: Add MPU9250 over I2C to detect_backends()
2015-09-28 13:52:01 -03:00
#elif HAL_INS_DEFAULT == HAL_INS_MPU9250_I2C
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.i2c_mgr->get_device(HAL_INS_MPU9250_I2C_BUS, HAL_INS_MPU9250_I2C_ADDR)));
AP_InertialSensor: added qflight driver
2015-12-13 23:20:06 -04:00
#elif HAL_INS_DEFAULT == HAL_INS_QFLIGHT
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_QFLIGHT::detect(*this));
AP_InertialSensor: added QURT driver
2015-12-09 17:48:25 -04:00
#elif HAL_INS_DEFAULT == HAL_INS_QURT
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_QURT::detect(*this));
AP_InertialSensor: Add second MPU9250 autodetection to BBBmini
2016-04-24 05:24:37 -03:00
#elif HAL_INS_DEFAULT == HAL_INS_BBBMINI
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)));
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME_EXT)));
AP_InertialSensor: use BMI160 for Intel Aero
2016-06-23 12:30:54 -03:00
#elif HAL_INS_DEFAULT == HAL_INS_AERO
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_BMI160::probe(*this, hal.spi->get_device("bmi160")));
AP_InertialSensor: add Robsense PhenixPro Devkit Board support
2017-10-30 06:03:04 -03:00
#elif HAL_INS_DEFAULT == HAL_INS_RST
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_RST::probe(*this, hal.spi->get_device(HAL_INS_RST_G_NAME),
hal.spi->get_device(HAL_INS_RST_A_NAME),
HAL_INS_DEFAULT_G_ROTATION,
HAL_INS_DEFAULT_A_ROTATION));
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: 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
_enable_mask.set(found_mask);
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: if no sensors found allow parameter change
2018-01-17 23:51:09 -04:00
AP_BoardConfig::sensor_config_error("INS: unable to initialise driver");
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()
{
batchsampler.periodic();
}
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: fixed AHRS_TRIM calculation again
2015-05-15 18:18:09 -03:00
*/
bool AP_InertialSensor::_calculate_trim(const Vector3f &accel_sample, float& trim_roll, float& trim_pitch)
{
AP_Math: Replace the pythagorous* functions with a variadic template The new function can deal with a variable number of function parameters. Additionally, I renamed the functions to norm(), because this is the standard name used in several other projects.
2016-04-16 06:58:46 -03:00
trim_pitch = atan2f(accel_sample.x, norm(accel_sample.y, accel_sample.z));
AP_InertialSensor: using atan2f() gives more accurate euler corrections thanks to Jon and Paul!
2015-05-15 19:19:18 -03:00
trim_roll = atan2f(-accel_sample.y, -accel_sample.z);
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
if (fabsf(trim_roll) > radians(10) ||
AP_InertialSensor: fixed AHRS_TRIM calculation again
2015-05-15 18:18:09 -03:00
fabsf(trim_pitch) > radians(10)) {
AP_InertialSensor: Unify from print or println to printf.
2017-01-21 00:39:09 -04:00
hal.console->printf("trim over maximum of 10 degrees\n");
AP_InertialSensor: fixed AHRS_TRIM calculation again
2015-05-15 18:18:09 -03:00
return false;
}
Replace use of UARTDriver::printf_P() with UARTDriver::printf() This also starts to show warnings on places that were already using wrong printf format strings.
2015-10-25 17:10:41 -03:00
hal.console->printf("Trim OK: roll=%.2f pitch=%.2f\n",
AP_InertialSensor: fixed build warnings
2015-05-30 09:51:19 -03:00
(double)degrees(trim_roll),
(double)degrees(trim_pitch));
AP_InertialSensor: fixed AHRS_TRIM calculation again
2015-05-15 18:18:09 -03:00
return true;
}
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
}
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++) {
if (_gyro_id[i] != 0) {
// 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;
}
return (get_gyro_health(instance) && _use[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_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: 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
*/
bool AP_InertialSensor::calibrate_trim(float &trim_roll, float &trim_pitch)
{
Vector3f level_sample;
// exit immediately if calibration is already in progress
if (_calibrating) {
return false;
}
_calibrating = true;
const uint8_t update_dt_milliseconds = (uint8_t)(1000.0f/get_sample_rate()+0.5f);
// 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);
}
uint32_t num_samples = 0;
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)) {
goto failed;
}
hal.scheduler->delay(update_dt_milliseconds);
num_samples++;
}
level_sample /= num_samples;
if (!_calculate_trim(level_sample, trim_roll, trim_pitch)) {
goto failed;
}
_calibrating = false;
return true;
failed:
_calibrating = 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
return false;
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
{
AP_IntertialSensor: don't require acceleration calibration in hil mode
2015-05-21 09:27:18 -03:00
// calibration is not applicable for HIL mode
AP_InertialSensor: minor formatting fixes No functional change
2016-12-23 23:21:29 -04:00
if (_hil_mode) {
AP_IntertialSensor: don't require acceleration calibration in hil mode
2015-05-21 09:27:18 -03:00
return true;
AP_InertialSensor: minor formatting fixes No functional change
2016-12-23 23:21:29 -04:00
}
AP_IntertialSensor: don't require acceleration calibration in hil mode
2015-05-21 09:27:18 -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 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
if (_accel_offset[i].get().is_zero()) {
return false;
}
// zero scaling also indicates not calibrated
if (_accel_scale[i].get().is_zero()) {
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++) {
if (_accel_id[i] != 0) {
// 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++) {
const Vector3f &scaling = _accel_scale[i].get();
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));
bool have_offsets = !_accel_offset[i].get().is_zero();
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;
}
return (get_accel_health(instance) && _use[instance]);
}
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: 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
if (_calibrating) {
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
_calibrating = true;
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: Unify from print or println to printf.
2017-01-21 00:39:09 -04:00
hal.console->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: try to avoid a compiler fault in travis
2015-03-10 20:39:47 -03: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
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
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: Unify from print or println to printf.
2017-01-21 00:39:09 -04:00
hal.console->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: Unify from print or println to printf.
2017-01-21 00:39:09 -04:00
hal.console->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: inform maximum gyro average difference While at it, define GYRO_INIT_MAX_DIFF_DPS.
2016-06-17 18:00:29 -03:00
hal.console->printf("gyro[%u] did not converge: diff=%f dps (expected < %f)\n",
(unsigned)k,
(double)ToDeg(best_diff[k]),
(double)GYRO_INIT_MAX_DIFF_DPS);
InertialSensor: reorder .cpp file to match .h No functional changes
2014-09-01 03:28:07 -03:00
_gyro_offset[k] = 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: 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
_gyro_offset[k] = new_gyro_offset[k];
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
_calibrating = false;
// stop flashing leds
AP_Notify::flags.initialising = false;
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++) {
InertialSensor: reorder .cpp file to match .h No functional changes
2014-09-01 03:28:07 -03:00
_gyro_offset[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
_gyro_id[i].save();
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++) {
_gyro_offset[i].set_and_save(Vector3f());
_gyro_id[i].set_and_save(0);
}
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
/*
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();
if (!_hil_mode) {
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: unify delta velocity calculation This commit basically moves delta velocity calculation that was previously done in AP_InertialSensor_PX4 to a common place. Instances must publish their accel raw sample rate to enable delta velocity calculation.
2015-09-08 11:42:28 -03:00
// clear accumulators
for (uint8_t i = 0; i < INS_MAX_INSTANCES; i++) {
_delta_velocity_acc[i].zero();
_delta_velocity_acc_dt[i] = 0;
AP_InertialSensor: unify delta angle calculation This commit basically moves delta angle calculation that was previously done in AP_InertialSensor_PX4 to a common place. Instances must publish their gyro raw sample rate to enable delta angle calculation.
2015-09-10 09:34:01 -03:00
_delta_angle_acc[i].zero();
AP_InertialSensor: added get_delta_angle_dt() API
2016-01-16 00:41:19 -04:00
_delta_angle_acc_dt[i] = 0;
AP_InertialSensor: unify delta velocity calculation This commit basically moves delta velocity calculation that was previously done in AP_InertialSensor_PX4 to a common place. Instances must publish their accel raw sample rate to enable delta velocity calculation.
2015-09-08 11:42:28 -03: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++) {
INS: add USE parameters
2015-08-01 03:40:40 -03: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++) {
INS: add USE parameters
2015-08-01 03:40:40 -03: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: implement notch filter on gyro
2017-08-16 02:27:58 -03:00
// apply notch filter to primary gyro
_gyro[_primary_gyro] = _notch_filter.apply(_gyro[_primary_gyro]);
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: 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:
if (!_hil_mode) {
AP_InertialSensor: simplify next sample time calculations this makes the calculation much clearer
2014-10-19 20:46:02 -03:00
// we also wait for at least one backend to have a sample of both
AP_InertialSensor: improved timing in all drivers
2014-10-15 20:32:40 -03:00
// accel and gyro. This normally completes immediately.
bool gyro_available = false;
bool accel_available = false;
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: 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: 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
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
gyro_available |= _new_gyro_data[i];
accel_available |= _new_accel_data[i];
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
if (gyro_available && accel_available) {
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
hal.scheduler->delay_microseconds(100);
AP_InertialSensor: converted PX4 driver to new API
2014-10-15 05:54:30 -03:00
}
AP_InertialSensor: converted HIL backend, which gets SITL working
2014-10-15 02:37:59 -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: support HIL functions for delta_velocity and delta_angles
2015-06-15 05:13:06 -03:00
if (_hil_mode && _hil.delta_time > 0) {
_delta_time = _hil.delta_time;
_hil.delta_time = 0;
} else {
_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: 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
bool AP_InertialSensor::get_delta_angle(uint8_t i, Vector3f &delta_angle) const
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
*/
bool AP_InertialSensor::get_delta_velocity(uint8_t i, Vector3f &delta_velocity) const
{
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;
}
/*
return delta_time for the delta_velocity
*/
float AP_InertialSensor::get_delta_velocity_dt(uint8_t i) const
{
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
float ret;
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]) {
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
ret = _delta_velocity_dt[i];
} else {
ret = get_delta_time();
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: 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
ret = MIN(ret, _loop_delta_t_max);
return ret;
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: added get_delta_angle_dt() API
2016-01-16 00:41:19 -04:00
/*
return delta_time for the delta_angle
*/
float AP_InertialSensor::get_delta_angle_dt(uint8_t i) const
{
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
float ret;
AP_InertialSensor: cope with zero delta angle time from Replay
2016-04-26 02:37:11 -03:00
if (_delta_angle_valid[i] && _delta_angle_dt[i] > 0) {
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
ret = _delta_angle_dt[i];
} else {
ret = get_delta_time();
AP_InertialSensor: added get_delta_angle_dt() API
2016-01-16 00:41:19 -04: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
ret = MIN(ret, _loop_delta_t_max);
return ret;
AP_InertialSensor: added get_delta_angle_dt() API
2016-01-16 00:41:19 -04:00
}
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: converted HIL backend, which gets SITL working
2014-10-15 02:37:59 -03:00
/*
support for setting accel and gyro vectors, for use by HIL
*/
void AP_InertialSensor::set_accel(uint8_t instance, const Vector3f &accel)
{
AP_InertialSensor: cope with 2 IMUs in SITL
2015-01-20 04:47:45 -04:00
if (_accel_count == 0) {
// we haven't initialised yet
return;
}
AP_InertialSensor: converted HIL backend, which gets SITL working
2014-10-15 02:37:59 -03:00
if (instance < INS_MAX_INSTANCES) {
_accel[instance] = accel;
AP_InertialSensor: fixed SITL with new HIL code
2014-10-24 01:05:44 -03:00
_accel_healthy[instance] = true;
AP_InertialSensor: fixed 2nd IMU in Replay this makes for much more accurate replay runs
2015-01-19 18:20:47 -04:00
if (_accel_count <= instance) {
_accel_count = instance+1;
}
AP_InertialSensor: fixed primary accel/gyro in replay
2015-05-10 22:16:57 -03:00
if (!_accel_healthy[_primary_accel]) {
_primary_accel = instance;
}
AP_InertialSensor: converted HIL backend, which gets SITL working
2014-10-15 02:37:59 -03:00
}
}
void AP_InertialSensor::set_gyro(uint8_t instance, const Vector3f &gyro)
{
AP_InertialSensor: cope with 2 IMUs in SITL
2015-01-20 04:47:45 -04:00
if (_gyro_count == 0) {
// we haven't initialised yet
return;
}
AP_InertialSensor: converted HIL backend, which gets SITL working
2014-10-15 02:37:59 -03:00
if (instance < INS_MAX_INSTANCES) {
_gyro[instance] = gyro;
AP_InertialSensor: fixed SITL with new HIL code
2014-10-24 01:05:44 -03:00
_gyro_healthy[instance] = true;
AP_InertialSensor: fixed 2nd IMU in Replay this makes for much more accurate replay runs
2015-01-19 18:20:47 -04:00
if (_gyro_count <= instance) {
_gyro_count = instance+1;
AP_InertialSensor: cope with 2 IMUs in SITL
2015-01-20 04:47:45 -04:00
_gyro_cal_ok[instance] = true;
AP_InertialSensor: fixed 2nd IMU in Replay this makes for much more accurate replay runs
2015-01-19 18:20:47 -04:00
}
AP_InertialSensor: fixed primary accel/gyro in replay
2015-05-10 22:16:57 -03:00
if (!_accel_healthy[_primary_accel]) {
_primary_accel = instance;
}
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: converted HIL backend, which gets SITL working
2014-10-15 02:37:59 -03:00
AP_InertialSensor: support HIL functions for delta_velocity and delta_angles
2015-06-15 05:13:06 -03:00
/*
set delta time for next ins.update()
*/
void AP_InertialSensor::set_delta_time(float delta_time)
{
_hil.delta_time = delta_time;
}
/*
set delta velocity for next update
*/
void AP_InertialSensor::set_delta_velocity(uint8_t instance, float deltavt, const Vector3f &deltav)
{
if (instance < INS_MAX_INSTANCES) {
_delta_velocity_valid[instance] = true;
_delta_velocity[instance] = deltav;
_delta_velocity_dt[instance] = deltavt;
}
}
/*
set delta angle for next update
*/
AP_InertialSensor: added set of delta angle time for replay
2016-04-26 02:50:46 -03:00
void AP_InertialSensor::set_delta_angle(uint8_t instance, const Vector3f &deltaa, float deltaat)
AP_InertialSensor: support HIL functions for delta_velocity and delta_angles
2015-06-15 05:13:06 -03:00
{
if (instance < INS_MAX_INSTANCES) {
_delta_angle_valid[instance] = true;
_delta_angle[instance] = deltaa;
AP_InertialSensor: added set of delta angle time for replay
2016-04-26 02:50:46 -03:00
_delta_angle_dt[instance] = deltaat;
AP_InertialSensor: support HIL functions for delta_velocity and delta_angles
2015-06-15 05:13:06 -03: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: 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
if (fabsf(accel.x) > AP_INERTIAL_SENSOR_ACCEL_CLIP_THRESH_MSS ||
fabsf(accel.y) > AP_INERTIAL_SENSOR_ACCEL_CLIP_THRESH_MSS ||
fabsf(accel.z) > AP_INERTIAL_SENSOR_ACCEL_CLIP_THRESH_MSS) {
_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
// 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;
}
_acal->update();
if (hal.util->get_soft_armed() && _acal->get_status() != ACCEL_CAL_NOT_STARTED) {
_acal->cancel();
}
}
/*
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);
_accel_offset[i].set_and_save(bias);
_accel_scale[i].set_and_save(gain);
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[i].save();
_accel_id_ok[i] = true;
AP_InertialSensor: support AP_AccelCal
2015-07-20 17:25:40 -03:00
} else {
AP_InertialSensor: detect missing accels or gyros
2016-11-05 06:20:43 -03:00
_accel_offset[i].set_and_save(Vector3f());
_accel_scale[i].set_and_save(Vector3f());
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++) {
_accel_id[i].set_and_save(0);
_accel_offset[i].set_and_save(Vector3f());
_accel_scale[i].set_and_save(Vector3f());
}
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_Math: Replace the pythagorous* functions with a variadic template The new function can deal with a variable number of function parameters. Additionally, I renamed the functions to norm(), because this is the standard name used in several other projects.
2016-04-16 06:58:46 -03:00
_trim_pitch = atan2f(aligned_sample.x, norm(aligned_sample.y, aligned_sample.z));
AP_InertialSensor: support AP_AccelCal
2015-07-20 17:25:40 -03:00
_trim_roll = atan2f(-aligned_sample.y, -aligned_sample.z);
_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();
_trim_roll = q.get_euler_roll();
_trim_pitch = q.get_euler_pitch();
_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: 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
if (fabsf(_trim_roll) > radians(10) ||
AP_InertialSensor: support AP_AccelCal
2015-07-20 17:25:40 -03:00
fabsf(_trim_pitch) > radians(10)) {
AP_InertialSensor: Unify from print or println to printf.
2017-01-21 00:39:09 -04:00
hal.console->printf("ERR: Trim over maximum of 10 degrees!!");
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: on accel cal failure don't save values to zero When the accel calibration fails leave the previous values saved but set them to defaults (scale default is ones, not zeros) and notify the GCS This fixes an arithmetic exception when doing a second accel cal after the first one failed
2016-11-12 06:36:33 -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
*/
bool AP_InertialSensor::get_new_trim(float& trim_roll, float &trim_pitch)
{
if (_new_trim) {
trim_roll = _trim_roll;
trim_pitch = _trim_pitch;
_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: support AP_AccelCal
2015-07-20 17:25:40 -03:00
ret.rotate(_board_orientation);
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;
ret.rotate(_board_orientation);
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: tighten type of simple_accel_cal result
2017-11-27 01:54:55 -04:00
MAV_RESULT AP_InertialSensor::simple_accel_cal(AP_AHRS &ahrs)
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
if (_calibrating) {
return MAV_RESULT_TEMPORARILY_REJECTED;
}
// record we are calibrating
_calibrating = true;
// flash leds to tell user to keep the IMU still
AP_Notify::flags.initialising = true;
hal.console->printf("Simple accel cal");
/*
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++) {
saved_offsets[k] = _accel_offset[k];
saved_scaling[k] = _accel_scale[k];
}
// remove existing accel offsets and scaling
for (uint8_t k=0; k<num_accels; k++) {
_accel_offset[k].set(Vector3f());
_accel_scale[k].set(Vector3f(1,1,1));
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));
hal.console->printf("*");
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) {
hal.console->printf("\nPASSED\n");
for (uint8_t k=0; k<num_accels; k++) {
// remove rotated gravity
new_accel_offset[k] -= rotated_gravity;
_accel_offset[k].set_and_save(new_accel_offset[k]);
_accel_scale[k].save();
_accel_id[k].save();
_accel_id_ok[k] = true;
}
// force trim to zero
ahrs.set_trim(Vector3f(0, 0, 0));
} else {
hal.console->printf("\nFAILED\n");
// restore old values
for (uint8_t k=0; k<num_accels; k++) {
_accel_offset[k] = saved_offsets[k];
_accel_scale[k] = saved_scaling[k];
}
}
// record calibration complete
_calibrating = false;
// 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
ahrs.reset();
// stop flashing leds
AP_Notify::flags.initialising = false;
return result;
}