Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-30 19:47:50 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

Added driver for QMC5883L Magnetometer (#2953)

Browse Source 
Co-authored-by: Open UAS <noreply@openuas.org>
This commit is contained in:
OpenUAS
2022-11-28 22:09:37 +01:00
committed by GitHub
parent 6a44445dd2
commit 6df663ff53
8 changed files with 555 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/boards/lisa_mxs/chibios/v1.0/board.h
+6 -6
View File
@@ -200,8 +200,8 @@
* PB3 - Digital input (JTAG_TDO/SWD)
* PB4 - Open Drain output 50MHz (LED2)
* PB5 - Digital input (IMU_MAG_DRDY)
* PB6 - Alternate Push Pull output 50MHz (SERVO7-Timer4Ch1)/USART1_TX
* PB7 - Alternate Push Pull output 50MHz (SERVO8-Timer4Ch2)/USART1_RX
* PB6 - Alternate Open Drain output 2MHz (SERVO7-Timer4Ch1)/USART1_TX
* PB7 - Alternate Open Drain output 2MHz (SERVO8-Timer4Ch2)/USART1_RX
* PB8 - Digital input. (CAN_RX)
* PB9 - Open Drain output 50MHz. (CAN_TX)
* PB10 - Alternate Open Drain output 2MHz.(I2C2_SCL)
@@ -233,8 +233,8 @@
PIN_OTYPE_PUSHPULL(3) | \
PIN_OTYPE_OPENDRAIN(4) | \
PIN_OTYPE_PUSHPULL(5) | \
PIN_OTYPE_PUSHPULL(6) | \
PIN_OTYPE_PUSHPULL(7) | \
PIN_OTYPE_OPENDRAIN(6) | \
PIN_OTYPE_OPENDRAIN(7) | \
PIN_OTYPE_PUSHPULL(8) | \
PIN_OTYPE_OPENDRAIN(9) | \
PIN_OTYPE_OPENDRAIN(10) | \
@@ -297,8 +297,8 @@
PIN_AFIO_AF(3, 0) | \
PIN_AFIO_AF(4, 0) | \
PIN_AFIO_AF(5, 0) | \
PIN_AFIO_AF(6, 7) | \
PIN_AFIO_AF(7, 7))
PIN_AFIO_AF(6, 4) | \
PIN_AFIO_AF(7, 4))
#define VAL_GPIOB_AFRH (PIN_AFIO_AF(8, 9) | \
PIN_AFIO_AF(9, 9) | \
PIN_AFIO_AF(10, 4) | \
sw/airborne/modules/core/abi_sender_ids.h
+4
View File
@@ -217,6 +217,10 @@
#define MAG_DATALINK_SENDER_ID 6
#endif
#ifndef MAG_QMC5883L_SENDER_ID
#define MAG_QMC5883L_SENDER_ID 7
#endif
#ifndef IMU_MAG_PITOT_ID
#define IMU_MAG_PITOT_ID 50
#endif
sw/airborne/modules/sensors/mag_qmc5883l.c
+136
View File
@@ -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);
}
sw/airborne/modules/sensors/mag_qmc5883l.h
+42
View File
@@ -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 */
sw/airborne/peripherals/qmc5883l.c
+191
View File
@@ -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;
}
}
sw/airborne/peripherals/qmc5883l.h
+92
View File
@@ -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 */