diff --git a/conf/modules/mag_lis3mdl.xml b/conf/modules/mag_lis3mdl.xml new file mode 100644 index 00000000000..f663d19893f --- /dev/null +++ b/conf/modules/mag_lis3mdl.xml @@ -0,0 +1,42 @@ + + + + + + ST LIS3MDL magnetometer. + + + + + + + + + + +
+ + + +
+ +
+ +
+ + + + + + + + + ifeq ($(MAG_LIS3MDL_I2C_DEV),) + $(error mag_lis3mdl module error: please configure MAG_LIS3MDL_I2C_DEV) + endif + + + + + + diff --git a/sw/airborne/modules/sensors/mag_lis3mdl.c b/sw/airborne/modules/sensors/mag_lis3mdl.c new file mode 100644 index 00000000000..4320592379c --- /dev/null +++ b/sw/airborne/modules/sensors/mag_lis3mdl.c @@ -0,0 +1,137 @@ +/* + * Copyright (C) 2019 Gautier Hattenberger + * + * 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_lis3mdl.c + * + * Module wrapper for ST LIS3MDL magnetometers. + */ + +#include "modules/sensors/mag_lis3mdl.h" +#include "mcu_periph/uart.h" +#include "pprzlink/messages.h" +#include "subsystems/datalink/downlink.h" +#include "generated/airframe.h" + +#ifndef LIS3MDL_CHAN_X +#define LIS3MDL_CHAN_X 0 +#endif +#ifndef LIS3MDL_CHAN_Y +#define LIS3MDL_CHAN_Y 1 +#endif +#ifndef LIS3MDL_CHAN_Z +#define LIS3MDL_CHAN_Z 2 +#endif +#ifndef LIS3MDL_CHAN_X_SIGN +#define LIS3MDL_CHAN_X_SIGN + +#endif +#ifndef LIS3MDL_CHAN_Y_SIGN +#define LIS3MDL_CHAN_Y_SIGN + +#endif +#ifndef LIS3MDL_CHAN_Z_SIGN +#define LIS3MDL_CHAN_Z_SIGN + +#endif + +#if MODULE_LIS3MDL_UPDATE_AHRS +#include "subsystems/imu.h" +#include "subsystems/abi.h" + +#if defined LIS3MDL_MAG_TO_IMU_PHI && defined LIS3MDL_MAG_TO_IMU_THETA && defined LIS3MDL_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 Lis3mdl mag_lis3mdl; + +void mag_lis3mdl_module_init(void) +{ + lis3mdl_init(&mag_lis3mdl, &(MAG_LIS3MDL_I2C_DEV), LIS3MDL_ADDR1, + LIS3MDL_DATA_RATE_80_HZ, + LIS3MDL_SCALE_4_GAUSS, + LIS3MDL_MODE_CONTINUOUS, + LIS3MDL_PERFORMANCE_ULTRA_HIGH); + +#if MODULE_LIS3MDL_UPDATE_AHRS && USE_MAG_TO_IMU + struct Int32Eulers mag_to_imu_eulers = { + ANGLE_BFP_OF_REAL(LIS3MDL_MAG_TO_IMU_PHI), + ANGLE_BFP_OF_REAL(LIS3MDL_MAG_TO_IMU_THETA), + ANGLE_BFP_OF_REAL(LIS3MDL_MAG_TO_IMU_PSI) + }; + int32_rmat_of_eulers(&mag_to_imu, &mag_to_imu_eulers); +#endif +} + +void mag_lis3mdl_module_periodic(void) +{ + lis3mdl_periodic(&mag_lis3mdl); +} + +void mag_lis3mdl_module_event(void) +{ + lis3mdl_event(&mag_lis3mdl); + + if (mag_lis3mdl.data_available) { +#if MODULE_LIS3MDL_UPDATE_AHRS + // current timestamp + uint32_t now_ts = get_sys_time_usec(); + + // set channel order + struct Int32Vect3 mag = { + LIS3MDL_CHAN_X_SIGN(int32_t)(mag_lis3mdl.data.value[LIS3MDL_CHAN_X]), + LIS3MDL_CHAN_Y_SIGN(int32_t)(mag_lis3mdl.data.value[LIS3MDL_CHAN_Y]), + LIS3MDL_CHAN_Z_SIGN(int32_t)(mag_lis3mdl.data.value[LIS3MDL_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(imu.mag_unscaled, imu_mag); +#else + // unscaled vector + VECT3_COPY(imu.mag_unscaled, mag); +#endif + // scale vector + imu_scale_mag(&imu); + + AbiSendMsgIMU_MAG_INT32(MAG_LIS3MDL_SENDER_ID, now_ts, &imu.mag); +#endif +#if MODULE_LIS3MDL_SYNC_SEND + mag_lis3mdl_report(); +#endif +#if MODULE_LIS3MDL_UPDATE_AHRS || MODULE_LIS3MDL_SYNC_SEND + mag_lis3mdl.data_available = false; +#endif + } +} + +void mag_lis3mdl_report(void) +{ + struct Int32Vect3 mag = { + LIS3MDL_CHAN_X_SIGN(int32_t)(mag_lis3mdl.data.value[LIS3MDL_CHAN_X]), + LIS3MDL_CHAN_Y_SIGN(int32_t)(mag_lis3mdl.data.value[LIS3MDL_CHAN_Y]), + LIS3MDL_CHAN_Z_SIGN(int32_t)(mag_lis3mdl.data.value[LIS3MDL_CHAN_Z]) + }; + DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice, &mag.x, &mag.y, &mag.z); +} diff --git a/sw/airborne/modules/sensors/mag_lis3mdl.h b/sw/airborne/modules/sensors/mag_lis3mdl.h new file mode 100644 index 00000000000..1edf80e531d --- /dev/null +++ b/sw/airborne/modules/sensors/mag_lis3mdl.h @@ -0,0 +1,39 @@ +/* + * Copyright (C) 2019 Gautier Hattenberger + * + * 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_lis3mdl.h + * + * Module wrapper for ST LIS3MDL magnetometers. + */ + +#ifndef MAG_LIS3MDL_H +#define MAG_LIS3MDL_H + +#include "peripherals/lis3mdl.h" + +extern struct Lis3mdl mag_lis3mdl; + +extern void mag_lis3mdl_module_init(void); +extern void mag_lis3mdl_module_periodic(void); +extern void mag_lis3mdl_module_event(void); +extern void mag_lis3mdl_report(void); + +#endif // MAG_LIS3MDL_H diff --git a/sw/airborne/peripherals/lis3mdl.c b/sw/airborne/peripherals/lis3mdl.c new file mode 100644 index 00000000000..4c5a344a6c6 --- /dev/null +++ b/sw/airborne/peripherals/lis3mdl.c @@ -0,0 +1,160 @@ +/* + * Copyright (C) 2019 Gautier Hattenberger + * + * 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/lis3mdl.c + * + * ST LIS3MDL 3-axis magnetometer driver interface (I2C). + */ + +#include "peripherals/lis3mdl.h" + +#define LIS3MDL_ENABLE_AUTO_INC (0x1<<7) + +#define LIS3MDL_STATUS_ZYXOR 0b10000000 +#define LIS3MDL_STATUS_ZOR 0b01000000 +#define LIS3MDL_STATUS_YOR 0b00100000 +#define LIS3MDL_STATUS_XOR 0b00010000 +#define LIS3MDL_STATUS_ZYXDA 0b00001000 +#define LIS3MDL_STATUS_ZDA 0b00000100 +#define LIS3MDL_STATUS_YDA 0b00000010 +#define LIS3MDL_STATUS_XDA 0b00000001 + +#define LIS3MDL_DEVICE_ID 0b00111101 + +#define LIS3MDL_REG_WHO_AM_I 0x0F +#define LIS3MDL_REG_CTL_1 0x20 +#define LIS3MDL_REG_CTL_2 0x21 +#define LIS3MDL_REG_CTL_3 0x22 +#define LIS3MDL_REG_CTL_4 0x23 +#define LIS3MDL_REG_STATUS 0x27 +#define LIS3MDL_REG_OUT_X_L 0x28 +#define LIS3MDL_REG_OUT_X_H 0x29 +#define LIS3MDL_REG_OUT_Y_L 0x2A +#define LIS3MDL_REG_OUT_Y_H 0x2B +#define LIS3MDL_REG_OUT_Z_L 0x2C +#define LIS3MDL_REG_OUT_Z_H 0x2D +#define LIS3MDL_REG_OUT_TEMP_L 0x2E +#define LIS3MDL_REG_OUT_TEMP_H 0x2F + +#define LIS3MDL_REG_CTL_1_TEMP_EN 0b10000000 +#define LIS3MDL_REG_CTL_2_RESET 0b00000100 + +void lis3mdl_init(struct Lis3mdl *mag, struct i2c_periph *i2c_p, uint8_t addr, uint8_t data_rate, uint8_t scale, uint8_t mode, uint8_t perf) +{ + /* set i2c_peripheral */ + mag->i2c_p = i2c_p; + /* set i2c address */ + mag->i2c_trans.slave_addr = addr; + mag->i2c_trans.status = I2CTransDone; + + /* prepare config request */ + mag->i2c_trans.buf[0] = LIS3MDL_REG_CTL_1 | LIS3MDL_ENABLE_AUTO_INC; + mag->i2c_trans.buf[1] = (data_rate << 2) | (perf << 5); + mag->i2c_trans.buf[2] = (scale << 5); + mag->i2c_trans.buf[3] = mode; + mag->i2c_trans.buf[4] = (perf << 2); + mag->i2c_trans.buf[5] = 0; + + mag->initialized = false; + mag->status = LIS3MDL_CONF_UNINIT; + mag->data_available = false; +} + +// Configure +void lis3mdl_configure(struct Lis3mdl *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 succesfull continue with next + if (mag->i2c_trans.status == I2CTransSuccess) { + mag->status++; + } + + mag->i2c_trans.status = I2CTransDone; + switch (mag->status) { + + case LIS3MDL_CONF_UNINIT: + // send config request + i2c_transmit(mag->i2c_p, &(mag->i2c_trans), mag->i2c_trans.slave_addr, 6); + break; + + case LIS3MDL_CONF_REG: + mag->status = LIS3MDL_STATUS_IDLE; + mag->initialized = true; + break; + + default: + break; + } +} + +void lis3mdl_read(struct Lis3mdl *mag) +{ + if (mag->status != LIS3MDL_STATUS_IDLE) { + return; + } + + mag->i2c_trans.buf[0] = LIS3MDL_REG_STATUS | LIS3MDL_ENABLE_AUTO_INC; + i2c_transceive(mag->i2c_p, &(mag->i2c_trans), mag->i2c_trans.slave_addr, 1, 7); + mag->status = LIS3MDL_STATUS_MEAS; +} + +// Get raw value +#define Int16FromBuf(_buf,_idx) ((int16_t)(_buf[_idx] | (_buf[_idx+1] << 8))) + +void lis3mdl_event(struct Lis3mdl *mag) +{ + if (!mag->initialized) { + return; + } + + switch (mag->status) { + + case LIS3MDL_STATUS_MEAS: + if (mag->i2c_trans.status == I2CTransSuccess) { + // Read status and error bytes + const bool dr = mag->i2c_trans.buf[0] & LIS3MDL_STATUS_ZYXDA; // data ready + if (dr > 0) { + // Copy the data + mag->data.vect.x = Int16FromBuf(mag->i2c_trans.buf, 1); + mag->data.vect.y = Int16FromBuf(mag->i2c_trans.buf, 3); + mag->data.vect.z = Int16FromBuf(mag->i2c_trans.buf, 5); + mag->data_available = true; + } + // End reading, back to idle + mag->status = LIS3MDL_STATUS_IDLE; + } + break; + + default: + if (mag->i2c_trans.status == I2CTransSuccess || mag->i2c_trans.status == I2CTransFailed) { + // Goto idle + mag->i2c_trans.status = I2CTransDone; + mag->status = LIS3MDL_STATUS_IDLE; + } + break; + } +} + diff --git a/sw/airborne/peripherals/lis3mdl.h b/sw/airborne/peripherals/lis3mdl.h new file mode 100644 index 00000000000..b3101469956 --- /dev/null +++ b/sw/airborne/peripherals/lis3mdl.h @@ -0,0 +1,97 @@ +/* + * Copyright (C) 2019 Gautier Hattenberger + * + * 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/lis3mdl.h + * + * ST LIS3MDL 3-axis magnetometer driver interface (I2C). + */ + +#ifndef LIS3MDL_H +#define LIS3MDL_H + +#include "std.h" +#include "mcu_periph/i2c.h" +#include "math/pprz_algebra_int.h" + +#define LIS3MDL_ADDR1 (0b0011110 << 1) +#define LIS3MDL_ADDR2 (0b0011100 << 1) + +#define LIS3MDL_PERFORMANCE_LOW_POWER 0b00 +#define LIS3MDL_PERFORMANCE_MEDIUM 0b01 +#define LIS3MDL_PERFORMANCE_HIGH 0b10 +#define LIS3MDL_PERFORMANCE_ULTRA_HIGH 0b11 + +#define LIS3MDL_DATA_RATE_0_625_HZ 0b000 +#define LIS3MDL_DATA_RATE_1_25_HZ 0b001 +#define LIS3MDL_DATA_RATE_2_5_HZ 0b010 +#define LIS3MDL_DATA_RATE_5_HZ 0b011 +#define LIS3MDL_DATA_RATE_10_HZ 0b100 +#define LIS3MDL_DATA_RATE_20_HZ 0b101 +#define LIS3MDL_DATA_RATE_40_HZ 0b110 +#define LIS3MDL_DATA_RATE_80_HZ 0b111 + +#define LIS3MDL_MODE_CONTINUOUS 0b00 +#define LIS3MDL_MODE_SINGLE 0b01 +#define LIS3MDL_MODE_POWER_DOWN 0b11 + +#define LIS3MDL_SCALE_4_GAUSS 0b00 +#define LIS3MDL_SCALE_8_GAUSS 0b01 +#define LIS3MDL_SCALE_12_GAUSS 0b10 +#define LIS3MDL_SCALE_16_GAUSS 0b11 + +/** config status states */ +enum Lis3mdlStatus { + LIS3MDL_CONF_UNINIT, + LIS3MDL_CONF_REG, + LIS3MDL_STATUS_IDLE, + LIS3MDL_STATUS_MEAS +}; + +// main structure +struct Lis3mdl { + struct i2c_periph *i2c_p; ///< peripheral used for communcation + struct i2c_transaction i2c_trans; ///< i2c transaction + bool initialized; ///< config done flag + enum Lis3mdlStatus status; ///< init status + volatile bool data_available; ///< data ready flag + union { + struct Int16Vect3 vect; ///< data vector in mag coordinate system + int16_t value[3]; ///< data values accessible by channel index + } data; +}; + +// Functions +extern void lis3mdl_init(struct Lis3mdl *mag, struct i2c_periph *i2c_p, uint8_t addr, uint8_t data_rate, uint8_t scale, uint8_t mode, uint8_t perf); +extern void lis3mdl_configure(struct Lis3mdl *mag); +extern void lis3mdl_event(struct Lis3mdl *mag); +extern void lis3mdl_read(struct Lis3mdl *mag); + +/// convenience function: read or start configuration if not already initialized +static inline void lis3mdl_periodic(struct Lis3mdl *mag) +{ + if (mag->initialized) { + lis3mdl_read(mag); + } else { + lis3mdl_configure(mag); + } +} + +#endif /* LIS3MDL_H */ diff --git a/sw/airborne/subsystems/abi_sender_ids.h b/sw/airborne/subsystems/abi_sender_ids.h index 9720427f09f..beef38c2216 100644 --- a/sw/airborne/subsystems/abi_sender_ids.h +++ b/sw/airborne/subsystems/abi_sender_ids.h @@ -165,6 +165,10 @@ #define MAG_HMC58XX_SENDER_ID 2 #endif +#ifndef MAG_LIS3MDL_SENDER_ID +#define MAG_LIS3MDL_SENDER_ID 3 +#endif + #ifndef IMU_MAG_PITOT_ID #define IMU_MAG_PITOT_ID 50 #endif