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:
parent
a871c87cad
commit
0efd3bacea
libraries/AP_Compass
@ -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));
|
||||
|
||||
|
@ -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);
|
||||
|
@ -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;
|
||||
}
|
||||
|
@ -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();
|
||||
}
|
||||
|
||||
|
@ -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
|
||||
|
@ -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;
|
||||
|
Loading…
Reference in New Issue
Block a user