From 0efd3bacea42f07dbc0a96b48cacf47a68811b4d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 13 Mar 2015 16:43:34 +1100 Subject: [PATCH] AP_Compass: make new backend match old PX4 behaviour when a compass is internal only apply board orientation, not user specified rotation --- libraries/AP_Compass/AP_Compass_AK8963.cpp | 6 +++--- libraries/AP_Compass/AP_Compass_Backend.cpp | 6 +++--- libraries/AP_Compass/AP_Compass_HMC5843.cpp | 6 ++++-- libraries/AP_Compass/Compass.cpp | 15 ++++++++++++--- libraries/AP_Compass/Compass.h | 5 +---- .../examples/AP_Compass_test/AP_Compass_test.pde | 2 -- 6 files changed, 23 insertions(+), 17 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass_AK8963.cpp b/libraries/AP_Compass/AP_Compass_AK8963.cpp index 5060d7509a..83c045e5fb 100644 --- a/libraries/AP_Compass/AP_Compass_AK8963.cpp +++ b/libraries/AP_Compass/AP_Compass_AK8963.cpp @@ -365,9 +365,6 @@ bool AP_Compass_AK8963::_self_test() bool AP_Compass_AK8963::init() { - // register the compass instance in the frontend - _compass_instance = register_compass(); - hal.scheduler->suspend_timer_procs(); if (!_backend->sem_take_blocking()) { error("_spi_sem->take failed\n"); @@ -422,6 +419,9 @@ bool AP_Compass_AK8963::init() _backend->sem_give(); + // register the compass instance in the frontend + _compass_instance = register_compass(); + hal.scheduler->resume_timer_procs(); hal.scheduler->register_timer_process( AP_HAL_MEMBERPROC(&AP_Compass_AK8963::_update)); diff --git a/libraries/AP_Compass/AP_Compass_Backend.cpp b/libraries/AP_Compass/AP_Compass_Backend.cpp index 40030c6c33..761be17e06 100644 --- a/libraries/AP_Compass/AP_Compass_Backend.cpp +++ b/libraries/AP_Compass/AP_Compass_Backend.cpp @@ -23,12 +23,12 @@ void AP_Compass_Backend::publish_field(const Vector3f &mag, uint8_t instance) // a noop on most boards state.field.rotate(MAG_BOARD_ORIENTATION); - // add user selectable orientation - state.field.rotate((enum Rotation)state.orientation.get()); - if (!state.external) { // and add in AHRS_ORIENTATION setting if not an external compass state.field.rotate(_compass._board_orientation); + } else { + // add user selectable orientation + state.field.rotate((enum Rotation)state.orientation.get()); } apply_corrections(state.field, instance); diff --git a/libraries/AP_Compass/AP_Compass_HMC5843.cpp b/libraries/AP_Compass/AP_Compass_HMC5843.cpp index 6d9bbc142a..82b4c69acb 100644 --- a/libraries/AP_Compass/AP_Compass_HMC5843.cpp +++ b/libraries/AP_Compass/AP_Compass_HMC5843.cpp @@ -336,8 +336,10 @@ AP_Compass_HMC5843::init() calibration[0], calibration[1], calibration[2]); #endif - // register the compass instance in the frontend - _compass_instance = register_compass(); + if (success) { + // register the compass instance in the frontend + _compass_instance = register_compass(); + } return success; } diff --git a/libraries/AP_Compass/Compass.cpp b/libraries/AP_Compass/Compass.cpp index b3089bf638..4e4ed0b39f 100644 --- a/libraries/AP_Compass/Compass.cpp +++ b/libraries/AP_Compass/Compass.cpp @@ -304,6 +304,11 @@ Compass::init() // detect available backends. Only called once _detect_backends(); } + if (_compass_count != 0) { + // get initial health status + hal.scheduler->delay(100); + read(); + } return true; } @@ -339,8 +344,10 @@ void Compass::_detect_backends(void) { -// Values defined in AP_HAL/AP_HAL_Boards.h -#if HAL_COMPASS_DEFAULT == HAL_COMPASS_HIL +#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX && CONFIG_HAL_BOARD_SUBTYPE != HAL_BOARD_SUBTYPE_LINUX_NONE + _add_backend(AP_Compass_HMC5843::detect); + _add_backend(AP_Compass_AK8963_MPU9250::detect); +#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_HIL _add_backend(AP_Compass_HIL::detect); #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_HMC5843 _add_backend(AP_Compass_HMC5843::detect); @@ -373,8 +380,10 @@ Compass::read(void) for (uint8_t i=0; i< _backend_count; i++) { // call read on each of the backend. This call updates field[i] _backends[i]->read(); - _state[i].healthy = (hal.scheduler->millis() - _state[i].last_update_ms < 500); } + for (uint8_t i=0; i < COMPASS_MAX_INSTANCES; i++) { + _state[i].healthy = (hal.scheduler->millis() - _state[i].last_update_ms < 500); + } return healthy(); } diff --git a/libraries/AP_Compass/Compass.h b/libraries/AP_Compass/Compass.h index e58d10379b..fb997d2dd8 100644 --- a/libraries/AP_Compass/Compass.h +++ b/libraries/AP_Compass/Compass.h @@ -51,12 +51,9 @@ maximum number of compass instances available on this platform. If more than 1 then redundent sensors may be available */ -#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_LINUX +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_LINUX || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN #define COMPASS_MAX_INSTANCES 3 #define COMPASS_MAX_BACKEND 3 -#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN -#define COMPASS_MAX_INSTANCES 2 -#define COMPASS_MAX_BACKEND 2 #else #define COMPASS_MAX_INSTANCES 1 #define COMPASS_MAX_BACKEND 1 diff --git a/libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde b/libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde index 720aa266b3..cac77edd72 100644 --- a/libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde +++ b/libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde @@ -39,8 +39,6 @@ const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER; -#define CONFIG_COMPASS HAL_COMPASS_DEFAULT - static Compass compass; uint32_t timer;