5
0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-05 23:48:31 -04:00

AP_Compass: make new backend match old PX4 behaviour

when a compass is internal only apply board orientation, not user
specified rotation
This commit is contained in:
Andrew Tridgell 2015-03-13 16:43:34 +11:00
parent a871c87cad
commit 0efd3bacea
6 changed files with 23 additions and 17 deletions

View File

@ -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));

View File

@ -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);

View File

@ -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;
}

View File

@ -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();
}

View File

@ -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

View File

@ -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;