Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: added COMPASS_OFFS_MAX

Browse Source 
this allows setup of airframes with hatch magnets
This commit is contained in:
Andrew Tridgell 2017-04-04 08:38:14 +10:00 committed by Francisco Ferreira
parent 98b7dac870
commit ee2afd3242
5 changed files with 27 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_Compass/AP_Compass.cpp
View File

@ -28,6 +28,10 @@ extern AP_HAL::HAL& hal;
#define COMPASS_LEARN_DEFAULT Compass::LEARN_INTERNAL
#endif
#ifndef AP_COMPASS_OFFSETS_MAX_DEFAULT
#define AP_COMPASS_OFFSETS_MAX_DEFAULT 600
#endif
const AP_Param::GroupInfo Compass::var_info[] = {
// index 0 was used for the old orientation matrix
@ -404,6 +408,14 @@ const AP_Param::GroupInfo Compass::var_info[] = {
// @User: Advanced
AP_GROUPINFO("CAL_FIT", 30, Compass, _calibration_threshold, AP_COMPASS_CALIBRATION_FITNESS_DEFAULT),
// @Param: OFFS_MAX
// @DisplayName: Compass maximum offset
// @Description: This sets the maximum allowed compass offset in calibration and arming checks
// @Range: 500 3000
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("OFFS_MAX", 31, Compass, _offset_max, AP_COMPASS_OFFSETS_MAX_DEFAULT),
AP_GROUPEND
};

7
libraries/AP_Compass/AP_Compass.h
View File

@ -281,6 +281,11 @@ public:
enum LearnType get_learn_type(void) const {
return (enum LearnType)_learn.get();
}
// return maximum allowed compass offsets
uint16_t get_offsets_max(void) const {
return (uint16_t)_offset_max.get();
}
private:
/// Register a new compas driver, allocating an instance number
@ -382,6 +387,8 @@ private:
enum Rotation rotation;
} _state[COMPASS_MAX_INSTANCES];
AP_Int16 _offset_max;
CompassCalibrator _calibrator[COMPASS_MAX_INSTANCES];
// if we want HIL only

2
libraries/AP_Compass/AP_Compass_Calibration.cpp
View File

@ -62,7 +62,7 @@ Compass::_start_calibration(uint8_t i, bool retry, float delay)
_calibrator[i].set_tolerance(_calibration_threshold*2);
}
_cal_saved[i] = false;
_calibrator[i].start(retry, delay);
_calibrator[i].start(retry, delay, get_offsets_max());
// disable compass learning both for calibration and after completion
_learn.set_and_save(0);

17
libraries/AP_Compass/CompassCalibrator.cpp
View File

@ -67,14 +67,6 @@ extern const AP_HAL::HAL& hal;
///////////////////// PUBLIC INTERFACE /////////////////////
////////////////////////////////////////////////////////////
#ifdef AP_ARMING_COMPASS_OFFSETS_MAX
#define COMPASS_CALIBRATOR_OFS_MAX AP_ARMING_COMPASS_OFFSETS_MAX
#elif CONFIG_HAL_BOARD == HAL_BOARD_SITL
#define COMPASS_CALIBRATOR_OFS_MAX 2000
#else
#define COMPASS_CALIBRATOR_OFS_MAX 1000
#endif
CompassCalibrator::CompassCalibrator():
_tolerance(COMPASS_CAL_DEFAULT_TOLERANCE),
_sample_buffer(nullptr)
@ -86,10 +78,11 @@ void CompassCalibrator::clear() {
set_status(COMPASS_CAL_NOT_STARTED);
}
void CompassCalibrator::start(bool retry, float delay) {
void CompassCalibrator::start(bool retry, float delay, uint16_t offset_max) {
if(running()) {
return;
}
_offset_max = offset_max;
_attempt = 1;
_retry = retry;
_delay_start_sec = delay;
@ -347,9 +340,9 @@ bool CompassCalibrator::set_status(compass_cal_status_t status) {
bool CompassCalibrator::fit_acceptable() {
if( !isnan(_fitness) &&
_params.radius > 150 && _params.radius < 950 && //Earth's magnetic field strength range: 250-850mG
fabsf(_params.offset.x) < COMPASS_CALIBRATOR_OFS_MAX &&
fabsf(_params.offset.y) < COMPASS_CALIBRATOR_OFS_MAX &&
fabsf(_params.offset.z) < COMPASS_CALIBRATOR_OFS_MAX &&
fabsf(_params.offset.x) < _offset_max &&
fabsf(_params.offset.y) < _offset_max &&
fabsf(_params.offset.z) < _offset_max &&
_params.diag.x > 0.2f && _params.diag.x < 5.0f &&
_params.diag.y > 0.2f && _params.diag.y < 5.0f &&
_params.diag.z > 0.2f && _params.diag.z < 5.0f &&

3
libraries/AP_Compass/CompassCalibrator.h
View File

@ -22,7 +22,7 @@ public:
CompassCalibrator();
void start(bool retry=false, float delay=0.0f);
void start(bool retry, float delay, uint16_t offset_max);
void clear();
void update(bool &failure);
@ -82,6 +82,7 @@ private:
bool _retry;
float _tolerance;
uint8_t _attempt;
uint16_t _offset_max;
completion_mask_t _completion_mask;