Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-07 08:23:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: support rotations on AK8963

Browse Source
This commit is contained in:
Andrew Tridgell 2016-11-06 21:49:18 +11:00
parent 2716ab8408
commit 05769640d1
3 changed files with 24 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_Compass/AP_Compass.cpp
View File

@ -520,7 +520,7 @@ void Compass::_detect_backends(void)
AP_Compass_AK8963::name, false); AP_Compass_AK8963::name, false);
} }
if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_TEST_V4) { if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_TEST_V4) {
_add_backend(AP_Compass_AK8963::probe_mpu9250(*this, 0), _add_backend(AP_Compass_AK8963::probe_mpu9250(*this, 0, ROTATION_ROLL_180_YAW_90),
AP_Compass_AK8963::name, false); AP_Compass_AK8963::name, false);
} }
// also add any px4 level drivers (for canbus magnetometers) // also add any px4 level drivers (for canbus magnetometers)

21
libraries/AP_Compass/AP_Compass_AK8963.cpp
View File

@ -52,9 +52,11 @@ struct PACKED sample_regs {
extern const AP_HAL::HAL &hal; extern const AP_HAL::HAL &hal;
AP_Compass_AK8963::AP_Compass_AK8963(Compass &compass, AP_AK8963_BusDriver *bus) AP_Compass_AK8963::AP_Compass_AK8963(Compass &compass, AP_AK8963_BusDriver *bus,
enum Rotation rotation)
: AP_Compass_Backend(compass) : AP_Compass_Backend(compass)
, _bus(bus) , _bus(bus)
, _rotation(rotation)
{ {
} }
@ -64,14 +66,15 @@ AP_Compass_AK8963::~AP_Compass_AK8963()
} }
AP_Compass_Backend *AP_Compass_AK8963::probe(Compass &compass, AP_Compass_Backend *AP_Compass_AK8963::probe(Compass &compass,
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev) AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
enum Rotation rotation)
{ {
AP_AK8963_BusDriver *bus = new AP_AK8963_BusDriver_HALDevice(std::move(dev)); AP_AK8963_BusDriver *bus = new AP_AK8963_BusDriver_HALDevice(std::move(dev));
if (!bus) { if (!bus) {
return nullptr; return nullptr;
} }
AP_Compass_AK8963 *sensor = new AP_Compass_AK8963(compass, bus); AP_Compass_AK8963 *sensor = new AP_Compass_AK8963(compass, bus, rotation);
if (!sensor || !sensor->init()) { if (!sensor || !sensor->init()) {
delete sensor; delete sensor;
return nullptr; return nullptr;
@ -81,17 +84,19 @@ AP_Compass_Backend *AP_Compass_AK8963::probe(Compass &compass,
} }
AP_Compass_Backend *AP_Compass_AK8963::probe_mpu9250(Compass &compass, AP_Compass_Backend *AP_Compass_AK8963::probe_mpu9250(Compass &compass,
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev) AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
enum Rotation rotation)
{ {
AP_InertialSensor &ins = *AP_InertialSensor::get_instance(); AP_InertialSensor &ins = *AP_InertialSensor::get_instance();
/* Allow MPU9250 to shortcut auxiliary bus and host bus */ /* Allow MPU9250 to shortcut auxiliary bus and host bus */
ins.detect_backends(); ins.detect_backends();
return probe(compass, std::move(dev)); return probe(compass, std::move(dev), rotation);
} }
AP_Compass_Backend *AP_Compass_AK8963::probe_mpu9250(Compass &compass, uint8_t mpu9250_instance) AP_Compass_Backend *AP_Compass_AK8963::probe_mpu9250(Compass &compass, uint8_t mpu9250_instance,
enum Rotation rotation)
{ {
AP_InertialSensor &ins = *AP_InertialSensor::get_instance(); AP_InertialSensor &ins = *AP_InertialSensor::get_instance();
@ -101,7 +106,7 @@ AP_Compass_Backend *AP_Compass_AK8963::probe_mpu9250(Compass &compass, uint8_t m
return nullptr; return nullptr;
} }
AP_Compass_AK8963 *sensor = new AP_Compass_AK8963(compass, bus); AP_Compass_AK8963 *sensor = new AP_Compass_AK8963(compass, bus, rotation);
if (!sensor || !sensor->init()) { if (!sensor || !sensor->init()) {
delete sensor; delete sensor;
return nullptr; return nullptr;
@ -149,6 +154,8 @@ bool AP_Compass_AK8963::init()
/* register the compass instance in the frontend */ /* register the compass instance in the frontend */
_compass_instance = register_compass(); _compass_instance = register_compass();
set_rotation(_compass_instance, _rotation);
_bus->set_device_type(DEVTYPE_AK8963); _bus->set_device_type(DEVTYPE_AK8963);
set_dev_id(_compass_instance, _bus->get_bus_id()); set_dev_id(_compass_instance, _bus->get_bus_id());

13
libraries/AP_Compass/AP_Compass_AK8963.h
View File

@ -19,14 +19,17 @@ class AP_Compass_AK8963 : public AP_Compass_Backend
public: public:
/* Probe for AK8963 standalone on I2C bus */ /* Probe for AK8963 standalone on I2C bus */
static AP_Compass_Backend *probe(Compass &compass, static AP_Compass_Backend *probe(Compass &compass,
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev); AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
enum Rotation rotation = ROTATION_NONE);
/* Probe for AK8963 on auxiliary bus of MPU9250, connected through I2C */ /* Probe for AK8963 on auxiliary bus of MPU9250, connected through I2C */
static AP_Compass_Backend *probe_mpu9250(Compass &compass, static AP_Compass_Backend *probe_mpu9250(Compass &compass,
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev); AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
enum Rotation rotation = ROTATION_NONE);
/* Probe for AK8963 on auxiliary bus of MPU9250, connected through SPI */ /* Probe for AK8963 on auxiliary bus of MPU9250, connected through SPI */
static AP_Compass_Backend *probe_mpu9250(Compass &compass, uint8_t mpu9250_instance); static AP_Compass_Backend *probe_mpu9250(Compass &compass, uint8_t mpu9250_instance,
enum Rotation rotation = ROTATION_NONE);
static constexpr const char *name = "AK8963"; static constexpr const char *name = "AK8963";
@ -36,7 +39,8 @@ public:
void read() override; void read() override;
private: private:
AP_Compass_AK8963(Compass &compass, AP_AK8963_BusDriver *bus); AP_Compass_AK8963(Compass &compass, AP_AK8963_BusDriver *bus,
enum Rotation rotation = ROTATION_NONE);
void _make_factory_sensitivity_adjustment(Vector3f &field) const; void _make_factory_sensitivity_adjustment(Vector3f &field) const;
void _make_adc_sensitivity_adjustment(Vector3f &field) const; void _make_adc_sensitivity_adjustment(Vector3f &field) const;
@ -61,6 +65,7 @@ private:
uint8_t _compass_instance; uint8_t _compass_instance;
bool _initialized; bool _initialized;
bool _timesliced; bool _timesliced;
enum Rotation _rotation;
}; };
class AP_AK8963_BusDriver class AP_AK8963_BusDriver