aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Compass: QMC5883 Add internal i2c bus and fix rotation error

Browse Source
This commit is contained in:
ljwang 2017-06-22 14:37:00 +08:00 committed by Andrew Tridgell
parent ae6df4fc04
commit 603e903ccd
3 changed files with 26 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_Compass/AP_Compass.cpp
View File

@ -551,10 +551,14 @@ void Compass::_detect_backends(void)
ADD_BACKEND(AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(0, HAL_COMPASS_HMC5843_I2C_ADDR),
both_i2c_external, both_i2c_external?ROTATION_ROLL_180:ROTATION_YAW_270),
AP_Compass_HMC5843::name, both_i2c_external);
//external i2c bus for QMC5883L
//external i2c bus
ADD_BACKEND(AP_Compass_QMC5883L::probe(*this, hal.i2c_mgr->get_device(1, HAL_COMPASS_QMC5883L_I2C_ADDR),
ROTATION_YAW_90), AP_Compass_QMC5883L::name, true);
true,ROTATION_ROLL_180),
AP_Compass_QMC5883L::name, true);
//internal i2c bus
ADD_BACKEND(AP_Compass_QMC5883L::probe(*this, hal.i2c_mgr->get_device(0, HAL_COMPASS_QMC5883L_I2C_ADDR),
both_i2c_external, both_i2c_external?ROTATION_ROLL_180:ROTATION_YAW_270),
AP_Compass_QMC5883L::name,both_i2c_external);
#if !HAL_MINIMIZE_FEATURES
// AK09916 on ICM20948

20
libraries/AP_Compass/AP_Compass_QMC5883L.cpp
View File

@ -15,6 +15,7 @@
* with this program. If not, see <http://www.gnu.org/licenses/>.
*
* Driver by RadioLink LjWang, Jun 2017
* GPS compass module See<http://www.radiolink.com>
*/
#include "AP_Compass_QMC5883L.h"
@ -58,13 +59,14 @@ extern const AP_HAL::HAL &hal;
AP_Compass_Backend *AP_Compass_QMC5883L::probe(Compass &compass,
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
enum Rotation rotation)
bool force_external,
enum Rotation rotation)
{
if (!dev) {
return nullptr;
}
AP_Compass_QMC5883L *sensor = new AP_Compass_QMC5883L(compass, std::move(dev),rotation);
AP_Compass_QMC5883L *sensor = new AP_Compass_QMC5883L(compass, std::move(dev),force_external,rotation);
if (!sensor || !sensor->init()) {
delete sensor;
return nullptr;
@ -75,10 +77,12 @@ AP_Compass_Backend *AP_Compass_QMC5883L::probe(Compass &compass,
AP_Compass_QMC5883L::AP_Compass_QMC5883L(Compass &compass,
AP_HAL::OwnPtr<AP_HAL::Device> dev,
enum Rotation rotation)
bool force_external,
enum Rotation rotation)
: AP_Compass_Backend(compass)
, _dev(std::move(dev))
, _rotation(rotation)
, _force_external(force_external)
{
}
@ -125,6 +129,10 @@ bool AP_Compass_QMC5883L::init()
_dev->set_device_type(DEVTYPE_QMC5883L);
set_dev_id(_instance, _dev->get_bus_id());
if (_force_external) {
set_external(_instance, true);
}
//Enable 100HZ
_dev->register_periodic_callback(10000,
FUNCTOR_BIND_MEMBER(&AP_Compass_QMC5883L::timer, void));
@ -173,6 +181,11 @@ void AP_Compass_QMC5883L::timer()
Vector3f field = Vector3f{x * range_scale , y * range_scale, z * range_scale };
// rotate to the desired orientation
if (is_external(_instance)) {
field.rotate(ROTATION_YAW_90);
}
/* rotate raw_field from sensor frame to body frame */
rotate_field(field, _instance);
@ -191,7 +204,6 @@ void AP_Compass_QMC5883L::timer()
_accum.z /= 2;
_accum_count = 10;
}
_sem->give();
}
}

3
libraries/AP_Compass/AP_Compass_QMC5883L.h
View File

@ -33,6 +33,7 @@ class AP_Compass_QMC5883L : public AP_Compass_Backend
public:
static AP_Compass_Backend *probe(Compass &compass,
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
bool force_external,
enum Rotation rotation = ROTATION_NONE);
void read() override;
@ -42,6 +43,7 @@ public:
private:
AP_Compass_QMC5883L(Compass &compass,
AP_HAL::OwnPtr<AP_HAL::Device> dev,
bool force_external,
enum Rotation rotation);
void timer();
@ -54,4 +56,5 @@ private:
enum Rotation _rotation;
uint8_t _instance;
bool _force_external:1;
};