mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-02 13:27:32 +08:00
Added driver for QMC5883L Magnetometer (#2953)
Co-authored-by: Open UAS <noreply@openuas.org>
This commit is contained in:
OpenUAS
GitHub
@@ -37,7 +37,7 @@ NOTES:
|
|||||||
</description>
|
</description>
|
||||||
<firmware name="fixedwing">
|
<firmware name="fixedwing">
|
||||||
<target name="ap" board="lisa_mxs_1.0_chibios">
|
<target name="ap" board="lisa_mxs_1.0_chibios">
|
||||||
<define name="REMAP_UART3" value="TRUE" />
|
<define name="REMAP_UART3" value="TRUE"/>
|
||||||
<!--<configure name="FLASH_MODE" value="SWD"/>--> <!--Enable when flashing with black magic probe v1.0-->
|
<!--<configure name="FLASH_MODE" value="SWD"/>--> <!--Enable when flashing with black magic probe v1.0-->
|
||||||
<!--<define name="USE_PERSISTENT_SETTINGS" value="TRUE"/>-->
|
<!--<define name="USE_PERSISTENT_SETTINGS" value="TRUE"/>-->
|
||||||
|
|
||||||
@@ -58,11 +58,11 @@ NOTES:
|
|||||||
|
|
||||||
<configure name="AHRS_PROPAGATE_FREQUENCY" value="500"/>
|
<configure name="AHRS_PROPAGATE_FREQUENCY" value="500"/>
|
||||||
<configure name="AHRS_CORRECT_FREQUENCY" value="500"/>
|
<configure name="AHRS_CORRECT_FREQUENCY" value="500"/>
|
||||||
<configure name="AHRS_MAG_CORRECT_FREQUENCY" value="50"/>
|
<!--<configure name="AHRS_MAG_CORRECT_FREQUENCY" value="50"/>
|
||||||
<configure name="NAVIGATION_FREQUENCY" value="16"/>
|
<configure name="NAVIGATION_FREQUENCY" value="16"/>
|
||||||
<configure name="CONTROL_FREQUENCY" value="120"/>
|
<configure name="CONTROL_FREQUENCY" value="120"/>
|
||||||
<configure name="TELEMETRY_FREQUENCY" value="60"/>
|
<configure name="TELEMETRY_FREQUENCY" value="60"/>
|
||||||
<configure name="MODULES_FREQUENCY" value="500"/>
|
<configure name="MODULES_FREQUENCY" value="500"/>-->
|
||||||
|
|
||||||
<!-- <module name="filter_1euro_imu">
|
<!-- <module name="filter_1euro_imu">
|
||||||
<define name="AHRS_ICQ_IMU_ID" value="IMU_F1E_ID"/>
|
<define name="AHRS_ICQ_IMU_ID" value="IMU_F1E_ID"/>
|
||||||
@@ -84,20 +84,18 @@ NOTES:
|
|||||||
<define name="IMU_MPU_USE_MEDIAN_FILTER" value="TRUE"/>
|
<define name="IMU_MPU_USE_MEDIAN_FILTER" value="TRUE"/>
|
||||||
</module>
|
</module>
|
||||||
|
|
||||||
<!-- enable when driver is available in master -->
|
<module name="mag" type="qmc5883l">
|
||||||
<!--
|
<configure name="MAG_QMC5883L_PERIODIC_FREQUENCY" value="200"/>
|
||||||
<module name="mag" type="qmc58xx">
|
<configure name="MAG_QMC5883L_I2C_DEV" value="I2C1"/>
|
||||||
<configure name="MAG_QMC58XX_I2C_DEV" value="i2c1"/>
|
<define name="MODULE_QMC5883L_SYNC_SEND" value="TRUE"/>
|
||||||
<define name="MODULE_QMC58XX_SYNC_SEND" value="TRUE"/>
|
<define name="MODULE_QMC5883L_UPDATE_AHRS" value="TRUE"/>
|
||||||
<define name="MODULE_QMC58XX_UPDATE_AHRS" value="TRUE"/>
|
<define name="QMC5883L_CHAN_X" value="1"/>
|
||||||
<define name="QMC58XX_CHAN_X" value="1"/>
|
<define name="QMC5883L_CHAN_Y" value="0"/>
|
||||||
<define name="QMC58XX_CHAN_Y" value="0"/>
|
<define name="QMC5883L_CHAN_Z" value="2"/>
|
||||||
<define name="QMC58XX_CHAN_Z" value="2"/>
|
<define name="QMC5883L_CHAN_X_SIGN" value="+"/>
|
||||||
<define name="QMC58XX_CHAN_X_SIGN" value="-"/>
|
<define name="QMC5883L_CHAN_Y_SIGN" value="+"/>
|
||||||
<define name="QMC58XX_CHAN_Y_SIGN" value="+"/>
|
<define name="QMC5883L_CHAN_Z_SIGN" value="+"/>
|
||||||
<define name="QMC58XX_CHAN_Z_SIGN" value="+"/>
|
|
||||||
</module>
|
</module>
|
||||||
-->
|
|
||||||
|
|
||||||
<module name="actuators" type="pwm"/>
|
<module name="actuators" type="pwm"/>
|
||||||
|
|
||||||
|
|||||||
@@ -0,0 +1,70 @@
|
|||||||
|
<!DOCTYPE module SYSTEM "module.dtd">
|
||||||
|
|
||||||
|
<module name="mag_qmc5883l" dir="sensors" task="sensors">
|
||||||
|
<doc>
|
||||||
|
<description>
|
||||||
|
Module to be able to use an QMC5883L magnetometer.
|
||||||
|
|
||||||
|
Intorduction: Honeywell discontinued the HMC5983/HMC5883L in 2016 and licensed the technology to QST Corporation, who now manufacture the replacement QMC5883L.
|
||||||
|
However, while the QMC5883L is pin compatible with HMC, it's registers are not the same as the Honeywell device. It's essentially a different chip.
|
||||||
|
|
||||||
|
QST Corporation have produced two versions of the QMC5883L:
|
||||||
|
A completely undocumented 'A' version with "DA 5883" on the QFN package that responds to the I2C address 0x1E.
|
||||||
|
It's identical to the HMC5983/HMC5883L, except that the status register doesn't work.
|
||||||
|
The DA 5883 is not supported by this driver since we have not seen any samples of this DA version in the wild
|
||||||
|
|
||||||
|
A documented 'B' version with "DB 5833" on the QFN package that responds to I2C address 0x0D. This works as per the QMC5883L datasheet.
|
||||||
|
|
||||||
|
TIP:
|
||||||
|
An arbitrary rotation between the sensor frame and the IMU frame can be compensated with a MAG_TO_IMU rotation, defined by three euler angles.
|
||||||
|
The three angles must be defined to enable this correction. Otherwise it is assumed that the IMU and MAG axis are aligned.
|
||||||
|
</description>
|
||||||
|
<configure name="MAG_QMC5883L_I2C_DEV" value="i2c1" description="I2C device to use (e.g. i2c2)"/>
|
||||||
|
<define name="MODULE_QMC5883L_SYNC_SEND" value="TRUE|FALSE" description="Send IMU_RAW message with each new measurement for debugging purposes (default: FALSE)"/>
|
||||||
|
<define name="MODULE_QMC5883L_UPDATE_AHRS" value="TRUE|FALSE" description="Copy measurements to IMU and send as ABI message (default: FALSE)"/>
|
||||||
|
<define name="QMC5883L_CHAN_X_SIGN" value="+|-" description="Set the polarity of x axis (default: +)"/>
|
||||||
|
<define name="QMC5883L_CHAN_Y_SIGN" value="+|-" description="Set the polarity of y axis (default: +)"/>
|
||||||
|
<define name="QMC5883L_CHAN_Z_SIGN" value="+|-" description="Set the polarity of z axis (default: +)"/>
|
||||||
|
<define name="QMC5883L_CHAN_X" value="0|1|2" description="Channel id of x axis (default: 0)"/>
|
||||||
|
<define name="QMC5883L_CHAN_Y" value="0|1|2" description="Channel id of y axis (default: 1)"/>
|
||||||
|
<define name="QMC5883L_CHAN_Z" value="0|1|2" description="Channel id of z axis (default: 2)"/>
|
||||||
|
<section name="MAG_QMC" prefix="QMC5883L_">
|
||||||
|
<define name="MAG_TO_IMU_PHI" value="0.0" description="Rotation between sensor frame and IMU frame (phi angle)"/>
|
||||||
|
<define name="MAG_TO_IMU_THETA" value="0.0" description="Rotation between sensor frame and IMU frame (theta angle)"/>
|
||||||
|
<define name="MAG_TO_IMU_PSI" value="0.0" description="Rotation between sensor frame and IMU frame (psi angle)"/>
|
||||||
|
<define name="DATA_RATE" value="QMC5883L_ODR_200" description="Continuous conversion data rate"/>
|
||||||
|
</section>
|
||||||
|
</doc>
|
||||||
|
<dep>
|
||||||
|
<depends>i2c,@imu</depends>
|
||||||
|
<provides>mag</provides>
|
||||||
|
</dep>
|
||||||
|
<header>
|
||||||
|
<file name="mag_qmc5883l.h"/>
|
||||||
|
</header>
|
||||||
|
<init fun="mag_qmc5883l_module_init()"/>
|
||||||
|
<periodic fun="mag_qmc5883l_module_periodic()" freq="MAG_QMC5883L_PERIODIC_FREQUENCY"/>
|
||||||
|
<periodic fun="mag_qmc5883l_report()" freq="10" autorun="FALSE"/>
|
||||||
|
<event fun="mag_qmc5883l_module_event()"/>
|
||||||
|
<makefile target="ap">
|
||||||
|
<file name="mag_qmc5883l.c"/>
|
||||||
|
<file name="qmc5883l.c" dir="peripherals"/>
|
||||||
|
<raw>
|
||||||
|
ifeq ($(MAG_QMC5883L_I2C_DEV),)
|
||||||
|
$(error mag_qmc5883l module error: please configure MAG_QMC5883L_I2C_DEV)
|
||||||
|
endif
|
||||||
|
</raw>
|
||||||
|
<configure name="MAG_QMC5883L_PERIODIC_FREQUENCY" default="50"/>
|
||||||
|
<configure name="MAG_QMC5883L_I2C_DEV" case="upper|lower"/>
|
||||||
|
<define name="MAG_QMC5883L_PERIODIC_FREQUENCY" value="$(MAG_QMC5883L_PERIODIC_FREQUENCY)"/>
|
||||||
|
<define name="USE_$(MAG_QMC5883L_I2C_DEV_UPPER)"/>
|
||||||
|
<define name="MAG_QMC5883L_I2C_DEV" value="$(MAG_QMC5883L_I2C_DEV_LOWER)"/>
|
||||||
|
<test>
|
||||||
|
<define name="MAG_QMC5883L_I2C_DEV" value="i2c1"/>
|
||||||
|
<define name="USE_I2C1"/>
|
||||||
|
<define name="DOWNLINK_TRANSPORT" value="pprz_tp"/>
|
||||||
|
<define name="DOWNLINK_DEVICE" value="uart1"/>
|
||||||
|
<define name="USE_UART1"/>
|
||||||
|
</test>
|
||||||
|
</makefile>
|
||||||
|
</module>
|
||||||
@@ -200,8 +200,8 @@
|
|||||||
* PB3 - Digital input (JTAG_TDO/SWD)
|
* PB3 - Digital input (JTAG_TDO/SWD)
|
||||||
* PB4 - Open Drain output 50MHz (LED2)
|
* PB4 - Open Drain output 50MHz (LED2)
|
||||||
* PB5 - Digital input (IMU_MAG_DRDY)
|
* PB5 - Digital input (IMU_MAG_DRDY)
|
||||||
* PB6 - Alternate Push Pull output 50MHz (SERVO7-Timer4Ch1)/USART1_TX
|
* PB6 - Alternate Open Drain output 2MHz (SERVO7-Timer4Ch1)/USART1_TX
|
||||||
* PB7 - Alternate Push Pull output 50MHz (SERVO8-Timer4Ch2)/USART1_RX
|
* PB7 - Alternate Open Drain output 2MHz (SERVO8-Timer4Ch2)/USART1_RX
|
||||||
* PB8 - Digital input. (CAN_RX)
|
* PB8 - Digital input. (CAN_RX)
|
||||||
* PB9 - Open Drain output 50MHz. (CAN_TX)
|
* PB9 - Open Drain output 50MHz. (CAN_TX)
|
||||||
* PB10 - Alternate Open Drain output 2MHz.(I2C2_SCL)
|
* PB10 - Alternate Open Drain output 2MHz.(I2C2_SCL)
|
||||||
@@ -233,8 +233,8 @@
|
|||||||
PIN_OTYPE_PUSHPULL(3) | \
|
PIN_OTYPE_PUSHPULL(3) | \
|
||||||
PIN_OTYPE_OPENDRAIN(4) | \
|
PIN_OTYPE_OPENDRAIN(4) | \
|
||||||
PIN_OTYPE_PUSHPULL(5) | \
|
PIN_OTYPE_PUSHPULL(5) | \
|
||||||
PIN_OTYPE_PUSHPULL(6) | \
|
PIN_OTYPE_OPENDRAIN(6) | \
|
||||||
PIN_OTYPE_PUSHPULL(7) | \
|
PIN_OTYPE_OPENDRAIN(7) | \
|
||||||
PIN_OTYPE_PUSHPULL(8) | \
|
PIN_OTYPE_PUSHPULL(8) | \
|
||||||
PIN_OTYPE_OPENDRAIN(9) | \
|
PIN_OTYPE_OPENDRAIN(9) | \
|
||||||
PIN_OTYPE_OPENDRAIN(10) | \
|
PIN_OTYPE_OPENDRAIN(10) | \
|
||||||
@@ -297,8 +297,8 @@
|
|||||||
PIN_AFIO_AF(3, 0) | \
|
PIN_AFIO_AF(3, 0) | \
|
||||||
PIN_AFIO_AF(4, 0) | \
|
PIN_AFIO_AF(4, 0) | \
|
||||||
PIN_AFIO_AF(5, 0) | \
|
PIN_AFIO_AF(5, 0) | \
|
||||||
PIN_AFIO_AF(6, 7) | \
|
PIN_AFIO_AF(6, 4) | \
|
||||||
PIN_AFIO_AF(7, 7))
|
PIN_AFIO_AF(7, 4))
|
||||||
#define VAL_GPIOB_AFRH (PIN_AFIO_AF(8, 9) | \
|
#define VAL_GPIOB_AFRH (PIN_AFIO_AF(8, 9) | \
|
||||||
PIN_AFIO_AF(9, 9) | \
|
PIN_AFIO_AF(9, 9) | \
|
||||||
PIN_AFIO_AF(10, 4) | \
|
PIN_AFIO_AF(10, 4) | \
|
||||||
|
|||||||
@@ -217,6 +217,10 @@
|
|||||||
#define MAG_DATALINK_SENDER_ID 6
|
#define MAG_DATALINK_SENDER_ID 6
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#ifndef MAG_QMC5883L_SENDER_ID
|
||||||
|
#define MAG_QMC5883L_SENDER_ID 7
|
||||||
|
#endif
|
||||||
|
|
||||||
#ifndef IMU_MAG_PITOT_ID
|
#ifndef IMU_MAG_PITOT_ID
|
||||||
#define IMU_MAG_PITOT_ID 50
|
#define IMU_MAG_PITOT_ID 50
|
||||||
#endif
|
#endif
|
||||||
|
|||||||
@@ -0,0 +1,136 @@
|
|||||||
|
/*
|
||||||
|
* Copyright (C) 2022 Paparazzi Team
|
||||||
|
*
|
||||||
|
* This file is part of paparazzi.
|
||||||
|
*
|
||||||
|
* paparazzi is free software; you can redistribute it and/or modify
|
||||||
|
* it under the terms of the GNU General Public License as published by
|
||||||
|
* the Free Software Foundation; either version 2, or (at your option)
|
||||||
|
* any later version.
|
||||||
|
*
|
||||||
|
* paparazzi is distributed in the hope that it will be useful,
|
||||||
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||||
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||||
|
* GNU General Public License for more details.
|
||||||
|
*
|
||||||
|
* You should have received a copy of the GNU General Public License
|
||||||
|
* along with paparazzi; see the file COPYING. If not, see
|
||||||
|
* <http://www.gnu.org/licenses/>.
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file modules/sensors/mag_qmc5883l.c
|
||||||
|
*
|
||||||
|
* Module for QST QMC5883L magnetometer, the DB version
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "modules/sensors/mag_qmc5883l.h"
|
||||||
|
#include "mcu_periph/uart.h"
|
||||||
|
#include "pprzlink/messages.h"
|
||||||
|
#include "modules/datalink/downlink.h"
|
||||||
|
#include "generated/airframe.h"
|
||||||
|
#include "modules/core/abi.h"
|
||||||
|
|
||||||
|
#ifndef QMC5883L_CHAN_X
|
||||||
|
#define QMC5883L_CHAN_X 0
|
||||||
|
#endif
|
||||||
|
#ifndef QMC5883L_CHAN_Y
|
||||||
|
#define QMC5883L_CHAN_Y 1
|
||||||
|
#endif
|
||||||
|
#ifndef QMC5883L_CHAN_Z
|
||||||
|
#define QMC5883L_CHAN_Z 2
|
||||||
|
#endif
|
||||||
|
#ifndef QMC5883L_CHAN_X_SIGN
|
||||||
|
#define QMC5883L_CHAN_X_SIGN +
|
||||||
|
#endif
|
||||||
|
#ifndef QMC5883L_CHAN_Y_SIGN
|
||||||
|
#define QMC5883L_CHAN_Y_SIGN +
|
||||||
|
#endif
|
||||||
|
#ifndef QMC5883L_CHAN_Z_SIGN
|
||||||
|
#define QMC5883L_CHAN_Z_SIGN +
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifndef QMC5883L_DATA_RATE
|
||||||
|
#define QMC5883L_DATA_RATE QMC5883L_ODR_DEFAULT
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifndef QMC5883L_ADDR
|
||||||
|
#define QMC5883L_ADDR QMC5883L_ADDR0
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#if MODULE_QMC5883L_UPDATE_AHRS
|
||||||
|
|
||||||
|
#if defined QMC5883L_MAG_TO_IMU_PHI && defined QMC5883L_MAG_TO_IMU_THETA && defined QMC5883L_MAG_TO_IMU_PSI
|
||||||
|
#define USE_MAG_TO_IMU 1
|
||||||
|
static struct Int32RMat mag_to_imu; ///< rotation from mag to imu frame
|
||||||
|
#else
|
||||||
|
#define USE_MAG_TO_IMU 0
|
||||||
|
#endif
|
||||||
|
#endif
|
||||||
|
|
||||||
|
struct Qmc5883l mag_qmc5883l;
|
||||||
|
|
||||||
|
void mag_qmc5883l_module_init(void)
|
||||||
|
{
|
||||||
|
qmc5883l_init(&mag_qmc5883l, &(MAG_QMC5883L_I2C_DEV), QMC5883L_ADDR, QMC5883L_DATA_RATE);
|
||||||
|
|
||||||
|
#if MODULE_QMC5883L_UPDATE_AHRS && USE_MAG_TO_IMU
|
||||||
|
struct Int32Eulers mag_to_imu_eulers = {
|
||||||
|
ANGLE_BFP_OF_REAL(QMC5883L_MAG_TO_IMU_PHI),
|
||||||
|
ANGLE_BFP_OF_REAL(QMC5883L_MAG_TO_IMU_THETA),
|
||||||
|
ANGLE_BFP_OF_REAL(QMC5883L_MAG_TO_IMU_PSI)
|
||||||
|
};
|
||||||
|
int32_rmat_of_eulers(&mag_to_imu, &mag_to_imu_eulers);
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
|
||||||
|
void mag_qmc5883l_module_periodic(void)
|
||||||
|
{
|
||||||
|
qmc5883l_periodic(&mag_qmc5883l);
|
||||||
|
}
|
||||||
|
|
||||||
|
void mag_qmc5883l_module_event(void)
|
||||||
|
{
|
||||||
|
qmc5883l_event(&mag_qmc5883l);
|
||||||
|
|
||||||
|
if (mag_qmc5883l.data_available) {
|
||||||
|
#if MODULE_QMC5883L_UPDATE_AHRS
|
||||||
|
// current timestamp
|
||||||
|
uint32_t now_ts = get_sys_time_usec();
|
||||||
|
|
||||||
|
// set channel order
|
||||||
|
struct Int32Vect3 mag = {
|
||||||
|
QMC5883L_CHAN_X_SIGN(int32_t)(mag_qmc5883l.data.value[QMC5883L_CHAN_X]),
|
||||||
|
QMC5883L_CHAN_Y_SIGN(int32_t)(mag_qmc5883l.data.value[QMC5883L_CHAN_Y]),
|
||||||
|
QMC5883L_CHAN_Z_SIGN(int32_t)(mag_qmc5883l.data.value[QMC5883L_CHAN_Z])
|
||||||
|
};
|
||||||
|
// only rotate if needed
|
||||||
|
#if USE_MAG_TO_IMU
|
||||||
|
struct Int32Vect3 imu_mag;
|
||||||
|
// rotate data from mag frame to imu frame
|
||||||
|
int32_rmat_vmult(&imu_mag, &mag_to_imu, &mag);
|
||||||
|
// unscaled vector
|
||||||
|
VECT3_COPY(mag, imu_mag);
|
||||||
|
#endif
|
||||||
|
|
||||||
|
AbiSendMsgIMU_MAG_RAW(MAG_QMC5883L_SENDER_ID, now_ts, &mag);
|
||||||
|
#endif
|
||||||
|
#if MODULE_QMC5883L_SYNC_SEND
|
||||||
|
mag_qmc5883l_report();
|
||||||
|
#endif
|
||||||
|
#if MODULE_QMC5883L_UPDATE_AHRS || MODULE_QMC5883L_SYNC_SEND
|
||||||
|
mag_qmc5883l.data_available = false;
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void mag_qmc5883l_report(void)
|
||||||
|
{
|
||||||
|
uint8_t id = MAG_QMC5883L_SENDER_ID;
|
||||||
|
struct Int32Vect3 mag = {
|
||||||
|
QMC5883L_CHAN_X_SIGN(int32_t)(mag_qmc5883l.data.value[QMC5883L_CHAN_X]),
|
||||||
|
QMC5883L_CHAN_Y_SIGN(int32_t)(mag_qmc5883l.data.value[QMC5883L_CHAN_Y]),
|
||||||
|
QMC5883L_CHAN_Z_SIGN(int32_t)(mag_qmc5883l.data.value[QMC5883L_CHAN_Z])
|
||||||
|
};
|
||||||
|
DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice, &id, &mag.x, &mag.y, &mag.z);
|
||||||
|
}
|
||||||
@@ -0,0 +1,42 @@
|
|||||||
|
/*
|
||||||
|
* Copyright (C) 2022 Paparazzi Team
|
||||||
|
*
|
||||||
|
* This file is part of paparazzi.
|
||||||
|
*
|
||||||
|
* paparazzi is free software; you can redistribute it and/or modify
|
||||||
|
* it under the terms of the GNU General Public License as published by
|
||||||
|
* the Free Software Foundation; either version 2, or (at your option)
|
||||||
|
* any later version.
|
||||||
|
*
|
||||||
|
* paparazzi is distributed in the hope that it will be useful,
|
||||||
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||||
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||||
|
* GNU General Public License for more details.
|
||||||
|
*
|
||||||
|
* You should have received a copy of the GNU General Public License
|
||||||
|
* along with paparazzi; see the file COPYING. If not, see
|
||||||
|
* <http://www.gnu.org/licenses/>.
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file modules/sensors/mag_qmc5883l.h
|
||||||
|
*
|
||||||
|
* Module wrapper for QNI QMC5883L magnetometer.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef MAG_QMC5883L_H
|
||||||
|
#define MAG_QMC5883L_H
|
||||||
|
|
||||||
|
#include "peripherals/qmc5883l.h"
|
||||||
|
|
||||||
|
extern struct Qmc5883l mag_qmc5883l;
|
||||||
|
|
||||||
|
extern void mag_qmc5883l_module_init(void);
|
||||||
|
extern void mag_qmc5883l_module_periodic(void);
|
||||||
|
extern void mag_qmc5883l_module_event(void);
|
||||||
|
extern void mag_qmc5883l_report(void);
|
||||||
|
|
||||||
|
#endif /* MAG_QMC5883L_H */
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
@@ -0,0 +1,191 @@
|
|||||||
|
/*
|
||||||
|
* Copyright (C) 2022 Paparazzi Team
|
||||||
|
*
|
||||||
|
* This file is part of paparazzi.
|
||||||
|
*
|
||||||
|
* paparazzi is free software; you can redistribute it and/or modify
|
||||||
|
* it under the terms of the GNU General Public License as published by
|
||||||
|
* the Free Software Foundation; either version 2, or (at your option)
|
||||||
|
* any later version.
|
||||||
|
*
|
||||||
|
* paparazzi is distributed in the hope that it will be useful,
|
||||||
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||||
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||||
|
* GNU General Public License for more details.
|
||||||
|
*
|
||||||
|
* You should have received a copy of the GNU General Public License
|
||||||
|
* along with paparazzi; see the file COPYING. If not, see
|
||||||
|
* <http://www.gnu.org/licenses/>.
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file peripherals/qmc5883l.c
|
||||||
|
*
|
||||||
|
* QST QMC5883L 3-axis magnetometer driver interface (I2C).
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "peripherals/qmc5883l.h"
|
||||||
|
|
||||||
|
/* Registers Axis X,Y,Z */
|
||||||
|
#define QMC5883L_REG_DATXL 0x00
|
||||||
|
#define QMC5883L_REG_DATXM 0x01
|
||||||
|
#define QMC5883L_REG_DATYL 0x02
|
||||||
|
#define QMC5883L_REG_DATYM 0x03
|
||||||
|
#define QMC5883L_REG_DATZL 0x04
|
||||||
|
#define QMC5883L_REG_DATZM 0x05
|
||||||
|
|
||||||
|
/* Register I2C bus transaction Status */
|
||||||
|
#define QMC5883L_REG_STATUS 0x06
|
||||||
|
|
||||||
|
/* Registers Temperature, relative thus not so useful ATM, therefore not implemented in reading */
|
||||||
|
#define QMC5883L_REG_TEMPM 0x07
|
||||||
|
#define QMC5883L_REG_TEMPL 0x08
|
||||||
|
|
||||||
|
/* Registers Config */
|
||||||
|
#define QMC5883L_REG_CONTROL_1 0x09 /* settings for MODE */
|
||||||
|
#define QMC5883L_REG_CONTROL_2 0x0A /* settings for INT_ENB */
|
||||||
|
#define QMC5883L_REG_RESET_PERIOD 0x0B
|
||||||
|
|
||||||
|
#define QMC5883L_REG_IDC 0x0C /* OEM reserved */
|
||||||
|
#define QMC5883L_REG_IDD 0x0D /* OEM reserved */
|
||||||
|
|
||||||
|
/* Options for CONTROL_1 */
|
||||||
|
#define QMC5883L_MODE_STBY 0x00
|
||||||
|
#define QMC5883L_MODE_CONT 0x01
|
||||||
|
|
||||||
|
/* Options for scale RaNGe(RNG) Gauss */
|
||||||
|
#define QMC5883L_RNG_2G 0x00
|
||||||
|
#define QMC5883L_RNG_8G 0x10
|
||||||
|
|
||||||
|
/* options for Over-Sample Ratio (OSR) */
|
||||||
|
#define QMC5883L_OSR_512 0x00 /* Use 512 if powerusage of chip is not an issue */
|
||||||
|
#define QMC5883L_OSR_256 0x40
|
||||||
|
#define QMC5883L_OSR_128 0x80
|
||||||
|
#define QMC5883L_OSR_64 0xC0
|
||||||
|
|
||||||
|
void qmc5883l_init(struct Qmc5883l *mag, struct i2c_periph *i2c_p, uint8_t addr, uint8_t data_rate)
|
||||||
|
{
|
||||||
|
/* set i2c_peripheral */
|
||||||
|
mag->i2c_p = i2c_p;
|
||||||
|
/* set i2c address */
|
||||||
|
mag->i2c_trans.slave_addr = addr;
|
||||||
|
mag->i2c_trans.status = I2CTransDone;
|
||||||
|
/* store data rate */
|
||||||
|
mag->data_rate = data_rate;
|
||||||
|
mag->initialized = false;
|
||||||
|
mag->status = QMC5883L_CONF_UNINIT;
|
||||||
|
mag->data_available = false;
|
||||||
|
}
|
||||||
|
|
||||||
|
void qmc5883l_configure(struct Qmc5883l *mag)
|
||||||
|
{
|
||||||
|
// Only configure when not busy
|
||||||
|
if (mag->i2c_trans.status != I2CTransSuccess && mag->i2c_trans.status != I2CTransFailed
|
||||||
|
&& mag->i2c_trans.status != I2CTransDone) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Only when successful continue with next
|
||||||
|
if (mag->i2c_trans.status == I2CTransSuccess) {
|
||||||
|
mag->status++; //Here the Enum Counter goes to the next one
|
||||||
|
}
|
||||||
|
|
||||||
|
mag->i2c_trans.status = I2CTransDone;
|
||||||
|
switch (mag->status) {
|
||||||
|
|
||||||
|
case QMC5883L_CONF_UNINIT:
|
||||||
|
/* prepare config request */
|
||||||
|
mag->i2c_trans.buf[0] = QMC5883L_REG_RESET_PERIOD;
|
||||||
|
mag->i2c_trans.buf[0] = 0x01;
|
||||||
|
/* send config request, ask for i2c frame for set/reset period */
|
||||||
|
i2c_transmit(mag->i2c_p, &(mag->i2c_trans), mag->i2c_trans.slave_addr, 2);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case QMC5883L_CONF_CCR_DONE:
|
||||||
|
mag->i2c_trans.buf[0] = QMC5883L_REG_CONTROL_1;
|
||||||
|
mag->i2c_trans.buf[1] = (QMC5883L_MODE_CONT|QMC5883L_ODR_200|QMC5883L_RNG_8G|QMC5883L_OSR_512);//todo datarate from settings mag->data_rate;
|
||||||
|
i2c_transmit(mag->i2c_p, &(mag->i2c_trans), mag->i2c_trans.slave_addr, 2);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case QMC5883L_CONF_TMRC_DONE:
|
||||||
|
mag->i2c_trans.buf[0] = QMC5883L_REG_CONTROL_1;
|
||||||
|
mag->i2c_trans.buf[1] = (QMC5883L_MODE_CONT|QMC5883L_ODR_200|QMC5883L_RNG_8G|QMC5883L_OSR_512);//todo datarate from settings //mag->data_rate;
|
||||||
|
i2c_transmit(mag->i2c_p, &(mag->i2c_trans), mag->i2c_trans.slave_addr, 2);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case QMC5883L_CONF_CCM_DONE:
|
||||||
|
mag->status = QMC5883L_STATUS_IDLE;
|
||||||
|
mag->initialized = true;
|
||||||
|
break;
|
||||||
|
|
||||||
|
default:
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void qmc5883l_read(struct Qmc5883l *mag)
|
||||||
|
{
|
||||||
|
if (mag->status != QMC5883L_STATUS_IDLE) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* get 3 x 2 bytes data = 6 Bytes and one status byte = 7 */
|
||||||
|
mag->i2c_trans.buf[0] = QMC5883L_REG_DATXL;
|
||||||
|
mag->i2c_trans.buf[1] = QMC5883L_REG_DATXM;
|
||||||
|
mag->i2c_trans.buf[2] = QMC5883L_REG_DATYL;
|
||||||
|
mag->i2c_trans.buf[3] = QMC5883L_REG_DATYM;
|
||||||
|
mag->i2c_trans.buf[4] = QMC5883L_REG_DATZL;
|
||||||
|
mag->i2c_trans.buf[5] = QMC5883L_REG_DATZM;
|
||||||
|
|
||||||
|
/* Chip transaction status, not used ATM in case of driver mishap once can considder using it */
|
||||||
|
mag->i2c_trans.buf[6] = QMC5883L_REG_STATUS;
|
||||||
|
// Add some code if you experience reading issue
|
||||||
|
// DRDY = ((mag->i2c_trans.buf[6]) >> 0) & 1;
|
||||||
|
// OVL = ((mag->i2c_trans.buf[6]) >> 1) & 1;
|
||||||
|
// DOR = ((uint8_t)(mag->i2c_trans.buf[6]) >> 2) & 1;
|
||||||
|
|
||||||
|
i2c_transceive(mag->i2c_p, &(mag->i2c_trans), mag->i2c_trans.slave_addr, 1, 7);
|
||||||
|
mag->status = QMC5883L_STATUS_MEAS;
|
||||||
|
}
|
||||||
|
/* Convert and align raw values */
|
||||||
|
#define Int16FromBuf(_buf,_idx) ((int16_t)(_buf[_idx] | (_buf[_idx+1] << 8)))
|
||||||
|
|
||||||
|
|
||||||
|
void qmc5883l_event(struct Qmc5883l *mag)
|
||||||
|
{
|
||||||
|
if (!mag->initialized) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
switch (mag->status) {
|
||||||
|
|
||||||
|
case QMC5883L_STATUS_MEAS:
|
||||||
|
if (mag->i2c_trans.status == I2CTransSuccess) {
|
||||||
|
mag->data.vect.x = Int16FromBuf(mag->i2c_trans.buf, 0);
|
||||||
|
mag->data.vect.y = Int16FromBuf(mag->i2c_trans.buf, 2);
|
||||||
|
mag->data.vect.z = Int16FromBuf(mag->i2c_trans.buf, 4);
|
||||||
|
|
||||||
|
/* only set available if measurements valid: -4096 if ADC under/overflow in sensor
|
||||||
|
The sensor sends out 12 bit wrapped in 16 bit, that sometimes gets corrupded */
|
||||||
|
if (mag->data.vect.x >= 4096 || mag->data.vect.y >= 4096 || mag->data.vect.z >= 4096) {
|
||||||
|
//mag->data.adc_overflow_cnt++;
|
||||||
|
mag->data_available = false;
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
mag->data_available = true;
|
||||||
|
}
|
||||||
|
/* End of measure reading, go back to idle */
|
||||||
|
mag->status = QMC5883L_STATUS_IDLE;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
default:
|
||||||
|
if (mag->i2c_trans.status == I2CTransSuccess || mag->i2c_trans.status == I2CTransFailed) {
|
||||||
|
/* Per default set to idle */
|
||||||
|
mag->i2c_trans.status = I2CTransDone;
|
||||||
|
mag->status = QMC5883L_STATUS_IDLE;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
@@ -0,0 +1,92 @@
|
|||||||
|
/*
|
||||||
|
* Copyright (C) 2022 Paparazzi Team
|
||||||
|
*
|
||||||
|
* This file is part of paparazzi.
|
||||||
|
*
|
||||||
|
* paparazzi is free software; you can redistribute it and/or modify
|
||||||
|
* it under the terms of the GNU General Public License as published by
|
||||||
|
* the Free Software Foundation; either version 2, or (at your option)
|
||||||
|
* any later version.
|
||||||
|
*
|
||||||
|
* paparazzi is distributed in the hope that it will be useful,
|
||||||
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||||
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||||
|
* GNU General Public License for more details.
|
||||||
|
*
|
||||||
|
* You should have received a copy of the GNU General Public License
|
||||||
|
* along with paparazzi; see the file COPYING. If not, see
|
||||||
|
* <http://www.gnu.org/licenses/>.
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file peripherals/qmc5883l.h
|
||||||
|
*
|
||||||
|
* QST QMC5883L 3-axis magnetometer driver interface (I2C).
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef QMC5883L_H
|
||||||
|
#define QMC5883L_H
|
||||||
|
|
||||||
|
#include "std.h"
|
||||||
|
#include "mcu_periph/i2c.h"
|
||||||
|
#include "math/pprz_algebra_int.h"
|
||||||
|
|
||||||
|
/**
|
||||||
|
* The default I2C 7bit address is 0D: 0001101. no option to change the address by e.g. bridging pins of the chip
|
||||||
|
* From the official datasheet "If other I2C address options are required, one must contact the chip factory"
|
||||||
|
* Just in cse one comes across an custom chip one can extend the ADDR option here
|
||||||
|
*/
|
||||||
|
#define QMC5883L_ADDR0 (0b00001101 << 1) //from 0x0D to 0x1A since 7bits and 1 for R/W setting
|
||||||
|
|
||||||
|
/* Continuous mode Output Data Rate (ODR) options in Hz */
|
||||||
|
#define QMC5883L_ODR_10 0x00
|
||||||
|
#define QMC5883L_ODR_50 0x04
|
||||||
|
#define QMC5883L_ODR_100 0x08
|
||||||
|
#define QMC5883L_ODR_200 0x0C
|
||||||
|
|
||||||
|
/* Default data rate */
|
||||||
|
#define QMC5883L_ODR_DEFAULT QMC5883L_ODR_200 //We are never interested in powersavings 200 is best
|
||||||
|
|
||||||
|
/* Status states */
|
||||||
|
enum Qmc5883lStatus {
|
||||||
|
QMC5883L_CONF_UNINIT, // Perform Initialization
|
||||||
|
QMC5883L_CONF_CCR_DONE, // Is done set CCR = Configure Continuous Rate done
|
||||||
|
QMC5883L_CONF_TMRC_DONE, // Is done set Datarate
|
||||||
|
QMC5883L_CONF_CCM_DONE, // Is done set Mode continuous or single CCM = Configure Continuous Measurement
|
||||||
|
QMC5883L_STATUS_IDLE, // Ready for data requests
|
||||||
|
QMC5883L_STATUS_MEAS // Get data
|
||||||
|
};
|
||||||
|
|
||||||
|
struct Qmc5883l {
|
||||||
|
struct i2c_periph *i2c_p; ///< peripheral used for communcation
|
||||||
|
struct i2c_transaction i2c_trans; ///< i2c transaction
|
||||||
|
uint8_t data_rate; ///< sensor data rate
|
||||||
|
bool initialized; ///< config done flag
|
||||||
|
enum Qmc5883lStatus status; ///< init status
|
||||||
|
volatile bool data_available; ///< data ready flag
|
||||||
|
union {
|
||||||
|
struct Int32Vect3 vect; ///< data vector in mag coordinate system
|
||||||
|
int32_t value[3]; ///< data values accessible by channel index
|
||||||
|
} data;
|
||||||
|
};
|
||||||
|
|
||||||
|
extern void qmc5883l_init(struct Qmc5883l *mag, struct i2c_periph *i2c_p, uint8_t addr, uint8_t data_rate);
|
||||||
|
extern void qmc5883l_configure(struct Qmc5883l *mag);
|
||||||
|
extern void qmc5883l_event(struct Qmc5883l *mag);
|
||||||
|
extern void qmc5883l_read(struct Qmc5883l *mag);
|
||||||
|
|
||||||
|
/// convenience function: read or start configuration if not already initialized
|
||||||
|
static inline void qmc5883l_periodic(struct Qmc5883l *mag)
|
||||||
|
{
|
||||||
|
if (mag->initialized) {
|
||||||
|
qmc5883l_read(mag);
|
||||||
|
} else {
|
||||||
|
qmc5883l_configure(mag);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif /* QMC5883L_H */
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
Reference in New Issue
Block a user