From cbb1512ebcbf0785999f1c40af57f31de19b538f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 11 May 2020 19:38:28 +1000 Subject: [PATCH] AP_Compass: merged in compass device ID changes from master --- libraries/AP_Compass/AP_Compass.cpp | 799 +++++++++++++----- libraries/AP_Compass/AP_Compass.h | 151 ++-- libraries/AP_Compass/AP_Compass_AK09916.cpp | 17 +- libraries/AP_Compass/AP_Compass_AK8963.cpp | 12 +- libraries/AP_Compass/AP_Compass_BMM150.cpp | 13 +- libraries/AP_Compass/AP_Compass_Backend.cpp | 56 +- libraries/AP_Compass/AP_Compass_Backend.h | 4 +- .../AP_Compass/AP_Compass_Calibration.cpp | 66 +- libraries/AP_Compass/AP_Compass_HIL.cpp | 7 +- libraries/AP_Compass/AP_Compass_HIL.h | 2 +- libraries/AP_Compass/AP_Compass_HMC5843.cpp | 13 +- libraries/AP_Compass/AP_Compass_IST8308.cpp | 14 +- libraries/AP_Compass/AP_Compass_IST8310.cpp | 14 +- libraries/AP_Compass/AP_Compass_LIS3MDL.cpp | 13 +- libraries/AP_Compass/AP_Compass_LSM303D.cpp | 14 +- libraries/AP_Compass/AP_Compass_LSM9DS1.cpp | 12 +- libraries/AP_Compass/AP_Compass_MAG3110.cpp | 13 +- libraries/AP_Compass/AP_Compass_MMC3416.cpp | 23 +- libraries/AP_Compass/AP_Compass_QMC5883L.cpp | 14 +- libraries/AP_Compass/AP_Compass_RM3100.cpp | 17 +- libraries/AP_Compass/AP_Compass_SITL.cpp | 39 +- libraries/AP_Compass/AP_Compass_SITL.h | 5 +- libraries/AP_Compass/AP_Compass_UAVCAN.cpp | 59 +- libraries/AP_Compass/AP_Compass_UAVCAN.h | 4 +- libraries/AP_Compass/CompassCalibrator.cpp | 11 +- libraries/AP_Compass/Compass_learn.cpp | 39 +- libraries/AP_Compass/Compass_learn.h | 1 - 27 files changed, 978 insertions(+), 454 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp index 5cf9e6dc2a..e5acdb8fb8 100644 --- a/libraries/AP_Compass/AP_Compass.cpp +++ b/libraries/AP_Compass/AP_Compass.cpp @@ -5,6 +5,7 @@ #include #include #include +#include #include "AP_Compass_SITL.h" #include "AP_Compass_AK8963.h" @@ -27,6 +28,7 @@ #include "AP_Compass_RM3100.h" #include "AP_Compass.h" #include "Compass_learn.h" +#include extern const AP_HAL::HAL& hal; @@ -39,17 +41,13 @@ extern const AP_HAL::HAL& hal; #endif #ifndef HAL_COMPASS_FILTER_DEFAULT - #define HAL_COMPASS_FILTER_DEFAULT 0 // turned off by default +#define HAL_COMPASS_FILTER_DEFAULT 0 // turned off by default #endif #ifndef HAL_COMPASS_AUTO_ROT_DEFAULT #define HAL_COMPASS_AUTO_ROT_DEFAULT 2 #endif -#ifndef HAL_COMPASS_MAX_SENSORS -#define HAL_COMPASS_MAX_SENSORS 3 -#endif - const AP_Param::GroupInfo Compass::var_info[] = { // index 0 was used for the old orientation matrix @@ -60,6 +58,7 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @Units: mGauss // @Increment: 1 // @User: Advanced + // @Calibration: 1 // @Param: OFS_Y // @DisplayName: Compass offsets in milligauss on the Y axis @@ -68,6 +67,7 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @Units: mGauss // @Increment: 1 // @User: Advanced + // @Calibration: 1 // @Param: OFS_Z // @DisplayName: Compass offsets in milligauss on the Z axis @@ -76,7 +76,7 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @Units: mGauss // @Increment: 1 // @User: Advanced - AP_GROUPINFO("OFS", 1, Compass, _state[0].offset, 0), + AP_GROUPINFO("OFS", 1, Compass, _state._priv_instance[0].offset, 0), // @Param: DEC // @DisplayName: Compass declination @@ -87,19 +87,21 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @User: Standard AP_GROUPINFO("DEC", 2, Compass, _declination, 0), +#if COMPASS_LEARN_ENABLED // @Param: LEARN // @DisplayName: Learn compass offsets automatically // @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. If InFlight learning is enabled then the compass with automatically start learning once a flight starts (must be armed). While InFlight learning is running you cannot use position control modes. // @Values: 0:Disabled,1:Internal-Learning,2:EKF-Learning,3:InFlight-Learning // @User: Advanced AP_GROUPINFO("LEARN", 3, Compass, _learn, COMPASS_LEARN_DEFAULT), +#endif // @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 - AP_GROUPINFO("USE", 4, Compass, _state[0].use_for_yaw, 1), // true if used for DCM yaw + AP_GROUPINFO("USE", 4, Compass, _use_for_yaw._priv_instance[0], 1), // true if used for DCM yaw // @Param: AUTODEC // @DisplayName: Auto Declination @@ -108,11 +110,13 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @User: Advanced AP_GROUPINFO("AUTODEC",5, Compass, _auto_declination, 1), +#if COMPASS_MOT_ENABLED // @Param: MOTCT // @DisplayName: Motor interference compensation type // @Description: Set motor interference compensation type to disabled, throttle or current. Do not change manually. // @Values: 0:Disabled,1:Use Throttle,2:Use Current // @User: Advanced + // @Calibration: 1 AP_GROUPINFO("MOTCT", 6, Compass, _motor_comp_type, AP_COMPASS_MOT_COMP_DISABLED), // @Param: MOT_X @@ -122,6 +126,7 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @Units: mGauss/A // @Increment: 1 // @User: Advanced + // @Calibration: 1 // @Param: MOT_Y // @DisplayName: Motor interference compensation for body frame Y axis @@ -130,6 +135,7 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @Units: mGauss/A // @Increment: 1 // @User: Advanced + // @Calibration: 1 // @Param: MOT_Z // @DisplayName: Motor interference compensation for body frame Z axis @@ -138,23 +144,24 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @Units: mGauss/A // @Increment: 1 // @User: Advanced - AP_GROUPINFO("MOT", 7, Compass, _state[0].motor_compensation, 0), + AP_GROUPINFO("MOT", 7, Compass, _state._priv_instance[0].motor_compensation, 0), +#endif // @Param: ORIENT // @DisplayName: Compass orientation // @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. - // @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 + // @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,100:Custom // @User: Advanced - AP_GROUPINFO("ORIENT", 8, Compass, _state[0].orientation, ROTATION_NONE), + AP_GROUPINFO("ORIENT", 8, Compass, _state._priv_instance[0].orientation, ROTATION_NONE), // @Param: EXTERNAL // @DisplayName: Compass is attached via an external cable // @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 // @User: Advanced - AP_GROUPINFO("EXTERNAL", 9, Compass, _state[0].external, 0), + AP_GROUPINFO("EXTERNAL", 9, Compass, _state._priv_instance[0].external, 0), -#if HAL_COMPASS_MAX_SENSORS > 1 +#if COMPASS_MAX_INSTANCES > 1 // @Param: OFS2_X // @DisplayName: Compass2 offsets in milligauss on the X axis // @Description: Offset to be added to compass2's x-axis values to compensate for metal in the frame @@ -162,6 +169,7 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @Units: mGauss // @Increment: 1 // @User: Advanced + // @Calibration: 1 // @Param: OFS2_Y // @DisplayName: Compass2 offsets in milligauss on the Y axis @@ -170,6 +178,7 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @Units: mGauss // @Increment: 1 // @User: Advanced + // @Calibration: 1 // @Param: OFS2_Z // @DisplayName: Compass2 offsets in milligauss on the Z axis @@ -178,7 +187,7 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @Units: mGauss // @Increment: 1 // @User: Advanced - AP_GROUPINFO("OFS2", 10, Compass, _state[1].offset, 0), + AP_GROUPINFO("OFS2", 10, Compass, _state._priv_instance[1].offset, 0), // @Param: MOT2_X // @DisplayName: Motor interference compensation to compass2 for body frame X axis @@ -187,6 +196,7 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @Units: mGauss/A // @Increment: 1 // @User: Advanced + // @Calibration: 1 // @Param: MOT2_Y // @DisplayName: Motor interference compensation to compass2 for body frame Y axis @@ -195,6 +205,7 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @Units: mGauss/A // @Increment: 1 // @User: Advanced + // @Calibration: 1 // @Param: MOT2_Z // @DisplayName: Motor interference compensation to compass2 for body frame Z axis @@ -203,17 +214,11 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @Units: mGauss/A // @Increment: 1 // @User: Advanced - AP_GROUPINFO("MOT2", 11, Compass, _state[1].motor_compensation, 0), + AP_GROUPINFO("MOT2", 11, Compass, _state._priv_instance[1].motor_compensation, 0), - // @Param: PRIMARY - // @DisplayName: Choose primary compass - // @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. - // @Values: 0:FirstCompass,1:SecondCompass,2:ThirdCompass - // @User: Advanced - AP_GROUPINFO("PRIMARY", 12, Compass, _primary, 0), -#endif // HAL_COMPASS_MAX_SENSORS +#endif // COMPASS_MAX_INSTANCES -#if HAL_COMPASS_MAX_SENSORS > 2 +#if COMPASS_MAX_INSTANCES > 2 // @Param: OFS3_X // @DisplayName: Compass3 offsets in milligauss on the X axis // @Description: Offset to be added to compass3's x-axis values to compensate for metal in the frame @@ -221,6 +226,7 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @Units: mGauss // @Increment: 1 // @User: Advanced + // @Calibration: 1 // @Param: OFS3_Y // @DisplayName: Compass3 offsets in milligauss on the Y axis @@ -229,6 +235,7 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @Units: mGauss // @Increment: 1 // @User: Advanced + // @Calibration: 1 // @Param: OFS3_Z // @DisplayName: Compass3 offsets in milligauss on the Z axis @@ -237,7 +244,7 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @Units: mGauss // @Increment: 1 // @User: Advanced - AP_GROUPINFO("OFS3", 13, Compass, _state[2].offset, 0), + AP_GROUPINFO("OFS3", 13, Compass, _state._priv_instance[2].offset, 0), // @Param: MOT3_X // @DisplayName: Motor interference compensation to compass3 for body frame X axis @@ -246,6 +253,7 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @Units: mGauss/A // @Increment: 1 // @User: Advanced + // @Calibration: 1 // @Param: MOT3_Y // @DisplayName: Motor interference compensation to compass3 for body frame Y axis @@ -254,6 +262,7 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @Units: mGauss/A // @Increment: 1 // @User: Advanced + // @Calibration: 1 // @Param: MOT3_Z // @DisplayName: Motor interference compensation to compass3 for body frame Z axis @@ -262,180 +271,193 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @Units: mGauss/A // @Increment: 1 // @User: Advanced - AP_GROUPINFO("MOT3", 14, Compass, _state[2].motor_compensation, 0), -#endif // HAL_COMPASS_MAX_SENSORS + AP_GROUPINFO("MOT3", 14, Compass, _state._priv_instance[2].motor_compensation, 0), +#endif // COMPASS_MAX_INSTANCES // @Param: DEV_ID // @DisplayName: Compass device id // @Description: Compass device id. Automatically detected, do not set manually // @ReadOnly: True // @User: Advanced - AP_GROUPINFO("DEV_ID", 15, Compass, _state[0].dev_id, 0), + AP_GROUPINFO("DEV_ID", 15, Compass, _state._priv_instance[0].dev_id, 0), -#if HAL_COMPASS_MAX_SENSORS > 1 +#if COMPASS_MAX_INSTANCES > 1 // @Param: DEV_ID2 // @DisplayName: Compass2 device id // @Description: Second compass's device id. Automatically detected, do not set manually // @ReadOnly: True // @User: Advanced - AP_GROUPINFO("DEV_ID2", 16, Compass, _state[1].dev_id, 0), -#endif // HAL_COMPASS_MAX_SENSORS + AP_GROUPINFO("DEV_ID2", 16, Compass, _state._priv_instance[1].dev_id, 0), +#endif // COMPASS_MAX_INSTANCES -#if HAL_COMPASS_MAX_SENSORS > 2 +#if COMPASS_MAX_INSTANCES > 2 // @Param: DEV_ID3 // @DisplayName: Compass3 device id // @Description: Third compass's device id. Automatically detected, do not set manually // @ReadOnly: True // @User: Advanced - AP_GROUPINFO("DEV_ID3", 17, Compass, _state[2].dev_id, 0), -#endif // HAL_COMPASS_MAX_SENSORS + AP_GROUPINFO("DEV_ID3", 17, Compass, _state._priv_instance[2].dev_id, 0), +#endif // COMPASS_MAX_INSTANCES -#if HAL_COMPASS_MAX_SENSORS > 1 +#if COMPASS_MAX_INSTANCES > 1 // @Param: USE2 // @DisplayName: Compass2 used for yaw - // @Description: Enable or disable the second compass for determining heading. + // @Description: Enable or disable the secondary compass for determining heading. // @Values: 0:Disabled,1:Enabled // @User: Advanced - AP_GROUPINFO("USE2", 18, Compass, _state[1].use_for_yaw, 1), + AP_GROUPINFO("USE2", 18, Compass, _use_for_yaw._priv_instance[1], 1), // @Param: ORIENT2 // @DisplayName: Compass2 orientation // @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. - // @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 + // @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,100:Custom // @User: Advanced - AP_GROUPINFO("ORIENT2", 19, Compass, _state[1].orientation, ROTATION_NONE), + AP_GROUPINFO("ORIENT2", 19, Compass, _state._priv_instance[1].orientation, ROTATION_NONE), // @Param: EXTERN2 // @DisplayName: Compass2 is attached via an external cable // @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 // @User: Advanced - AP_GROUPINFO("EXTERN2",20, Compass, _state[1].external, 0), -#endif // HAL_COMPASS_MAX_SENSORS + AP_GROUPINFO("EXTERN2",20, Compass, _state._priv_instance[1].external, 0), +#endif // COMPASS_MAX_INSTANCES -#if HAL_COMPASS_MAX_SENSORS > 2 +#if COMPASS_MAX_INSTANCES > 2 // @Param: USE3 // @DisplayName: Compass3 used for yaw - // @Description: Enable or disable the third compass for determining heading. + // @Description: Enable or disable the tertiary compass for determining heading. // @Values: 0:Disabled,1:Enabled // @User: Advanced - AP_GROUPINFO("USE3", 21, Compass, _state[2].use_for_yaw, 1), + AP_GROUPINFO("USE3", 21, Compass, _use_for_yaw._priv_instance[2], 1), // @Param: ORIENT3 // @DisplayName: Compass3 orientation // @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. - // @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 + // @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,100:Custom // @User: Advanced - AP_GROUPINFO("ORIENT3", 22, Compass, _state[2].orientation, ROTATION_NONE), + AP_GROUPINFO("ORIENT3", 22, Compass, _state._priv_instance[2].orientation, ROTATION_NONE), // @Param: EXTERN3 // @DisplayName: Compass3 is attached via an external cable // @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 // @User: Advanced - AP_GROUPINFO("EXTERN3",23, Compass, _state[2].external, 0), -#endif // HAL_COMPASS_MAX_SENSORS + AP_GROUPINFO("EXTERN3",23, Compass, _state._priv_instance[2].external, 0), +#endif // COMPASS_MAX_INSTANCES // @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]] // @User: Advanced + // @Calibration: 1 // @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]] // @User: Advanced + // @Calibration: 1 // @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]] // @User: Advanced - AP_GROUPINFO("DIA", 24, Compass, _state[0].diagonals, 0), + AP_GROUPINFO("DIA", 24, Compass, _state._priv_instance[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]] // @User: Advanced + // @Calibration: 1 // @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]] // @User: Advanced + // @Calibration: 1 // @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]] // @User: Advanced - AP_GROUPINFO("ODI", 25, Compass, _state[0].offdiagonals, 0), + AP_GROUPINFO("ODI", 25, Compass, _state._priv_instance[0].offdiagonals, 0), -#if HAL_COMPASS_MAX_SENSORS > 1 +#if COMPASS_MAX_INSTANCES > 1 // @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]] // @User: Advanced + // @Calibration: 1 // @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]] // @User: Advanced + // @Calibration: 1 // @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]] // @User: Advanced - AP_GROUPINFO("DIA2", 26, Compass, _state[1].diagonals, 0), + AP_GROUPINFO("DIA2", 26, Compass, _state._priv_instance[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]] // @User: Advanced + // @Calibration: 1 // @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]] // @User: Advanced + // @Calibration: 1 // @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]] // @User: Advanced - AP_GROUPINFO("ODI2", 27, Compass, _state[1].offdiagonals, 0), -#endif // HAL_COMPASS_MAX_SENSORS + AP_GROUPINFO("ODI2", 27, Compass, _state._priv_instance[1].offdiagonals, 0), +#endif // COMPASS_MAX_INSTANCES -#if HAL_COMPASS_MAX_SENSORS > 2 +#if COMPASS_MAX_INSTANCES > 2 // @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]] // @User: Advanced + // @Calibration: 1 // @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]] // @User: Advanced + // @Calibration: 1 // @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]] // @User: Advanced - AP_GROUPINFO("DIA3", 28, Compass, _state[2].diagonals, 0), + AP_GROUPINFO("DIA3", 28, Compass, _state._priv_instance[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]] // @User: Advanced + // @Calibration: 1 // @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]] // @User: Advanced + // @Calibration: 1 // @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]] // @User: Advanced - AP_GROUPINFO("ODI3", 29, Compass, _state[2].offdiagonals, 0), -#endif // HAL_COMPASS_MAX_SENSORS + AP_GROUPINFO("ODI3", 29, Compass, _state._priv_instance[2].offdiagonals, 0), +#endif // COMPASS_MAX_INSTANCES +#if COMPASS_CAL_ENABLED // @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. @@ -444,6 +466,7 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @Increment: 0.1 // @User: Advanced AP_GROUPINFO("CAL_FIT", 30, Compass, _calibration_threshold, AP_COMPASS_CALIBRATION_FITNESS_DEFAULT), +#endif // @Param: OFFS_MAX // @DisplayName: Compass maximum offset @@ -474,33 +497,38 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @Increment: 1 AP_GROUPINFO("FLTR_RNG", 34, Compass, _filter_range, HAL_COMPASS_FILTER_DEFAULT), +#if COMPASS_CAL_ENABLED // @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), +#endif - // @Param: EXP_DID - // @DisplayName: Compass device id expected - // @Description: The expected value of COMPASS_DEV_ID, used by arming checks. Setting this to -1 means "don't care." +#if COMPASS_MAX_INSTANCES > 1 + // @Param: PRIO1_ID + // @DisplayName: Compass device id with 1st order priority + // @Description: Compass device id with 1st order priority, set automatically if 0. Reboot required after change. + // @RebootRequired: True // @User: Advanced - AP_GROUPINFO("EXP_DID", 36, Compass, _state[0].expected_dev_id, -1), + AP_GROUPINFO("PRIO1_ID", 36, Compass, _priority_did_stored_list._priv_instance[0], 0), -#if HAL_COMPASS_MAX_SENSORS > 1 - // @Param: EXP_DID2 - // @DisplayName: Compass2 device id expected - // @Description: The expected value of COMPASS_DEV_ID2, used by arming checks. Setting this to -1 means "don't care." + // @Param: PRIO2_ID + // @DisplayName: Compass device id with 2nd order priority + // @Description: Compass device id with 2nd order priority, set automatically if 0. Reboot required after change. + // @RebootRequired: True // @User: Advanced - AP_GROUPINFO("EXP_DID2", 37, Compass, _state[1].expected_dev_id, -1), -#endif // HAL_COMPASS_MAX_SENSORS + AP_GROUPINFO("PRIO2_ID", 37, Compass, _priority_did_stored_list._priv_instance[1], 0), +#endif // COMPASS_MAX_INSTANCES -#if HAL_COMPASS_MAX_SENSORS > 2 - // @Param: EXP_DID3 - // @DisplayName: Compass3 device id expected - // @Description: The expected value of COMPASS_DEV_ID3, used by arming checks. Setting this to -1 means "don't care." +#if COMPASS_MAX_INSTANCES > 2 + // @Param: PRIO3_ID + // @DisplayName: Compass device id with 3rd order priority + // @Description: Compass device id with 3rd order priority, set automatically if 0. Reboot required after change. + // @RebootRequired: True // @User: Advanced - AP_GROUPINFO("EXP_DID3", 38, Compass, _state[2].expected_dev_id, -1), -#endif // HAL_COMPASS_MAX_SENSORS + AP_GROUPINFO("PRIO3_ID", 38, Compass, _priority_did_stored_list._priv_instance[2], 0), +#endif // COMPASS_MAX_INSTANCES // @Param: ENABLE // @DisplayName: Enable Compass @@ -514,24 +542,24 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @Description: Scaling factor for first compass to compensate for sensor scaling errors. If this is 0 then no scaling is done // @User: Standard // @Range: 0 1.3 - AP_GROUPINFO("SCALE", 40, Compass, _state[0].scale_factor, 0), + AP_GROUPINFO("SCALE", 40, Compass, _state._priv_instance[0].scale_factor, 0), -#if HAL_COMPASS_MAX_SENSORS > 1 +#if COMPASS_MAX_INSTANCES > 1 // @Param: SCALE2 // @DisplayName: Compass2 scale factor // @Description: Scaling factor for 2nd compass to compensate for sensor scaling errors. If this is 0 then no scaling is done // @User: Standard // @Range: 0 1.3 - AP_GROUPINFO("SCALE2", 41, Compass, _state[1].scale_factor, 0), + AP_GROUPINFO("SCALE2", 41, Compass, _state._priv_instance[1].scale_factor, 0), #endif -#if HAL_COMPASS_MAX_SENSORS > 2 +#if COMPASS_MAX_INSTANCES > 2 // @Param: SCALE3 // @DisplayName: Compass3 scale factor // @Description: Scaling factor for 3rd compass to compensate for sensor scaling errors. If this is 0 then no scaling is done // @User: Standard // @Range: 0 1.3 - AP_GROUPINFO("SCALE3", 42, Compass, _state[2].scale_factor, 0), + AP_GROUPINFO("SCALE3", 42, Compass, _state._priv_instance[2].scale_factor, 0), #endif // @Param: OPTIONS @@ -541,6 +569,82 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @User: Advanced AP_GROUPINFO("OPTIONS", 43, Compass, _options, 0), +#if COMPASS_MAX_UNREG_DEV > 0 + // @Param: DEV_ID4 + // @DisplayName: Compass4 device id + // @Description: Extra 4th compass's device id. Automatically detected, do not set manually + // @ReadOnly: True + // @User: Advanced + AP_GROUPINFO("DEV_ID4", 44, Compass, extra_dev_id[0], 0), +#endif // COMPASS_MAX_UNREG_DEV + +#if COMPASS_MAX_UNREG_DEV > 1 + // @Param: DEV_ID5 + // @DisplayName: Compass5 device id + // @Description: Extra 5th compass's device id. Automatically detected, do not set manually + // @ReadOnly: True + // @User: Advanced + AP_GROUPINFO("DEV_ID5", 45, Compass, extra_dev_id[1], 0), +#endif // COMPASS_MAX_UNREG_DEV + +#if COMPASS_MAX_UNREG_DEV > 2 + // @Param: DEV_ID6 + // @DisplayName: Compass6 device id + // @Description: Extra 6th compass's device id. Automatically detected, do not set manually + // @ReadOnly: True + // @User: Advanced + AP_GROUPINFO("DEV_ID6", 46, Compass, extra_dev_id[2], 0), +#endif // COMPASS_MAX_UNREG_DEV + +#if COMPASS_MAX_UNREG_DEV > 3 + // @Param: DEV_ID7 + // @DisplayName: Compass7 device id + // @Description: Extra 7th compass's device id. Automatically detected, do not set manually + // @ReadOnly: True + // @User: Advanced + AP_GROUPINFO("DEV_ID7", 47, Compass, extra_dev_id[3], 0), +#endif // COMPASS_MAX_UNREG_DEV + +#if COMPASS_MAX_UNREG_DEV > 4 + // @Param: DEV_ID8 + // @DisplayName: Compass8 device id + // @Description: Extra 8th compass's device id. Automatically detected, do not set manually + // @ReadOnly: True + // @User: Advanced + AP_GROUPINFO("DEV_ID8", 48, Compass, extra_dev_id[4], 0), +#endif // COMPASS_MAX_UNREG_DEV + +#if !APM_BUILD_TYPE(APM_BUILD_AP_Periph) + // @Param: CUS_ROLL + // @DisplayName: Custom orientation roll offset + // @Description: Compass mounting position roll offset. Positive values = roll right, negative values = roll left. This parameter is only used when COMPASS_ORIENT/2/3 is set to CUSTOM. + // @Range: -180 180 + // @Units: deg + // @Increment: 1 + // @RebootRequired: True + // @User: Advanced + AP_GROUPINFO("CUS_ROLL", 49, Compass, _custom_roll, 0), + + // @Param: CUS_PIT + // @DisplayName: Custom orientation pitch offset + // @Description: Compass mounting position pitch offset. Positive values = pitch up, negative values = pitch down. This parameter is only used when COMPASS_ORIENT/2/3 is set to CUSTOM. + // @Range: -180 180 + // @Units: deg + // @Increment: 1 + // @RebootRequired: True + // @User: Advanced + AP_GROUPINFO("CUS_PIT", 50, Compass, _custom_pitch, 0), + + // @Param: CUS_YAW + // @DisplayName: Custom orientation yaw offset + // @Description: Compass mounting position yaw offset. Positive values = yaw right, negative values = yaw left. This parameter is only used when COMPASS_ORIENT/2/3 is set to CUSTOM. + // @Range: -180 180 + // @Units: deg + // @Increment: 1 + // @RebootRequired: True + // @User: Advanced + AP_GROUPINFO("CUS_YAW", 51, Compass, _custom_yaw, 0), +#endif AP_GROUPEND }; @@ -568,10 +672,67 @@ void Compass::init() return; } +#if COMPASS_MAX_INSTANCES > 1 + // Look if there was a primary compass setup in previous version + // if so and the the primary compass is not set in current setup + // make the devid as primary. + if (_priority_did_stored_list[Priority(0)] == 0) { + uint16_t k_param_compass; + if (AP_Param::find_top_level_key_by_pointer(this, k_param_compass)) { + const AP_Param::ConversionInfo primary_compass_old_param = {k_param_compass, 12, AP_PARAM_INT8, ""}; + AP_Int8 value; + value.set(0); + bool primary_param_exists = AP_Param::find_old_parameter(&primary_compass_old_param, &value); + int8_t oldvalue = value.get(); + if ((oldvalue!=0) && (oldvaluedelay(100); @@ -582,21 +743,10 @@ void Compass::init() // 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; iprintf("Compass initialisation failed\n"); -#endif -#ifndef HAL_NO_LOGGING - AP::logger().Write_Error(LogErrorSubsystem::COMPASS, LogErrorCode::FAILED_TO_INITIALISE); -#endif - return; + for (StateIndex i(0); i 1 || COMPASS_MAX_UNREG_DEV +// Update Priority List for Mags, by default, we just +// load them as they come up the first time +Compass::Priority Compass::_update_priority_list(int32_t dev_id) +{ + // Check if already in priority list + for (Priority i(0); i= _compass_count) { + _compass_count = uint8_t(i)+1; + } + return i; + } + } + + // We are not in priority list, let's add at first empty + for (Priority i(0); i= _compass_count) { + _compass_count = uint8_t(i)+1; + } + return i; + } + } + return Priority(COMPASS_MAX_INSTANCES); +} +#endif + + +// This method reorganises devid list to match +// priority list, only call before detection at boot +void Compass::_reorder_compass_params() +{ + mag_state swap_state; + StateIndex curr_state_id; + for (Priority i(0); i= COMPASS_MAX_UNREG_DEV) { AP_HAL::panic("Too many compass instances"); } - return _compass_count++; + + for (uint8_t i=0; i= _unreg_compass_count) { + _unreg_compass_count = i+1; + } + instance = i+COMPASS_MAX_INSTANCES; + return false; + } else if (extra_dev_id[i] == 0) { + extra_dev_id[_unreg_compass_count++].set(dev_id); + instance = i+COMPASS_MAX_INSTANCES; + return false; + } + } +#else + AP_HAL::panic("Too many compass instances"); +#endif + + return false; +} + +Compass::StateIndex Compass::_get_state_id(Compass::Priority priority) const +{ +#if COMPASS_MAX_INSTANCES > 1 + for (StateIndex i(0); i 0 +#define CHECK_UNREG_LIMIT_RETURN if (_unreg_compass_count == COMPASS_MAX_UNREG_DEV) return +#else +#define CHECK_UNREG_LIMIT_RETURN +#endif + /* 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) \ do { if (_driver_enabled(driver_type)) { _add_backend(backend); } \ - if (_backend_count == COMPASS_MAX_BACKEND || \ - _compass_count == COMPASS_MAX_INSTANCES) { \ - return; \ - } \ + CHECK_UNREG_LIMIT_RETURN; \ } while (0) #define GET_I2C_DEVICE(bus, address) _have_i2c_driver(bus, address)?nullptr:hal.i2c_mgr->get_device(bus, address) @@ -676,7 +975,7 @@ void Compass::_probe_external_i2c_compasses(void) // external i2c bus FOREACH_I2C_EXTERNAL(i) { ADD_BACKEND(DRIVER_HMC5843, AP_Compass_HMC5843::probe(GET_I2C_DEVICE(i, HAL_COMPASS_HMC5843_I2C_ADDR), - true, ROTATION_ROLL_180)); + true, ROTATION_ROLL_180)); } if (AP_BoardConfig::get_board_type() != AP_BoardConfig::PX4_BOARD_MINDPXV2 && @@ -684,14 +983,14 @@ void Compass::_probe_external_i2c_compasses(void) // internal i2c bus FOREACH_I2C_INTERNAL(i) { ADD_BACKEND(DRIVER_HMC5843, AP_Compass_HMC5843::probe(GET_I2C_DEVICE(i, HAL_COMPASS_HMC5843_I2C_ADDR), - all_external, all_external?ROTATION_ROLL_180:ROTATION_YAW_270)); + all_external, all_external?ROTATION_ROLL_180:ROTATION_YAW_270)); } } //external i2c bus FOREACH_I2C_EXTERNAL(i) { ADD_BACKEND(DRIVER_QMC5883L, AP_Compass_QMC5883L::probe(GET_I2C_DEVICE(i, HAL_COMPASS_QMC5883L_I2C_ADDR), - true, HAL_COMPASS_QMC5883L_ORIENTATION_EXTERNAL)); + true, HAL_COMPASS_QMC5883L_ORIENTATION_EXTERNAL)); } // internal i2c bus @@ -699,57 +998,59 @@ void Compass::_probe_external_i2c_compasses(void) // only probe QMC5883L on internal if we are treating internals as externals FOREACH_I2C_INTERNAL(i) { ADD_BACKEND(DRIVER_QMC5883L, AP_Compass_QMC5883L::probe(GET_I2C_DEVICE(i, HAL_COMPASS_QMC5883L_I2C_ADDR), - all_external, - all_external?HAL_COMPASS_QMC5883L_ORIENTATION_EXTERNAL:HAL_COMPASS_QMC5883L_ORIENTATION_INTERNAL)); + all_external, + all_external?HAL_COMPASS_QMC5883L_ORIENTATION_EXTERNAL:HAL_COMPASS_QMC5883L_ORIENTATION_INTERNAL)); } } #if !HAL_MINIMIZE_FEATURES +#ifndef HAL_BUILD_AP_PERIPH // AK09916 on ICM20948 FOREACH_I2C_EXTERNAL(i) { ADD_BACKEND(DRIVER_ICM20948, AP_Compass_AK09916::probe_ICM20948(GET_I2C_DEVICE(i, HAL_COMPASS_AK09916_I2C_ADDR), - GET_I2C_DEVICE(i, HAL_COMPASS_ICM20948_I2C_ADDR), - true, ROTATION_PITCH_180_YAW_90)); + GET_I2C_DEVICE(i, HAL_COMPASS_ICM20948_I2C_ADDR), + true, ROTATION_PITCH_180_YAW_90)); } FOREACH_I2C_INTERNAL(i) { ADD_BACKEND(DRIVER_ICM20948, AP_Compass_AK09916::probe_ICM20948(GET_I2C_DEVICE(i, HAL_COMPASS_AK09916_I2C_ADDR), - GET_I2C_DEVICE(i, HAL_COMPASS_ICM20948_I2C_ADDR), - all_external, ROTATION_PITCH_180_YAW_90)); + GET_I2C_DEVICE(i, HAL_COMPASS_ICM20948_I2C_ADDR), + all_external, ROTATION_PITCH_180_YAW_90)); } +#endif // HAL_BUILD_AP_PERIPH // lis3mdl on bus 0 with default address FOREACH_I2C_INTERNAL(i) { ADD_BACKEND(DRIVER_LIS3MDL, AP_Compass_LIS3MDL::probe(GET_I2C_DEVICE(i, HAL_COMPASS_LIS3MDL_I2C_ADDR), - all_external, all_external?ROTATION_YAW_90:ROTATION_NONE)); + all_external, all_external?ROTATION_YAW_90:ROTATION_NONE)); } // lis3mdl on bus 0 with alternate address FOREACH_I2C_INTERNAL(i) { ADD_BACKEND(DRIVER_LIS3MDL, AP_Compass_LIS3MDL::probe(GET_I2C_DEVICE(i, HAL_COMPASS_LIS3MDL_I2C_ADDR2), - all_external, all_external?ROTATION_YAW_90:ROTATION_NONE)); + all_external, all_external?ROTATION_YAW_90:ROTATION_NONE)); } // external lis3mdl on bus 1 with default address FOREACH_I2C_EXTERNAL(i) { ADD_BACKEND(DRIVER_LIS3MDL, AP_Compass_LIS3MDL::probe(GET_I2C_DEVICE(i, HAL_COMPASS_LIS3MDL_I2C_ADDR), - true, ROTATION_YAW_90)); + true, ROTATION_YAW_90)); } // external lis3mdl on bus 1 with alternate address FOREACH_I2C_EXTERNAL(i) { ADD_BACKEND(DRIVER_LIS3MDL, AP_Compass_LIS3MDL::probe(GET_I2C_DEVICE(i, HAL_COMPASS_LIS3MDL_I2C_ADDR2), - true, ROTATION_YAW_90)); + true, ROTATION_YAW_90)); } // 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(GET_I2C_DEVICE(i, HAL_COMPASS_AK09916_I2C_ADDR), - true, ROTATION_YAW_270)); + true, ROTATION_YAW_270)); } FOREACH_I2C_INTERNAL(i) { ADD_BACKEND(DRIVER_AK09916, AP_Compass_AK09916::probe(GET_I2C_DEVICE(i, HAL_COMPASS_AK09916_I2C_ADDR), - all_external, all_external?ROTATION_YAW_270:ROTATION_NONE)); + all_external, all_external?ROTATION_YAW_270:ROTATION_NONE)); } // IST8310 on external and internal bus @@ -768,11 +1069,11 @@ void Compass::_probe_external_i2c_compasses(void) for (uint8_t a=0; aget_device(HAL_COMPASS_HMC5843_NAME), - false, ROTATION_PITCH_180)); + false, ROTATION_PITCH_180)); ADD_BACKEND(DRIVER_LSM303D, AP_Compass_LSM303D::probe(hal.spi->get_device(HAL_INS_LSM9DS0_A_NAME), ROTATION_NONE)); break; @@ -902,22 +1197,22 @@ void Compass::_detect_backends(void) case AP_BoardConfig::PX4_BOARD_FMUV6: FOREACH_I2C_EXTERNAL(i) { ADD_BACKEND(DRIVER_IST8310, AP_Compass_IST8310::probe(GET_I2C_DEVICE(i, HAL_COMPASS_IST8310_I2C_ADDR), - true, ROTATION_ROLL_180_YAW_90)); + true, ROTATION_ROLL_180_YAW_90)); } FOREACH_I2C_INTERNAL(i) { ADD_BACKEND(DRIVER_IST8310, AP_Compass_IST8310::probe(GET_I2C_DEVICE(i, HAL_COMPASS_IST8310_I2C_ADDR), - false, ROTATION_ROLL_180_YAW_90)); + false, ROTATION_ROLL_180_YAW_90)); } break; - + case AP_BoardConfig::PX4_BOARD_SP01: ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(1, ROTATION_NONE)); break; - + case AP_BoardConfig::PX4_BOARD_PIXHAWK_PRO: ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(0, ROTATION_ROLL_180_YAW_90)); ADD_BACKEND(DRIVER_LIS3MDL, AP_Compass_LIS3MDL::probe(hal.spi->get_device(HAL_COMPASS_LIS3MDL_NAME), - false, ROTATION_NONE)); + false, ROTATION_NONE)); break; case AP_BoardConfig::PX4_BOARD_PHMINI: @@ -927,17 +1222,17 @@ void Compass::_detect_backends(void) case AP_BoardConfig::PX4_BOARD_AUAV21: ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(0, ROTATION_ROLL_180_YAW_90)); break; - + case AP_BoardConfig::PX4_BOARD_PH2SLIM: ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(0, ROTATION_YAW_270)); break; case AP_BoardConfig::PX4_BOARD_MINDPXV2: ADD_BACKEND(DRIVER_HMC5843, AP_Compass_HMC5843::probe(GET_I2C_DEVICE(0, HAL_COMPASS_HMC5843_I2C_ADDR), - false, ROTATION_YAW_90)); + false, ROTATION_YAW_90)); ADD_BACKEND(DRIVER_LSM303D, AP_Compass_LSM303D::probe(hal.spi->get_device(HAL_INS_LSM9DS0_A_NAME), ROTATION_PITCH_180_YAW_270)); break; - + default: break; } @@ -945,19 +1240,19 @@ void Compass::_detect_backends(void) #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_NONE // no compass, or only external probe #else - #error Unrecognised HAL_COMPASS_TYPE setting +#error Unrecognised HAL_COMPASS_TYPE setting #endif -/* for chibios external board coniguration */ -#ifdef HAL_EXT_COMPASS_HMC5843_I2C_BUS - ADD_BACKEND(DRIVER_HMC5843, AP_Compass_HMC5843::probe(GET_I2C_DEVICE(HAL_EXT_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR), - true, ROTATION_ROLL_180)); -#endif - #if HAL_WITH_UAVCAN - for (uint8_t i = 0; i < COMPASS_MAX_BACKEND; i++) { - ADD_BACKEND(DRIVER_UAVCAN, AP_Compass_UAVCAN::probe()); + if (_driver_enabled(DRIVER_UAVCAN)) { + for (uint8_t i=0; iget_soft_armed()) { + return; + } + static uint32_t last_try; + //Try once every second + if ((AP_HAL::millis() - last_try) < 1000) { + return; + } + last_try = AP_HAL::millis(); + if (_driver_enabled(DRIVER_UAVCAN)) { + for (uint8_t i=0; iread(); } uint32_t time = AP_HAL::millis(); - for (uint8_t i=0; i < COMPASS_MAX_INSTANCES; i++) { + for (StateIndex i(0); i < COMPASS_MAX_INSTANCES; i++) { _state[i].healthy = (time - _state[i].last_update_ms < 500); } #if COMPASS_LEARN_ENABLED @@ -1005,9 +1330,9 @@ uint8_t Compass::get_healthy_mask() const { uint8_t healthy_mask = 0; - for(uint8_t i=0; i 0.0f ) - { + if ( fabsf(_declination) > 0.0f ) { heading = heading + _declination; - if (heading > M_PI) // Angle normalization (-180 deg, 180 deg) + if (heading > M_PI) { // Angle normalization (-180 deg, 180 deg) heading -= (2.0f * M_PI); - else if (heading < -M_PI) + } else if (heading < -M_PI) { heading += (2.0f * M_PI); + } } return heading; @@ -1195,40 +1567,55 @@ bool Compass::configured(uint8_t i) return false; } + StateIndex id = _get_state_id(Priority(i)); // exit immediately if dev_id hasn't been detected - if (_state[i].detected_dev_id == 0) { + if (_state[id].detected_dev_id == 0) { return false; } // back up cached value of dev_id - int32_t dev_id_cache_value = _state[i].dev_id; + int32_t dev_id_cache_value = _state[id].dev_id; // load dev_id from eeprom - _state[i].dev_id.load(); + _state[id].dev_id.load(); // if dev_id loaded from eeprom is different from detected dev id or dev_id loaded from eeprom is different from cached dev_id, compass is unconfigured - if (_state[i].dev_id != _state[i].detected_dev_id || _state[i].dev_id != dev_id_cache_value) { + if (_state[id].dev_id != _state[id].detected_dev_id || _state[id].dev_id != dev_id_cache_value) { // restore cached value - _state[i].dev_id = dev_id_cache_value; + _state[id].dev_id = dev_id_cache_value; // return failure return false; } - // if expected_dev_id is configured and the detected dev_id is different, return false - if (_state[i].expected_dev_id != -1 && _state[i].expected_dev_id != _state[i].dev_id) { - return false; - } - // if we got here then it must be configured return true; } -bool Compass::configured(void) +bool Compass::configured(char *failure_msg, uint8_t failure_msg_len) { +#if COMPASS_MAX_INSTANCES > 1 + // Check if any of the registered devs are not registered + for (Priority i(0); i= get_count() || - _state[i].scale_factor < COMPASS_MIN_SCALE_FACTOR || - _state[i].scale_factor > COMPASS_MAX_SCALE_FACTOR) { + StateIndex id = _get_state_id(Priority(i)); + if (id >= COMPASS_MAX_INSTANCES || + _state[id].scale_factor < COMPASS_MIN_SCALE_FACTOR || + _state[id].scale_factor > COMPASS_MAX_SCALE_FACTOR) { return false; } return true; @@ -1377,7 +1765,8 @@ bool Compass::have_scale_factor(uint8_t i) const // singleton instance Compass *Compass::_singleton; -namespace AP { +namespace AP +{ Compass &compass() { diff --git a/libraries/AP_Compass/AP_Compass.h b/libraries/AP_Compass/AP_Compass.h index ba9fae9004..31b56cb3fa 100644 --- a/libraries/AP_Compass/AP_Compass.h +++ b/libraries/AP_Compass/AP_Compass.h @@ -12,6 +12,7 @@ #include "CompassCalibrator.h" #include "AP_Compass_Backend.h" #include "Compass_PerMotor.h" +#include // motor compensation types (for use with motor_comp_enabled) #define AP_COMPASS_MOT_COMP_DISABLED 0x00 @@ -45,8 +46,22 @@ maximum number of compass instances available on this platform. If more than 1 then redundant sensors may be available */ -#define COMPASS_MAX_INSTANCES 3 -#define COMPASS_MAX_BACKEND 3 +#ifndef HAL_BUILD_AP_PERIPH +#ifndef HAL_COMPASS_MAX_SENSORS +#define HAL_COMPASS_MAX_SENSORS 3 +#endif +#define COMPASS_MAX_UNREG_DEV 5 +#else +#ifndef HAL_COMPASS_MAX_SENSORS +#define HAL_COMPASS_MAX_SENSORS 1 +#endif +#define COMPASS_MAX_UNREG_DEV 0 +#endif + +#define COMPASS_MAX_INSTANCES HAL_COMPASS_MAX_SENSORS +#define COMPASS_MAX_BACKEND HAL_COMPASS_MAX_SENSORS + +#define MAX_CONNECTED_MAGS (COMPASS_MAX_UNREG_DEV+COMPASS_MAX_INSTANCES) class CompassLearn; @@ -87,7 +102,7 @@ public: /// @returns heading in radians /// float calculate_heading(const Matrix3f &dcm_matrix) const { - return calculate_heading(dcm_matrix, get_primary()); + return calculate_heading(dcm_matrix, 0); } float calculate_heading(const Matrix3f &dcm_matrix, uint8_t i) const; @@ -107,6 +122,7 @@ public: void set_and_save_diagonals(uint8_t i, const Vector3f &diagonals); void set_and_save_offdiagonals(uint8_t i, const Vector3f &diagonals); void set_and_save_scale_factor(uint8_t i, float scale_factor); + void set_and_save_orientation(uint8_t i, Rotation orientation); /// Saves the current offset x/y/z values for one or all compasses /// @@ -121,9 +137,12 @@ public: // return the number of compass instances uint8_t get_count(void) const { return _compass_count; } + // return the number of enabled sensors + uint8_t get_num_enabled(void) const; + /// Return the current field as a Vector3f in milligauss - const Vector3f &get_field(uint8_t i) const { return _state[i].field; } - const Vector3f &get_field(void) const { return get_field(get_primary()); } + const Vector3f &get_field(uint8_t i) const { return _get_state(Priority(i)).field; } + const Vector3f &get_field(void) const { return get_field(0); } /// Return true if we have set a scale factor for a compass bool have_scale_factor(uint8_t i) const; @@ -166,22 +185,22 @@ public: bool consistent() const; /// Return the health of a compass - bool healthy(uint8_t i) const { return _state[i].healthy; } - bool healthy(void) const { return healthy(get_primary()); } + bool healthy(uint8_t i) const { return _get_state(Priority(i)).healthy; } + bool healthy(void) const { return healthy(0); } uint8_t get_healthy_mask() const; /// Returns the current offset values /// /// @returns The current compass offsets in milligauss. /// - const Vector3f &get_offsets(uint8_t i) const { return _state[i].offset; } - const Vector3f &get_offsets(void) const { return get_offsets(get_primary()); } + const Vector3f &get_offsets(uint8_t i) const { return _get_state(Priority(i)).offset; } + const Vector3f &get_offsets(void) const { return get_offsets(0); } - const Vector3f &get_diagonals(uint8_t i) const { return _state[i].diagonals; } - const Vector3f &get_diagonals(void) const { return get_diagonals(get_primary()); } + const Vector3f &get_diagonals(uint8_t i) const { return _get_state(Priority(i)).diagonals; } + const Vector3f &get_diagonals(void) const { return get_diagonals(0); } - const Vector3f &get_offdiagonals(uint8_t i) const { return _state[i].offdiagonals; } - const Vector3f &get_offdiagonals(void) const { return get_offdiagonals(get_primary()); } + const Vector3f &get_offdiagonals(uint8_t i) const { return _get_state(Priority(i)).offdiagonals; } + const Vector3f &get_offdiagonals(void) const { return get_offdiagonals(0); } // learn offsets accessor bool learn_offsets_enabled() const { return _learn == LEARN_INFLIGHT; } @@ -190,9 +209,7 @@ public: bool use_for_yaw(uint8_t i) const; bool use_for_yaw(void) const; - void set_use_for_yaw(uint8_t i, bool use) { - _state[i].use_for_yaw.set(use); - } + void set_use_for_yaw(uint8_t i, bool use); /// Sets the local magnetic field declination. /// @@ -229,8 +246,8 @@ public: void set_motor_compensation(uint8_t i, const Vector3f &motor_comp_factor); /// get motor compensation factors as a vector - const Vector3f& get_motor_compensation(uint8_t i) const { return _state[i].motor_compensation; } - const Vector3f& get_motor_compensation(void) const { return get_motor_compensation(get_primary()); } + const Vector3f& get_motor_compensation(uint8_t i) const { return _get_state(Priority(i)).motor_compensation; } + const Vector3f& get_motor_compensation(void) const { return get_motor_compensation(0); } /// Saves the current motor compensation x/y/z values. /// @@ -242,8 +259,8 @@ public: /// /// @returns The current compass offsets in milligauss. /// - const Vector3f &get_motor_offsets(uint8_t i) const { return _state[i].motor_offset; } - const Vector3f &get_motor_offsets(void) const { return get_motor_offsets(get_primary()); } + const Vector3f &get_motor_offsets(uint8_t i) const { return _get_state(Priority(i)).motor_offset; } + const Vector3f &get_motor_offsets(void) const { return get_motor_offsets(0); } /// Set the throttle as a percentage from 0.0 to 1.0 /// @param thr_pct throttle expressed as a percentage from 0 to 1.0 @@ -265,13 +282,7 @@ public: /// @returns True if compass has been configured /// bool configured(uint8_t i); - bool configured(void); - - /// Returns the instance of the primary compass - /// - /// @returns the instance number of the primary compass - /// - uint8_t get_primary(void) const { return _primary; } + bool configured(char *failure_msg, uint8_t failure_msg_len); // HIL methods void setHIL(uint8_t instance, float roll, float pitch, float yaw); @@ -283,11 +294,11 @@ public: void set_hil_mode(void) { _hil_mode = true; } // return last update time in microseconds - uint32_t last_update_usec(void) const { return _state[get_primary()].last_update_usec; } - uint32_t last_update_usec(uint8_t i) const { return _state[i].last_update_usec; } + uint32_t last_update_usec(void) const { return last_update_usec(0); } + uint32_t last_update_usec(uint8_t i) const { return _get_state(Priority(i)).last_update_usec; } - uint32_t last_update_ms(void) const { return _state[get_primary()].last_update_ms; } - uint32_t last_update_ms(uint8_t i) const { return _state[i].last_update_ms; } + uint32_t last_update_ms(void) const { return last_update_ms(0); } + uint32_t last_update_ms(uint8_t i) const { return _get_state(Priority(i)).last_update_ms; } static const struct AP_Param::GroupInfo var_info[]; @@ -335,10 +346,20 @@ public: private: static Compass *_singleton; + + // Use Priority and StateIndex typesafe index types + // to distinguish between two different type of indexing + // We use StateIndex for access by Backend + // and Priority for access by Frontend + DECLARE_TYPESAFE_INDEX(Priority, uint8_t); + DECLARE_TYPESAFE_INDEX(StateIndex, uint8_t); + /// Register a new compas driver, allocating an instance number /// - /// @return number of compass instances - uint8_t register_compass(void); + /// @param dev_id Dev ID of compass to register against + /// + /// @return instance number saved against the dev id or first available empty instance number + bool register_compass(int32_t dev_id, uint8_t& instance); // load backend drivers bool _add_backend(AP_Compass_Backend *backend); @@ -366,7 +387,7 @@ private: #if COMPASS_CAL_ENABLED //keep track of which calibrators have been saved - bool _cal_saved[COMPASS_MAX_INSTANCES]; + RestrictIDTypeArray _cal_saved; bool _cal_autosave; #endif @@ -407,16 +428,17 @@ private: // number of registered compasses. uint8_t _compass_count; + // number of unregistered compasses. + uint8_t _unreg_compass_count; + // settable parameters AP_Int8 _learn; // board orientation from AHRS enum Rotation _board_orientation = ROTATION_NONE; + // custom rotation matrix Matrix3f* _custom_rotation; - // primary instance - AP_Int8 _primary; - // declination in radians AP_Float _declination; @@ -429,15 +451,17 @@ private: // stores which bit is used to indicate we should log compass readings uint32_t _log_bit = -1; - // used by offset correction - static const uint8_t _mag_history_size = 20; - // motor compensation type // 0 = disabled, 1 = enabled for throttle, 2 = enabled for current AP_Int8 _motor_comp_type; // automatic compass orientation on calibration AP_Int8 _rotate_auto; + + // custom compass rotation + AP_Float _custom_roll; + AP_Float _custom_pitch; + AP_Float _custom_yaw; // throttle expressed as a percentage from 0 ~ 1.0, used for motor compensation float _thr; @@ -445,6 +469,8 @@ private: struct mag_state { AP_Int8 external; bool healthy; + bool registered; + Compass::Priority priority; AP_Int8 orientation; AP_Vector3f offset; AP_Vector3f diagonals; @@ -455,13 +481,8 @@ private: // saved to eeprom when offsets are saved allowing ram & // eeprom values to be compared as consistency check AP_Int32 dev_id; - AP_Int32 expected_dev_id; int32_t detected_dev_id; - - AP_Int8 use_for_yaw; - - uint8_t mag_history_index; - Vector3i mag_history[_mag_history_size]; + int32_t expected_dev_id; // factors multiplied by throttle and added to compass outputs AP_Vector3f motor_compensation; @@ -482,7 +503,34 @@ private: // accumulated samples, protected by _sem, used by AP_Compass_Backend Vector3f accum; uint32_t accum_count; - } _state[COMPASS_MAX_INSTANCES]; + // We only copy persistent params + void copy_from(const mag_state& state); + }; + + //Create an Array of mag_state to be accessible by StateIndex only + RestrictIDTypeArray _state; + + //Convert Priority to StateIndex + StateIndex _get_state_id(Priority priority) const; + //Get State Struct by Priority + const struct mag_state& _get_state(Priority priority) const { return _state[_get_state_id(priority)]; } + //Convert StateIndex to Priority + Priority _get_priority(StateIndex state_id) { return _state[state_id].priority; } + //Method to detect compass beyond initialisation stage + void _detect_runtime(void); + // This method reorganises devid list to match + // priority list, only call before detection at boot + void _reorder_compass_params(); + // Update Priority List for Mags, by default, we just + // load them as they come up the first time + Priority _update_priority_list(int32_t dev_id); + + //Create Arrays to be accessible by Priority only + RestrictIDTypeArray _use_for_yaw; +#if COMPASS_MAX_INSTANCES > 1 + RestrictIDTypeArray _priority_did_stored_list; + RestrictIDTypeArray _priority_did_list; +#endif AP_Int16 _offset_max; @@ -493,7 +541,7 @@ private: AP_Int16 _options; #if COMPASS_CAL_ENABLED - CompassCalibrator _calibrator[COMPASS_MAX_INSTANCES]; + RestrictIDTypeArray _calibrator; #endif #if COMPASS_MOT_ENABLED @@ -508,7 +556,12 @@ private: // mask of driver types to not load. Bit positions match DEVTYPE_ in backend AP_Int32 _driver_type_mask; - + +#if COMPASS_MAX_UNREG_DEV + // Put extra dev ids detected + AP_Int32 extra_dev_id[COMPASS_MAX_UNREG_DEV]; +#endif + AP_Int8 _filter_range; CompassLearn *learn; diff --git a/libraries/AP_Compass/AP_Compass_AK09916.cpp b/libraries/AP_Compass/AP_Compass_AK09916.cpp index 67a2e37db2..e846fceb7c 100644 --- a/libraries/AP_Compass/AP_Compass_AK09916.cpp +++ b/libraries/AP_Compass/AP_Compass_AK09916.cpp @@ -104,9 +104,7 @@ AP_Compass_Backend *AP_Compass_AK09916::probe_ICM20948(AP_HAL::OwnPtrget_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { - return nullptr; - } + dev->get_semaphore()->take_blocking(); /* Allow ICM20x48 to shortcut auxiliary bus and host bus */ uint8_t rval; @@ -192,9 +190,10 @@ bool AP_Compass_AK09916::init() { AP_HAL::Semaphore *bus_sem = _bus->get_semaphore(); - if (!bus_sem || !_bus->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { + if (!bus_sem) { return false; } + _bus->get_semaphore()->take_blocking(); if (!_bus->configure()) { hal.console->printf("AK09916: Could not configure the bus\n"); @@ -202,7 +201,6 @@ bool AP_Compass_AK09916::init() } if (!_reset()) { - hal.console->printf("AK09916: Reset Failed\n"); goto fail; } @@ -224,7 +222,11 @@ bool AP_Compass_AK09916::init() _initialized = true; /* register the compass instance in the frontend */ - _compass_instance = register_compass(); + _bus->set_device_type(DEVTYPE_AK09916); + if (!register_compass(_bus->get_bus_id(), _compass_instance)) { + goto fail; + } + set_dev_id(_compass_instance, _bus->get_bus_id()); if (_force_external) { set_external(_compass_instance, true); @@ -232,9 +234,6 @@ bool AP_Compass_AK09916::init() set_rotation(_compass_instance, _rotation); - _bus->set_device_type(DEVTYPE_AK09916); - set_dev_id(_compass_instance, _bus->get_bus_id()); - bus_sem->give(); _bus->register_periodic_callback(10000, FUNCTOR_BIND_MEMBER(&AP_Compass_AK09916::_update, void)); diff --git a/libraries/AP_Compass/AP_Compass_AK8963.cpp b/libraries/AP_Compass/AP_Compass_AK8963.cpp index b94698fbe1..6aff89c3b6 100644 --- a/libraries/AP_Compass/AP_Compass_AK8963.cpp +++ b/libraries/AP_Compass/AP_Compass_AK8963.cpp @@ -122,10 +122,10 @@ bool AP_Compass_AK8963::init() { AP_HAL::Semaphore *bus_sem = _bus->get_semaphore(); - if (!bus_sem || !_bus->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { - hal.console->printf("AK8963: Unable to get bus semaphore\n"); + if (!bus_sem) { return false; } + _bus->get_semaphore()->take_blocking(); if (!_bus->configure()) { hal.console->printf("AK8963: Could not configure the bus\n"); @@ -155,13 +155,13 @@ bool AP_Compass_AK8963::init() _initialized = true; /* register the compass instance in the frontend */ - _compass_instance = register_compass(); - - set_rotation(_compass_instance, _rotation); - _bus->set_device_type(DEVTYPE_AK8963); + if (!register_compass(_bus->get_bus_id(), _compass_instance)) { + goto fail; + } set_dev_id(_compass_instance, _bus->get_bus_id()); + set_rotation(_compass_instance, _rotation); bus_sem->give(); _bus->register_periodic_callback(10000, FUNCTOR_BIND_MEMBER(&AP_Compass_AK8963::_update, void)); diff --git a/libraries/AP_Compass/AP_Compass_BMM150.cpp b/libraries/AP_Compass/AP_Compass_BMM150.cpp index 75bd068e7a..d6fc5d6506 100644 --- a/libraries/AP_Compass/AP_Compass_BMM150.cpp +++ b/libraries/AP_Compass/AP_Compass_BMM150.cpp @@ -142,10 +142,7 @@ bool AP_Compass_BMM150::init() uint8_t val = 0; bool ret; - if (!_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { - hal.console->printf("BMM150: Unable to get bus semaphore\n"); - return false; - } + _dev->get_semaphore()->take_blocking(); // 10 retries for init _dev->set_retries(10); @@ -214,12 +211,14 @@ bool AP_Compass_BMM150::init() _dev->get_semaphore()->give(); /* register the compass instance in the frontend */ - _compass_instance = register_compass(); + _dev->set_device_type(DEVTYPE_BMM150); + if (!register_compass(_dev->get_bus_id(), _compass_instance)) { + return false; + } + set_dev_id(_compass_instance, _dev->get_bus_id()); set_rotation(_compass_instance, _rotation); - _dev->set_device_type(DEVTYPE_BMM150); - set_dev_id(_compass_instance, _dev->get_bus_id()); _perf_err = hal.util->perf_alloc(AP_HAL::Util::PC_COUNT, "BMM150_err"); diff --git a/libraries/AP_Compass/AP_Compass_Backend.cpp b/libraries/AP_Compass/AP_Compass_Backend.cpp index afec18b51f..48b55bb2ee 100644 --- a/libraries/AP_Compass/AP_Compass_Backend.cpp +++ b/libraries/AP_Compass/AP_Compass_Backend.cpp @@ -4,6 +4,7 @@ #include "AP_Compass_Backend.h" #include +#include extern const AP_HAL::HAL& hal; @@ -14,8 +15,10 @@ AP_Compass_Backend::AP_Compass_Backend() void AP_Compass_Backend::rotate_field(Vector3f &mag, uint8_t instance) { - Compass::mag_state &state = _compass._state[instance]; - mag.rotate(MAG_BOARD_ORIENTATION); + Compass::mag_state &state = _compass._state[Compass::StateIndex(instance)]; + if (MAG_BOARD_ORIENTATION != ROTATION_NONE) { + mag.rotate(MAG_BOARD_ORIENTATION); + } mag.rotate(state.rotation); if (!state.external) { @@ -27,26 +30,35 @@ void AP_Compass_Backend::rotate_field(Vector3f &mag, uint8_t instance) } } else { // add user selectable orientation +#if !APM_BUILD_TYPE(APM_BUILD_AP_Periph) + Rotation rotation = Rotation(state.orientation.get()); + if (rotation == ROTATION_CUSTOM && _compass._custom_rotation) { + mag = *_compass._custom_rotation * mag; + } else { + mag.rotate(rotation); + } +#else mag.rotate((enum Rotation)state.orientation.get()); +#endif } } void AP_Compass_Backend::publish_raw_field(const Vector3f &mag, uint8_t instance) { - Compass::mag_state &state = _compass._state[instance]; + Compass::mag_state &state = _compass._state[Compass::StateIndex(instance)]; // note that we do not set last_update_usec here as otherwise the // EKF and DCM would end up consuming compass data at the full // sensor rate. We want them to consume only the filtered fields state.last_update_ms = AP_HAL::millis(); #if COMPASS_CAL_ENABLED - _compass._calibrator[instance].new_sample(mag); + _compass._calibrator[_compass._get_priority(Compass::StateIndex(instance))].new_sample(mag); #endif } void AP_Compass_Backend::correct_field(Vector3f &mag, uint8_t i) { - Compass::mag_state &state = _compass._state[i]; + Compass::mag_state &state = _compass._state[Compass::StateIndex(i)]; if (state.diagonals.get().is_zero()) { state.diagonals.set(Vector3f(1.0f,1.0f,1.0f)); @@ -123,7 +135,7 @@ void AP_Compass_Backend::accumulate_sample(Vector3f &field, uint8_t instance, WITH_SEMAPHORE(_sem); - Compass::mag_state &state = _compass._state[instance]; + Compass::mag_state &state = _compass._state[Compass::StateIndex(instance)]; state.accum += field; state.accum_count++; if (max_samples && state.accum_count >= max_samples) { @@ -137,7 +149,7 @@ void AP_Compass_Backend::drain_accumulated_samples(uint8_t instance, { WITH_SEMAPHORE(_sem); - Compass::mag_state &state = _compass._state[instance]; + Compass::mag_state &state = _compass._state[Compass::StateIndex(instance)]; if (state.accum_count == 0) { return; @@ -159,7 +171,7 @@ void AP_Compass_Backend::drain_accumulated_samples(uint8_t instance, */ void AP_Compass_Backend::publish_filtered_field(const Vector3f &mag, uint8_t instance) { - Compass::mag_state &state = _compass._state[instance]; + Compass::mag_state &state = _compass._state[Compass::StateIndex(instance)]; state.field = mag; @@ -169,7 +181,7 @@ void AP_Compass_Backend::publish_filtered_field(const Vector3f &mag, uint8_t ins void AP_Compass_Backend::set_last_update_usec(uint32_t last_update, uint8_t instance) { - Compass::mag_state &state = _compass._state[instance]; + Compass::mag_state &state = _compass._state[Compass::StateIndex(instance)]; state.last_update_usec = last_update; } @@ -177,9 +189,9 @@ void AP_Compass_Backend::set_last_update_usec(uint32_t last_update, uint8_t inst register a new backend with frontend, returning instance which should be used in publish_field() */ -uint8_t AP_Compass_Backend::register_compass(void) const +bool AP_Compass_Backend::register_compass(int32_t dev_id, uint8_t& instance) const { - return _compass.register_compass(); + return _compass.register_compass(dev_id, instance); } @@ -188,8 +200,9 @@ uint8_t AP_Compass_Backend::register_compass(void) const */ void AP_Compass_Backend::set_dev_id(uint8_t instance, uint32_t dev_id) { - _compass._state[instance].dev_id.set_and_notify(dev_id); - _compass._state[instance].detected_dev_id = dev_id; + _compass._state[Compass::StateIndex(instance)].dev_id.set_and_notify(dev_id); + _compass._state[Compass::StateIndex(instance)].detected_dev_id = dev_id; + _compass._state[Compass::StateIndex(instance)].expected_dev_id = dev_id; } /* @@ -197,7 +210,7 @@ void AP_Compass_Backend::set_dev_id(uint8_t instance, uint32_t dev_id) */ void AP_Compass_Backend::save_dev_id(uint8_t instance) { - _compass._state[instance].dev_id.save(); + _compass._state[Compass::StateIndex(instance)].dev_id.save(); } /* @@ -205,20 +218,27 @@ void AP_Compass_Backend::save_dev_id(uint8_t instance) */ void AP_Compass_Backend::set_external(uint8_t instance, bool external) { - if (_compass._state[instance].external != 2) { - _compass._state[instance].external.set_and_notify(external); + if (_compass._state[Compass::StateIndex(instance)].external != 2) { + _compass._state[Compass::StateIndex(instance)].external.set_and_notify(external); } } bool AP_Compass_Backend::is_external(uint8_t instance) { - return _compass._state[instance].external; + return _compass._state[Compass::StateIndex(instance)].external; } // set rotation of an instance void AP_Compass_Backend::set_rotation(uint8_t instance, enum Rotation rotation) { - _compass._state[instance].rotation = rotation; + _compass._state[Compass::StateIndex(instance)].rotation = rotation; +#if !APM_BUILD_TYPE(APM_BUILD_AP_Periph) + // lazily create the custom rotation matrix + if (!_compass._custom_rotation && Rotation(_compass._state[Compass::StateIndex(instance)].orientation.get()) == ROTATION_CUSTOM) { + _compass._custom_rotation = new Matrix3f(); + _compass._custom_rotation->from_euler(radians(_compass._custom_roll), radians(_compass._custom_pitch), radians(_compass._custom_yaw)); + } +#endif } static constexpr float FILTER_KOEF = 0.1f; diff --git a/libraries/AP_Compass/AP_Compass_Backend.h b/libraries/AP_Compass/AP_Compass_Backend.h index 49f90b9139..e52ee2ea3d 100644 --- a/libraries/AP_Compass/AP_Compass_Backend.h +++ b/libraries/AP_Compass/AP_Compass_Backend.h @@ -89,7 +89,7 @@ protected: void drain_accumulated_samples(uint8_t instance, const Vector3f *scale = NULL); // register a new compass instance with the frontend - uint8_t register_compass(void) const; + bool register_compass(int32_t dev_id, uint8_t& instance) const; // set dev_id for an instance void set_dev_id(uint8_t instance, uint32_t dev_id); @@ -113,7 +113,7 @@ protected: Compass &_compass; // semaphore for access to shared frontend data - HAL_Semaphore_Recursive _sem; + HAL_Semaphore _sem; // Check that the compass field is valid by using a mean filter on the vector length bool field_ok(const Vector3f &field); diff --git a/libraries/AP_Compass/AP_Compass_Calibration.cpp b/libraries/AP_Compass/AP_Compass_Calibration.cpp index 2087e80545..442892c4b2 100644 --- a/libraries/AP_Compass/AP_Compass_Calibration.cpp +++ b/libraries/AP_Compass/AP_Compass_Calibration.cpp @@ -7,7 +7,7 @@ #include "AP_Compass.h" -extern AP_HAL::HAL& hal; +const extern AP_HAL::HAL& hal; #if COMPASS_CAL_ENABLED @@ -19,7 +19,7 @@ void Compass::cal_update() bool running = false; - for (uint8_t i=0; i=2); + _calibrator[prio].set_orientation(r, _get_state(prio).external, _rotate_auto>=2); } } - _cal_saved[i] = false; - _calibrator[i].start(retry, delay, get_offsets_max(), i); + _cal_saved[prio] = false; + _calibrator[prio].start(retry, delay, get_offsets_max(), i); // disable compass learning both for calibration and after completion _learn.set_and_save(0); @@ -120,12 +125,13 @@ void Compass::start_calibration_all(bool retry, bool autosave, float delay, bool void Compass::_cancel_calibration(uint8_t i) { AP_Notify::events.initiated_compass_cal = 0; + Priority prio = Priority(i); - if (_calibrator[i].running() || _calibrator[i].get_status() == CompassCalibrator::Status::WAITING_TO_START) { + if (_calibrator[prio].running() || _calibrator[prio].get_status() == CompassCalibrator::Status::WAITING_TO_START) { AP_Notify::events.compass_cal_canceled = 1; } - _cal_saved[i] = false; - _calibrator[i].stop(); + _cal_saved[prio] = false; + _calibrator[prio].stop(); } void Compass::_cancel_calibration_mask(uint8_t mask) @@ -144,14 +150,16 @@ void Compass::cancel_calibration_all() bool Compass::_accept_calibration(uint8_t i) { - CompassCalibrator& cal = _calibrator[i]; + Priority prio = Priority(i); + + CompassCalibrator& cal = _calibrator[prio]; const CompassCalibrator::Status cal_status = cal.get_status(); - if (_cal_saved[i] || cal_status == CompassCalibrator::Status::NOT_STARTED) { + if (_cal_saved[prio] || cal_status == CompassCalibrator::Status::NOT_STARTED) { return true; } else if (cal_status == CompassCalibrator::Status::SUCCESS) { _cal_complete_requires_reboot = true; - _cal_saved[i] = true; + _cal_saved[prio] = true; Vector3f ofs, diag, offdiag; float scale_factor; @@ -162,8 +170,8 @@ bool Compass::_accept_calibration(uint8_t i) set_and_save_offdiagonals(i,offdiag); set_and_save_scale_factor(i,scale_factor); - if (_state[i].external && _rotate_auto >= 2) { - _state[i].orientation.set_and_save_ifchanged(cal.get_orientation()); + if (_get_state(prio).external && _rotate_auto >= 2) { + set_and_save_orientation(i, cal.get_orientation()); } if (!is_calibrating()) { @@ -178,9 +186,9 @@ bool Compass::_accept_calibration(uint8_t i) bool Compass::_accept_calibration_mask(uint8_t mask) { bool success = true; - for (uint8_t i=0; iget_soft_armed()) { - hal.console->printf("Disarm for compass calibration\n"); + gcs().send_text(MAV_SEVERITY_NOTICE, "Disarm to allow compass calibration"); result = MAV_RESULT_FAILED; break; } diff --git a/libraries/AP_Compass/AP_Compass_HIL.cpp b/libraries/AP_Compass/AP_Compass_HIL.cpp index 284b2884ee..c19be067ef 100644 --- a/libraries/AP_Compass/AP_Compass_HIL.cpp +++ b/libraries/AP_Compass/AP_Compass_HIL.cpp @@ -48,9 +48,12 @@ AP_Compass_Backend *AP_Compass_HIL::detect() bool AP_Compass_HIL::init(void) { - // register two compass instances + // register compass instances for (uint8_t i=0; iget_semaphore(); - if (!bus_sem || !bus_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { + if (!bus_sem) { hal.console->printf("HMC5843: Unable to get bus semaphore\n"); return false; } + bus_sem->take_blocking(); // high retries for init _bus->set_retries(10); @@ -193,13 +194,15 @@ bool AP_Compass_HMC5843::init() // perform an initial read read(); - _compass_instance = register_compass(); + //register compass instance + _bus->set_device_type(DEVTYPE_HMC5883); + if (!register_compass(_bus->get_bus_id(), _compass_instance)) { + return false; + } + set_dev_id(_compass_instance, _bus->get_bus_id()); set_rotation(_compass_instance, _rotation); - _bus->set_device_type(DEVTYPE_HMC5883); - set_dev_id(_compass_instance, _bus->get_bus_id()); - if (_force_external) { set_external(_compass_instance, true); } diff --git a/libraries/AP_Compass/AP_Compass_IST8308.cpp b/libraries/AP_Compass/AP_Compass_IST8308.cpp index c90aa7a8db..b90958a79d 100644 --- a/libraries/AP_Compass/AP_Compass_IST8308.cpp +++ b/libraries/AP_Compass/AP_Compass_IST8308.cpp @@ -109,9 +109,7 @@ bool AP_Compass_IST8308::init() { uint8_t reset_count = 0; - if (!_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { - return false; - } + _dev->get_semaphore()->take_blocking(); // high retries for init _dev->set_retries(10); @@ -160,16 +158,18 @@ bool AP_Compass_IST8308::init() _dev->get_semaphore()->give(); - _instance = register_compass(); + //register compass instance + _dev->set_device_type(DEVTYPE_IST8308); + if (!register_compass(_dev->get_bus_id(), _instance)) { + return false; + } + set_dev_id(_instance, _dev->get_bus_id()); printf("%s found on bus %u id %u address 0x%02x\n", name, _dev->bus_num(), _dev->get_bus_id(), _dev->get_bus_address()); set_rotation(_instance, _rotation); - _dev->set_device_type(DEVTYPE_IST8308); - set_dev_id(_instance, _dev->get_bus_id()); - if (_force_external) { set_external(_instance, true); } diff --git a/libraries/AP_Compass/AP_Compass_IST8310.cpp b/libraries/AP_Compass/AP_Compass_IST8310.cpp index 7adc0eee80..52716e4c71 100644 --- a/libraries/AP_Compass/AP_Compass_IST8310.cpp +++ b/libraries/AP_Compass/AP_Compass_IST8310.cpp @@ -105,9 +105,7 @@ bool AP_Compass_IST8310::init() { uint8_t reset_count = 0; - if (!_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { - return false; - } + _dev->get_semaphore()->take_blocking(); // high retries for init _dev->set_retries(10); @@ -153,16 +151,18 @@ bool AP_Compass_IST8310::init() _dev->get_semaphore()->give(); - _instance = register_compass(); + // register compass instance + _dev->set_device_type(DEVTYPE_IST8310); + if (!register_compass(_dev->get_bus_id(), _instance)) { + return false; + } + set_dev_id(_instance, _dev->get_bus_id()); printf("%s found on bus %u id %u address 0x%02x\n", name, _dev->bus_num(), _dev->get_bus_id(), _dev->get_bus_address()); set_rotation(_instance, _rotation); - _dev->set_device_type(DEVTYPE_IST8310); - set_dev_id(_instance, _dev->get_bus_id()); - if (_force_external) { set_external(_instance, true); } diff --git a/libraries/AP_Compass/AP_Compass_LIS3MDL.cpp b/libraries/AP_Compass/AP_Compass_LIS3MDL.cpp index 8d9e28c40a..f65a07e218 100644 --- a/libraries/AP_Compass/AP_Compass_LIS3MDL.cpp +++ b/libraries/AP_Compass/AP_Compass_LIS3MDL.cpp @@ -75,9 +75,7 @@ AP_Compass_LIS3MDL::AP_Compass_LIS3MDL(AP_HAL::OwnPtr _dev, bool AP_Compass_LIS3MDL::init() { - if (!dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { - return false; - } + dev->get_semaphore()->take_blocking(); if (dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI) { dev->set_read_flag(0xC0); @@ -107,7 +105,11 @@ bool AP_Compass_LIS3MDL::init() dev->get_semaphore()->give(); /* register the compass instance in the frontend */ - compass_instance = register_compass(); + dev->set_device_type(DEVTYPE_LIS3MDL); + if (!register_compass(dev->get_bus_id(), compass_instance)) { + return false; + } + set_dev_id(compass_instance, dev->get_bus_id()); printf("Found a LIS3MDL on 0x%x as compass %u\n", dev->get_bus_id(), compass_instance); @@ -117,9 +119,6 @@ bool AP_Compass_LIS3MDL::init() set_external(compass_instance, true); } - dev->set_device_type(DEVTYPE_LIS3MDL); - set_dev_id(compass_instance, dev->get_bus_id()); - // call timer() at 80Hz dev->register_periodic_callback(1000000U/80U, FUNCTOR_BIND_MEMBER(&AP_Compass_LIS3MDL::timer, void)); diff --git a/libraries/AP_Compass/AP_Compass_LSM303D.cpp b/libraries/AP_Compass/AP_Compass_LSM303D.cpp index b6e7375893..840d6d4699 100644 --- a/libraries/AP_Compass/AP_Compass_LSM303D.cpp +++ b/libraries/AP_Compass/AP_Compass_LSM303D.cpp @@ -266,13 +266,14 @@ bool AP_Compass_LSM303D::init(enum Rotation rotation) _initialised = true; /* register the compass instance in the frontend */ - _compass_instance = register_compass(); + _dev->set_device_type(DEVTYPE_LSM303D); + if (!register_compass(_dev->get_bus_id(), _compass_instance)) { + return false; + } + set_dev_id(_compass_instance, _dev->get_bus_id()); set_rotation(_compass_instance, rotation); - _dev->set_device_type(DEVTYPE_LSM303D); - set_dev_id(_compass_instance, _dev->get_bus_id()); - // read at 91Hz. We don't run at 100Hz as fetching data too fast can cause some very // odd periodic changes in the output data _dev->register_periodic_callback(11000, FUNCTOR_BIND_MEMBER(&AP_Compass_LSM303D::_update, void)); @@ -282,9 +283,7 @@ bool AP_Compass_LSM303D::init(enum Rotation rotation) bool AP_Compass_LSM303D::_hardware_init() { - if (!_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { - AP_HAL::panic("LSM303D: Unable to get semaphore"); - } + _dev->get_semaphore()->take_blocking(); // initially run the bus at low speed _dev->set_speed(AP_HAL::Device::SPEED_LOW); @@ -292,7 +291,6 @@ bool AP_Compass_LSM303D::_hardware_init() // Test WHOAMI uint8_t whoami = _register_read(ADDR_WHO_AM_I); if (whoami != WHO_I_AM) { - hal.console->printf("LSM303D: unexpected WHOAMI 0x%x\n", (unsigned)whoami); goto fail_whoami; } diff --git a/libraries/AP_Compass/AP_Compass_LSM9DS1.cpp b/libraries/AP_Compass/AP_Compass_LSM9DS1.cpp index 073332f983..f40ea2e05a 100644 --- a/libraries/AP_Compass/AP_Compass_LSM9DS1.cpp +++ b/libraries/AP_Compass/AP_Compass_LSM9DS1.cpp @@ -83,10 +83,11 @@ bool AP_Compass_LSM9DS1::init() { AP_HAL::Semaphore *bus_sem = _dev->get_semaphore(); - if (!bus_sem || !bus_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { + if (!bus_sem) { hal.console->printf("LSM9DS1: Unable to get bus semaphore\n"); return false; } + bus_sem->take_blocking(); if (!_check_id()) { hal.console->printf("LSM9DS1: Could not check id\n"); @@ -103,12 +104,15 @@ bool AP_Compass_LSM9DS1::init() goto errout; } - _compass_instance = register_compass(); + //register compass instance + _dev->set_device_type(DEVTYPE_LSM9DS1); + if (!register_compass(_dev->get_bus_id(), _compass_instance)) { + goto errout; + } + set_dev_id(_compass_instance, _dev->get_bus_id()); set_rotation(_compass_instance, _rotation); - _dev->set_device_type(DEVTYPE_LSM9DS1); - set_dev_id(_compass_instance, _dev->get_bus_id()); _dev->register_periodic_callback(10000, FUNCTOR_BIND_MEMBER(&AP_Compass_LSM9DS1::_update, void)); diff --git a/libraries/AP_Compass/AP_Compass_MAG3110.cpp b/libraries/AP_Compass/AP_Compass_MAG3110.cpp index 780de585ff..37cc16761c 100644 --- a/libraries/AP_Compass/AP_Compass_MAG3110.cpp +++ b/libraries/AP_Compass/AP_Compass_MAG3110.cpp @@ -116,13 +116,14 @@ bool AP_Compass_MAG3110::init(enum Rotation rotation) read(); /* register the compass instance in the frontend */ - _compass_instance = register_compass(); + _dev->set_device_type(DEVTYPE_MAG3110); + if (!register_compass(_dev->get_bus_id(), _compass_instance)) { + return false; + } + set_dev_id(_compass_instance, _dev->get_bus_id()); set_rotation(_compass_instance, rotation); - _dev->set_device_type(DEVTYPE_MAG3110); - set_dev_id(_compass_instance, _dev->get_bus_id()); - set_external(_compass_instance, true); // read at 75Hz @@ -135,9 +136,7 @@ bool AP_Compass_MAG3110::_hardware_init() { AP_HAL::Semaphore *bus_sem = _dev->get_semaphore(); - if (!bus_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { - AP_HAL::panic("MAG3110: Unable to get semaphore"); - } + bus_sem->take_blocking(); // initially run the bus at low speed _dev->set_speed(AP_HAL::Device::SPEED_LOW); diff --git a/libraries/AP_Compass/AP_Compass_MMC3416.cpp b/libraries/AP_Compass/AP_Compass_MMC3416.cpp index 71a22b4e40..d7ba223e16 100644 --- a/libraries/AP_Compass/AP_Compass_MMC3416.cpp +++ b/libraries/AP_Compass/AP_Compass_MMC3416.cpp @@ -68,9 +68,7 @@ AP_Compass_MMC3416::AP_Compass_MMC3416(AP_HAL::OwnPtr _dev, bool AP_Compass_MMC3416::init() { - if (!dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { - return false; - } + dev->get_semaphore()->take_blocking(); dev->set_retries(10); @@ -92,7 +90,12 @@ bool AP_Compass_MMC3416::init() dev->get_semaphore()->give(); /* register the compass instance in the frontend */ - compass_instance = register_compass(); + dev->set_device_type(DEVTYPE_MMC3416); + if (!register_compass(dev->get_bus_id(), compass_instance)) { + return false; + } + + set_dev_id(compass_instance, dev->get_bus_id()); printf("Found a MMC3416 on 0x%x as compass %u\n", dev->get_bus_id(), compass_instance); @@ -102,9 +105,6 @@ bool AP_Compass_MMC3416::init() set_external(compass_instance, true); } - dev->set_device_type(DEVTYPE_MMC3416); - set_dev_id(compass_instance, dev->get_bus_id()); - dev->set_retries(1); // call timer() at 100Hz @@ -224,6 +224,15 @@ void AP_Compass_MMC3416::timer() } #if 0 +// @LoggerMessage: MMO +// @Description: MMC3416 compass data +// @Field: TimeUS: Time since system startup +// @Field: Nx: new measurement X axis +// @Field: Ny: new measurement Y axis +// @Field: Nz: new measurement Z axis +// @Field: Ox: new offset X axis +// @Field: Oy: new offset Y axis +// @Field: Oz: new offset Z axis AP::logger().Write("MMO", "TimeUS,Nx,Ny,Nz,Ox,Oy,Oz", "Qffffff", AP_HAL::micros64(), (double)new_offset.x, diff --git a/libraries/AP_Compass/AP_Compass_QMC5883L.cpp b/libraries/AP_Compass/AP_Compass_QMC5883L.cpp index 4097fc1357..adc7b5ba93 100644 --- a/libraries/AP_Compass/AP_Compass_QMC5883L.cpp +++ b/libraries/AP_Compass/AP_Compass_QMC5883L.cpp @@ -85,9 +85,7 @@ AP_Compass_QMC5883L::AP_Compass_QMC5883L(AP_HAL::OwnPtr dev, bool AP_Compass_QMC5883L::init() { - if (!_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { - return false; - } + _dev->get_semaphore()->take_blocking(); _dev->set_retries(10); @@ -115,16 +113,18 @@ bool AP_Compass_QMC5883L::init() _dev->get_semaphore()->give(); - _instance = register_compass(); + //register compass instance + _dev->set_device_type(DEVTYPE_QMC5883L); + if (!register_compass(_dev->get_bus_id(), _instance)) { + return false; + } + set_dev_id(_instance, _dev->get_bus_id()); printf("%s found on bus %u id %u address 0x%02x\n", name, _dev->bus_num(), _dev->get_bus_id(), _dev->get_bus_address()); set_rotation(_instance, _rotation); - _dev->set_device_type(DEVTYPE_QMC5883L); - set_dev_id(_instance, _dev->get_bus_id()); - if (_force_external) { set_external(_instance, true); } diff --git a/libraries/AP_Compass/AP_Compass_RM3100.cpp b/libraries/AP_Compass/AP_Compass_RM3100.cpp index 797bec003c..c39cdd4b29 100644 --- a/libraries/AP_Compass/AP_Compass_RM3100.cpp +++ b/libraries/AP_Compass/AP_Compass_RM3100.cpp @@ -93,9 +93,7 @@ AP_Compass_RM3100::AP_Compass_RM3100(AP_HAL::OwnPtr _dev, bool AP_Compass_RM3100::init() { - if (!dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { - return false; - } + dev->get_semaphore()->take_blocking(); if (dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI) { // read has high bit set for SPI @@ -145,19 +143,20 @@ bool AP_Compass_RM3100::init() dev->get_semaphore()->give(); /* register the compass instance in the frontend */ - compass_instance = register_compass(); + dev->set_device_type(DEVTYPE_RM3100); + if (!register_compass(dev->get_bus_id(), compass_instance)) { + return false; + } + set_dev_id(compass_instance, dev->get_bus_id()); hal.console->printf("RM3100: Found at address 0x%x as compass %u\n", dev->get_bus_address(), compass_instance); - + set_rotation(compass_instance, rotation); if (force_external) { set_external(compass_instance, true); } - - dev->set_device_type(DEVTYPE_RM3100); - set_dev_id(compass_instance, dev->get_bus_id()); - + // call timer() at 80Hz dev->register_periodic_callback(1000000U/80U, FUNCTOR_BIND_MEMBER(&AP_Compass_RM3100::timer, void)); diff --git a/libraries/AP_Compass/AP_Compass_SITL.cpp b/libraries/AP_Compass/AP_Compass_SITL.cpp index a9790b2e12..d059e4cc49 100644 --- a/libraries/AP_Compass/AP_Compass_SITL.cpp +++ b/libraries/AP_Compass/AP_Compass_SITL.cpp @@ -10,17 +10,29 @@ AP_Compass_SITL::AP_Compass_SITL() { if (_sitl != nullptr) { _compass._setup_earth_field(); - for (uint8_t i=0; imag_devid[i]; + if (dev_id == 0) { + continue; + } + uint8_t instance; + if (!register_compass(dev_id, instance)) { + continue; + } else if (_num_compassmag_ofs); } - - _compass_instance[i] = register_compass(); - set_dev_id(_compass_instance[i], AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SITL, i, 0, DEVTYPE_SITL)); - - // save so the compass always comes up configured in SITL - save_dev_id(_compass_instance[i]); } // make first compass external @@ -109,12 +121,17 @@ void AP_Compass_SITL::_timer() new_mag_data = _eliptical_corr * new_mag_data; new_mag_data -= _sitl->mag_ofs.get(); - for (uint8_t i=0; imag_orient.get()); - f.rotate(get_board_orientation()); + // and add in AHRS_ORIENTATION setting if not an external compass + if (get_board_orientation() == ROTATION_CUSTOM) { + f = _sitl->ahrs_rotation * f; + } else { + f.rotate(get_board_orientation()); + } // scale the first compass to simulate sensor scale factor errors f *= _sitl->mag_scaling; @@ -126,7 +143,7 @@ void AP_Compass_SITL::_timer() void AP_Compass_SITL::read() { - for (uint8_t i=0; i #include -#define SITL_NUM_COMPASSES 3 +#define MAX_SITL_COMPASSES 3 class AP_Compass_SITL : public AP_Compass_Backend { public: @@ -18,7 +18,8 @@ public: void read(void) override; private: - uint8_t _compass_instance[SITL_NUM_COMPASSES]; + uint8_t _compass_instance[MAX_SITL_COMPASSES]; + uint8_t _num_compass; SITL::SITL *_sitl; // delay buffer variables diff --git a/libraries/AP_Compass/AP_Compass_UAVCAN.cpp b/libraries/AP_Compass/AP_Compass_UAVCAN.cpp index 3d4068e81b..3e66c31edf 100644 --- a/libraries/AP_Compass/AP_Compass_UAVCAN.cpp +++ b/libraries/AP_Compass/AP_Compass_UAVCAN.cpp @@ -69,44 +69,46 @@ void AP_Compass_UAVCAN::subscribe_msgs(AP_UAVCAN* ap_uavcan) } } -AP_Compass_Backend* AP_Compass_UAVCAN::probe() +AP_Compass_Backend* AP_Compass_UAVCAN::probe(uint8_t index) { - WITH_SEMAPHORE(_sem_registry); - AP_Compass_UAVCAN* driver = nullptr; - for (uint8_t i = 0; i < COMPASS_MAX_BACKEND; i++) { - if (!_detected_modules[i].driver && _detected_modules[i].ap_uavcan) { - // Register new Compass mode to a backend - driver = new AP_Compass_UAVCAN(_detected_modules[i].ap_uavcan, _detected_modules[i].node_id, _detected_modules[i].sensor_id); - if (driver) { - _detected_modules[i].driver = driver; - driver->init(); - debug_mag_uavcan(2, - _detected_modules[i].ap_uavcan->get_driver_index(), - "Found Mag Node %d on Bus %d Sensor ID %d\n", - _detected_modules[i].node_id, - _detected_modules[i].ap_uavcan->get_driver_index(), - _detected_modules[i].sensor_id); + if (!_detected_modules[index].driver && _detected_modules[index].ap_uavcan) { + WITH_SEMAPHORE(_sem_registry); + // Register new Compass mode to a backend + driver = new AP_Compass_UAVCAN(_detected_modules[index].ap_uavcan, _detected_modules[index].node_id, _detected_modules[index].sensor_id); + if (driver) { + if (!driver->init()) { + delete driver; + return nullptr; } - break; + _detected_modules[index].driver = driver; + debug_mag_uavcan(2, + _detected_modules[index].ap_uavcan->get_driver_index(), + "Found Mag Node %d on Bus %d Sensor ID %d\n", + _detected_modules[index].node_id, + _detected_modules[index].ap_uavcan->get_driver_index(), + _detected_modules[index].sensor_id); } } return driver; } -void AP_Compass_UAVCAN::init() +bool AP_Compass_UAVCAN::init() { - _instance = register_compass(); - uint32_t devid = AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_UAVCAN, _ap_uavcan->get_driver_index(), _node_id, - 1); // the 1 is arbitrary + _sensor_id + 1); // we use sensor_id as devtype + // Adding 1 is necessary to allow backward compatibilty, where this field was set as 1 by default + if (!register_compass(devid, _instance)) { + return false; + } set_dev_id(_instance, devid); set_external(_instance, true); debug_mag_uavcan(2, _ap_uavcan->get_driver_index(), "AP_Compass_UAVCAN loaded\n\r"); + return true; } AP_Compass_UAVCAN* AP_Compass_UAVCAN::get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id, uint8_t sensor_id) @@ -144,6 +146,21 @@ AP_Compass_UAVCAN* AP_Compass_UAVCAN::get_uavcan_backend(AP_UAVCAN* ap_uavcan, u } } } + + struct DetectedModules tempslot; + // Sort based on the node_id, larger values first + // we do this, so that we have repeatable compass + // registration, especially in cases of extraneous + // CAN compass is connected. + for (uint8_t i = 1; i < COMPASS_MAX_BACKEND; i++) { + for (uint8_t j = i; j > 0; j--) { + if (_detected_modules[j].node_id > _detected_modules[j-1].node_id) { + tempslot = _detected_modules[j]; + _detected_modules[j] = _detected_modules[j-1]; + _detected_modules[j-1] = tempslot; + } + } + } return nullptr; } diff --git a/libraries/AP_Compass/AP_Compass_UAVCAN.h b/libraries/AP_Compass/AP_Compass_UAVCAN.h index 4ec1aec21d..f487b4287c 100644 --- a/libraries/AP_Compass/AP_Compass_UAVCAN.h +++ b/libraries/AP_Compass/AP_Compass_UAVCAN.h @@ -15,13 +15,13 @@ public: void read(void) override; static void subscribe_msgs(AP_UAVCAN* ap_uavcan); - static AP_Compass_Backend* probe(); + static AP_Compass_Backend* probe(uint8_t index); static void handle_magnetic_field(AP_UAVCAN* ap_uavcan, uint8_t node_id, const MagCb &cb); static void handle_magnetic_field_2(AP_UAVCAN* ap_uavcan, uint8_t node_id, const Mag2Cb &cb); private: - void init(); + bool init(); // callback for UAVCAN messages void handle_mag_msg(const Vector3f &mag); diff --git a/libraries/AP_Compass/CompassCalibrator.cpp b/libraries/AP_Compass/CompassCalibrator.cpp index 49334f4147..a403d7d5b5 100644 --- a/libraries/AP_Compass/CompassCalibrator.cpp +++ b/libraries/AP_Compass/CompassCalibrator.cpp @@ -878,15 +878,16 @@ bool CompassCalibrator::calculate_orientation(void) pass = _orientation_confidence > variance_threshold; } if (!pass) { - gcs().send_text(MAV_SEVERITY_CRITICAL, "Mag(%u) bad orientation: %u/%u %.1f", _compass_idx, + GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Mag(%u) bad orientation: %u/%u %.1f", _compass_idx, besti, besti2, (double)_orientation_confidence); + (void)besti2; } else if (besti == _orientation) { // no orientation change - gcs().send_text(MAV_SEVERITY_INFO, "Mag(%u) good orientation: %u %.1f", _compass_idx, besti, (double)_orientation_confidence); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Mag(%u) good orientation: %u %.1f", _compass_idx, besti, (double)_orientation_confidence); } else if (!_is_external || !_fix_orientation) { - gcs().send_text(MAV_SEVERITY_CRITICAL, "Mag(%u) internal bad orientation: %u %.1f", _compass_idx, besti, (double)_orientation_confidence); + GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Mag(%u) internal bad orientation: %u %.1f", _compass_idx, besti, (double)_orientation_confidence); } else { - gcs().send_text(MAV_SEVERITY_INFO, "Mag(%u) new orientation: %u was %u %.1f", _compass_idx, besti, _orientation, (double)_orientation_confidence); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Mag(%u) new orientation: %u was %u %.1f", _compass_idx, besti, _orientation, (double)_orientation_confidence); } if (!pass) { @@ -956,7 +957,7 @@ bool CompassCalibrator::fix_radius(void) if (correction > COMPASS_MAX_SCALE_FACTOR || correction < COMPASS_MIN_SCALE_FACTOR) { // don't allow more than 30% scale factor correction - gcs().send_text(MAV_SEVERITY_ERROR, "Mag(%u) bad radius %.0f expected %.0f", + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Mag(%u) bad radius %.0f expected %.0f", _compass_idx, _params.radius, expected_radius); diff --git a/libraries/AP_Compass/Compass_learn.cpp b/libraries/AP_Compass/Compass_learn.cpp index 9498b17fb6..8927df6a90 100644 --- a/libraries/AP_Compass/Compass_learn.cpp +++ b/libraries/AP_Compass/Compass_learn.cpp @@ -9,6 +9,7 @@ #include #include +#include #if COMPASS_LEARN_ENABLED @@ -19,11 +20,11 @@ CompassLearn::CompassLearn(Compass &_compass) : compass(_compass) { gcs().send_text(MAV_SEVERITY_INFO, "CompassLearn: Initialised"); - for (uint8_t i=0; i