diff --git a/conf/airframes/OPENUAS/openuas_wltech_xk_a160.xml b/conf/airframes/OPENUAS/openuas_wltech_xk_a160.xml index 734c20b123..68040ebe55 100644 --- a/conf/airframes/OPENUAS/openuas_wltech_xk_a160.xml +++ b/conf/airframes/OPENUAS/openuas_wltech_xk_a160.xml @@ -37,7 +37,7 @@ NOTES: - + @@ -58,11 +58,11 @@ NOTES: - + - diff --git a/conf/modules/mag_qmc5883l.xml b/conf/modules/mag_qmc5883l.xml new file mode 100644 index 0000000000..0b8426b8c6 --- /dev/null +++ b/conf/modules/mag_qmc5883l.xml @@ -0,0 +1,70 @@ + + + + + + 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. + + + + + + + + + + +
+ + + + +
+ + + i2c,@imu + mag + +
+ +
+ + + + + + + + + ifeq ($(MAG_QMC5883L_I2C_DEV),) + $(error mag_qmc5883l module error: please configure MAG_QMC5883L_I2C_DEV) + endif + + + + + + + + + + + + + + + diff --git a/sw/airborne/boards/lisa_mxs/chibios/v1.0/board.h b/sw/airborne/boards/lisa_mxs/chibios/v1.0/board.h index 8ad7d6ef67..b57d039e00 100644 --- a/sw/airborne/boards/lisa_mxs/chibios/v1.0/board.h +++ b/sw/airborne/boards/lisa_mxs/chibios/v1.0/board.h @@ -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) | \ diff --git a/sw/airborne/modules/core/abi_sender_ids.h b/sw/airborne/modules/core/abi_sender_ids.h index c0910847f1..17ba007404 100644 --- a/sw/airborne/modules/core/abi_sender_ids.h +++ b/sw/airborne/modules/core/abi_sender_ids.h @@ -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 diff --git a/sw/airborne/modules/sensors/mag_qmc5883l.c b/sw/airborne/modules/sensors/mag_qmc5883l.c new file mode 100644 index 0000000000..662f1533ff --- /dev/null +++ b/sw/airborne/modules/sensors/mag_qmc5883l.c @@ -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 + * . + */ + +/** + * @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); +} diff --git a/sw/airborne/modules/sensors/mag_qmc5883l.h b/sw/airborne/modules/sensors/mag_qmc5883l.h new file mode 100644 index 0000000000..43082b6898 --- /dev/null +++ b/sw/airborne/modules/sensors/mag_qmc5883l.h @@ -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 + * . + */ + +/** + * @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 */ + + + diff --git a/sw/airborne/peripherals/qmc5883l.c b/sw/airborne/peripherals/qmc5883l.c new file mode 100644 index 0000000000..8de97ddbb9 --- /dev/null +++ b/sw/airborne/peripherals/qmc5883l.c @@ -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 + * . + */ + +/** + * @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; + } +} + diff --git a/sw/airborne/peripherals/qmc5883l.h b/sw/airborne/peripherals/qmc5883l.h new file mode 100644 index 0000000000..c2dccc6101 --- /dev/null +++ b/sw/airborne/peripherals/qmc5883l.h @@ -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 + * . + */ + +/** + * @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 */ + + + +