Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-09 01:13:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: move enabled parameter into compass library

Browse Source
This commit is contained in:
Peter Barker 2019-03-24 14:24:56 +11:00 committed by Andrew Tridgell
parent a372428ec5
commit 54e3959a99
2 changed files with 29 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
libraries/AP_Compass/AP_Compass.cpp
View File

@ -4,6 +4,7 @@
#endif #endif
#include <AP_Vehicle/AP_Vehicle.h> #include <AP_Vehicle/AP_Vehicle.h>
#include <AP_BoardConfig/AP_BoardConfig.h> #include <AP_BoardConfig/AP_BoardConfig.h>
#include <AP_Logger/AP_Logger.h>
#include "AP_Compass_SITL.h" #include "AP_Compass_SITL.h"
#include "AP_Compass_AK8963.h" #include "AP_Compass_AK8963.h"
@ -477,6 +478,13 @@ const AP_Param::GroupInfo Compass::var_info[] = {
// @User: Advanced // @User: Advanced
AP_GROUPINFO("EXP_DID3", 38, Compass, _state[2].expected_dev_id, -1), AP_GROUPINFO("EXP_DID3", 38, Compass, _state[2].expected_dev_id, -1),
// @Param: ENABLE
// @DisplayName: Enable Compass
// @Description: Setting this to Enabled(1) will enable the compass. Setting this to Disabled(0) will disable the compass. Note that this is separate from COMPASS_USE. This will enable the low level senor, and will enable logging of magnetometer data. To use the compass for navigation you must also set COMPASS_USE to 1.
// @User: Standard
// @Values: 0:Disabled,1:Enabled
AP_GROUPINFO("ENABLE", 39, Compass, _enabled, 1),
AP_GROUPEND AP_GROUPEND
}; };
@ -500,6 +508,10 @@ Compass::Compass(void)
// //
void Compass::init() void Compass::init()
{ {
if (!AP::compass().enabled()) {
return;
}
if (_compass_count == 0) { if (_compass_count == 0) {
// detect available backends. Only called once // detect available backends. Only called once
_detect_backends(); _detect_backends();
@ -517,6 +529,17 @@ void Compass::init()
for (uint8_t i=_compass_count; i<COMPASS_MAX_INSTANCES; i++) { for (uint8_t i=_compass_count; i<COMPASS_MAX_INSTANCES; i++) {
_state[i].dev_id.set(0); _state[i].dev_id.set(0);
} }
// check that we are actually working before passing the compass
// through to ARHS to use.
if (!read()) {
_enabled = false;
hal.console->printf("Compass initialisation failed\n");
AP::logger().Write_Error(LogErrorSubsystem::COMPASS, LogErrorCode::FAILED_TO_INITIALISE);
return;
}
AP::ahrs().set_compass(this);
} }
// Register a new compass instance // Register a new compass instance

5
libraries/AP_Compass/AP_Compass.h
View File

@ -76,6 +76,8 @@ public:
/// ///
bool read(); bool read();
bool enabled() const { return _enabled; }
/// Calculate the tilt-compensated heading_ variables. /// Calculate the tilt-compensated heading_ variables.
/// ///
/// @param dcm_matrix The current orientation rotation matrix /// @param dcm_matrix The current orientation rotation matrix
@ -388,6 +390,9 @@ private:
AP_Compass_Backend *_backends[COMPASS_MAX_BACKEND]; AP_Compass_Backend *_backends[COMPASS_MAX_BACKEND];
uint8_t _backend_count; uint8_t _backend_count;
// whether to enable the compass drivers at all
AP_Int8 _enabled;
// number of registered compasses. // number of registered compasses.
uint8_t _compass_count; uint8_t _compass_count;