Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: Scale adjusted QMC5883L 

geändert:       libraries/AP_Compass/AP_Compass_QMC5883L.cpp
This commit is contained in:
Jonny Roeker 2024-05-25 17:42:40 +02:00
parent 8a58affe24
commit 17743dc413
1 changed files with 13 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libraries/AP_Compass/AP_Compass_QMC5883L.cpp
View File

@ -27,12 +27,13 @@
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include <AP_HAL/utility/sparse-endian.h> #include <AP_HAL/utility/sparse-endian.h>
#include <AP_Math/AP_Math.h> #include <AP_Math/AP_Math.h>
#include <GCS_MAVLink/GCS.h>
#define QMC5883L_REG_CONF1 0x09 #define QMC5883L_REG_CONF1 0x09
#define QMC5883L_REG_CONF2 0x0A #define QMC5883L_REG_CONF2 0x0A
// data output rates for 5883L // data output rates for 5883L
#define QMC5883L_ODR_10HZ (0x00 << 2) #define QMC5883L_ODR_10HZ (0x00 << 2)
#define QMC5883L_ODR_50HZ (0x01 << 2) #define QMC5883L_ODR_50HZ (0x01 << 2)
#define QMC5883L_ODR_100HZ (0x02 << 2) #define QMC5883L_ODR_100HZ (0x02 << 2)
#define QMC5883L_ODR_200HZ (0x03 << 2) #define QMC5883L_ODR_200HZ (0x03 << 2)
@ -46,8 +47,8 @@
#define QMC5883L_OSR_512 (0x00 << 6) #define QMC5883L_OSR_512 (0x00 << 6)
#define QMC5883L_OSR_256 (0x01 << 6) #define QMC5883L_OSR_256 (0x01 << 6)
#define QMC5883L_OSR_128 (0x10 << 6) #define QMC5883L_OSR_128 (0x10 << 6)
#define QMC5883L_OSR_64 (0x11 << 6) #define QMC5883L_OSR_64 (0x11 << 6)
#define QMC5883L_RST 0x80 #define QMC5883L_RST 0x80
@ -99,14 +100,14 @@ bool AP_Compass_QMC5883L::init()
goto fail; goto fail;
} }
/* Registers 0x20 and 0x21 are not documented in the data sheet and do not show any function during operation */
if (!_dev->write_register(0x0B, 0x01)|| if (!_dev->write_register(0x0B, 0x01)||
!_dev->write_register(0x20, 0x40)|| !_dev->write_register(QMC5883L_REG_CONF1,
!_dev->write_register(0x21, 0x01)|| QMC5883L_MODE_CONTINUOUS|
!_dev->write_register(QMC5883L_REG_CONF1, QMC5883L_ODR_100HZ|
QMC5883L_MODE_CONTINUOUS| QMC5883L_OSR_512|
QMC5883L_ODR_100HZ| QMC5883L_RNG_8G)) {
QMC5883L_OSR_512|
QMC5883L_RNG_8G)) {
goto fail; goto fail;
} }
@ -165,7 +166,8 @@ void AP_Compass_QMC5883L::timer()
le16_t rz; le16_t rz;
} buffer; } buffer;
const float range_scale = 1000.0f / 3000.0f; /* Most modules deliver a value four times higher */
const float range_scale = 1000.0f / (3000.0f * 4.0f);
uint8_t status; uint8_t status;
if(!_dev->read_registers(QMC5883L_REG_STATUS,&status,1)){ if(!_dev->read_registers(QMC5883L_REG_STATUS,&status,1)){