Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 00:28:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: Use common function in MPU9250 for initialization

Browse Source
This commit is contained in:
Lucas De Marchi 2015-07-02 21:01:04 -03:00 committed by Andrew Tridgell
parent eb4e2ac2e5
commit 1deb837e70
1 changed files with 30 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
libraries/AP_Compass/AP_Compass_AK8963.cpp
View File

@ -24,7 +24,9 @@
#include <AP_Math.h> #include <AP_Math.h>
#include <AP_HAL.h> #include <AP_HAL.h>
#include "AP_Compass_AK8963.h" #include "AP_Compass_AK8963.h"
#include "../AP_InertialSensor/AP_InertialSensor_MPU9250.h"
#define READ_FLAG 0x80 #define READ_FLAG 0x80
#define MPUREG_I2C_SLV0_ADDR 0x25 #define MPUREG_I2C_SLV0_ADDR 0x25
@ -148,43 +150,58 @@ bool AP_Compass_AK8963::init()
_spi_sem = _spi->get_semaphore(); _spi_sem = _spi->get_semaphore();
hal.scheduler->suspend_timer_procs();
if (!_spi_sem->take(100)) {
hal.console->printf("AK8963: Unable to get MPU9250 semaphore");
goto fail_sem;
}
if (!_configure_mpu9250()) { if (!_configure_mpu9250()) {
hal.console->printf_P(PSTR("AK8963: MPU9250 not configured for AK8963\n")); hal.console->printf_P(PSTR("AK8963: MPU9250 not configured for AK8963\n"));
return false; goto fail;
} }
if (!_configure()) { if (!_configure()) {
hal.console->printf_P(PSTR("AK8963: not configured\n")); hal.console->printf_P(PSTR("AK8963: not configured\n"));
return false; goto fail;
} }
if (!_check_id()) { if (!_check_id()) {
hal.console->printf_P(PSTR("AK8963: wrong id\n")); hal.console->printf_P(PSTR("AK8963: wrong id\n"));
return false; goto fail;
} }
if (!_calibrate()) { if (!_calibrate()) {
hal.console->printf_P(PSTR("AK8963: not calibrated\n")); hal.console->printf_P(PSTR("AK8963: not calibrated\n"));
return false; goto fail;
} }
if (!_start_conversion()) { if (!_start_conversion()) {
hal.console->printf_P(PSTR("AK8963: conversion not started\n")); hal.console->printf_P(PSTR("AK8963: conversion not started\n"));
return false; goto fail;
} }
_state = STATE_SAMPLE; _state = STATE_SAMPLE;
_initialized = true; _initialized = true;
hal.scheduler->suspend_timer_procs();
/* register the compass instance in the frontend */ /* register the compass instance in the frontend */
_compass_instance = register_compass(); _compass_instance = register_compass();
hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_Compass_AK8963::_update, void)); hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_Compass_AK8963::_update, void));
_spi_sem->give();
hal.scheduler->resume_timer_procs(); hal.scheduler->resume_timer_procs();
return true; return true;
fail:
_spi_sem->give();
fail_sem:
hal.scheduler->resume_timer_procs();
return false;
} }
void AP_Compass_AK8963::read() void AP_Compass_AK8963::read()
@ -256,7 +273,12 @@ bool AP_Compass_AK8963::_check_id()
bool AP_Compass_AK8963::_configure_mpu9250() bool AP_Compass_AK8963::_configure_mpu9250()
{ {
_bus_write(MPUREG_USER_CTRL, BIT_USER_CTRL_I2C_IF_DIS | BIT_USER_CTRL_I2C_MST_EN); if (!AP_InertialSensor_MPU9250::initialize_driver_state())
return false;
uint8_t user_ctrl;
_register_read(MPUREG_USER_CTRL, &user_ctrl, 1);
_bus_write(MPUREG_USER_CTRL, user_ctrl | BIT_USER_CTRL_I2C_MST_EN);
_bus_write(MPUREG_I2C_MST_CTRL, I2C_MST_CLOCK_400KHZ); _bus_write(MPUREG_I2C_MST_CTRL, I2C_MST_CLOCK_400KHZ);
return true; return true;