2015-08-11 03:28:42 -03:00
# include <AP_HAL/AP_HAL.h>
2016-03-23 20:10:21 -03:00
# if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
# include <AP_HAL_Linux/I2CDevice.h>
# endif
2015-08-11 03:28:42 -03:00
# include <AP_Vehicle/AP_Vehicle.h>
2016-10-31 22:20:31 -03:00
# include <AP_BoardConfig/AP_BoardConfig.h>
2015-02-23 19:17:44 -04:00
2017-06-21 14:39:07 -03:00
# include "AP_Compass_SITL.h"
2016-03-10 20:41:18 -04:00
# include "AP_Compass_AK8963.h"
# include "AP_Compass_Backend.h"
2016-07-05 11:54:25 -03:00
# include "AP_Compass_BMM150.h"
2016-03-10 20:41:18 -04:00
# include "AP_Compass_HIL.h"
# include "AP_Compass_HMC5843.h"
2017-02-10 19:33:23 -04:00
# include "AP_Compass_IST8310.h"
2016-03-10 20:41:18 -04:00
# include "AP_Compass_LSM303D.h"
2016-04-22 12:12:23 -03:00
# include "AP_Compass_LSM9DS1.h"
2016-11-24 02:54:44 -04:00
# include "AP_Compass_LIS3MDL.h"
2016-11-24 03:46:47 -04:00
# include "AP_Compass_AK09916.h"
2017-06-16 02:50:09 -03:00
# include "AP_Compass_QMC5883L.h"
2017-04-02 11:56:15 -03:00
# if HAL_WITH_UAVCAN
2017-05-06 06:13:19 -03:00
# include <AP_BoardConfig/AP_BoardConfig_CAN.h>
2017-04-02 11:56:15 -03:00
# include "AP_Compass_UAVCAN.h"
# endif
2016-12-19 02:53:48 -04:00
# include "AP_Compass_MMC3416.h"
2018-02-02 05:25:24 -04:00
# include "AP_Compass_MAG3110.h"
2016-03-10 20:41:18 -04:00
# include "AP_Compass.h"
2015-02-23 19:17:44 -04:00
extern AP_HAL : : HAL & hal ;
2016-12-09 19:15:12 -04:00
# if APM_BUILD_TYPE(APM_BUILD_ArduCopter) || APM_BUILD_TYPE(APM_BUILD_ArduSub)
2016-04-28 05:51:06 -03:00
# define COMPASS_LEARN_DEFAULT Compass::LEARN_NONE
2015-02-23 19:17:44 -04:00
# else
2016-04-28 05:51:06 -03:00
# define COMPASS_LEARN_DEFAULT Compass::LEARN_INTERNAL
2015-02-23 19:17:44 -04:00
# endif
2011-02-14 00:25:20 -04:00
2017-04-03 19:38:14 -03:00
# ifndef AP_COMPASS_OFFSETS_MAX_DEFAULT
2017-07-06 08:25:19 -03:00
# define AP_COMPASS_OFFSETS_MAX_DEFAULT 850
2017-04-03 19:38:14 -03:00
# endif
2018-03-12 04:59:32 -03:00
# ifndef HAL_COMPASS_FILTER_DEFAULT
# define HAL_COMPASS_FILTER_DEFAULT 0 // turned off by default
# endif
2018-08-01 06:10:32 -03:00
# ifndef HAL_COMPASS_AUTO_ROT_DEFAULT
2018-09-14 02:16:13 -03:00
# define HAL_COMPASS_AUTO_ROT_DEFAULT 2
2018-08-01 06:10:32 -03:00
# endif
2015-10-25 14:03:46 -03:00
const AP_Param : : GroupInfo Compass : : var_info [ ] = {
2012-03-09 22:45:35 -04:00
// index 0 was used for the old orientation matrix
2013-01-02 03:07:33 -04:00
// @Param: OFS_X
2015-09-29 16:51:21 -03:00
// @DisplayName: Compass offsets in milligauss on the X axis
2013-01-02 03:07:33 -04:00
// @Description: Offset to be added to the compass x-axis values to compensate for metal in the frame
// @Range: -400 400
2017-05-02 10:42:55 -03:00
// @Units: mGauss
2013-01-02 03:07:33 -04:00
// @Increment: 1
2016-07-26 02:35:15 -03:00
// @User: Advanced
2013-01-02 03:07:33 -04:00
// @Param: OFS_Y
2015-09-29 16:51:21 -03:00
// @DisplayName: Compass offsets in milligauss on the Y axis
2013-01-02 03:07:33 -04:00
// @Description: Offset to be added to the compass y-axis values to compensate for metal in the frame
// @Range: -400 400
2017-05-02 10:42:55 -03:00
// @Units: mGauss
2013-01-02 03:07:33 -04:00
// @Increment: 1
2016-07-26 02:35:15 -03:00
// @User: Advanced
2013-01-02 03:07:33 -04:00
// @Param: OFS_Z
2015-09-29 16:51:21 -03:00
// @DisplayName: Compass offsets in milligauss on the Z axis
2013-01-02 03:07:33 -04:00
// @Description: Offset to be added to the compass z-axis values to compensate for metal in the frame
// @Range: -400 400
2017-05-02 10:42:55 -03:00
// @Units: mGauss
2013-01-02 03:07:33 -04:00
// @Increment: 1
2016-07-26 02:35:15 -03:00
// @User: Advanced
2015-02-23 19:17:44 -04:00
AP_GROUPINFO ( " OFS " , 1 , Compass , _state [ 0 ] . offset , 0 ) ,
2013-01-02 03:07:33 -04:00
// @Param: DEC
// @DisplayName: Compass declination
// @Description: An angle to compensate between the true north and magnetic north
// @Range: -3.142 3.142
2017-05-02 10:42:55 -03:00
// @Units: rad
2013-01-02 03:07:33 -04:00
// @Increment: 0.01
// @User: Standard
2012-08-06 22:02:14 -03:00
AP_GROUPINFO ( " DEC " , 2 , Compass , _declination , 0 ) ,
2013-01-02 03:07:33 -04:00
// @Param: LEARN
// @DisplayName: Learn compass offsets automatically
2016-04-28 05:51:06 -03:00
// @Description: Enable or disable the automatic learning of compass offsets. You can enable learning either using a compass-only method that is suitable only for fixed wing aircraft or using the offsets learnt by the active EKF state estimator. If this option is enabled then the learnt offsets are saved when you disarm the vehicle.
// @Values: 0:Disabled,1:Internal-Learning,2:EKF-Learning
2013-01-02 03:07:33 -04:00
// @User: Advanced
2015-02-23 19:17:44 -04:00
AP_GROUPINFO ( " LEARN " , 3 , Compass , _learn , COMPASS_LEARN_DEFAULT ) ,
2013-01-02 03:07:33 -04:00
// @Param: USE
// @DisplayName: Use compass for yaw
// @Description: Enable or disable the use of the compass (instead of the GPS) for determining heading
// @Values: 0:Disabled,1:Enabled
// @User: Advanced
2015-02-23 19:17:44 -04:00
AP_GROUPINFO ( " USE " , 4 , Compass , _state [ 0 ] . use_for_yaw , 1 ) , // true if used for DCM yaw
2013-01-02 03:07:33 -04:00
// @Param: AUTODEC
// @DisplayName: Auto Declination
// @Description: Enable or disable the automatic calculation of the declination based on gps location
// @Values: 0:Disabled,1:Enabled
// @User: Advanced
2012-08-06 22:02:14 -03:00
AP_GROUPINFO ( " AUTODEC " , 5 , Compass , _auto_declination , 1 ) ,
2013-02-27 03:57:04 -04:00
2013-03-03 10:02:12 -04:00
// @Param: MOTCT
// @DisplayName: Motor interference compensation type
2013-05-11 02:50:36 -03:00
// @Description: Set motor interference compensation type to disabled, throttle or current. Do not change manually.
2013-03-03 10:02:12 -04:00
// @Values: 0:Disabled,1:Use Throttle,2:Use Current
2015-10-15 08:32:47 -03:00
// @User: Advanced
2013-03-03 10:02:12 -04:00
AP_GROUPINFO ( " MOTCT " , 6 , Compass , _motor_comp_type , AP_COMPASS_MOT_COMP_DISABLED ) ,
2013-02-27 03:57:04 -04:00
// @Param: MOT_X
// @DisplayName: Motor interference compensation for body frame X axis
2017-05-02 10:42:55 -03:00
// @Description: Multiplied by the current throttle and added to the compass's x-axis values to compensate for motor interference (Offset per Amp or at Full Throttle)
2013-03-01 10:59:53 -04:00
// @Range: -1000 1000
2017-05-02 10:42:55 -03:00
// @Units: mGauss/A
2013-02-27 03:57:04 -04:00
// @Increment: 1
2016-07-26 02:35:15 -03:00
// @User: Advanced
2013-02-27 03:57:04 -04:00
// @Param: MOT_Y
// @DisplayName: Motor interference compensation for body frame Y axis
2017-05-02 10:42:55 -03:00
// @Description: Multiplied by the current throttle and added to the compass's y-axis values to compensate for motor interference (Offset per Amp or at Full Throttle)
2013-03-01 10:59:53 -04:00
// @Range: -1000 1000
2017-05-02 10:42:55 -03:00
// @Units: mGauss/A
2013-02-27 03:57:04 -04:00
// @Increment: 1
2016-07-26 02:35:15 -03:00
// @User: Advanced
2013-02-27 03:57:04 -04:00
// @Param: MOT_Z
// @DisplayName: Motor interference compensation for body frame Z axis
2017-05-02 10:42:55 -03:00
// @Description: Multiplied by the current throttle and added to the compass's z-axis values to compensate for motor interference (Offset per Amp or at Full Throttle)
2013-03-01 10:59:53 -04:00
// @Range: -1000 1000
2017-05-02 10:42:55 -03:00
// @Units: mGauss/A
2013-02-27 03:57:04 -04:00
// @Increment: 1
2016-07-26 02:35:15 -03:00
// @User: Advanced
2015-02-23 19:17:44 -04:00
AP_GROUPINFO ( " MOT " , 7 , Compass , _state [ 0 ] . motor_compensation , 0 ) ,
2013-02-27 03:57:04 -04:00
2013-05-01 23:27:35 -03:00
// @Param: ORIENT
// @DisplayName: Compass orientation
2018-03-08 11:00:53 -04:00
// @Description: The orientation of the first external compass relative to the vehicle frame. This value will be ignored unless this compass is set as an external compass. When set correctly in the northern hemisphere, pointing the nose and right side down should increase the MagX and MagY values respectively. Rolling the vehicle upside down should decrease the MagZ value. For southern hemisphere, switch increase and decrease. NOTE: For internal compasses, AHRS_ORIENT is used.
2018-04-04 13:25:04 -03:00
// @Values: 0:None,1:Yaw45,2:Yaw90,3:Yaw135,4:Yaw180,5:Yaw225,6:Yaw270,7:Yaw315,8:Roll180,9:Roll180Yaw45,10:Roll180Yaw90,11:Roll180Yaw135,12:Pitch180,13:Roll180Yaw225,14:Roll180Yaw270,15:Roll180Yaw315,16:Roll90,17:Roll90Yaw45,18:Roll90Yaw90,19:Roll90Yaw135,20:Roll270,21:Roll270Yaw45,22:Roll270Yaw90,23:Roll270Yaw135,24:Pitch90,25:Pitch270,26:Pitch180Yaw90,27:Pitch180Yaw270,28:Roll90Pitch90,29:Roll180Pitch90,30:Roll270Pitch90,31:Roll90Pitch180,32:Roll270Pitch180,33:Roll90Pitch270,34:Roll180Pitch270,35:Roll270Pitch270,36:Roll90Pitch180Yaw90,37:Roll90Yaw270,38:Yaw293Pitch68Roll180,39:Pitch315,40:Roll90Pitch315
2015-10-15 08:32:47 -03:00
// @User: Advanced
2015-02-23 19:17:44 -04:00
AP_GROUPINFO ( " ORIENT " , 8 , Compass , _state [ 0 ] . orientation , ROTATION_NONE ) ,
2013-05-01 23:27:35 -03:00
2013-08-30 01:02:09 -03:00
// @Param: EXTERNAL
// @DisplayName: Compass is attached via an external cable
2016-04-28 20:16:14 -03:00
// @Description: Configure compass so it is attached externally. This is auto-detected on PX4 and Pixhawk. Set to 1 if the compass is externally connected. When externally connected the COMPASS_ORIENT option operates independently of the AHRS_ORIENTATION board orientation option. If set to 0 or 1 then auto-detection by bus connection can override the value. If set to 2 then auto-detection will be disabled.
// @Values: 0:Internal,1:External,2:ForcedExternal
2013-08-30 01:02:09 -03:00
// @User: Advanced
2015-02-23 19:17:44 -04:00
AP_GROUPINFO ( " EXTERNAL " , 9 , Compass , _state [ 0 ] . external , 0 ) ,
2013-08-30 01:02:09 -03:00
2014-09-27 05:59:26 -03:00
// @Param: OFS2_X
2015-09-29 16:51:21 -03:00
// @DisplayName: Compass2 offsets in milligauss on the X axis
2014-09-27 05:59:26 -03:00
// @Description: Offset to be added to compass2's x-axis values to compensate for metal in the frame
// @Range: -400 400
2017-05-02 10:42:55 -03:00
// @Units: mGauss
2014-09-27 05:59:26 -03:00
// @Increment: 1
2016-07-26 02:35:15 -03:00
// @User: Advanced
2014-09-27 05:59:26 -03:00
// @Param: OFS2_Y
2015-09-29 16:51:21 -03:00
// @DisplayName: Compass2 offsets in milligauss on the Y axis
2014-09-27 05:59:26 -03:00
// @Description: Offset to be added to compass2's y-axis values to compensate for metal in the frame
// @Range: -400 400
2017-05-02 10:42:55 -03:00
// @Units: mGauss
2014-09-27 05:59:26 -03:00
// @Increment: 1
2016-07-26 02:35:15 -03:00
// @User: Advanced
2014-09-27 05:59:26 -03:00
// @Param: OFS2_Z
2015-09-29 16:51:21 -03:00
// @DisplayName: Compass2 offsets in milligauss on the Z axis
2014-09-27 05:59:26 -03:00
// @Description: Offset to be added to compass2's z-axis values to compensate for metal in the frame
// @Range: -400 400
2017-05-02 10:42:55 -03:00
// @Units: mGauss
2014-09-27 05:59:26 -03:00
// @Increment: 1
2016-07-26 02:35:15 -03:00
// @User: Advanced
2015-02-23 19:17:44 -04:00
AP_GROUPINFO ( " OFS2 " , 10 , Compass , _state [ 1 ] . offset , 0 ) ,
2014-09-27 05:59:26 -03:00
// @Param: MOT2_X
// @DisplayName: Motor interference compensation to compass2 for body frame X axis
2017-05-02 10:42:55 -03:00
// @Description: Multiplied by the current throttle and added to compass2's x-axis values to compensate for motor interference (Offset per Amp or at Full Throttle)
2014-09-27 05:59:26 -03:00
// @Range: -1000 1000
2017-05-02 10:42:55 -03:00
// @Units: mGauss/A
2014-09-27 05:59:26 -03:00
// @Increment: 1
2016-07-26 02:35:15 -03:00
// @User: Advanced
2014-09-27 05:59:26 -03:00
// @Param: MOT2_Y
// @DisplayName: Motor interference compensation to compass2 for body frame Y axis
2017-05-02 10:42:55 -03:00
// @Description: Multiplied by the current throttle and added to compass2's y-axis values to compensate for motor interference (Offset per Amp or at Full Throttle)
2014-09-27 05:59:26 -03:00
// @Range: -1000 1000
2017-05-02 10:42:55 -03:00
// @Units: mGauss/A
2014-09-27 05:59:26 -03:00
// @Increment: 1
2016-07-26 02:35:15 -03:00
// @User: Advanced
2014-09-27 05:59:26 -03:00
// @Param: MOT2_Z
// @DisplayName: Motor interference compensation to compass2 for body frame Z axis
2017-05-02 10:42:55 -03:00
// @Description: Multiplied by the current throttle and added to compass2's z-axis values to compensate for motor interference (Offset per Amp or at Full Throttle)
2014-09-27 05:59:26 -03:00
// @Range: -1000 1000
2017-05-02 10:42:55 -03:00
// @Units: mGauss/A
2014-09-27 05:59:26 -03:00
// @Increment: 1
2016-07-26 02:35:15 -03:00
// @User: Advanced
2015-02-23 19:17:44 -04:00
AP_GROUPINFO ( " MOT2 " , 11 , Compass , _state [ 1 ] . motor_compensation , 0 ) ,
2014-05-21 23:52:25 -03:00
// @Param: PRIMARY
// @DisplayName: Choose primary compass
2018-03-08 11:00:53 -04:00
// @Description: If more than one compass is available, this selects which compass is the primary. When external compasses are connected, they will be ordered first. NOTE: If no external compass is attached, this parameter is ignored.
2015-07-02 01:49:48 -03:00
// @Values: 0:FirstCompass,1:SecondCompass,2:ThirdCompass
2014-05-21 23:52:25 -03:00
// @User: Advanced
AP_GROUPINFO ( " PRIMARY " , 12 , Compass , _primary , 0 ) ,
2013-12-09 02:33:54 -04:00
2014-09-27 05:59:26 -03:00
// @Param: OFS3_X
2015-09-29 16:51:21 -03:00
// @DisplayName: Compass3 offsets in milligauss on the X axis
2014-09-27 05:59:26 -03:00
// @Description: Offset to be added to compass3's x-axis values to compensate for metal in the frame
// @Range: -400 400
2017-05-02 10:42:55 -03:00
// @Units: mGauss
2014-09-27 05:59:26 -03:00
// @Increment: 1
2016-07-26 02:35:15 -03:00
// @User: Advanced
2014-09-27 05:59:26 -03:00
// @Param: OFS3_Y
2015-09-29 16:51:21 -03:00
// @DisplayName: Compass3 offsets in milligauss on the Y axis
2014-09-27 05:59:26 -03:00
// @Description: Offset to be added to compass3's y-axis values to compensate for metal in the frame
// @Range: -400 400
2017-05-02 10:42:55 -03:00
// @Units: mGauss
2014-09-27 05:59:26 -03:00
// @Increment: 1
2016-07-26 02:35:15 -03:00
// @User: Advanced
2014-09-27 05:59:26 -03:00
// @Param: OFS3_Z
2015-09-29 16:51:21 -03:00
// @DisplayName: Compass3 offsets in milligauss on the Z axis
2014-09-27 05:59:26 -03:00
// @Description: Offset to be added to compass3's z-axis values to compensate for metal in the frame
// @Range: -400 400
2017-05-02 10:42:55 -03:00
// @Units: mGauss
2014-09-27 05:59:26 -03:00
// @Increment: 1
2016-07-26 02:35:15 -03:00
// @User: Advanced
2015-02-23 19:17:44 -04:00
AP_GROUPINFO ( " OFS3 " , 13 , Compass , _state [ 2 ] . offset , 0 ) ,
2014-09-27 05:59:26 -03:00
// @Param: MOT3_X
// @DisplayName: Motor interference compensation to compass3 for body frame X axis
2017-05-02 10:42:55 -03:00
// @Description: Multiplied by the current throttle and added to compass3's x-axis values to compensate for motor interference (Offset per Amp or at Full Throttle)
2014-09-27 05:59:26 -03:00
// @Range: -1000 1000
2017-05-02 10:42:55 -03:00
// @Units: mGauss/A
2014-09-27 05:59:26 -03:00
// @Increment: 1
2016-07-26 02:35:15 -03:00
// @User: Advanced
2014-09-27 05:59:26 -03:00
// @Param: MOT3_Y
// @DisplayName: Motor interference compensation to compass3 for body frame Y axis
2017-05-02 10:42:55 -03:00
// @Description: Multiplied by the current throttle and added to compass3's y-axis values to compensate for motor interference (Offset per Amp or at Full Throttle)
2014-09-27 05:59:26 -03:00
// @Range: -1000 1000
2017-05-02 10:42:55 -03:00
// @Units: mGauss/A
2014-09-27 05:59:26 -03:00
// @Increment: 1
2016-07-26 02:35:15 -03:00
// @User: Advanced
2014-09-27 05:59:26 -03:00
// @Param: MOT3_Z
// @DisplayName: Motor interference compensation to compass3 for body frame Z axis
2017-05-02 10:42:55 -03:00
// @Description: Multiplied by the current throttle and added to compass3's z-axis values to compensate for motor interference (Offset per Amp or at Full Throttle)
2014-09-27 05:59:26 -03:00
// @Range: -1000 1000
2017-05-02 10:42:55 -03:00
// @Units: mGauss/A
2014-09-27 05:59:26 -03:00
// @Increment: 1
2016-07-26 02:35:15 -03:00
// @User: Advanced
2015-02-23 19:17:44 -04:00
AP_GROUPINFO ( " MOT3 " , 14 , Compass , _state [ 2 ] . motor_compensation , 0 ) ,
2014-07-03 23:07:47 -03:00
2014-07-07 09:24:18 -03:00
// @Param: DEV_ID
// @DisplayName: Compass device id
// @Description: Compass device id. Automatically detected, do not set manually
2016-07-26 02:35:15 -03:00
// @User: Advanced
2015-02-23 19:17:44 -04:00
AP_GROUPINFO ( " DEV_ID " , 15 , Compass , _state [ 0 ] . dev_id , 0 ) ,
2014-07-07 09:24:18 -03:00
// @Param: DEV_ID2
// @DisplayName: Compass2 device id
// @Description: Second compass's device id. Automatically detected, do not set manually
2016-07-26 02:35:15 -03:00
// @User: Advanced
2015-02-23 19:17:44 -04:00
AP_GROUPINFO ( " DEV_ID2 " , 16 , Compass , _state [ 1 ] . dev_id , 0 ) ,
2014-07-07 09:24:18 -03:00
// @Param: DEV_ID3
// @DisplayName: Compass3 device id
// @Description: Third compass's device id. Automatically detected, do not set manually
2016-07-26 02:35:15 -03:00
// @User: Advanced
2015-02-23 19:17:44 -04:00
AP_GROUPINFO ( " DEV_ID3 " , 17 , Compass , _state [ 2 ] . dev_id , 0 ) ,
2014-07-07 09:24:18 -03:00
2014-09-24 10:26:42 -03:00
// @Param: USE2
// @DisplayName: Compass2 used for yaw
// @Description: Enable or disable the second compass for determining heading.
// @Values: 0:Disabled,1:Enabled
// @User: Advanced
2015-02-23 19:17:44 -04:00
AP_GROUPINFO ( " USE2 " , 18 , Compass , _state [ 1 ] . use_for_yaw , 1 ) ,
2014-09-24 10:26:42 -03:00
// @Param: ORIENT2
// @DisplayName: Compass2 orientation
2018-03-08 11:00:53 -04:00
// @Description: The orientation of a second external compass relative to the vehicle frame. This value will be ignored unless this compass is set as an external compass. When set correctly in the northern hemisphere, pointing the nose and right side down should increase the MagX and MagY values respectively. Rolling the vehicle upside down should decrease the MagZ value. For southern hemisphere, switch increase and decrease. NOTE: For internal compasses, AHRS_ORIENT is used.
2018-04-04 13:25:04 -03:00
// @Values: 0:None,1:Yaw45,2:Yaw90,3:Yaw135,4:Yaw180,5:Yaw225,6:Yaw270,7:Yaw315,8:Roll180,9:Roll180Yaw45,10:Roll180Yaw90,11:Roll180Yaw135,12:Pitch180,13:Roll180Yaw225,14:Roll180Yaw270,15:Roll180Yaw315,16:Roll90,17:Roll90Yaw45,18:Roll90Yaw90,19:Roll90Yaw135,20:Roll270,21:Roll270Yaw45,22:Roll270Yaw90,23:Roll270Yaw135,24:Pitch90,25:Pitch270,26:Pitch180Yaw90,27:Pitch180Yaw270,28:Roll90Pitch90,29:Roll180Pitch90,30:Roll270Pitch90,31:Roll90Pitch180,32:Roll270Pitch180,33:Roll90Pitch270,34:Roll180Pitch270,35:Roll270Pitch270,36:Roll90Pitch180Yaw90,37:Roll90Yaw270,38:Yaw293Pitch68Roll180,39:Pitch315,40:Roll90Pitch315
2015-10-15 08:32:47 -03:00
// @User: Advanced
2015-02-23 19:17:44 -04:00
AP_GROUPINFO ( " ORIENT2 " , 19 , Compass , _state [ 1 ] . orientation , ROTATION_NONE ) ,
2014-09-24 10:26:42 -03:00
2014-11-07 21:21:22 -04:00
// @Param: EXTERN2
2014-09-24 10:26:42 -03:00
// @DisplayName: Compass2 is attached via an external cable
2016-04-28 20:16:14 -03:00
// @Description: Configure second compass so it is attached externally. This is auto-detected on PX4 and Pixhawk. If set to 0 or 1 then auto-detection by bus connection can override the value. If set to 2 then auto-detection will be disabled.
// @Values: 0:Internal,1:External,2:ForcedExternal
2014-09-24 10:26:42 -03:00
// @User: Advanced
2015-02-23 19:17:44 -04:00
AP_GROUPINFO ( " EXTERN2 " , 20 , Compass , _state [ 1 ] . external , 0 ) ,
2014-09-24 10:26:42 -03:00
// @Param: USE3
// @DisplayName: Compass3 used for yaw
// @Description: Enable or disable the third compass for determining heading.
// @Values: 0:Disabled,1:Enabled
// @User: Advanced
2015-02-23 19:17:44 -04:00
AP_GROUPINFO ( " USE3 " , 21 , Compass , _state [ 2 ] . use_for_yaw , 1 ) ,
2014-09-24 10:26:42 -03:00
// @Param: ORIENT3
// @DisplayName: Compass3 orientation
2018-03-08 11:00:53 -04:00
// @Description: The orientation of a third external compass relative to the vehicle frame. This value will be ignored unless this compass is set as an external compass. When set correctly in the northern hemisphere, pointing the nose and right side down should increase the MagX and MagY values respectively. Rolling the vehicle upside down should decrease the MagZ value. For southern hemisphere, switch increase and decrease. NOTE: For internal compasses, AHRS_ORIENT is used.
2018-04-04 13:25:04 -03:00
// @Values: 0:None,1:Yaw45,2:Yaw90,3:Yaw135,4:Yaw180,5:Yaw225,6:Yaw270,7:Yaw315,8:Roll180,9:Roll180Yaw45,10:Roll180Yaw90,11:Roll180Yaw135,12:Pitch180,13:Roll180Yaw225,14:Roll180Yaw270,15:Roll180Yaw315,16:Roll90,17:Roll90Yaw45,18:Roll90Yaw90,19:Roll90Yaw135,20:Roll270,21:Roll270Yaw45,22:Roll270Yaw90,23:Roll270Yaw135,24:Pitch90,25:Pitch270,26:Pitch180Yaw90,27:Pitch180Yaw270,28:Roll90Pitch90,29:Roll180Pitch90,30:Roll270Pitch90,31:Roll90Pitch180,32:Roll270Pitch180,33:Roll90Pitch270,34:Roll180Pitch270,35:Roll270Pitch270,36:Roll90Pitch180Yaw90,37:Roll90Yaw270,38:Yaw293Pitch68Roll180,39:Pitch315,40:Roll90Pitch315
2015-10-15 08:32:47 -03:00
// @User: Advanced
2015-02-23 19:17:44 -04:00
AP_GROUPINFO ( " ORIENT3 " , 22 , Compass , _state [ 2 ] . orientation , ROTATION_NONE ) ,
2014-09-24 10:26:42 -03:00
2014-11-07 21:21:22 -04:00
// @Param: EXTERN3
2014-09-24 10:26:42 -03:00
// @DisplayName: Compass3 is attached via an external cable
2016-04-28 20:16:14 -03:00
// @Description: Configure third compass so it is attached externally. This is auto-detected on PX4 and Pixhawk. If set to 0 or 1 then auto-detection by bus connection can override the value. If set to 2 then auto-detection will be disabled.
// @Values: 0:Internal,1:External,2:ForcedExternal
2014-09-24 10:26:42 -03:00
// @User: Advanced
2015-02-23 19:17:44 -04:00
AP_GROUPINFO ( " EXTERN3 " , 23 , Compass , _state [ 2 ] . external , 0 ) ,
2014-09-24 10:26:42 -03:00
2015-03-18 20:18:47 -03:00
// @Param: DIA_X
// @DisplayName: Compass soft-iron diagonal X component
// @Description: DIA_X in the compass soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
2016-07-26 02:35:15 -03:00
// @User: Advanced
2015-03-18 20:18:47 -03:00
// @Param: DIA_Y
// @DisplayName: Compass soft-iron diagonal Y component
// @Description: DIA_Y in the compass soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
2016-07-26 02:35:15 -03:00
// @User: Advanced
2015-03-18 20:18:47 -03:00
// @Param: DIA_Z
// @DisplayName: Compass soft-iron diagonal Z component
// @Description: DIA_Z in the compass soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
2016-07-26 02:35:15 -03:00
// @User: Advanced
2015-03-18 20:18:47 -03:00
AP_GROUPINFO ( " DIA " , 24 , Compass , _state [ 0 ] . diagonals , 0 ) ,
// @Param: ODI_X
// @DisplayName: Compass soft-iron off-diagonal X component
// @Description: ODI_X in the compass soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
2016-07-26 02:35:15 -03:00
// @User: Advanced
2015-03-18 20:18:47 -03:00
// @Param: ODI_Y
// @DisplayName: Compass soft-iron off-diagonal Y component
// @Description: ODI_Y in the compass soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
2016-07-26 02:35:15 -03:00
// @User: Advanced
2015-03-18 20:18:47 -03:00
// @Param: ODI_Z
// @DisplayName: Compass soft-iron off-diagonal Z component
// @Description: ODI_Z in the compass soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
2016-07-26 02:35:15 -03:00
// @User: Advanced
2015-03-18 20:18:47 -03:00
AP_GROUPINFO ( " ODI " , 25 , Compass , _state [ 0 ] . offdiagonals , 0 ) ,
// @Param: DIA2_X
// @DisplayName: Compass2 soft-iron diagonal X component
// @Description: DIA_X in the compass2 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
2016-07-26 02:35:15 -03:00
// @User: Advanced
2015-03-18 20:18:47 -03:00
// @Param: DIA2_Y
// @DisplayName: Compass2 soft-iron diagonal Y component
// @Description: DIA_Y in the compass2 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
2016-07-26 02:35:15 -03:00
// @User: Advanced
2015-03-18 20:18:47 -03:00
// @Param: DIA2_Z
// @DisplayName: Compass2 soft-iron diagonal Z component
// @Description: DIA_Z in the compass2 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
2016-07-26 02:35:15 -03:00
// @User: Advanced
2015-03-18 20:18:47 -03:00
AP_GROUPINFO ( " DIA2 " , 26 , Compass , _state [ 1 ] . diagonals , 0 ) ,
// @Param: ODI2_X
// @DisplayName: Compass2 soft-iron off-diagonal X component
// @Description: ODI_X in the compass2 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
2016-07-26 02:35:15 -03:00
// @User: Advanced
2015-03-18 20:18:47 -03:00
// @Param: ODI2_Y
// @DisplayName: Compass2 soft-iron off-diagonal Y component
// @Description: ODI_Y in the compass2 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
2016-07-26 02:35:15 -03:00
// @User: Advanced
2015-03-18 20:18:47 -03:00
// @Param: ODI2_Z
// @DisplayName: Compass2 soft-iron off-diagonal Z component
// @Description: ODI_Z in the compass2 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
2016-07-26 02:35:15 -03:00
// @User: Advanced
2015-03-18 20:18:47 -03:00
AP_GROUPINFO ( " ODI2 " , 27 , Compass , _state [ 1 ] . offdiagonals , 0 ) ,
// @Param: DIA3_X
// @DisplayName: Compass3 soft-iron diagonal X component
// @Description: DIA_X in the compass3 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
2016-07-26 02:35:15 -03:00
// @User: Advanced
2015-03-18 20:18:47 -03:00
// @Param: DIA3_Y
// @DisplayName: Compass3 soft-iron diagonal Y component
// @Description: DIA_Y in the compass3 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
2016-07-26 02:35:15 -03:00
// @User: Advanced
2015-03-18 20:18:47 -03:00
// @Param: DIA3_Z
// @DisplayName: Compass3 soft-iron diagonal Z component
// @Description: DIA_Z in the compass3 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
2016-07-26 02:35:15 -03:00
// @User: Advanced
2015-03-18 20:18:47 -03:00
AP_GROUPINFO ( " DIA3 " , 28 , Compass , _state [ 2 ] . diagonals , 0 ) ,
// @Param: ODI3_X
// @DisplayName: Compass3 soft-iron off-diagonal X component
// @Description: ODI_X in the compass3 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
2016-07-26 02:35:15 -03:00
// @User: Advanced
2015-03-18 20:18:47 -03:00
// @Param: ODI3_Y
// @DisplayName: Compass3 soft-iron off-diagonal Y component
// @Description: ODI_Y in the compass3 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
2016-07-26 02:35:15 -03:00
// @User: Advanced
2015-03-18 20:18:47 -03:00
// @Param: ODI3_Z
// @DisplayName: Compass3 soft-iron off-diagonal Z component
// @Description: ODI_Z in the compass3 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
2016-07-26 02:35:15 -03:00
// @User: Advanced
2015-03-18 20:18:47 -03:00
AP_GROUPINFO ( " ODI3 " , 29 , Compass , _state [ 2 ] . offdiagonals , 0 ) ,
2015-09-14 04:01:14 -03:00
// @Param: CAL_FIT
// @DisplayName: Compass calibration fitness
// @Description: This controls the fitness level required for a successful compass calibration. A lower value makes for a stricter fit (less likely to pass). This is the value used for the primary magnetometer. Other magnetometers get double the value.
2016-09-13 21:57:52 -03:00
// @Range: 4 32
2017-01-25 04:27:53 -04:00
// @Values: 4:Very Strict,8:Strict,16:Default,32:Relaxed
2015-09-14 04:01:14 -03:00
// @Increment: 0.1
// @User: Advanced
2017-01-20 21:15:11 -04:00
AP_GROUPINFO ( " CAL_FIT " , 30 , Compass , _calibration_threshold , AP_COMPASS_CALIBRATION_FITNESS_DEFAULT ) ,
2016-03-01 15:16:04 -04:00
2017-04-03 19:38:14 -03:00
// @Param: OFFS_MAX
// @DisplayName: Compass maximum offset
// @Description: This sets the maximum allowed compass offset in calibration and arming checks
// @Range: 500 3000
// @Increment: 1
// @User: Advanced
AP_GROUPINFO ( " OFFS_MAX " , 31 , Compass , _offset_max , AP_COMPASS_OFFSETS_MAX_DEFAULT ) ,
2017-06-18 04:26:25 -03:00
2018-01-05 06:08:04 -04:00
// @Group: PMOT
// @Path: Compass_PerMotor.cpp
AP_SUBGROUPINFO ( _per_motor , " PMOT " , 32 , Compass , Compass_PerMotor ) ,
2017-06-18 04:26:25 -03:00
// @Param: TYPEMASK
// @DisplayName: Compass disable driver type mask
// @Description: This is a bitmask of driver types to disable. If a driver type is set in this mask then that driver will not try to find a sensor at startup
2018-05-11 09:26:51 -03:00
// @Bitmask: 0:HMC5883,1:LSM303D,2:AK8963,3:BMM150,4:LSM9DS1,5:LIS3MDL,6:AK09916,7:IST8310,8:ICM20948,9:MMC3416,11:UAVCAN,12:QMC5883
2017-06-18 04:26:25 -03:00
// @User: Advanced
AP_GROUPINFO ( " TYPEMASK " , 33 , Compass , _driver_type_mask , 0 ) ,
2018-03-12 04:59:32 -03:00
// @Param: FLTR_RNG
// @DisplayName: Range in which sample is accepted
// @Description: This sets the range around the average value that new samples must be within to be accepted. This can help reduce the impact of noise on sensors that are on long I2C cables. The value is a percentage from the average value. A value of zero disables this filter.
// @Units: %
// @Range: 0 100
// @Increment: 1
AP_GROUPINFO ( " FLTR_RNG " , 34 , Compass , _filter_range , HAL_COMPASS_FILTER_DEFAULT ) ,
2018-08-01 06:10:32 -03:00
// @Param: AUTO_ROT
// @DisplayName: Automatically check orientation
// @Description: When enabled this will automatically check the orientation of compasses on successful completion of compass calibration. If set to 2 then external compasses will have their orientation automatically corrected.
// @Values: 0:Disabled,1:CheckOnly,2:CheckAndFix
AP_GROUPINFO ( " AUTO_ROT " , 35 , Compass , _rotate_auto , HAL_COMPASS_AUTO_ROT_DEFAULT ) ,
2017-04-03 19:38:14 -03:00
2012-02-12 03:23:19 -04:00
AP_GROUPEND
2012-02-11 07:53:30 -04:00
} ;
2011-02-14 00:25:20 -04:00
// Default constructor.
// Note that the Vector/Matrix constructors already implicitly zero
// their values.
//
2018-03-28 20:57:17 -03:00
Compass : : Compass ( void )
2011-02-14 00:25:20 -04:00
{
2018-03-29 01:13:55 -03:00
if ( _singleton ! = nullptr ) {
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
AP_HAL : : panic ( " Compass must be singleton " ) ;
# endif
return ;
}
_singleton = this ;
2012-12-12 17:43:51 -04:00
AP_Param : : setup_object_defaults ( this , var_info ) ;
2011-02-14 00:25:20 -04:00
}
2014-11-15 21:58:23 -04:00
// Default init method
2011-02-14 00:25:20 -04:00
//
bool
Compass : : init ( )
{
2014-11-15 21:58:23 -04:00
if ( _compass_count = = 0 ) {
// detect available backends. Only called once
_detect_backends ( ) ;
}
2015-03-13 02:43:34 -03:00
if ( _compass_count ! = 0 ) {
// get initial health status
hal . scheduler - > delay ( 100 ) ;
read ( ) ;
}
2018-07-29 21:42:59 -03:00
// set the dev_id to 0 for undetected compasses, to make it easier
// for users to see how many compasses are detected. We don't do a
// set_and_save() as the user may have temporarily removed the
// compass, and we don't want to force a re-cal if they plug it
// back in again
for ( uint8_t i = _compass_count ; i < COMPASS_MAX_INSTANCES ; i + + ) {
_state [ i ] . dev_id . set ( 0 ) ;
}
2011-02-14 00:25:20 -04:00
return true ;
}
2014-11-15 21:58:23 -04:00
// Register a new compass instance
//
uint8_t Compass : : register_compass ( void )
{
if ( _compass_count = = COMPASS_MAX_INSTANCES ) {
2015-11-19 23:09:17 -04:00
AP_HAL : : panic ( " Too many compass instances " ) ;
2014-11-15 21:58:23 -04:00
}
return _compass_count + + ;
}
2016-05-13 14:37:30 -03:00
bool Compass : : _add_backend ( AP_Compass_Backend * backend , const char * name , bool external )
2014-11-15 21:58:23 -04:00
{
2016-05-13 14:37:30 -03:00
if ( ! backend ) {
return false ;
}
if ( _backend_count = = COMPASS_MAX_BACKEND ) {
2015-11-19 23:09:17 -04:00
AP_HAL : : panic ( " Too many compass backends " ) ;
2016-05-13 14:37:30 -03:00
}
2015-08-05 17:02:32 -03:00
_backends [ _backend_count + + ] = backend ;
2016-05-13 14:37:30 -03:00
return true ;
2014-11-15 21:58:23 -04:00
}
2017-06-18 04:26:25 -03:00
/*
return true if a driver type is enabled
*/
bool Compass : : _driver_enabled ( enum DriverType driver_type )
{
uint32_t mask = ( 1U < < uint8_t ( driver_type ) ) ;
return ( mask & uint32_t ( _driver_type_mask . get ( ) ) ) = = 0 ;
}
2017-10-02 04:23:57 -03:00
/*
2018-08-01 06:10:32 -03:00
wrapper around hal . i2c_mgr - > get_device ( ) that prevents duplicate devices being opened
2017-10-02 04:23:57 -03:00
*/
2018-08-01 06:10:32 -03:00
bool Compass : : _have_i2c_driver ( uint8_t bus , uint8_t address ) const
2017-10-02 04:23:57 -03:00
{
for ( uint8_t i = 0 ; i < _compass_count ; i + + ) {
2018-08-01 06:10:32 -03:00
if ( AP_HAL : : Device : : make_bus_id ( AP_HAL : : Device : : BUS_TYPE_I2C , bus , address , 0 ) = =
AP_HAL : : Device : : change_bus_id ( uint32_t ( _state [ i ] . dev_id . get ( ) ) , 0 ) ) {
// we are already using this device
2017-10-02 04:23:57 -03:00
return true ;
}
}
return false ;
}
2018-08-01 06:10:32 -03:00
/*
macro to add a backend with check for too many backends or compass
instances . We don ' t try to start more than the maximum allowed
*/
# define ADD_BACKEND(driver_type, backend, name, external) \
do { if ( _driver_enabled ( driver_type ) ) { _add_backend ( backend , name , external ) ; } \
if ( _backend_count = = COMPASS_MAX_BACKEND | | \
_compass_count = = COMPASS_MAX_INSTANCES ) { \
return ; \
} \
} while ( 0 )
# define GET_I2C_DEVICE(bus, address) _have_i2c_driver(bus, address)?nullptr:hal.i2c_mgr->get_device(bus, address)
/*
look for compasses on external i2c buses
*/
void Compass : : _probe_external_i2c_compasses ( void )
{
bool both_i2c_external = ( AP_BoardConfig : : get_board_type ( ) = = AP_BoardConfig : : PX4_BOARD_PIXHAWK2 ) ;
// external i2c bus
FOREACH_I2C_EXTERNAL ( i ) {
ADD_BACKEND ( DRIVER_HMC5883 , AP_Compass_HMC5843 : : probe ( * this , GET_I2C_DEVICE ( i , HAL_COMPASS_HMC5843_I2C_ADDR ) ,
true , ROTATION_ROLL_180 ) ,
AP_Compass_HMC5843 : : name , true ) ;
}
if ( AP_BoardConfig : : get_board_type ( ) ! = AP_BoardConfig : : PX4_BOARD_MINDPXV2 ) {
// internal i2c bus
FOREACH_I2C_INTERNAL ( i ) {
ADD_BACKEND ( DRIVER_HMC5883 , AP_Compass_HMC5843 : : probe ( * this , GET_I2C_DEVICE ( i , HAL_COMPASS_HMC5843_I2C_ADDR ) ,
both_i2c_external , both_i2c_external ? ROTATION_ROLL_180 : ROTATION_YAW_270 ) ,
AP_Compass_HMC5843 : : name , both_i2c_external ) ;
}
}
//external i2c bus
FOREACH_I2C_EXTERNAL ( i ) {
ADD_BACKEND ( DRIVER_QMC5883 , AP_Compass_QMC5883L : : probe ( * this , GET_I2C_DEVICE ( i , HAL_COMPASS_QMC5883L_I2C_ADDR ) ,
true , HAL_COMPASS_QMC5883L_ORIENTATION_EXTERNAL ) ,
AP_Compass_QMC5883L : : name , true ) ;
}
//internal i2c bus
FOREACH_I2C_INTERNAL ( i ) {
ADD_BACKEND ( DRIVER_QMC5883 , AP_Compass_QMC5883L : : probe ( * this , GET_I2C_DEVICE ( i , HAL_COMPASS_QMC5883L_I2C_ADDR ) ,
both_i2c_external ,
both_i2c_external ? HAL_COMPASS_QMC5883L_ORIENTATION_EXTERNAL : HAL_COMPASS_QMC5883L_ORIENTATION_INTERNAL ) ,
AP_Compass_QMC5883L : : name , both_i2c_external ) ;
}
# if !HAL_MINIMIZE_FEATURES
// AK09916 on ICM20948
FOREACH_I2C_EXTERNAL ( i ) {
ADD_BACKEND ( DRIVER_ICM20948 , AP_Compass_AK09916 : : probe_ICM20948 ( * this ,
GET_I2C_DEVICE ( i , HAL_COMPASS_AK09916_I2C_ADDR ) ,
GET_I2C_DEVICE ( i , HAL_COMPASS_ICM20948_I2C_ADDR ) ,
true , ROTATION_PITCH_180_YAW_90 ) ,
AP_Compass_AK09916 : : name , true ) ;
}
FOREACH_I2C_INTERNAL ( i ) {
ADD_BACKEND ( DRIVER_ICM20948 , AP_Compass_AK09916 : : probe_ICM20948 ( * this ,
GET_I2C_DEVICE ( i , HAL_COMPASS_AK09916_I2C_ADDR ) ,
GET_I2C_DEVICE ( i , HAL_COMPASS_ICM20948_I2C_ADDR ) ,
both_i2c_external , ROTATION_PITCH_180_YAW_90 ) ,
AP_Compass_AK09916 : : name , true ) ;
}
// lis3mdl on bus 0 with default address
FOREACH_I2C_INTERNAL ( i ) {
ADD_BACKEND ( DRIVER_LIS3MDL , AP_Compass_LIS3MDL : : probe ( * this , GET_I2C_DEVICE ( i , HAL_COMPASS_LIS3MDL_I2C_ADDR ) ,
both_i2c_external , both_i2c_external ? ROTATION_YAW_90 : ROTATION_NONE ) ,
AP_Compass_LIS3MDL : : name , both_i2c_external ) ;
}
// lis3mdl on bus 0 with alternate address
FOREACH_I2C_INTERNAL ( i ) {
ADD_BACKEND ( DRIVER_LIS3MDL , AP_Compass_LIS3MDL : : probe ( * this , GET_I2C_DEVICE ( i , HAL_COMPASS_LIS3MDL_I2C_ADDR2 ) ,
both_i2c_external , both_i2c_external ? ROTATION_YAW_90 : ROTATION_NONE ) ,
AP_Compass_LIS3MDL : : name , both_i2c_external ) ;
}
// external lis3mdl on bus 1 with default address
FOREACH_I2C_EXTERNAL ( i ) {
ADD_BACKEND ( DRIVER_LIS3MDL , AP_Compass_LIS3MDL : : probe ( * this , GET_I2C_DEVICE ( i , HAL_COMPASS_LIS3MDL_I2C_ADDR ) ,
true , ROTATION_YAW_90 ) ,
AP_Compass_LIS3MDL : : name , true ) ;
}
// external lis3mdl on bus 1 with alternate address
FOREACH_I2C_EXTERNAL ( i ) {
ADD_BACKEND ( DRIVER_LIS3MDL , AP_Compass_LIS3MDL : : probe ( * this , GET_I2C_DEVICE ( i , HAL_COMPASS_LIS3MDL_I2C_ADDR2 ) ,
true , ROTATION_YAW_90 ) ,
AP_Compass_LIS3MDL : : name , true ) ;
}
// AK09916. This can be found twice, due to the ICM20948 i2c bus pass-thru, so we need to be careful to avoid that
FOREACH_I2C_EXTERNAL ( i ) {
ADD_BACKEND ( DRIVER_AK09916 , AP_Compass_AK09916 : : probe ( * this , GET_I2C_DEVICE ( i , HAL_COMPASS_AK09916_I2C_ADDR ) ,
true , ROTATION_YAW_270 ) ,
AP_Compass_AK09916 : : name , true ) ;
}
FOREACH_I2C_INTERNAL ( i ) {
ADD_BACKEND ( DRIVER_AK09916 , AP_Compass_AK09916 : : probe ( * this , GET_I2C_DEVICE ( i , HAL_COMPASS_AK09916_I2C_ADDR ) ,
both_i2c_external , both_i2c_external ? ROTATION_YAW_270 : ROTATION_NONE ) ,
AP_Compass_AK09916 : : name , both_i2c_external ) ;
}
// IST8310 on external and internal bus
if ( AP_BoardConfig : : get_board_type ( ) ! = AP_BoardConfig : : PX4_BOARD_FMUV5 ) {
FOREACH_I2C_EXTERNAL ( i ) {
ADD_BACKEND ( DRIVER_IST8310 , AP_Compass_IST8310 : : probe ( * this , GET_I2C_DEVICE ( i , HAL_COMPASS_IST8310_I2C_ADDR ) ,
true , ROTATION_PITCH_180 ) , AP_Compass_IST8310 : : name , true ) ;
}
FOREACH_I2C_INTERNAL ( i ) {
ADD_BACKEND ( DRIVER_IST8310 , AP_Compass_IST8310 : : probe ( * this , GET_I2C_DEVICE ( i , HAL_COMPASS_IST8310_I2C_ADDR ) ,
both_i2c_external , ROTATION_PITCH_180 ) , AP_Compass_IST8310 : : name , both_i2c_external ) ;
}
}
# endif // HAL_MINIMIZE_FEATURES
}
2014-11-15 21:58:23 -04:00
/*
detect available backends for this board
*/
2015-08-05 17:02:32 -03:00
void Compass : : _detect_backends ( void )
2014-11-15 21:58:23 -04:00
{
2015-03-13 08:32:35 -03:00
if ( _hil_mode ) {
2016-05-13 14:37:30 -03:00
_add_backend ( AP_Compass_HIL : : detect ( * this ) , nullptr , false ) ;
2015-03-13 08:32:35 -03:00
return ;
}
2014-11-15 21:58:23 -04:00
2018-01-10 06:34:13 -04:00
# if AP_FEATURE_BOARD_DETECT
2017-06-18 04:26:25 -03:00
if ( AP_BoardConfig : : get_board_type ( ) = = AP_BoardConfig : : PX4_BOARD_PIXHAWK2 ) {
// default to disabling LIS3MDL on pixhawk2 due to hardware issue
2017-09-27 08:19:54 -03:00
_driver_type_mask . set_default ( 1U < < DRIVER_LIS3MDL ) ;
2017-06-18 04:26:25 -03:00
}
# endif
2017-06-21 14:39:07 -03:00
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
2017-06-18 04:26:25 -03:00
ADD_BACKEND ( DRIVER_SITL , new AP_Compass_SITL ( * this ) , nullptr , false ) ;
2017-06-21 14:39:07 -03:00
return ;
# endif
2018-08-01 06:10:32 -03:00
# ifdef HAL_PROBE_EXTERNAL_I2C_COMPASSES
// allow boards to ask for external probing of all i2c compass types in hwdef.dat
_probe_external_i2c_compasses ( ) ;
if ( _backend_count = = COMPASS_MAX_BACKEND | |
_compass_count = = COMPASS_MAX_INSTANCES ) {
return ;
}
# endif
2017-05-06 06:13:19 -03:00
2016-03-04 13:59:06 -04:00
# if HAL_COMPASS_DEFAULT == HAL_COMPASS_HIL
2017-06-18 04:26:25 -03:00
ADD_BACKEND ( DRIVER_SITL , AP_Compass_HIL : : detect ( * this ) , nullptr , false ) ;
2018-01-10 06:34:13 -04:00
# elif AP_FEATURE_BOARD_DETECT
2016-12-19 13:10:47 -04:00
switch ( AP_BoardConfig : : get_board_type ( ) ) {
case AP_BoardConfig : : PX4_BOARD_PX4V1 :
case AP_BoardConfig : : PX4_BOARD_PIXHAWK :
case AP_BoardConfig : : PX4_BOARD_PHMINI :
2017-02-28 21:42:23 -04:00
case AP_BoardConfig : : PX4_BOARD_AUAV21 :
2016-12-19 13:10:47 -04:00
case AP_BoardConfig : : PX4_BOARD_PH2SLIM :
case AP_BoardConfig : : PX4_BOARD_PIXHAWK2 :
2017-07-15 02:13:44 -03:00
case AP_BoardConfig : : PX4_BOARD_PIXRACER :
2018-01-11 17:26:29 -04:00
case AP_BoardConfig : : PX4_BOARD_MINDPXV2 :
2018-05-29 08:49:40 -03:00
case AP_BoardConfig : : PX4_BOARD_FMUV5 :
2018-08-01 06:10:32 -03:00
case AP_BoardConfig : : PX4_BOARD_PIXHAWK_PRO :
_probe_external_i2c_compasses ( ) ;
if ( _backend_count = = COMPASS_MAX_BACKEND | |
_compass_count = = COMPASS_MAX_INSTANCES ) {
return ;
2016-12-19 13:10:47 -04:00
}
break ;
2017-06-21 04:03:13 -03:00
case AP_BoardConfig : : PX4_BOARD_PCNC1 :
ADD_BACKEND ( DRIVER_BMM150 ,
2018-08-01 06:10:32 -03:00
AP_Compass_BMM150 : : probe ( * this , GET_I2C_DEVICE ( 0 , 0x10 ) ) ,
2017-06-21 04:03:13 -03:00
AP_Compass_BMM150 : : name , true ) ;
break ;
2017-02-10 19:33:23 -04:00
case AP_BoardConfig : : PX4_BOARD_AEROFC :
# ifdef HAL_COMPASS_IST8310_I2C_BUS
2018-08-01 06:10:32 -03:00
ADD_BACKEND ( DRIVER_IST8310 , AP_Compass_IST8310 : : probe ( * this , GET_I2C_DEVICE ( HAL_COMPASS_IST8310_I2C_BUS , HAL_COMPASS_IST8310_I2C_ADDR ) ,
2017-09-15 23:03:43 -03:00
true , ROTATION_PITCH_180_YAW_90 ) , AP_Compass_IST8310 : : name , true ) ;
2017-02-10 19:33:23 -04:00
# endif
break ;
2018-02-03 09:56:02 -04:00
case AP_BoardConfig : : VRX_BOARD_BRAIN54 : {
// external i2c bus
2018-08-01 06:10:32 -03:00
ADD_BACKEND ( DRIVER_HMC5883 , AP_Compass_HMC5843 : : probe ( * this , GET_I2C_DEVICE ( 1 , HAL_COMPASS_HMC5843_I2C_ADDR ) ,
2018-02-03 09:56:02 -04:00
true , ROTATION_ROLL_180 ) ,
AP_Compass_HMC5843 : : name , true ) ;
}
// internal i2c bus
2018-08-01 06:10:32 -03:00
ADD_BACKEND ( DRIVER_HMC5883 , AP_Compass_HMC5843 : : probe ( * this , GET_I2C_DEVICE ( 0 , HAL_COMPASS_HMC5843_I2C_ADDR ) ,
2018-02-03 09:56:02 -04:00
false , ROTATION_YAW_270 ) ,
AP_Compass_HMC5843 : : name , false ) ;
break ;
case AP_BoardConfig : : VRX_BOARD_BRAIN51 :
case AP_BoardConfig : : VRX_BOARD_BRAIN52 :
case AP_BoardConfig : : VRX_BOARD_BRAIN52E :
case AP_BoardConfig : : VRX_BOARD_CORE10 :
case AP_BoardConfig : : VRX_BOARD_UBRAIN51 :
case AP_BoardConfig : : VRX_BOARD_UBRAIN52 : {
// external i2c bus
2018-08-01 06:10:32 -03:00
ADD_BACKEND ( DRIVER_HMC5883 , AP_Compass_HMC5843 : : probe ( * this , GET_I2C_DEVICE ( 1 , HAL_COMPASS_HMC5843_I2C_ADDR ) ,
2018-02-03 09:56:02 -04:00
true , ROTATION_ROLL_180 ) ,
AP_Compass_HMC5843 : : name , true ) ;
}
break ;
2016-12-19 13:10:47 -04:00
default :
break ;
2016-11-09 06:19:24 -04:00
}
2016-12-19 13:10:47 -04:00
switch ( AP_BoardConfig : : get_board_type ( ) ) {
case AP_BoardConfig : : PX4_BOARD_PIXHAWK :
2017-06-18 04:26:25 -03:00
ADD_BACKEND ( DRIVER_HMC5883 , AP_Compass_HMC5843 : : probe ( * this , hal . spi - > get_device ( HAL_COMPASS_HMC5843_NAME ) ,
false , ROTATION_PITCH_180 ) ,
AP_Compass_HMC5843 : : name , false ) ;
ADD_BACKEND ( DRIVER_LSM303D , AP_Compass_LSM303D : : probe ( * this , hal . spi - > get_device ( HAL_INS_LSM9DS0_A_NAME ) ) ,
AP_Compass_LSM303D : : name , false ) ;
2016-12-19 13:10:47 -04:00
break ;
case AP_BoardConfig : : PX4_BOARD_PIXHAWK2 :
2017-06-18 04:26:25 -03:00
ADD_BACKEND ( DRIVER_LSM303D , AP_Compass_LSM303D : : probe ( * this , hal . spi - > get_device ( HAL_INS_LSM9DS0_EXT_A_NAME ) , ROTATION_YAW_270 ) ,
AP_Compass_LSM303D : : name , false ) ;
2016-11-09 22:05:22 -04:00
// we run the AK8963 only on the 2nd MPU9250, which leaves the
// first MPU9250 to run without disturbance at high rate
2017-06-18 04:26:25 -03:00
ADD_BACKEND ( DRIVER_AK8963 , AP_Compass_AK8963 : : probe_mpu9250 ( * this , 1 , ROTATION_YAW_270 ) ,
AP_Compass_AK8963 : : name , false ) ;
2016-12-19 13:10:47 -04:00
break ;
2018-05-29 08:49:40 -03:00
case AP_BoardConfig : : PX4_BOARD_FMUV5 :
2018-07-10 03:09:16 -03:00
FOREACH_I2C_EXTERNAL ( i ) {
2018-08-01 06:10:32 -03:00
ADD_BACKEND ( DRIVER_IST8310 , AP_Compass_IST8310 : : probe ( * this , GET_I2C_DEVICE ( i , HAL_COMPASS_IST8310_I2C_ADDR ) ,
2018-07-10 03:09:16 -03:00
true , ROTATION_ROLL_180_YAW_90 ) , AP_Compass_IST8310 : : name , true ) ;
}
FOREACH_I2C_INTERNAL ( i ) {
2018-08-01 06:10:32 -03:00
ADD_BACKEND ( DRIVER_IST8310 , AP_Compass_IST8310 : : probe ( * this , GET_I2C_DEVICE ( i , HAL_COMPASS_IST8310_I2C_ADDR ) ,
2018-07-10 03:09:16 -03:00
false , ROTATION_ROLL_180_YAW_90 ) , AP_Compass_IST8310 : : name , false ) ;
}
2018-05-29 08:49:40 -03:00
break ;
2018-02-14 01:35:51 -04:00
case AP_BoardConfig : : PX4_BOARD_SP01 :
ADD_BACKEND ( DRIVER_AK8963 , AP_Compass_AK8963 : : probe_mpu9250 ( * this , 1 , ROTATION_NONE ) ,
AP_Compass_AK8963 : : name , false ) ;
break ;
2016-12-19 13:10:47 -04:00
case AP_BoardConfig : : PX4_BOARD_PIXRACER :
2017-06-18 04:26:25 -03:00
ADD_BACKEND ( DRIVER_HMC5883 , AP_Compass_HMC5843 : : probe ( * this , hal . spi - > get_device ( HAL_COMPASS_HMC5843_NAME ) ,
false , ROTATION_PITCH_180 ) ,
AP_Compass_HMC5843 : : name , false ) ;
2017-12-01 20:11:12 -04:00
// R15 has LIS3MDL on spi bus instead of HMC; same CS pin
ADD_BACKEND ( DRIVER_LIS3MDL , AP_Compass_LIS3MDL : : probe ( * this , hal . spi - > get_device ( HAL_COMPASS_LIS3MDL_NAME ) ,
false , ROTATION_NONE ) ,
AP_Compass_LIS3MDL : : name , false ) ;
2017-06-18 04:26:25 -03:00
ADD_BACKEND ( DRIVER_AK8963 , AP_Compass_AK8963 : : probe_mpu9250 ( * this , 0 , ROTATION_ROLL_180_YAW_90 ) ,
AP_Compass_AK8963 : : name , false ) ;
2017-09-18 05:46:05 -03:00
break ;
2017-07-15 02:13:44 -03:00
case AP_BoardConfig : : PX4_BOARD_PIXHAWK_PRO :
ADD_BACKEND ( DRIVER_AK8963 , AP_Compass_AK8963 : : probe_mpu9250 ( * this , 0 , ROTATION_ROLL_180_YAW_90 ) ,
AP_Compass_AK8963 : : name , false ) ;
2017-07-15 02:18:13 -03:00
ADD_BACKEND ( DRIVER_LIS3MDL , AP_Compass_LIS3MDL : : probe ( * this , hal . spi - > get_device ( HAL_COMPASS_LIS3MDL_NAME ) ,
2017-06-26 13:35:04 -03:00
false , ROTATION_NONE ) ,
2017-07-15 02:18:13 -03:00
AP_Compass_LIS3MDL : : name , false ) ;
2016-12-19 13:10:47 -04:00
break ;
case AP_BoardConfig : : PX4_BOARD_PHMINI :
2017-06-18 04:26:25 -03:00
ADD_BACKEND ( DRIVER_AK8963 , AP_Compass_AK8963 : : probe_mpu9250 ( * this , 0 , ROTATION_ROLL_180 ) ,
AP_Compass_AK8963 : : name , false ) ;
2016-12-19 13:10:47 -04:00
break ;
2017-02-28 21:42:23 -04:00
case AP_BoardConfig : : PX4_BOARD_AUAV21 :
2017-06-18 04:26:25 -03:00
ADD_BACKEND ( DRIVER_AK8963 , AP_Compass_AK8963 : : probe_mpu9250 ( * this , 0 , ROTATION_ROLL_180_YAW_90 ) ,
AP_Compass_AK8963 : : name , false ) ;
2017-02-28 21:42:23 -04:00
break ;
2016-12-19 13:10:47 -04:00
case AP_BoardConfig : : PX4_BOARD_PH2SLIM :
2017-06-18 04:26:25 -03:00
ADD_BACKEND ( DRIVER_AK8963 , AP_Compass_AK8963 : : probe_mpu9250 ( * this , 0 , ROTATION_YAW_270 ) ,
AP_Compass_AK8963 : : name , false ) ;
2016-12-19 13:10:47 -04:00
break ;
2018-01-11 21:16:53 -04:00
case AP_BoardConfig : : PX4_BOARD_MINDPXV2 :
2018-08-01 06:10:32 -03:00
ADD_BACKEND ( DRIVER_HMC5883 , AP_Compass_HMC5843 : : probe ( * this , GET_I2C_DEVICE ( 0 , HAL_COMPASS_HMC5843_I2C_ADDR ) ,
2018-01-11 21:16:53 -04:00
false , ROTATION_YAW_90 ) ,
AP_Compass_HMC5843 : : name , false ) ;
ADD_BACKEND ( DRIVER_LSM303D , AP_Compass_LSM303D : : probe ( * this , hal . spi - > get_device ( HAL_INS_LSM9DS0_A_NAME ) , ROTATION_PITCH_180_YAW_270 ) ,
AP_Compass_LSM303D : : name , false ) ;
break ;
2016-12-19 13:10:47 -04:00
default :
break ;
2016-11-09 06:19:24 -04:00
}
2017-04-13 14:49:15 -03:00
2015-11-28 05:37:40 -04:00
# elif HAL_COMPASS_DEFAULT == HAL_COMPASS_BH
2018-08-01 06:10:32 -03:00
ADD_BACKEND ( DRIVER_HMC5883 , AP_Compass_HMC5843 : : probe ( * this , GET_I2C_DEVICE ( HAL_COMPASS_HMC5843_I2C_BUS , HAL_COMPASS_HMC5843_I2C_ADDR ) ) ,
2016-11-24 22:55:42 -04:00
AP_Compass_HMC5843 : : name , false ) ;
2017-06-18 04:26:25 -03:00
ADD_BACKEND ( DRIVER_AK8963 , AP_Compass_AK8963 : : probe_mpu9250 ( * this , 0 ) , AP_Compass_AK8963 : : name , false ) ;
2016-04-24 05:23:47 -03:00
# elif HAL_COMPASS_DEFAULT == HAL_COMPASS_BBBMINI
2018-08-01 06:10:32 -03:00
ADD_BACKEND ( DRIVER_HMC5883 , AP_Compass_HMC5843 : : probe ( * this , GET_I2C_DEVICE ( HAL_COMPASS_HMC5843_I2C_BUS , HAL_COMPASS_HMC5843_I2C_ADDR ) , true ) ,
2017-06-18 04:26:25 -03:00
AP_Compass_HMC5843 : : name , true ) ;
ADD_BACKEND ( DRIVER_AK8963 , AP_Compass_AK8963 : : probe_mpu9250 ( * this , 0 ) ,
AP_Compass_AK8963 : : name , false ) ;
ADD_BACKEND ( DRIVER_AK8963 , AP_Compass_AK8963 : : probe_mpu9250 ( * this , 1 ) ,
AP_Compass_AK8963 : : name , true ) ;
2016-04-25 12:37:49 -03:00
# elif HAL_COMPASS_DEFAULT == HAL_COMPASS_NAVIO2
2017-06-18 04:26:25 -03:00
ADD_BACKEND ( DRIVER_LSM9DS1 , AP_Compass_LSM9DS1 : : probe ( * this , hal . spi - > get_device ( " lsm9ds1_m " ) , ROTATION_ROLL_180 ) ,
AP_Compass_LSM9DS1 : : name , false ) ;
ADD_BACKEND ( DRIVER_AK8963 , AP_Compass_AK8963 : : probe_mpu9250 ( * this , 0 ) ,
AP_Compass_AK8963 : : name , false ) ;
2018-08-01 06:10:32 -03:00
ADD_BACKEND ( DRIVER_HMC5883 , AP_Compass_HMC5843 : : probe ( * this , GET_I2C_DEVICE ( HAL_COMPASS_HMC5843_I2C_BUS , HAL_COMPASS_HMC5843_I2C_ADDR ) , true ) ,
2017-06-18 04:26:25 -03:00
AP_Compass_HMC5843 : : name , true ) ;
2016-04-29 09:55:17 -03:00
# elif HAL_COMPASS_DEFAULT == HAL_COMPASS_NAVIO
2017-06-18 04:26:25 -03:00
ADD_BACKEND ( DRIVER_AK8963 , AP_Compass_AK8963 : : probe_mpu9250 ( * this , 0 ) ,
AP_Compass_AK8963 : : name , false ) ;
2018-08-01 06:10:32 -03:00
ADD_BACKEND ( DRIVER_HMC5883 , AP_Compass_HMC5843 : : probe ( * this , GET_I2C_DEVICE ( HAL_COMPASS_HMC5843_I2C_BUS , HAL_COMPASS_HMC5843_I2C_ADDR ) , true ) ,
2017-06-18 04:26:25 -03:00
AP_Compass_HMC5843 : : name , true ) ;
2017-08-17 19:42:17 -03:00
# elif HAL_COMPASS_DEFAULT == HAL_COMPASS_OCPOC_ZYNQ
2018-08-01 06:10:32 -03:00
ADD_BACKEND ( DRIVER_HMC5883 , AP_Compass_HMC5843 : : probe ( * this , GET_I2C_DEVICE ( HAL_COMPASS_HMC5843_I2C_BUS , HAL_COMPASS_HMC5843_I2C_ADDR ) ) ,
2017-08-17 19:42:17 -03:00
AP_Compass_HMC5843 : : name , true ) ;
ADD_BACKEND ( DRIVER_AK8963 , AP_Compass_AK8963 : : probe_mpu9250 ( * this , 0 ) ,
AP_Compass_AK8963 : : name , false ) ;
2017-05-25 12:40:35 -03:00
# elif HAL_COMPASS_DEFAULT == HAL_COMPASS_EDGE
2018-08-01 06:10:32 -03:00
ADD_BACKEND ( DRIVER_HMC5883 , AP_Compass_HMC5843 : : probe ( * this , GET_I2C_DEVICE ( HAL_COMPASS_HMC5843_I2C_BUS , HAL_COMPASS_HMC5843_I2C_ADDR ) , true ) ,
2017-05-25 12:40:35 -03:00
AP_Compass_HMC5843 : : name , true ) ;
2016-05-16 14:19:59 -03:00
# elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF || \
CONFIG_HAL_BOARD_SUBTYPE = = HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2 | | \
CONFIG_HAL_BOARD_SUBTYPE = = HAL_BOARD_SUBTYPE_LINUX_PXFMINI
2017-06-18 04:26:25 -03:00
ADD_BACKEND ( DRIVER_AK8963 , AP_Compass_AK8963 : : probe_mpu9250 ( * this , 0 ) ,
AP_Compass_AK8963 : : name , false ) ;
2018-08-01 06:10:32 -03:00
ADD_BACKEND ( DRIVER_HMC5883 , AP_Compass_HMC5843 : : probe ( * this , GET_I2C_DEVICE ( HAL_COMPASS_HMC5843_I2C_BUS , HAL_COMPASS_HMC5843_I2C_ADDR ) , true ) ,
2017-06-18 04:26:25 -03:00
AP_Compass_HMC5843 : : name , true ) ;
2017-05-30 16:38:57 -03:00
# elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BLUE
2018-08-01 06:10:32 -03:00
ADD_BACKEND ( DRIVER_AK8963 , AP_Compass_AK8963 : : probe_mpu9250 ( * this , GET_I2C_DEVICE ( HAL_COMPASS_AK8963_I2C_BUS , HAL_COMPASS_AK8963_I2C_ADDR ) ) ,
2017-06-18 04:26:25 -03:00
AP_Compass_AK8963 : : name , false ) ;
2018-08-01 06:10:32 -03:00
ADD_BACKEND ( DRIVER_HMC5883 , AP_Compass_HMC5843 : : probe ( * this , GET_I2C_DEVICE ( HAL_COMPASS_HMC5843_I2C_BUS , HAL_COMPASS_HMC5843_I2C_ADDR ) , true ) ,
2017-05-30 16:38:57 -03:00
AP_Compass_HMC5843 : : name , true ) ;
2018-05-11 12:48:03 -03:00
# elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_POCKET
ADD_BACKEND ( DRIVER_AK8963 , AP_Compass_AK8963 : : probe_mpu9250 ( * this , 0 ) ,
AP_Compass_AK8963 : : name , false ) ;
2018-08-01 06:10:32 -03:00
ADD_BACKEND ( DRIVER_HMC5883 , AP_Compass_HMC5843 : : probe ( * this , GET_I2C_DEVICE ( HAL_COMPASS_HMC5843_I2C_BUS , HAL_COMPASS_HMC5843_I2C_ADDR ) , true ) ,
2018-05-11 12:48:03 -03:00
AP_Compass_HMC5843 : : name , true ) ;
2016-03-04 13:59:06 -04:00
# elif HAL_COMPASS_DEFAULT == HAL_COMPASS_AK8963_MPU9250
2017-06-18 04:26:25 -03:00
ADD_BACKEND ( DRIVER_AK8963 , AP_Compass_AK8963 : : probe_mpu9250 ( * this , 0 ) ,
AP_Compass_AK8963 : : name , false ) ;
2014-11-15 21:58:23 -04:00
# elif HAL_COMPASS_DEFAULT == HAL_COMPASS_HMC5843
2018-02-04 06:43:12 -04:00
# ifndef HAL_COMPASS_HMC5843_ROTATION
# define HAL_COMPASS_HMC5843_ROTATION ROTATION_NONE
# endif
2018-08-01 06:10:32 -03:00
ADD_BACKEND ( DRIVER_HMC5883 , AP_Compass_HMC5843 : : probe ( * this , GET_I2C_DEVICE ( HAL_COMPASS_HMC5843_I2C_BUS , HAL_COMPASS_HMC5843_I2C_ADDR ) ,
2018-02-04 06:43:12 -04:00
false , HAL_COMPASS_HMC5843_ROTATION ) ,
2017-06-18 04:26:25 -03:00
AP_Compass_HMC5843 : : name , false ) ;
2018-02-28 03:09:04 -04:00
# if defined(HAL_COMPASS_HMC5843_I2C_EXT_BUS) && CONFIG_HAL_BOARD == HAL_BOARD_F4LIGHT
2018-08-01 06:10:32 -03:00
ADD_BACKEND ( DRIVER_HMC5883 , AP_Compass_HMC5843 : : probe ( * this , GET_I2C_DEVICE ( HAL_COMPASS_HMC5843_I2C_EXT_BUS , HAL_COMPASS_HMC5843_I2C_ADDR ) , true ) ,
2018-02-28 03:09:04 -04:00
AP_Compass_HMC5843 : : name , true ) ;
# endif
2015-08-10 20:30:32 -03:00
# elif HAL_COMPASS_DEFAULT == HAL_COMPASS_HMC5843_MPU6000
2017-06-18 04:26:25 -03:00
ADD_BACKEND ( DRIVER_HMC5883 , AP_Compass_HMC5843 : : probe_mpu6000 ( * this ) ,
AP_Compass_HMC5843 : : name , false ) ;
2016-03-04 13:59:06 -04:00
# elif HAL_COMPASS_DEFAULT == HAL_COMPASS_AK8963_I2C
2018-08-01 06:10:32 -03:00
ADD_BACKEND ( DRIVER_AK8963 , AP_Compass_AK8963 : : probe ( * this , GET_I2C_DEVICE ( HAL_COMPASS_AK8963_I2C_BUS , HAL_COMPASS_AK8963_I2C_ADDR ) ) ,
2017-06-18 04:26:25 -03:00
AP_Compass_AK8963 : : name , false ) ;
2015-09-28 17:55:07 -03:00
# elif HAL_COMPASS_DEFAULT == HAL_COMPASS_AK8963_MPU9250_I2C
2018-08-01 06:10:32 -03:00
ADD_BACKEND ( DRIVER_AK8963 , AP_Compass_AK8963 : : probe_mpu9250 ( * this , GET_I2C_DEVICE ( HAL_COMPASS_AK8963_I2C_BUS , HAL_COMPASS_AK8963_I2C_ADDR ) ) ,
2017-06-18 04:26:25 -03:00
AP_Compass_AK8963 : : name , false ) ;
2016-06-24 17:39:19 -03:00
# elif HAL_COMPASS_DEFAULT == HAL_COMPASS_AERO
2018-08-01 06:10:32 -03:00
ADD_BACKEND ( DRIVER_BMM150 , AP_Compass_BMM150 : : probe ( * this , GET_I2C_DEVICE ( HAL_COMPASS_BMM150_I2C_BUS , HAL_COMPASS_BMM150_I2C_ADDR ) ) ,
2017-06-18 04:26:25 -03:00
AP_Compass_BMM150 : : name , false ) ;
2018-08-01 06:10:32 -03:00
ADD_BACKEND ( DRIVER_HMC5883 , AP_Compass_HMC5843 : : probe ( * this , GET_I2C_DEVICE ( HAL_COMPASS_HMC5843_I2C_BUS , HAL_COMPASS_HMC5843_I2C_ADDR ) , true ) ,
2017-09-20 19:08:44 -03:00
AP_Compass_HMC5843 : : name , true ) ;
2018-08-01 06:10:32 -03:00
ADD_BACKEND ( DRIVER_IST8310 , AP_Compass_IST8310 : : probe ( * this , GET_I2C_DEVICE ( HAL_COMPASS_IST8310_I2C_BUS , HAL_COMPASS_IST8310_I2C_ADDR ) ,
2017-09-20 19:08:44 -03:00
true , ROTATION_PITCH_180_YAW_90 ) , AP_Compass_IST8310 : : name , true ) ;
2017-10-30 06:03:04 -03:00
# elif HAL_COMPASS_DEFAULT == HAL_COMPASS_LIS3MDL
ADD_BACKEND ( DRIVER_LIS3MDL , AP_Compass_LIS3MDL : : probe ( * this , hal . spi - > get_device ( HAL_COMPASS_LIS3MDL_NAME ) , false , ROTATION_ROLL_180_YAW_90 ) ,
AP_Compass_LIS3MDL : : name , false ) ;
2018-02-02 05:25:24 -04:00
# elif HAL_COMPASS_DEFAULT == HAL_COMPASS_MAG3110
2018-08-01 06:10:32 -03:00
ADD_BACKEND ( DRIVER_MAG3110 , AP_Compass_MAG3110 : : probe ( * this , GET_I2C_DEVICE ( HAL_MAG3110_I2C_BUS , HAL_MAG3110_I2C_ADDR ) , ROTATION_NONE ) ,
2018-02-02 05:25:24 -04:00
AP_Compass_MAG3110 : : name , true ) ;
2018-02-25 02:03:00 -04:00
# elif HAL_COMPASS_DEFAULT == HAL_COMPASS_IST8310
2018-08-01 06:10:32 -03:00
ADD_BACKEND ( DRIVER_IST8310 , AP_Compass_IST8310 : : probe ( * this , GET_I2C_DEVICE ( HAL_COMPASS_IST8310_I2C_BUS , HAL_COMPASS_IST8310_I2C_ADDR ) ,
2018-02-25 02:03:00 -04:00
true , ROTATION_PITCH_180_YAW_90 ) , AP_Compass_IST8310 : : name , true ) ;
2018-02-06 06:39:55 -04:00
# elif HAL_COMPASS_DEFAULT == HAL_COMPASS_QMC5883L
2018-08-01 06:10:32 -03:00
ADD_BACKEND ( DRIVER_QMC5883 , AP_Compass_QMC5883L : : probe ( * this , GET_I2C_DEVICE ( 1 , HAL_COMPASS_QMC5883L_I2C_ADDR ) ,
true , HAL_COMPASS_QMC5883L_ORIENTATION_EXTERNAL ) ,
2018-02-06 06:39:55 -04:00
AP_Compass_QMC5883L : : name , true ) ;
2018-08-01 06:10:32 -03:00
ADD_BACKEND ( DRIVER_QMC5883 , AP_Compass_QMC5883L : : probe ( * this , GET_I2C_DEVICE ( 0 , HAL_COMPASS_QMC5883L_I2C_ADDR ) ,
false , HAL_COMPASS_QMC5883L_ORIENTATION_INTERNAL ) ,
2018-02-11 04:28:14 -04:00
AP_Compass_QMC5883L : : name , false ) ;
2016-03-04 13:59:06 -04:00
# elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX && CONFIG_HAL_BOARD_SUBTYPE != HAL_BOARD_SUBTYPE_LINUX_NONE
2018-08-01 06:10:32 -03:00
ADD_BACKEND ( DRIVER_HMC5883 , AP_Compass_HMC5843 : : probe ( * this , GET_I2C_DEVICE ( HAL_COMPASS_HMC5843_I2C_BUS , HAL_COMPASS_HMC5843_I2C_ADDR ) ) ,
2017-06-18 04:26:25 -03:00
AP_Compass_HMC5843 : : name , false ) ;
ADD_BACKEND ( DRIVER_AK8963 , AP_Compass_AK8963 : : probe_mpu9250 ( * this , 0 ) ,
AP_Compass_AK8963 : : name , false ) ;
ADD_BACKEND ( DRIVER_LSM9DS1 , AP_Compass_LSM9DS1 : : probe ( * this , hal . spi - > get_device ( " lsm9ds1_m " ) ) ,
AP_Compass_LSM9DS1 : : name , false ) ;
2018-01-19 01:06:45 -04:00
# elif HAL_COMPASS_DEFAULT == HAL_COMPASS_BMM150_I2C
2018-08-01 06:10:32 -03:00
ADD_BACKEND ( DRIVER_BMM150 , AP_Compass_BMM150 : : probe ( * this , GET_I2C_DEVICE ( HAL_COMPASS_BMM150_I2C_BUS , HAL_COMPASS_BMM150_I2C_ADDR ) ) ,
2018-01-05 03:05:11 -04:00
AP_Compass_BMM150 : : name , true ) ;
2018-03-01 20:45:02 -04:00
# elif HAL_COMPASS_DEFAULT == HAL_COMPASS_NONE
// no compass
2014-11-15 21:58:23 -04:00
# else
# error Unrecognised HAL_COMPASS_TYPE setting
# endif
2018-02-28 03:09:04 -04:00
# if defined(BOARD_I2C_BUS_EXT) && CONFIG_HAL_BOARD == HAL_BOARD_F4LIGHT
// autodetect external i2c bus
2018-08-01 06:10:32 -03:00
ADD_BACKEND ( DRIVER_QMC5883 , AP_Compass_QMC5883L : : probe ( * this , GET_I2C_DEVICE ( BOARD_I2C_BUS_EXT , HAL_COMPASS_QMC5883L_I2C_ADDR ) ,
2018-02-28 03:09:04 -04:00
true , ROTATION_NONE ) ,
AP_Compass_QMC5883L : : name , true ) ;
2018-08-01 06:10:32 -03:00
ADD_BACKEND ( DRIVER_HMC5883 , AP_Compass_HMC5843 : : probe ( * this , GET_I2C_DEVICE ( BOARD_I2C_BUS_EXT , HAL_COMPASS_HMC5843_I2C_ADDR ) ) ,
2018-02-28 03:09:04 -04:00
AP_Compass_HMC5843 : : name , true ) ;
// lis3mdl
2018-08-01 06:10:32 -03:00
ADD_BACKEND ( DRIVER_LIS3MDL , AP_Compass_LIS3MDL : : probe ( * this , GET_I2C_DEVICE ( BOARD_I2C_BUS_EXT , HAL_COMPASS_LIS3MDL_I2C_ADDR ) ,
2018-02-28 03:09:04 -04:00
true , ROTATION_NONE ) ,
AP_Compass_LIS3MDL : : name , true ) ;
// AK09916
2018-08-01 06:10:32 -03:00
ADD_BACKEND ( DRIVER_AK09916 , AP_Compass_AK09916 : : probe ( * this , GET_I2C_DEVICE ( BOARD_I2C_BUS_EXT , HAL_COMPASS_AK09916_I2C_ADDR ) ,
2018-02-28 03:09:04 -04:00
true , ROTATION_NONE ) ,
AP_Compass_AK09916 : : name , true ) ;
2018-08-01 06:10:32 -03:00
ADD_BACKEND ( DRIVER_IST8310 , AP_Compass_IST8310 : : probe ( * this , GET_I2C_DEVICE ( BOARD_I2C_BUS_EXT , HAL_COMPASS_IST8310_I2C_ADDR ) ,
2018-02-28 03:09:04 -04:00
ROTATION_NONE ) ,
AP_Compass_IST8310 : : name , true ) ;
2018-04-29 13:00:05 -03:00
# ifdef HAL_COMPASS_BMM150_I2C_ADDR
2018-08-01 06:10:32 -03:00
ADD_BACKEND ( DRIVER_BMM150 , AP_Compass_BMM150 : : probe ( * this , GET_I2C_DEVICE ( BOARD_I2C_BUS_EXT , HAL_COMPASS_BMM150_I2C_ADDR ) ) ,
2018-02-28 03:09:04 -04:00
AP_Compass_BMM150 : : name , true ) ;
2018-04-29 13:00:05 -03:00
# endif
2018-02-28 03:09:04 -04:00
2018-08-01 06:10:32 -03:00
ADD_BACKEND ( DRIVER_MAG3110 , AP_Compass_MAG3110 : : probe ( * this , GET_I2C_DEVICE ( BOARD_I2C_BUS_EXT , HAL_MAG3110_I2C_ADDR ) , ROTATION_NONE ) ,
2018-02-28 03:09:04 -04:00
AP_Compass_MAG3110 : : name , true ) ;
2018-08-01 06:10:32 -03:00
ADD_BACKEND ( DRIVER_QMC5883 , AP_Compass_QMC5883L : : probe ( * this , GET_I2C_DEVICE ( BOARD_I2C_BUS_EXT , HAL_COMPASS_QMC5883L_I2C_ADDR ) , ROTATION_NONE ) ,
2018-02-28 03:09:04 -04:00
AP_Compass_QMC5883L : : name , true ) ;
# endif
2018-04-13 20:31:11 -03:00
/* for chibios external board coniguration */
# ifdef HAL_EXT_COMPASS_HMC5843_I2C_BUS
2018-08-01 06:10:32 -03:00
ADD_BACKEND ( DRIVER_HMC5883 , AP_Compass_HMC5843 : : probe ( * this , GET_I2C_DEVICE ( HAL_EXT_COMPASS_HMC5843_I2C_BUS , HAL_COMPASS_HMC5843_I2C_ADDR ) ,
2018-04-13 20:31:11 -03:00
true , ROTATION_ROLL_180 ) ,
AP_Compass_HMC5843 : : name , true ) ;
# endif
2017-06-18 04:26:25 -03:00
# if HAL_WITH_UAVCAN
if ( _driver_enabled ( DRIVER_UAVCAN ) ) {
bool added ;
do {
added = _add_backend ( AP_Compass_UAVCAN : : probe ( * this ) , " UAVCAN " , true ) ;
if ( _backend_count = = COMPASS_MAX_BACKEND | | _compass_count = = COMPASS_MAX_INSTANCES ) {
return ;
}
} while ( added ) ;
}
# endif
2014-11-15 21:58:23 -04:00
if ( _backend_count = = 0 | |
_compass_count = = 0 ) {
2017-01-21 00:36:34 -04:00
hal . console - > printf ( " No Compass backends available \n " ) ;
2014-11-15 21:58:23 -04:00
}
}
2016-03-01 15:16:04 -04:00
void
2014-11-15 21:58:23 -04:00
Compass : : accumulate ( void )
2016-03-01 15:16:04 -04:00
{
2014-11-15 21:58:23 -04:00
for ( uint8_t i = 0 ; i < _backend_count ; i + + ) {
// call accumulate on each of the backend
_backends [ i ] - > accumulate ( ) ;
}
}
2016-03-01 15:16:04 -04:00
bool
2014-11-15 21:58:23 -04:00
Compass : : read ( void )
{
2015-02-23 19:17:44 -04:00
for ( uint8_t i = 0 ; i < _backend_count ; i + + ) {
2014-11-15 21:58:23 -04:00
// call read on each of the backend. This call updates field[i]
_backends [ i ] - > read ( ) ;
2016-03-01 15:16:04 -04:00
}
2017-09-05 22:58:35 -03:00
uint32_t time = AP_HAL : : millis ( ) ;
2015-03-13 02:43:34 -03:00
for ( uint8_t i = 0 ; i < COMPASS_MAX_INSTANCES ; i + + ) {
2017-09-05 22:58:35 -03:00
_state [ i ] . healthy = ( time - _state [ i ] . last_update_ms < 500 ) ;
2015-03-13 02:43:34 -03:00
}
2015-02-23 19:17:44 -04:00
return healthy ( ) ;
2014-11-15 21:58:23 -04:00
}
2015-03-18 20:42:30 -03:00
uint8_t
Compass : : get_healthy_mask ( ) const
{
uint8_t healthy_mask = 0 ;
for ( uint8_t i = 0 ; i < COMPASS_MAX_INSTANCES ; i + + ) {
if ( healthy ( i ) ) {
healthy_mask | = 1 < < i ;
}
}
return healthy_mask ;
}
2014-10-14 19:16:31 -03:00
void
Compass : : set_offsets ( uint8_t i , const Vector3f & offsets )
{
// sanity check compass instance provided
if ( i < COMPASS_MAX_INSTANCES ) {
2015-02-23 19:17:44 -04:00
_state [ i ] . offset . set ( offsets ) ;
2014-10-14 19:16:31 -03:00
}
}
2011-02-14 00:25:20 -04:00
void
2014-07-09 05:09:02 -03:00
Compass : : set_and_save_offsets ( uint8_t i , const Vector3f & offsets )
2011-02-14 00:25:20 -04:00
{
2014-07-10 02:37:27 -03:00
// sanity check compass instance provided
if ( i < COMPASS_MAX_INSTANCES ) {
2015-02-23 19:17:44 -04:00
_state [ i ] . offset . set ( offsets ) ;
2014-07-10 02:37:27 -03:00
save_offsets ( i ) ;
}
2011-02-14 00:25:20 -04:00
}
2015-03-18 20:18:47 -03:00
void
Compass : : set_and_save_diagonals ( uint8_t i , const Vector3f & diagonals )
{
// sanity check compass instance provided
if ( i < COMPASS_MAX_INSTANCES ) {
_state [ i ] . diagonals . set_and_save ( diagonals ) ;
}
}
void
Compass : : set_and_save_offdiagonals ( uint8_t i , const Vector3f & offdiagonals )
{
// sanity check compass instance provided
if ( i < COMPASS_MAX_INSTANCES ) {
_state [ i ] . offdiagonals . set_and_save ( offdiagonals ) ;
}
}
2011-02-14 00:25:20 -04:00
void
2014-07-09 05:07:30 -03:00
Compass : : save_offsets ( uint8_t i )
2011-02-14 00:25:20 -04:00
{
2015-02-23 19:17:44 -04:00
_state [ i ] . offset . save ( ) ; // save offsets
_state [ i ] . dev_id . save ( ) ; // save device id corresponding to these offsets
2014-07-09 05:07:30 -03:00
}
void
Compass : : save_offsets ( void )
{
for ( uint8_t i = 0 ; i < COMPASS_MAX_INSTANCES ; i + + ) {
save_offsets ( i ) ;
2013-12-09 02:33:54 -04:00
}
2011-02-14 00:25:20 -04:00
}
2013-02-27 03:57:04 -04:00
void
2014-07-22 04:29:43 -03:00
Compass : : set_motor_compensation ( uint8_t i , const Vector3f & motor_comp_factor )
2013-02-27 03:57:04 -04:00
{
2015-02-23 19:17:44 -04:00
_state [ i ] . motor_compensation . set ( motor_comp_factor ) ;
2013-02-27 03:57:04 -04:00
}
void
Compass : : save_motor_compensation ( )
{
2013-03-03 10:02:12 -04:00
_motor_comp_type . save ( ) ;
2013-12-09 04:45:31 -04:00
for ( uint8_t k = 0 ; k < COMPASS_MAX_INSTANCES ; k + + ) {
2015-02-23 19:17:44 -04:00
_state [ k ] . motor_compensation . save ( ) ;
2013-12-09 04:45:31 -04:00
}
2013-02-27 03:57:04 -04:00
}
2012-03-30 00:18:06 -03:00
void
2012-08-18 08:41:38 -03:00
Compass : : set_initial_location ( int32_t latitude , int32_t longitude )
2012-03-10 19:19:04 -04:00
{
2012-03-30 00:18:06 -03:00
// if automatic declination is configured, then compute
// the declination based on the initial GPS fix
2012-08-17 03:19:22 -03:00
if ( _auto_declination ) {
// Set the declination based on the lat/lng from GPS
2012-10-11 17:48:39 -03:00
_declination . set ( radians (
AP_Declination : : get_declination (
( float ) latitude / 10000000 ,
( float ) longitude / 10000000 ) ) ) ;
2012-08-17 03:19:22 -03:00
}
2012-03-10 19:19:04 -04:00
}
2014-09-18 09:56:13 -03:00
/// return true if the compass should be used for yaw calculations
bool
Compass : : use_for_yaw ( void ) const
{
uint8_t prim = get_primary ( ) ;
return healthy ( prim ) & & use_for_yaw ( prim ) ;
}
/// return true if the specified compass can be used for yaw calculations
bool
Compass : : use_for_yaw ( uint8_t i ) const
{
2017-08-18 05:58:08 -03:00
// when we are doing in-flight compass learning the state
// estimator must not use the compass. The learning code turns off
// inflight learning when it has converged
return _state [ i ] . use_for_yaw & & _learn . get ( ) ! = LEARN_INFLIGHT ;
2014-09-18 09:56:13 -03:00
}
2011-02-14 00:25:20 -04:00
void
2013-04-15 09:50:44 -03:00
Compass : : set_declination ( float radians , bool save_to_eeprom )
2011-02-14 00:25:20 -04:00
{
2013-04-15 09:50:44 -03:00
if ( save_to_eeprom ) {
_declination . set_and_save ( radians ) ;
} else {
_declination . set ( radians ) ;
}
2011-02-14 00:25:20 -04:00
}
2011-02-18 23:57:53 -04:00
float
2013-05-08 20:22:00 -03:00
Compass : : get_declination ( ) const
2011-02-18 23:57:53 -04:00
{
2012-08-17 03:19:22 -03:00
return _declination . get ( ) ;
2011-02-18 23:57:53 -04:00
}
2013-08-18 08:08:34 -03:00
/*
calculate a compass heading given the attitude from DCM and the mag vector
*/
2012-06-20 06:30:30 -03:00
float
2016-01-22 13:30:59 -04:00
Compass : : calculate_heading ( const Matrix3f & dcm_matrix , uint8_t i ) const
2011-02-14 00:25:20 -04:00
{
2013-08-18 08:08:34 -03:00
float cos_pitch_sq = 1.0f - ( dcm_matrix . c . x * dcm_matrix . c . x ) ;
2011-05-08 15:15:29 -03:00
// Tilt compensated magnetic field Y component:
2016-01-22 13:30:59 -04:00
const Vector3f & field = get_field ( i ) ;
2015-02-23 19:17:44 -04:00
float headY = field . y * dcm_matrix . c . z - field . z * dcm_matrix . c . y ;
2012-02-23 19:42:03 -04:00
2011-02-14 00:25:20 -04:00
// Tilt compensated magnetic field X component:
2015-02-23 19:17:44 -04:00
float headX = field . x * cos_pitch_sq - dcm_matrix . c . x * ( field . y * dcm_matrix . c . y + field . z * dcm_matrix . c . z ) ;
2013-05-08 20:19:01 -03:00
2011-02-14 00:25:20 -04:00
// magnetic heading
2011-07-08 00:56:04 -03:00
// 6/4/11 - added constrain to keep bad values from ruining DCM Yaw - Jason S.
2013-05-08 20:19:01 -03:00
float heading = constrain_float ( atan2f ( - headY , headX ) , - 3.15f , 3.15f ) ;
2011-02-14 00:25:20 -04:00
// Declination correction (if supplied)
2013-01-10 14:42:24 -04:00
if ( fabsf ( _declination ) > 0.0f )
2011-02-14 00:25:20 -04:00
{
heading = heading + _declination ;
2016-02-25 13:13:02 -04:00
if ( heading > M_PI ) // Angle normalization (-180 deg, 180 deg)
heading - = ( 2.0f * M_PI ) ;
else if ( heading < - M_PI )
heading + = ( 2.0f * M_PI ) ;
2011-02-14 00:25:20 -04:00
}
2012-06-20 06:30:30 -03:00
return heading ;
2011-02-14 00:25:20 -04:00
}
2014-07-07 09:24:18 -03:00
/// Returns True if the compasses have been configured (i.e. offsets saved)
///
/// @returns True if compass has been configured
///
bool Compass : : configured ( uint8_t i )
{
// exit immediately if instance is beyond the number of compasses we have available
if ( i > get_count ( ) ) {
return false ;
}
// exit immediately if all offsets are zero
2015-09-28 16:56:49 -03:00
if ( is_zero ( get_offsets ( i ) . length ( ) ) ) {
2015-08-24 11:53:17 -03:00
return false ;
}
2014-07-07 09:24:18 -03:00
// backup detected dev_id
2015-02-23 19:17:44 -04:00
int32_t dev_id_orig = _state [ i ] . dev_id ;
2014-07-07 09:24:18 -03:00
// load dev_id from eeprom
2015-02-23 19:17:44 -04:00
_state [ i ] . dev_id . load ( ) ;
2014-07-07 09:24:18 -03:00
// if different then the device has not been configured
2015-02-23 19:17:44 -04:00
if ( _state [ i ] . dev_id ! = dev_id_orig ) {
2014-07-07 09:24:18 -03:00
// restore device id
2015-02-23 19:17:44 -04:00
_state [ i ] . dev_id = dev_id_orig ;
2014-07-07 09:24:18 -03:00
// return failure
return false ;
}
2012-03-27 01:16:00 -03:00
2014-07-07 09:24:18 -03:00
// if we got here then it must be configured
return true ;
}
bool Compass : : configured ( void )
{
bool all_configured = true ;
for ( uint8_t i = 0 ; i < get_count ( ) ; i + + ) {
2015-03-17 22:38:55 -03:00
all_configured = all_configured & & ( ! use_for_yaw ( i ) | | configured ( i ) ) ;
2014-07-07 09:24:18 -03:00
}
return all_configured ;
}
2014-07-31 22:19:53 -03:00
2014-11-15 21:58:23 -04:00
// Update raw magnetometer values from HIL data
//
2015-05-15 01:04:00 -03:00
void Compass : : setHIL ( uint8_t instance , float roll , float pitch , float yaw )
2014-11-15 21:58:23 -04:00
{
Matrix3f R ;
// create a rotation matrix for the given attitude
R . from_euler ( roll , pitch , yaw ) ;
2015-05-04 23:35:20 -03:00
if ( ! is_equal ( _hil . last_declination , get_declination ( ) ) ) {
2014-11-15 21:58:23 -04:00
_setup_earth_field ( ) ;
2015-02-23 19:17:44 -04:00
_hil . last_declination = get_declination ( ) ;
2014-11-15 21:58:23 -04:00
}
// convert the earth frame magnetic vector to body frame, and
// apply the offsets
2015-05-15 01:04:00 -03:00
_hil . field [ instance ] = R . mul_transpose ( _hil . Bearth ) ;
2014-11-15 21:58:23 -04:00
// apply default board orientation for this compass type. This is
// a noop on most boards
2015-05-15 01:04:00 -03:00
_hil . field [ instance ] . rotate ( MAG_BOARD_ORIENTATION ) ;
2014-11-15 21:58:23 -04:00
// add user selectable orientation
2015-05-15 01:04:00 -03:00
_hil . field [ instance ] . rotate ( ( enum Rotation ) _state [ 0 ] . orientation . get ( ) ) ;
2014-11-15 21:58:23 -04:00
2015-02-23 19:17:44 -04:00
if ( ! _state [ 0 ] . external ) {
2014-11-15 21:58:23 -04:00
// and add in AHRS_ORIENTATION setting if not an external compass
2018-03-08 22:26:18 -04:00
if ( _board_orientation = = ROTATION_CUSTOM & & _custom_rotation ) {
_hil . field [ instance ] = * _custom_rotation * _hil . field [ instance ] ;
} else {
_hil . field [ instance ] . rotate ( _board_orientation ) ;
}
2014-11-15 21:58:23 -04:00
}
2015-05-15 01:04:00 -03:00
_hil . healthy [ instance ] = true ;
2014-11-15 21:58:23 -04:00
}
// Update raw magnetometer values from HIL mag vector
//
2016-05-04 04:11:02 -03:00
void Compass : : setHIL ( uint8_t instance , const Vector3f & mag , uint32_t update_usec )
2014-11-15 21:58:23 -04:00
{
2015-05-15 01:04:00 -03:00
_hil . field [ instance ] = mag ;
_hil . healthy [ instance ] = true ;
2016-05-04 04:11:02 -03:00
_state [ instance ] . last_update_usec = update_usec ;
2014-11-15 21:58:23 -04:00
}
2016-03-01 15:16:04 -04:00
const Vector3f & Compass : : getHIL ( uint8_t instance ) const
2015-05-15 01:04:00 -03:00
{
return _hil . field [ instance ] ;
2014-11-15 21:58:23 -04:00
}
// setup _Bearth
void Compass : : _setup_earth_field ( void )
{
// assume a earth field strength of 400
2015-02-23 19:17:44 -04:00
_hil . Bearth ( 400 , 0 , 0 ) ;
2016-03-01 15:16:04 -04:00
2014-11-15 21:58:23 -04:00
// rotate _Bearth for inclination and declination. -66 degrees
// is the inclination in Canberra, Australia
Matrix3f R ;
R . from_euler ( 0 , ToRad ( 66 ) , get_declination ( ) ) ;
2015-02-23 19:17:44 -04:00
_hil . Bearth = R * _hil . Bearth ;
}
/*
set the type of motor compensation to use
*/
void Compass : : motor_compensation_type ( const uint8_t comp_type )
{
if ( _motor_comp_type < = AP_COMPASS_MOT_COMP_CURRENT & & _motor_comp_type ! = ( int8_t ) comp_type ) {
_motor_comp_type = ( int8_t ) comp_type ;
2018-03-01 17:42:31 -04:00
_thr = 0 ; // set current throttle to zero
2015-02-23 19:17:44 -04:00
for ( uint8_t i = 0 ; i < COMPASS_MAX_INSTANCES ; i + + ) {
set_motor_compensation ( i , Vector3f ( 0 , 0 , 0 ) ) ; // clear out invalid compensation vectors
}
}
2014-11-15 21:58:23 -04:00
}
2015-08-14 21:27:23 -03:00
2015-09-15 03:54:26 -03:00
bool Compass : : consistent ( ) const
2015-08-14 21:27:23 -03:00
{
2018-06-21 20:43:30 -03:00
const Vector3f & primary_mag_field = get_field ( ) ;
const Vector2f primary_mag_field_xy = Vector2f ( primary_mag_field . x , primary_mag_field . y ) ;
2015-08-14 21:27:23 -03:00
2018-06-21 20:43:30 -03:00
if ( primary_mag_field_xy . is_zero ( ) ) {
2015-08-14 21:27:23 -03:00
return false ;
}
2018-06-21 20:43:30 -03:00
const Vector3f primary_mag_field_norm = primary_mag_field . normalized ( ) ;
const Vector2f primary_mag_field_xy_norm = primary_mag_field_xy . normalized ( ) ;
2015-08-14 21:27:23 -03:00
2015-09-15 03:54:26 -03:00
for ( uint8_t i = 0 ; i < get_count ( ) ; i + + ) {
2018-06-21 20:43:30 -03:00
if ( ! use_for_yaw ( i ) ) {
// configured not-to-be-used
continue ;
}
2015-08-14 21:27:23 -03:00
2018-06-21 20:43:30 -03:00
Vector3f mag_field = get_field ( i ) ;
Vector2f mag_field_xy = Vector2f ( mag_field . x , mag_field . y ) ;
2015-08-14 21:27:23 -03:00
2018-06-21 20:43:30 -03:00
if ( mag_field_xy . is_zero ( ) ) {
return false ;
}
2015-08-14 21:27:23 -03:00
2018-06-21 20:43:30 -03:00
const float xy_len_diff = ( primary_mag_field_xy - mag_field_xy ) . length ( ) ;
2015-08-14 21:27:23 -03:00
2018-06-21 20:43:30 -03:00
mag_field . normalize ( ) ;
mag_field_xy . normalize ( ) ;
2015-08-14 21:27:23 -03:00
2018-06-21 20:43:30 -03:00
const float xyz_ang_diff = acosf ( constrain_float ( mag_field * primary_mag_field_norm , - 1.0f , 1.0f ) ) ;
const float xy_ang_diff = acosf ( constrain_float ( mag_field_xy * primary_mag_field_xy_norm , - 1.0f , 1.0f ) ) ;
2015-08-14 21:27:23 -03:00
2018-06-21 20:43:30 -03:00
// check for gross misalignment on all axes
if ( xyz_ang_diff > AP_COMPASS_MAX_XYZ_ANG_DIFF ) {
return false ;
}
2015-08-14 21:27:23 -03:00
2018-06-21 20:43:30 -03:00
// check for an unacceptable angle difference on the xy plane
if ( xy_ang_diff > AP_COMPASS_MAX_XY_ANG_DIFF ) {
return false ;
}
// check for an unacceptable length difference on the xy plane
if ( xy_len_diff > AP_COMPASS_MAX_XY_LENGTH_DIFF ) {
return false ;
2015-08-14 21:27:23 -03:00
}
}
return true ;
}
2015-09-15 03:54:26 -03:00
2018-03-29 01:13:55 -03:00
// singleton instance
Compass * Compass : : _singleton ;
namespace AP {
Compass & compass ( )
{
return * Compass : : get_singleton ( ) ;
}
}
