aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Compass: modify compass driver to support consistent ordering and hotplugging

Browse Source
This commit is contained in:
Siddharth Purohit 2019-11-20 15:18:10 +08:00 committed by Randy Mackay
parent 545331024f
commit 8d227d401a
26 changed files with 667 additions and 324 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

502
libraries/AP_Compass/AP_Compass.cpp
View File

@ -27,6 +27,8 @@
#include "AP_Compass_RM3100.h"
#include "AP_Compass.h"
#include "Compass_learn.h"
#include <stdio.h>
extern const AP_HAL::HAL& hal;
@ -78,8 +80,7 @@ const AP_Param::GroupInfo Compass::var_info[] = {
// @Units: mGauss
// @Increment: 1
// @User: Advanced
// @Calibration: 1
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
@ -102,7 +103,7 @@ const AP_Param::GroupInfo Compass::var_info[] = {
// @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
@ -144,22 +145,21 @@ const AP_Param::GroupInfo Compass::var_info[] = {
// @Units: mGauss/A
// @Increment: 1
// @User: Advanced
// @Calibration: 1
AP_GROUPINFO("MOT", 7, Compass, _state[0].motor_compensation, 0),
AP_GROUPINFO("MOT", 7, Compass, _state._priv_instance[0].motor_compensation, 0),
// @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
// @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
// @Param: OFS2_X
@ -187,8 +187,7 @@ const AP_Param::GroupInfo Compass::var_info[] = {
// @Units: mGauss
// @Increment: 1
// @User: Advanced
// @Calibration: 1
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
@ -215,8 +214,7 @@ const AP_Param::GroupInfo Compass::var_info[] = {
// @Units: mGauss/A
// @Increment: 1
// @User: Advanced
// @Calibration: 1
AP_GROUPINFO("MOT2", 11, Compass, _state[1].motor_compensation, 0),
AP_GROUPINFO("MOT2", 11, Compass, _state._priv_instance[1].motor_compensation, 0),
#endif // HAL_COMPASS_MAX_SENSORS
@ -246,8 +244,7 @@ const AP_Param::GroupInfo Compass::var_info[] = {
// @Units: mGauss
// @Increment: 1
// @User: Advanced
// @Calibration: 1
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
@ -274,8 +271,7 @@ const AP_Param::GroupInfo Compass::var_info[] = {
// @Units: mGauss/A
// @Increment: 1
// @User: Advanced
// @Calibration: 1
AP_GROUPINFO("MOT3", 14, Compass, _state[2].motor_compensation, 0),
AP_GROUPINFO("MOT3", 14, Compass, _state._priv_instance[2].motor_compensation, 0),
#endif // HAL_COMPASS_MAX_SENSORS
// @Param: DEV_ID
@ -283,69 +279,70 @@ const AP_Param::GroupInfo Compass::var_info[] = {
// @Description: Compass device id. Automatically detected, do not set manually
// @ReadOnly: True
// @User: Advanced
// @Calibration: 1
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
// @Param: DEV_ID2
// @DisplayName: Compass2 device id
// @Description: Second compass's device id. Automatically detected, do not set manually
// @ReadOnly: True
// @User: Advanced
// @Calibration: 1
AP_GROUPINFO("DEV_ID2", 16, Compass, _state[1].dev_id, 0),
AP_GROUPINFO("DEV_ID2", 16, Compass, _state._priv_instance[1].dev_id, 0),
#endif // HAL_COMPASS_MAX_SENSORS
#if HAL_COMPASS_MAX_SENSORS > 2
// @Param: DEV_ID3
// @DisplayName: Compass3 device id
// @Description: Third compass's device id. Automatically detected, do not set manually
// @ReadOnly: True
// @User: Advanced
// @Calibration: 1
AP_GROUPINFO("DEV_ID3", 17, Compass, _state[2].dev_id, 0),
AP_GROUPINFO("DEV_ID3", 17, Compass, _state._priv_instance[2].dev_id, 0),
#endif // HAL_COMPASS_MAX_SENSORS
#if HAL_COMPASS_MAX_SENSORS > 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
// @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),
AP_GROUPINFO("EXTERN2",20, Compass, _state._priv_instance[1].external, 0),
#endif // HAL_COMPASS_MAX_SENSORS
#if HAL_COMPASS_MAX_SENSORS > 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
// @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),
AP_GROUPINFO("EXTERN3",23, Compass, _state._priv_instance[2].external, 0),
#endif // HAL_COMPASS_MAX_SENSORS
// @Param: DIA_X
@ -364,8 +361,7 @@ const AP_Param::GroupInfo Compass::var_info[] = {
// @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
// @Calibration: 1
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
@ -383,8 +379,7 @@ const AP_Param::GroupInfo Compass::var_info[] = {
// @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
// @Calibration: 1
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
// @Param: DIA2_X
@ -403,8 +398,7 @@ const AP_Param::GroupInfo Compass::var_info[] = {
// @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
// @Calibration: 1
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
@ -422,8 +416,7 @@ const AP_Param::GroupInfo Compass::var_info[] = {
// @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
// @Calibration: 1
AP_GROUPINFO("ODI2", 27, Compass, _state[1].offdiagonals, 0),
AP_GROUPINFO("ODI2", 27, Compass, _state._priv_instance[1].offdiagonals, 0),
#endif // HAL_COMPASS_MAX_SENSORS
#if HAL_COMPASS_MAX_SENSORS > 2
@ -443,8 +436,7 @@ const AP_Param::GroupInfo Compass::var_info[] = {
// @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
// @Calibration: 1
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
@ -462,8 +454,7 @@ const AP_Param::GroupInfo Compass::var_info[] = {
// @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
// @Calibration: 1
AP_GROUPINFO("ODI3", 29, Compass, _state[2].offdiagonals, 0),
AP_GROUPINFO("ODI3", 29, Compass, _state._priv_instance[2].offdiagonals, 0),
#endif // HAL_COMPASS_MAX_SENSORS
// @Param: CAL_FIT
@ -510,26 +501,26 @@ const AP_Param::GroupInfo Compass::var_info[] = {
// @Values: 0:Disabled,1:CheckOnly,2:CheckAndFix
AP_GROUPINFO("AUTO_ROT", 35, Compass, _rotate_auto, HAL_COMPASS_AUTO_ROT_DEFAULT),
// @Param: PRI_DEVID
// @DisplayName: Primary Compass device id
// @Description: Primary Compass device id, set automatically if 0. Reboot required after change.
// @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.
// @User: Advanced
AP_GROUPINFO("PRI_DEV", 36, Compass, _priority_did_stored_list[0], 0),
AP_GROUPINFO("PRIO1_ID", 36, Compass, _priority_did_stored_list._priv_instance[0], 0),
#if HAL_COMPASS_MAX_SENSORS > 1
// @Param: SEC_DID
// @DisplayName: Secondary Compass device id
// @Description: Secondary Compass device id, set automatically if 0. Reboot required after change.
// @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.
// @User: Advanced
AP_GROUPINFO("SEC_DEV", 37, Compass, _priority_did_stored_list[1], 0),
AP_GROUPINFO("PRIO2_ID", 37, Compass, _priority_did_stored_list._priv_instance[1], 0),
#endif // HAL_COMPASS_MAX_SENSORS
#if HAL_COMPASS_MAX_SENSORS > 2
// @Param: TER_DID
// @DisplayName: Tertiary Compass device id
// @Description: Tertiary Compass device id, set automatically if 0. Reboot required after change.
// @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.
// @User: Advanced
AP_GROUPINFO("TER_DEV", 38, Compass, _priority_did_stored_list[2], 0),
AP_GROUPINFO("PRIO3_ID", 38, Compass, _priority_did_stored_list._priv_instance[2], 0),
#endif // HAL_COMPASS_MAX_SENSORS
// @Param: ENABLE
@ -544,7 +535,7 @@ 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
// @Param: SCALE2
@ -552,7 +543,7 @@ const AP_Param::GroupInfo Compass::var_info[] = {
// @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
@ -561,7 +552,7 @@ const AP_Param::GroupInfo Compass::var_info[] = {
// @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
@ -571,43 +562,50 @@ 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
AP_GROUPEND
};
@ -636,10 +634,65 @@ void Compass::init()
return;
}
// 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) && (oldvalue<COMPASS_MAX_INSTANCES) && primary_param_exists) {
_priority_did_stored_list[Priority(0)].set_and_save_ifchanged(_state[StateIndex(oldvalue)].dev_id);
}
}
}
// Load priority list from storage, the changes to priority list
// by user only take effect post reboot, after this
for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
if (_priority_did_stored_list[i] != 0) {
_priority_did_list[i] = _priority_did_stored_list[i];
} else {
// Maintain a list without gaps
for (Priority j(i+1); j<COMPASS_MAX_INSTANCES; j++) {
int32_t temp;
if (_priority_did_stored_list[j] == 0) {
continue;
}
temp = _priority_did_stored_list[j];
_priority_did_stored_list[j].set_and_save_ifchanged(0);
_priority_did_list[i] = temp;
_priority_did_stored_list[i].set_and_save_ifchanged(temp);
break;
}
}
}
// cache expected dev ids for use during runtime detection
for (StateIndex i(0); i<COMPASS_MAX_INSTANCES; i++) {
_state[i].expected_dev_id = _state[i].dev_id;
}
#if COMPASS_MAX_UNREG_DEV
// set the dev_id to 0 for undetected compasses. extra_dev_id is just an
// interface for users to see unreg compasses, we actually never store it
// in storage.
for (uint8_t i=_unreg_compass_count; i<COMPASS_MAX_UNREG_DEV; i++) {
extra_dev_id[i].set(0);
}
#endif
_reorder_compass_params();
if (_compass_count == 0) {
// detect available backends. Only called once
_detect_backends();
}
if (_compass_count != 0) {
// get initial health status
hal.scheduler->delay(100);
@ -650,21 +703,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; i<COMPASS_MAX_INSTANCES; i++) {
_state[i].dev_id.set(0);
}
// check that we are actually working before passing the compass
// through to ARHS to use.
if (!read()) {
#ifndef HAL_BUILD_AP_PERIPH
_enabled = false;
hal.console->printf("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<COMPASS_MAX_INSTANCES; i++) {
if (!_state[i].registered) {
_state[i].dev_id.set(0);
}
}
#ifndef HAL_BUILD_AP_PERIPH
@ -672,14 +714,137 @@ void Compass::init()
#endif
}
// 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_MAX_INSTANCES; i++) {
if (_priority_did_list[i] == dev_id) {
if (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_MAX_INSTANCES; i++) {
if (_priority_did_stored_list[i] == 0) {
_priority_did_stored_list[i].set_and_save(dev_id);
_priority_did_list[i] = dev_id;
if (i >= _compass_count) {
_compass_count = uint8_t(i)+1;
}
return i;
}
}
return Priority(COMPASS_MAX_INSTANCES);
}
// 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_INSTANCES; i++) {
curr_state_id = _get_state_id(i);
if (curr_state_id != COMPASS_MAX_INSTANCES && uint8_t(curr_state_id) != uint8_t(i)) {
//let's swap
swap_state.copy_from(_state[curr_state_id]);
_state[curr_state_id].copy_from(_state[StateIndex(uint8_t(i))]);
_state[StateIndex(uint8_t(i))].copy_from(swap_state);
}
}
}
void Compass::mag_state::copy_from(const Compass::mag_state& state)
{
external.set_and_save(state.external);
orientation.set_and_save(state.orientation);
offset.set_and_save(state.offset);
diagonals.set_and_save(state.diagonals);
offdiagonals.set_and_save(state.offdiagonals);
scale_factor.set_and_save(state.scale_factor);
dev_id.set_and_save(state.dev_id);
motor_compensation.set_and_save(state.motor_compensation);
expected_dev_id = state.expected_dev_id;
}
// Register a new compass instance
//
uint8_t Compass::register_compass(void)
bool Compass::register_compass(int32_t dev_id, uint8_t& instance)
{
if (_compass_count == COMPASS_MAX_INSTANCES) {
Priority priority;
// Check if we already have this dev_id registered
for (StateIndex i(0); i<COMPASS_MAX_INSTANCES; i++) {
priority = _update_priority_list(dev_id);
if (_state[i].expected_dev_id == dev_id && priority < COMPASS_MAX_INSTANCES) {
_state[i].registered = true;
_state[i].priority = priority;
instance = uint8_t(i);
return true;
}
}
// This is an unregistered compass, check if any free slot is available
for (StateIndex i(0); i<COMPASS_MAX_INSTANCES; i++) {
priority = _update_priority_list(dev_id);
if (_state[i].dev_id == 0 && priority < COMPASS_MAX_INSTANCES) {
_state[i].registered = true;
_state[i].priority = priority;
instance = uint8_t(i);
return true;
}
}
// This might be a replacement compass module, find any unregistered compass
// instance and replace that
for (StateIndex i(0); i<COMPASS_MAX_INSTANCES; i++) {
priority = _update_priority_list(dev_id);
if (!_state[i].registered && priority < COMPASS_MAX_INSTANCES) {
_state[i].registered = true;
_state[i].priority = priority;
instance = uint8_t(i);
return true;
}
}
#if COMPASS_MAX_UNREG_DEV
// Set extra dev id
if (_unreg_compass_count >= COMPASS_MAX_UNREG_DEV) {
AP_HAL::panic("Too many compass instances");
}
return _compass_count++;
for (uint8_t i=0; i<COMPASS_MAX_UNREG_DEV; i++) {
if (extra_dev_id[i] == dev_id) {
if (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
{
for(StateIndex i(0); i<COMPASS_MAX_INSTANCES; i++) {
if (_priority_did_list[priority] == _state[i].expected_dev_id) {
return i;
}
}
return StateIndex(COMPASS_MAX_INSTANCES);
}
bool Compass::_add_backend(AP_Compass_Backend *backend)
@ -689,7 +854,7 @@ bool Compass::_add_backend(AP_Compass_Backend *backend)
}
if (_backend_count == COMPASS_MAX_BACKEND) {
AP_HAL::panic("Too many compass backends");
return false;
}
_backends[_backend_count++] = backend;
@ -711,7 +876,10 @@ bool Compass::_driver_enabled(enum DriverType driver_type)
*/
bool Compass::_have_i2c_driver(uint8_t bus, uint8_t address) const
{
for (uint8_t i=0; i<_compass_count; i++) {
for (StateIndex i(0); i<COMPASS_MAX_INSTANCES; i++) {
if (!_state[i].registered) {
continue;
}
if (AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_I2C, bus, address, 0) ==
AP_HAL::Device::change_bus_id(uint32_t(_state[i].dev_id.get()), 0)) {
// we are already using this device
@ -727,8 +895,7 @@ bool Compass::_have_i2c_driver(uint8_t bus, uint8_t address) const
*/
#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) { \
if (_unreg_compass_count == COMPASS_MAX_UNREG_DEV) { \
return; \
} \
} while (0)
@ -893,8 +1060,7 @@ void Compass::_detect_backends(void)
#ifdef HAL_PROBE_EXTERNAL_I2C_COMPASSES
// allow boards to ask for external probing of all i2c compass types in hwdef.dat
_probe_external_i2c_compasses();
if (_backend_count == COMPASS_MAX_BACKEND ||
_compass_count == COMPASS_MAX_INSTANCES) {
if (_unreg_compass_count == COMPASS_MAX_UNREG_DEV) {
return;
}
#endif
@ -918,8 +1084,7 @@ void Compass::_detect_backends(void)
case AP_BoardConfig::PX4_BOARD_PIXHAWK_PRO:
case AP_BoardConfig::PX4_BOARD_AEROFC:
_probe_external_i2c_compasses();
if (_backend_count == COMPASS_MAX_BACKEND ||
_compass_count == COMPASS_MAX_INSTANCES) {
if (_unreg_compass_count == COMPASS_MAX_UNREG_DEV) {
return;
}
break;
@ -1026,8 +1191,16 @@ void Compass::_detect_backends(void)
#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; i<COMPASS_MAX_BACKEND; i++) {
AP_Compass_Backend* _uavcan_backend = AP_Compass_UAVCAN::probe(i);
if (_uavcan_backend) {
_add_backend(_uavcan_backend);
}
if (_unreg_compass_count == COMPASS_MAX_UNREG_DEV) {
return;
}
}
}
#endif
@ -1037,6 +1210,35 @@ void Compass::_detect_backends(void)
}
}
// Look for devices beyond initialisation
void
Compass::_detect_runtime(void)
{
#if HAL_WITH_UAVCAN
//Don't try to add device while armed
if (hal.util->get_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; i<COMPASS_MAX_BACKEND; i++) {
AP_Compass_Backend* _uavcan_backend = AP_Compass_UAVCAN::probe(i);
if (_uavcan_backend) {
_add_backend(_uavcan_backend);
}
if (_unreg_compass_count == COMPASS_MAX_UNREG_DEV) {
return;
}
}
}
#endif
}
bool
Compass::read(void)
{
@ -1045,12 +1247,15 @@ Compass::read(void)
try_set_initial_location();
}
#endif
_detect_runtime();
for (uint8_t i=0; i< _backend_count; i++) {
// call read on each of the backend. This call updates field[i]
_backends[i]->read();
}
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
@ -1075,9 +1280,9 @@ uint8_t
Compass::get_healthy_mask() const
{
uint8_t healthy_mask = 0;
for(uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
if(healthy(i)) {
healthy_mask |= 1 << i;
for(Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
if(healthy(uint8_t(i))) {
healthy_mask |= 1 << uint8_t(i);
}
}
return healthy_mask;
@ -1087,8 +1292,9 @@ void
Compass::set_offsets(uint8_t i, const Vector3f &offsets)
{
// sanity check compass instance provided
if (i < COMPASS_MAX_INSTANCES) {
_state[i].offset.set(offsets);
StateIndex id = _get_state_id(Priority(i));
if (id < COMPASS_MAX_INSTANCES){
_state[id].offset.set(offsets);
}
}
@ -1096,8 +1302,9 @@ void
Compass::set_and_save_offsets(uint8_t i, const Vector3f &offsets)
{
// sanity check compass instance provided
if (i < COMPASS_MAX_INSTANCES) {
_state[i].offset.set(offsets);
StateIndex id = _get_state_id(Priority(i));
if (id < COMPASS_MAX_INSTANCES){
_state[id].offset.set(offsets);
save_offsets(i);
}
}
@ -1106,8 +1313,9 @@ void
Compass::set_and_save_diagonals(uint8_t i, const Vector3f &diagonals)
{
// sanity check compass instance provided
if (i < COMPASS_MAX_INSTANCES) {
_state[i].diagonals.set_and_save(diagonals);
StateIndex id = _get_state_id(Priority(i));
if (id < COMPASS_MAX_INSTANCES){
_state[id].diagonals.set_and_save(diagonals);
}
}
@ -1115,46 +1323,67 @@ void
Compass::set_and_save_offdiagonals(uint8_t i, const Vector3f &offdiagonals)
{
// sanity check compass instance provided
if (i < COMPASS_MAX_INSTANCES) {
_state[i].offdiagonals.set_and_save(offdiagonals);
StateIndex id = _get_state_id(Priority(i));
if (id < COMPASS_MAX_INSTANCES){
_state[id].offdiagonals.set_and_save(offdiagonals);
}
}
void
Compass::set_and_save_scale_factor(uint8_t i, float scale_factor)
{
StateIndex id = _get_state_id(Priority(i));
if (i < COMPASS_MAX_INSTANCES) {
_state[i].scale_factor.set_and_save(scale_factor);
_state[id].scale_factor.set_and_save(scale_factor);
}
}
void
Compass::save_offsets(uint8_t i)
{
_state[i].offset.save(); // save offsets
_state[i].dev_id.set_and_save(_state[i].detected_dev_id);
StateIndex id = _get_state_id(Priority(i));
if (id < COMPASS_MAX_INSTANCES){
_state[id].offset.save(); // save offsets
_state[id].dev_id.set_and_save(_state[id].detected_dev_id);
}
}
void
Compass::set_and_save_orientation(uint8_t i, Rotation orientation)
{
StateIndex id = _get_state_id(Priority(i));
if (id < COMPASS_MAX_INSTANCES){
_state[id].orientation.set_and_save_ifchanged(orientation);
}
}
void
Compass::save_offsets(void)
{
for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
save_offsets(i);
for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
save_offsets(uint8_t(i));
}
}
void
Compass::set_motor_compensation(uint8_t i, const Vector3f &motor_comp_factor)
{
_state[i].motor_compensation.set(motor_comp_factor);
StateIndex id = _get_state_id(Priority(i));
if (id < COMPASS_MAX_INSTANCES){
_state[id].motor_compensation.set(motor_comp_factor);
}
}
void
Compass::save_motor_compensation()
{
StateIndex id;
_motor_comp_type.save();
for (uint8_t k=0; k<COMPASS_MAX_INSTANCES; k++) {
_state[k].motor_compensation.save();
for (Priority k(0); k<COMPASS_MAX_INSTANCES; k++) {
id = _get_state_id(k);
if (id<COMPASS_MAX_INSTANCES){
_state[id].motor_compensation.save();
}
}
}
@ -1186,8 +1415,7 @@ void Compass::try_set_initial_location()
bool
Compass::use_for_yaw(void) const
{
uint8_t prim = get_primary();
return healthy(prim) && use_for_yaw(prim);
return healthy(0) && use_for_yaw(0);
}
/// return true if the specified compass can be used for yaw calculations
@ -1197,7 +1425,13 @@ Compass::use_for_yaw(uint8_t i) const
// when we are doing in-flight compass learning the state
// estimator must not use the compass. The learning code turns off
// inflight learning when it has converged
return _state[i].use_for_yaw && _learn.get() != LEARN_INFLIGHT;
return _use_for_yaw[Priority(i)] && _learn.get() != LEARN_INFLIGHT;
}
void
Compass::set_use_for_yaw(uint8_t i, bool use)
{
_use_for_yaw[Priority(i)].set(use);
}
void
@ -1265,40 +1499,53 @@ 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)
{
// Check if any of the registered devs are not registered
for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
if (_priority_did_list[i] != 0 && use_for_yaw(uint8_t(i))) {
if (!_get_state(i).registered) {
snprintf(failure_msg, failure_msg_len, "Compass %d not Found", uint8_t(i));
return false;
}
if (_priority_did_list[i] != _priority_did_stored_list[i]) {
snprintf(failure_msg, failure_msg_len, "Compass order change requires reboot");
return false;
}
}
}
bool all_configured = true;
for(uint8_t i=0; i<get_count(); i++) {
all_configured = all_configured && (!use_for_yaw(i) || configured(i));
}
if (!all_configured) {
snprintf(failure_msg, failure_msg_len, "Compass not calibrated");
}
return all_configured;
}
@ -1325,9 +1572,9 @@ void Compass::setHIL(uint8_t instance, float roll, float pitch, float yaw)
_hil.field[instance].rotate(MAG_BOARD_ORIENTATION);
// add user selectable orientation
_hil.field[instance].rotate((enum Rotation)_state[0].orientation.get());
_hil.field[instance].rotate((enum Rotation)_state[StateIndex(0)].orientation.get());
if (!_state[0].external) {
if (!_state[StateIndex(0)].external) {
// and add in AHRS_ORIENTATION setting if not an external compass
if (_board_orientation == ROTATION_CUSTOM && _custom_rotation) {
_hil.field[instance] = *_custom_rotation * _hil.field[instance];
@ -1344,7 +1591,7 @@ void Compass::setHIL(uint8_t instance, const Vector3f &mag, uint32_t update_usec
{
_hil.field[instance] = mag;
_hil.healthy[instance] = true;
_state[instance].last_update_usec = update_usec;
_state[StateIndex(instance)].last_update_usec = update_usec;
}
const Vector3f& Compass::getHIL(uint8_t instance) const
@ -1435,9 +1682,10 @@ bool Compass::consistent() const
*/
bool Compass::have_scale_factor(uint8_t i) const
{
if (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;

131
libraries/AP_Compass/AP_Compass.h
View File

@ -12,6 +12,7 @@
#include "CompassCalibrator.h"
#include "AP_Compass_Backend.h"
#include "Compass_PerMotor.h"
#include <AP_Common/TSIndex.h>
// motor compensation types (for use with motor_comp_enabled)
#define AP_COMPASS_MOT_COMP_DISABLED 0x00
@ -45,8 +46,17 @@
maximum number of compass instances available on this platform. If more
than 1 then redundant sensors may be available
*/
#ifndef HAL_BUILD_AP_PERIPH
#define COMPASS_MAX_INSTANCES 3
#define COMPASS_MAX_BACKEND 3
#define COMPASS_MAX_UNREG_DEV 5
#else
#define COMPASS_MAX_INSTANCES 1
#define COMPASS_MAX_BACKEND 1
#define COMPASS_MAX_UNREG_DEV 0
#endif
#define MAX_CONNECTED_MAGS (COMPASS_MAX_UNREG_DEV+COMPASS_MAX_INSTANCES)
class CompassLearn;
@ -87,7 +97,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 +117,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
///
@ -122,8 +133,8 @@ public:
uint8_t get_count(void) const { return _compass_count; }
/// 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 +177,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 +201,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.
///
@ -227,8 +236,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.
///
@ -240,8 +249,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
@ -263,13 +272,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);
@ -281,11 +284,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[];
@ -333,10 +336,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);
@ -364,7 +377,7 @@ private:
#if COMPASS_CAL_ENABLED
//keep track of which calibrators have been saved
bool _cal_saved[COMPASS_MAX_INSTANCES];
RestrictIDTypeArray<bool, COMPASS_MAX_INSTANCES, Priority> _cal_saved;
bool _cal_autosave;
#endif
@ -405,6 +418,9 @@ private:
// number of registered compasses.
uint8_t _compass_count;
// number of unregistered compasses.
uint8_t _unreg_compass_count;
// settable parameters
AP_Int8 _learn;
@ -412,9 +428,6 @@ private:
enum Rotation _board_orientation = ROTATION_NONE;
Matrix3f* _custom_rotation;
// primary instance
AP_Int8 _primary;
// declination in radians
AP_Float _declination;
@ -427,9 +440,6 @@ 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;
@ -443,6 +453,8 @@ private:
struct mag_state {
AP_Int8 external;
bool healthy;
bool registered;
Compass::Priority priority;
AP_Int8 orientation;
AP_Vector3f offset;
AP_Vector3f diagonals;
@ -453,13 +465,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;
@ -480,7 +487,32 @@ 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<mag_state, COMPASS_MAX_INSTANCES+1, StateIndex> _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<AP_Int8, COMPASS_MAX_INSTANCES, Priority> _use_for_yaw;
RestrictIDTypeArray<AP_Int32, COMPASS_MAX_INSTANCES, Priority> _priority_did_stored_list;
RestrictIDTypeArray<int32_t, COMPASS_MAX_INSTANCES, Priority> _priority_did_list;
AP_Int16 _offset_max;
@ -491,7 +523,7 @@ private:
AP_Int16 _options;
#if COMPASS_CAL_ENABLED
CompassCalibrator _calibrator[COMPASS_MAX_INSTANCES];
RestrictIDTypeArray<CompassCalibrator, COMPASS_MAX_INSTANCES, Priority> _calibrator;
#endif
#if COMPASS_MOT_ENABLED
@ -506,7 +538,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;

9
libraries/AP_Compass/AP_Compass_AK09916.cpp
View File

@ -222,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);
@ -230,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));

8
libraries/AP_Compass/AP_Compass_AK8963.cpp
View File

@ -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));

8
libraries/AP_Compass/AP_Compass_BMM150.cpp
View File

@ -211,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");

35
libraries/AP_Compass/AP_Compass_Backend.cpp
View File

@ -14,7 +14,7 @@ AP_Compass_Backend::AP_Compass_Backend()
void AP_Compass_Backend::rotate_field(Vector3f &mag, uint8_t instance)
{
Compass::mag_state &state = _compass._state[instance];
Compass::mag_state &state = _compass._state[Compass::StateIndex(instance)];
mag.rotate(MAG_BOARD_ORIENTATION);
mag.rotate(state.rotation);
@ -33,20 +33,20 @@ void AP_Compass_Backend::rotate_field(Vector3f &mag, uint8_t instance)
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 +123,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 +137,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 +159,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 +169,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 +177,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 +188,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 +198,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 +206,20 @@ 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;
}
static constexpr float FILTER_KOEF = 0.1f;

2
libraries/AP_Compass/AP_Compass_Backend.h
View File

@ -88,7 +88,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);

62
libraries/AP_Compass/AP_Compass_Calibration.cpp
View File

@ -19,7 +19,7 @@ void Compass::cal_update()
bool running = false;
for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
bool failure;
_calibrator[i].update(failure);
if (failure) {
@ -34,7 +34,7 @@ void Compass::cal_update()
if (_calibrator[i].running()) {
running = true;
} else if (_cal_autosave && !_cal_saved[i] && _calibrator[i].get_status() == CompassCalibrator::Status::SUCCESS) {
_accept_calibration(i);
_accept_calibration(uint8_t(i));
}
}
@ -57,6 +57,11 @@ bool Compass::_start_calibration(uint8_t i, bool retry, float delay)
if (!use_for_yaw(i)) {
return false;
}
Priority prio = Priority(i);
if (_priority_did_list[prio] != _priority_did_stored_list[prio]) {
gcs().send_text(MAV_SEVERITY_ERROR, "Compass cal requires reboot after priority change");
return false;
}
if (_options.get() & uint16_t(Option::CAL_REQUIRE_GPS)) {
if (AP::gps().status() < AP_GPS::GPS_OK_FIX_2D) {
gcs().send_text(MAV_SEVERITY_ERROR, "Compass cal requires GPS lock");
@ -66,22 +71,22 @@ bool Compass::_start_calibration(uint8_t i, bool retry, float delay)
if (!is_calibrating()) {
AP_Notify::events.initiated_compass_cal = 1;
}
if (i == get_primary() && _state[i].external != 0) {
_calibrator[i].set_tolerance(_calibration_threshold);
if (i == 0 && _get_state(prio).external != 0) {
_calibrator[prio].set_tolerance(_calibration_threshold);
} else {
// internal compasses or secondary compasses get twice the
// threshold. This is because internal compasses tend to be a
// lot noisier
_calibrator[i].set_tolerance(_calibration_threshold*2);
_calibrator[prio].set_tolerance(_calibration_threshold*2);
}
if (_rotate_auto) {
enum Rotation r = _state[i].external?(enum Rotation)_state[i].orientation.get():ROTATION_NONE;
enum Rotation r = _get_state(prio).external?(enum Rotation)_get_state(prio).orientation.get():ROTATION_NONE;
if (r != ROTATION_CUSTOM) {
_calibrator[i].set_orientation(r, _state[i].external, _rotate_auto>=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; i<COMPASS_MAX_INSTANCES; i++) {
if ((1<<i) & mask) {
if (!_accept_calibration(i)) {
for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
if ((1<<uint8_t(i)) & mask) {
if (!_accept_calibration(uint8_t(i))) {
success = false;
}
_calibrator[i].stop();
@ -194,7 +202,7 @@ bool Compass::send_mag_cal_progress(const GCS_MAVLINK& link)
{
uint8_t cal_mask = _get_cal_mask();
for (uint8_t compass_id=0; compass_id<COMPASS_MAX_INSTANCES; compass_id++) {
for (Priority compass_id(0); compass_id<COMPASS_MAX_INSTANCES; compass_id++) {
// ensure we don't try to send with no space available
if (!HAVE_PAYLOAD_SPACE(link.get_chan(), MAG_CAL_PROGRESS)) {
// ideally we would only send one progress message per
@ -216,7 +224,7 @@ bool Compass::send_mag_cal_progress(const GCS_MAVLINK& link)
mavlink_msg_mag_cal_progress_send(
link.get_chan(),
compass_id, cal_mask,
uint8_t(compass_id), cal_mask,
(uint8_t)cal_status, attempt, completion_pct, completion_mask,
direction.x, direction.y, direction.z
);
@ -230,7 +238,7 @@ bool Compass::send_mag_cal_report(const GCS_MAVLINK& link)
{
uint8_t cal_mask = _get_cal_mask();
for (uint8_t compass_id=0; compass_id<COMPASS_MAX_INSTANCES; compass_id++) {
for (Priority compass_id(0); compass_id<COMPASS_MAX_INSTANCES; compass_id++) {
// ensure we don't try to send with no space available
if (!HAVE_PAYLOAD_SPACE(link.get_chan(), MAG_CAL_REPORT)) {
// ideally we would only send one progress message per
@ -251,7 +259,7 @@ bool Compass::send_mag_cal_report(const GCS_MAVLINK& link)
mavlink_msg_mag_cal_report_send(
link.get_chan(),
compass_id, cal_mask,
uint8_t(compass_id), cal_mask,
(uint8_t)cal_status, autosaved,
fitness,
ofs.x, ofs.y, ofs.z,
@ -269,7 +277,7 @@ bool Compass::send_mag_cal_report(const GCS_MAVLINK& link)
bool Compass::is_calibrating() const
{
for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
switch(_calibrator[i].get_status()) {
case CompassCalibrator::Status::NOT_STARTED:
case CompassCalibrator::Status::SUCCESS:
@ -286,9 +294,9 @@ bool Compass::is_calibrating() const
uint8_t Compass::_get_cal_mask() const
{
uint8_t cal_mask = 0;
for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
if (_calibrator[i].get_status() != CompassCalibrator::Status::NOT_STARTED) {
cal_mask |= 1 << i;
cal_mask |= 1 << uint8_t(i);
}
}
return cal_mask;

5
libraries/AP_Compass/AP_Compass_HIL.cpp
View File

@ -50,7 +50,10 @@ AP_Compass_HIL::init(void)
{
// register two compass instances
for (uint8_t i=0; i<HIL_NUM_COMPASSES; i++) {
_compass_instance[i] = register_compass();
uint32_t dev_id = AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SITL, i, 0, DEVTYPE_SITL);
if (!register_compass(dev_id, _compass_instance[i])) {
return false;
}
}
return true;
}

2
libraries/AP_Compass/AP_Compass_HIL.h
View File

@ -2,7 +2,7 @@
#include "AP_Compass.h"
#define HIL_NUM_COMPASSES 2
#define HIL_NUM_COMPASSES 1
class AP_Compass_HIL : public AP_Compass_Backend
{

10
libraries/AP_Compass/AP_Compass_HMC5843.cpp
View File

@ -194,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);
}

10
libraries/AP_Compass/AP_Compass_IST8308.cpp
View File

@ -158,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);
}

10
libraries/AP_Compass/AP_Compass_IST8310.cpp
View File

@ -151,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);
}

9
libraries/AP_Compass/AP_Compass_LIS3MDL.cpp
View File

@ -105,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);
@ -115,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));

9
libraries/AP_Compass/AP_Compass_LSM303D.cpp
View File

@ -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));

9
libraries/AP_Compass/AP_Compass_LSM9DS1.cpp
View File

@ -104,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));

9
libraries/AP_Compass/AP_Compass_MAG3110.cpp
View File

@ -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

10
libraries/AP_Compass/AP_Compass_MMC3416.cpp
View File

@ -90,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);
@ -100,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

10
libraries/AP_Compass/AP_Compass_QMC5883L.cpp
View File

@ -113,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);
}

13
libraries/AP_Compass/AP_Compass_RM3100.cpp
View File

@ -143,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));

32
libraries/AP_Compass/AP_Compass_SITL.cpp
View File

@ -10,17 +10,29 @@ AP_Compass_SITL::AP_Compass_SITL()
{
if (_sitl != nullptr) {
_compass._setup_earth_field();
for (uint8_t i=0; i<SITL_NUM_COMPASSES; i++) {
// default offsets to correct value
for (uint8_t i=0; i<MAX_CONNECTED_MAGS; i++) {
uint32_t dev_id = _sitl->mag_devid[i];
if (dev_id == 0) {
continue;
}
uint8_t instance;
if (!register_compass(dev_id, instance)) {
continue;
} else if (_num_compass<MAX_SITL_COMPASSES) {
_compass_instance[_num_compass] = instance;
set_dev_id(_compass_instance[_num_compass], dev_id);
// save so the compass always comes up configured in SITL
save_dev_id(_compass_instance[_num_compass]);
_num_compass++;
}
}
// Scroll through the registered compasses, and set the offsets
for (uint8_t i=0; i<_num_compass; i++) {
if (_compass.get_offsets(i).is_zero()) {
_compass.set_offsets(i, _sitl->mag_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,7 +121,7 @@ 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; i<SITL_NUM_COMPASSES; i++) {
for (uint8_t i=0; i<_num_compass; i++) {
Vector3f f = new_mag_data;
if (i == 0) {
// rotate the first compass, allowing for testing of external compass rotation
@ -126,7 +138,7 @@ void AP_Compass_SITL::_timer()
void AP_Compass_SITL::read()
{
for (uint8_t i=0; i<SITL_NUM_COMPASSES; i++) {
for (uint8_t i=0; i<_num_compass; i++) {
drain_accumulated_samples(_compass_instance[i], nullptr);
}
}

5
libraries/AP_Compass/AP_Compass_SITL.h
View File

@ -9,7 +9,7 @@
#include <AP_Math/AP_Math.h>
#include <AP_Declination/AP_Declination.h>
#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

59
libraries/AP_Compass/AP_Compass_UAVCAN.cpp
View File

@ -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;
}

4
libraries/AP_Compass/AP_Compass_UAVCAN.h
View File

@ -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);

27
libraries/AP_Compass/Compass_learn.cpp
View File

@ -20,11 +20,11 @@ CompassLearn::CompassLearn(Compass &_compass) :
compass(_compass)
{
gcs().send_text(MAV_SEVERITY_INFO, "CompassLearn: Initialised");
for (uint8_t i=0; i<compass.get_count(); i++) {
if (compass._state[i].use_for_yaw) {
for (Compass::Priority i(0); i<compass.get_count(); i++) {
if (compass._use_for_yaw[i]) {
// reset scale factors, we can't learn scale factors in
// flight
compass.set_and_save_scale_factor(i, 0.0);
compass.set_and_save_scale_factor(uint8_t(i), 0.0);
}
}
}
@ -50,9 +50,6 @@ void CompassLearn::update(void)
return;
}
// remember primary mag
primary_mag = compass.get_primary();
// setup the expected earth field in mGauss at this location
mag_ef = AP_Declination::get_earth_field_ga(loc) * 1000;
have_earth_field = true;
@ -60,8 +57,8 @@ void CompassLearn::update(void)
// form eliptical correction matrix and invert it. This is
// needed to remove the effects of the eliptical correction
// when calculating new offsets
const Vector3f &diagonals = compass.get_diagonals(primary_mag);
const Vector3f &offdiagonals = compass.get_offdiagonals(primary_mag);
const Vector3f &diagonals = compass.get_diagonals(0);
const Vector3f &offdiagonals = compass.get_offdiagonals(0);
mat = Matrix3f(
diagonals.x, offdiagonals.x, offdiagonals.y,
offdiagonals.x, diagonals.y, offdiagonals.z,
@ -89,7 +86,7 @@ void CompassLearn::update(void)
return;
}
Vector3f field = compass.get_field(primary_mag);
Vector3f field = compass.get_field(0);
Vector3f field_change = field - last_field;
if (field_change.length() < min_field_change) {
return;
@ -99,7 +96,7 @@ void CompassLearn::update(void)
WITH_SEMAPHORE(sem);
// give a sample to the backend to process
new_sample.field = field;
new_sample.offsets = compass.get_offsets(primary_mag);
new_sample.offsets = compass.get_offsets(0);
new_sample.attitude = Vector3f(ahrs.roll, ahrs.pitch, ahrs.yaw);
sample_available = true;
last_field = field;
@ -122,12 +119,12 @@ void CompassLearn::update(void)
WITH_SEMAPHORE(sem);
// set offsets to current best guess
compass.set_offsets(primary_mag, best_offsets);
compass.set_offsets(0, best_offsets);
// set non-primary offsets to match primary
Vector3f field_primary = compass.get_field(primary_mag);
for (uint8_t i=0; i<compass.get_count(); i++) {
if (i == primary_mag || !compass._state[i].use_for_yaw) {
Vector3f field_primary = compass.get_field(0);
for (uint8_t i=1; i<compass.get_count(); i++) {
if (!compass.use_for_yaw(i)) {
continue;
}
Vector3f field2 = compass.get_field(i);
@ -140,7 +137,7 @@ void CompassLearn::update(void)
// set the offsets and enable compass for EKF use. Let the
// EKF learn the remaining compass offset error
for (uint8_t i=0; i<compass.get_count(); i++) {
if (compass._state[i].use_for_yaw) {
if (compass.use_for_yaw(i)) {
compass.save_offsets(i);
compass.set_and_save_scale_factor(i, 0.0);
compass.set_use_for_yaw(i, true);

1
libraries/AP_Compass/Compass_learn.h
View File

@ -51,7 +51,6 @@ private:
float best_yaw_deg;
float worst_error;
bool converged;
uint8_t primary_mag;
void io_timer(void);
void process_sample(const struct sample &s);