diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp index 3bbcdcd6a6..6976102eef 100644 --- a/libraries/AP_Compass/AP_Compass.cpp +++ b/libraries/AP_Compass/AP_Compass.cpp @@ -60,6 +60,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 +69,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,6 +78,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), // @Param: DEC @@ -113,6 +116,7 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @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,6 +144,7 @@ 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), // @Param: ORIENT @@ -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,6 +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), // @Param: MOT2_X @@ -187,6 +197,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 +206,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,6 +215,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), // @Param: PRIMARY @@ -221,6 +234,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 +243,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,6 +252,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), // @Param: MOT3_X @@ -246,6 +262,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 +271,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,6 +280,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), #endif // HAL_COMPASS_MAX_SENSORS @@ -270,6 +289,7 @@ 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), #if HAL_COMPASS_MAX_SENSORS > 1 @@ -278,6 +298,7 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @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), #endif // HAL_COMPASS_MAX_SENSORS @@ -287,6 +308,7 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @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), #endif // HAL_COMPASS_MAX_SENSORS @@ -340,32 +362,38 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @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 + // @Calibration: 1 AP_GROUPINFO("DIA", 24, Compass, _state[0].diagonals, 0), // @Param: ODI_X // @DisplayName: Compass soft-iron off-diagonal X component // @Description: ODI_X in the compass soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]] // @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 + // @Calibration: 1 AP_GROUPINFO("ODI", 25, Compass, _state[0].offdiagonals, 0), #if HAL_COMPASS_MAX_SENSORS > 1 @@ -373,32 +401,38 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @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 + // @Calibration: 1 AP_GROUPINFO("DIA2", 26, Compass, _state[1].diagonals, 0), // @Param: ODI2_X // @DisplayName: Compass2 soft-iron off-diagonal X component // @Description: ODI_X in the compass2 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]] // @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 + // @Calibration: 1 AP_GROUPINFO("ODI2", 27, Compass, _state[1].offdiagonals, 0), #endif // HAL_COMPASS_MAX_SENSORS @@ -407,32 +441,38 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @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 + // @Calibration: 1 AP_GROUPINFO("DIA3", 28, Compass, _state[2].diagonals, 0), // @Param: ODI3_X // @DisplayName: Compass3 soft-iron off-diagonal X component // @Description: ODI_X in the compass3 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]] // @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 + // @Calibration: 1 AP_GROUPINFO("ODI3", 29, Compass, _state[2].offdiagonals, 0), #endif // HAL_COMPASS_MAX_SENSORS