Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-07 00:13:59 -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);
}
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);
}
// 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;
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)
, _bus(bus)
, _rotation(rotation)
{
}
@ -64,14 +66,15 @@ AP_Compass_AK8963::~AP_Compass_AK8963()
}
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));
if (!bus) {
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()) {
delete sensor;
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_HAL::OwnPtr<AP_HAL::I2CDevice> dev)
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
enum Rotation rotation)
{
AP_InertialSensor &ins = *AP_InertialSensor::get_instance();
/* Allow MPU9250 to shortcut auxiliary bus and host bus */
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();
@ -101,7 +106,7 @@ AP_Compass_Backend *AP_Compass_AK8963::probe_mpu9250(Compass &compass, uint8_t m
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()) {
delete sensor;
return nullptr;
@ -149,6 +154,8 @@ bool AP_Compass_AK8963::init()
/* register the compass instance in the frontend */
_compass_instance = register_compass();
set_rotation(_compass_instance, _rotation);
_bus->set_device_type(DEVTYPE_AK8963);
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:
/* Probe for AK8963 standalone on I2C bus */
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 */
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 */
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";
@ -36,7 +39,8 @@ public:
void read() override;
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_adc_sensitivity_adjustment(Vector3f &field) const;
@ -61,6 +65,7 @@ private:
uint8_t _compass_instance;
bool _initialized;
bool _timesliced;
enum Rotation _rotation;
};
class AP_AK8963_BusDriver