Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-05 07:28:29 -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 #define COMPASS_LEARN_DEFAULT Compass::LEARN_INTERNAL
#endif #endif
#ifndef AP_COMPASS_OFFSETS_MAX_DEFAULT
#define AP_COMPASS_OFFSETS_MAX_DEFAULT 600
#endif
const AP_Param::GroupInfo Compass::var_info[] = { const AP_Param::GroupInfo Compass::var_info[] = {
// index 0 was used for the old orientation matrix // index 0 was used for the old orientation matrix
@ -404,6 +408,14 @@ const AP_Param::GroupInfo Compass::var_info[] = {
// @User: Advanced // @User: Advanced
AP_GROUPINFO("CAL_FIT", 30, Compass, _calibration_threshold, AP_COMPASS_CALIBRATION_FITNESS_DEFAULT), 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 AP_GROUPEND
}; };

7
libraries/AP_Compass/AP_Compass.h
View File

@ -282,6 +282,11 @@ public:
return (enum LearnType)_learn.get(); return (enum LearnType)_learn.get();
} }
// return maximum allowed compass offsets
uint16_t get_offsets_max(void) const {
return (uint16_t)_offset_max.get();
}
private: private:
/// Register a new compas driver, allocating an instance number /// Register a new compas driver, allocating an instance number
/// ///
@ -382,6 +387,8 @@ private:
enum Rotation rotation; enum Rotation rotation;
} _state[COMPASS_MAX_INSTANCES]; } _state[COMPASS_MAX_INSTANCES];
AP_Int16 _offset_max;
CompassCalibrator _calibrator[COMPASS_MAX_INSTANCES]; CompassCalibrator _calibrator[COMPASS_MAX_INSTANCES];
// if we want HIL only // 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); _calibrator[i].set_tolerance(_calibration_threshold*2);
} }
_cal_saved[i] = false; _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 // disable compass learning both for calibration and after completion
_learn.set_and_save(0); _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 ///////////////////// ///////////////////// 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(): CompassCalibrator::CompassCalibrator():
_tolerance(COMPASS_CAL_DEFAULT_TOLERANCE), _tolerance(COMPASS_CAL_DEFAULT_TOLERANCE),
_sample_buffer(nullptr) _sample_buffer(nullptr)
@ -86,10 +78,11 @@ void CompassCalibrator::clear() {
set_status(COMPASS_CAL_NOT_STARTED); 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()) { if(running()) {
return; return;
} }
_offset_max = offset_max;
_attempt = 1; _attempt = 1;
_retry = retry; _retry = retry;
_delay_start_sec = delay; _delay_start_sec = delay;
@ -347,9 +340,9 @@ bool CompassCalibrator::set_status(compass_cal_status_t status) {
bool CompassCalibrator::fit_acceptable() { bool CompassCalibrator::fit_acceptable() {
if( !isnan(_fitness) && if( !isnan(_fitness) &&
_params.radius > 150 && _params.radius < 950 && //Earth's magnetic field strength range: 250-850mG _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.x) < _offset_max &&
fabsf(_params.offset.y) < COMPASS_CALIBRATOR_OFS_MAX && fabsf(_params.offset.y) < _offset_max &&
fabsf(_params.offset.z) < COMPASS_CALIBRATOR_OFS_MAX && fabsf(_params.offset.z) < _offset_max &&
_params.diag.x > 0.2f && _params.diag.x < 5.0f && _params.diag.x > 0.2f && _params.diag.x < 5.0f &&
_params.diag.y > 0.2f && _params.diag.y < 5.0f && _params.diag.y > 0.2f && _params.diag.y < 5.0f &&
_params.diag.z > 0.2f && _params.diag.z < 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(); CompassCalibrator();
void start(bool retry=false, float delay=0.0f); void start(bool retry, float delay, uint16_t offset_max);
void clear(); void clear();
void update(bool &failure); void update(bool &failure);
@ -82,6 +82,7 @@ private:
bool _retry; bool _retry;
float _tolerance; float _tolerance;
uint8_t _attempt; uint8_t _attempt;
uint16_t _offset_max;
completion_mask_t _completion_mask; completion_mask_t _completion_mask;