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

AP_Compass: correct compilation when COMPASS_MAX_INSTANCE>1 and no diagonals

Browse Source
This commit is contained in:
Peter Barker 2022-10-05 11:22:17 +11:00 committed by Peter Barker
parent 4668e1b61e
commit d6c5a0ab2b
4 changed files with 19 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_Compass/AP_Compass.cpp
View File

@ -357,7 +357,7 @@ const AP_Param::GroupInfo Compass::var_info[] = {
AP_GROUPINFO("EXTERN3",23, Compass, _state._priv_instance[2].external, 0),
#endif // COMPASS_MAX_INSTANCES
#ifndef HAL_BUILD_AP_PERIPH
#if AP_COMPASS_DIAGONALS_ENABLED
// @Param: DIA_X
// @DisplayName: Compass soft-iron diagonal X component
// @Description: DIA_X in the compass soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
@ -393,7 +393,6 @@ const AP_Param::GroupInfo Compass::var_info[] = {
// @Description: ODI_Z in the compass soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
// @User: Advanced
AP_GROUPINFO("ODI", 25, Compass, _state._priv_instance[0].offdiagonals, 0),
#endif // HAL_BUILD_AP_PERIPH
#if COMPASS_MAX_INSTANCES > 1
// @Param: DIA2_X
@ -470,6 +469,7 @@ const AP_Param::GroupInfo Compass::var_info[] = {
// @User: Advanced
AP_GROUPINFO("ODI3", 29, Compass, _state._priv_instance[2].offdiagonals, 0),
#endif // COMPASS_MAX_INSTANCES
#endif // AP_COMPASS_DIAGONALS_ENABLED
#if COMPASS_CAL_ENABLED
// @Param: CAL_FIT
@ -1678,7 +1678,7 @@ Compass::set_and_save_offsets(uint8_t i, const Vector3f &offsets)
}
}
#ifndef HAL_BUILD_AP_PERIPH
#if AP_COMPASS_DIAGONALS_ENABLED
void
Compass::set_and_save_diagonals(uint8_t i, const Vector3f &diagonals)
{
@ -1698,7 +1698,7 @@ Compass::set_and_save_offdiagonals(uint8_t i, const Vector3f &offdiagonals)
_state[id].offdiagonals.set_and_save(offdiagonals);
}
}
#endif // HAL_BUILD_AP_PERIPH
#endif // AP_COMPASS_DIAGONALS_ENABLED
void
Compass::set_and_save_scale_factor(uint8_t i, float scale_factor)

4
libraries/AP_Compass/AP_Compass.h
View File

@ -1,5 +1,7 @@
#pragma once
#include "AP_Compass_config.h"
#include <inttypes.h>
#include <AP_Common/AP_Common.h>
@ -133,8 +135,10 @@ public:
/// @param offsets Offsets to the raw mag_ values in milligauss.
///
void set_and_save_offsets(uint8_t i, const Vector3f &offsets);
#if AP_COMPASS_DIAGONALS_ENABLED
void set_and_save_diagonals(uint8_t i, const Vector3f &diagonals);
void set_and_save_offdiagonals(uint8_t i, const Vector3f &diagonals);
#endif
void set_and_save_scale_factor(uint8_t i, float scale_factor);
void set_and_save_orientation(uint8_t i, Rotation orientation);

4
libraries/AP_Compass/AP_Compass_Calibration.cpp
View File

@ -203,8 +203,10 @@ bool Compass::_accept_calibration(uint8_t i)
float scale_factor = cal_report.scale_factor;
set_and_save_offsets(i, ofs);
#if AP_COMPASS_DIAGONALS_ENABLED
set_and_save_diagonals(i,diag);
set_and_save_offdiagonals(i,offdiag);
#endif
set_and_save_scale_factor(i,scale_factor);
if (cal_report.check_orientation && _get_state(prio).external && _rotate_auto >= 2) {
@ -542,10 +544,12 @@ MAV_RESULT Compass::mag_cal_fixed_yaw(float yaw_deg, uint8_t compass_mask,
Vector3f offsets = field - measurement;
set_and_save_offsets(i, offsets);
#if AP_COMPASS_DIAGONALS_ENABLED
Vector3f one{1,1,1};
set_and_save_diagonals(i, one);
Vector3f zero{0,0,0};
set_and_save_offdiagonals(i, zero);
#endif
}
return MAV_RESULT_ACCEPTED;

7
libraries/AP_Compass/AP_Compass_config.h Normal file
View File

@ -0,0 +1,7 @@
#pragma once
#include <AP_HAL/AP_HAL_Boards.h>
#ifndef AP_COMPASS_DIAGONALS_ENABLED
#define AP_COMPASS_DIAGONALS_ENABLED (!defined(HAL_BUILD_AP_PERIPH))
#endif