Browse Source

AP_Compass: correct use of incorrect backend-count macro

c415-sdk
Peter Barker 5 years ago committed by Andrew Tridgell
parent
commit
0665c379de
  1. 48
      libraries/AP_Compass/AP_Compass.cpp
  2. 13
      libraries/AP_Compass/AP_Compass.h

48
libraries/AP_Compass/AP_Compass.cpp

@ -48,10 +48,6 @@ extern const AP_HAL::HAL& hal; @@ -48,10 +48,6 @@ extern const AP_HAL::HAL& hal;
#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
@ -161,7 +157,7 @@ const AP_Param::GroupInfo Compass::var_info[] = { @@ -161,7 +157,7 @@ const AP_Param::GroupInfo Compass::var_info[] = {
// @User: Advanced
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
@ -216,9 +212,9 @@ const AP_Param::GroupInfo Compass::var_info[] = { @@ -216,9 +212,9 @@ const AP_Param::GroupInfo Compass::var_info[] = {
// @User: Advanced
AP_GROUPINFO("MOT2", 11, Compass, _state._priv_instance[1].motor_compensation, 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
@ -272,7 +268,7 @@ const AP_Param::GroupInfo Compass::var_info[] = { @@ -272,7 +268,7 @@ const AP_Param::GroupInfo Compass::var_info[] = {
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("MOT3", 14, Compass, _state._priv_instance[2].motor_compensation, 0),
#endif // HAL_COMPASS_MAX_SENSORS
#endif // COMPASS_MAX_INSTANCES
// @Param: DEV_ID
// @DisplayName: Compass device id
@ -281,25 +277,25 @@ const AP_Param::GroupInfo Compass::var_info[] = { @@ -281,25 +277,25 @@ const AP_Param::GroupInfo Compass::var_info[] = {
// @User: Advanced
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._priv_instance[1].dev_id, 0),
#endif // HAL_COMPASS_MAX_SENSORS
#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._priv_instance[2].dev_id, 0),
#endif // HAL_COMPASS_MAX_SENSORS
#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 secondary compass for determining heading.
@ -320,9 +316,9 @@ const AP_Param::GroupInfo Compass::var_info[] = { @@ -320,9 +316,9 @@ const AP_Param::GroupInfo Compass::var_info[] = {
// @Values: 0:Internal,1:External,2:ForcedExternal
// @User: Advanced
AP_GROUPINFO("EXTERN2",20, Compass, _state._priv_instance[1].external, 0),
#endif // HAL_COMPASS_MAX_SENSORS
#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 tertiary compass for determining heading.
@ -343,7 +339,7 @@ const AP_Param::GroupInfo Compass::var_info[] = { @@ -343,7 +339,7 @@ const AP_Param::GroupInfo Compass::var_info[] = {
// @Values: 0:Internal,1:External,2:ForcedExternal
// @User: Advanced
AP_GROUPINFO("EXTERN3",23, Compass, _state._priv_instance[2].external, 0),
#endif // HAL_COMPASS_MAX_SENSORS
#endif // COMPASS_MAX_INSTANCES
// @Param: DIA_X
// @DisplayName: Compass soft-iron diagonal X component
@ -381,7 +377,7 @@ const AP_Param::GroupInfo Compass::var_info[] = { @@ -381,7 +377,7 @@ const AP_Param::GroupInfo Compass::var_info[] = {
// @User: Advanced
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]]
@ -417,9 +413,9 @@ const AP_Param::GroupInfo Compass::var_info[] = { @@ -417,9 +413,9 @@ const AP_Param::GroupInfo Compass::var_info[] = {
// @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._priv_instance[1].offdiagonals, 0),
#endif // HAL_COMPASS_MAX_SENSORS
#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]]
@ -455,7 +451,7 @@ const AP_Param::GroupInfo Compass::var_info[] = { @@ -455,7 +451,7 @@ const AP_Param::GroupInfo Compass::var_info[] = {
// @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._priv_instance[2].offdiagonals, 0),
#endif // HAL_COMPASS_MAX_SENSORS
#endif // COMPASS_MAX_INSTANCES
// @Param: CAL_FIT
// @DisplayName: Compass calibration fitness
@ -507,21 +503,21 @@ const AP_Param::GroupInfo Compass::var_info[] = { @@ -507,21 +503,21 @@ const AP_Param::GroupInfo Compass::var_info[] = {
// @User: Advanced
AP_GROUPINFO("PRIO1_ID", 36, Compass, _priority_did_stored_list._priv_instance[0], 0),
#if HAL_COMPASS_MAX_SENSORS > 1
#if COMPASS_MAX_INSTANCES > 1
// @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("PRIO2_ID", 37, Compass, _priority_did_stored_list._priv_instance[1], 0),
#endif // HAL_COMPASS_MAX_SENSORS
#endif // COMPASS_MAX_INSTANCES
#if HAL_COMPASS_MAX_SENSORS > 2
#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.
// @User: Advanced
AP_GROUPINFO("PRIO3_ID", 38, Compass, _priority_did_stored_list._priv_instance[2], 0),
#endif // HAL_COMPASS_MAX_SENSORS
#endif // COMPASS_MAX_INSTANCES
// @Param: ENABLE
// @DisplayName: Enable Compass
@ -537,7 +533,7 @@ const AP_Param::GroupInfo Compass::var_info[] = { @@ -537,7 +533,7 @@ const AP_Param::GroupInfo Compass::var_info[] = {
// @Range: 0 1.3
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
@ -546,7 +542,7 @@ const AP_Param::GroupInfo Compass::var_info[] = { @@ -546,7 +542,7 @@ const AP_Param::GroupInfo Compass::var_info[] = {
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

13
libraries/AP_Compass/AP_Compass.h

@ -47,15 +47,20 @@ @@ -47,15 +47,20 @@
than 1 then redundant sensors may be available
*/
#ifndef HAL_BUILD_AP_PERIPH
#define COMPASS_MAX_INSTANCES 3
#define COMPASS_MAX_BACKEND 3
#ifndef HAL_COMPASS_MAX_SENSORS
#define HAL_COMPASS_MAX_SENSORS 3
#endif
#define COMPASS_MAX_UNREG_DEV 5
#else
#define COMPASS_MAX_INSTANCES 1
#define COMPASS_MAX_BACKEND 1
#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;

Loading…
Cancel
Save