[mag] add lis3mdl driver and module

conf/modules/mag_lis3mdl.xml

@@ -0,0 +1,42 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="mag_lis3mdl" dir="sensors">
+  <doc>
+    <description>
+      ST LIS3MDL magnetometer.
+    </description>
+    <configure name="MAG_LIS3MDL_I2C_DEV" value="i2c1" description="I2C device to use (e.g. i2c1)"/>
+    <define name="MODULE_LIS3MDL_SYNC_SEND" value="TRUE|FALSE" description="Send IMU_RAW message with each new measurement (default: FALSE)"/>
+    <define name="MODULE_LIS3MDL_UPDATE_AHRS" value="TRUE|FALSE" description="Copy measurements to imu and send as ABI message (default: FALSE)"/>
+    <define name="LIS3MDL_CHAN_X_SIGN" value="+|-" description="Reverse polarity of x axis (default: +)"/>
+    <define name="LIS3MDL_CHAN_Y_SIGN" value="+|-" description="Reverse polarity of y axis (default: +)"/>
+    <define name="LIS3MDL_CHAN_Z_SIGN" value="+|-" description="Reverse polarity of z axis (default: +)"/>
+    <define name="LIS3MDL_CHAN_X" value="0|1|2" description="Channel id of x axis (default: 0)"/>
+    <define name="LIS3MDL_CHAN_Y" value="0|1|2" description="Channel id of y axis (default: 1)"/>
+    <define name="LIS3MDL_CHAN_Z" value="0|1|2" description="Channel id of z axis (default: 2)"/>
+    <section name="MAG_LIS3MDL" prefix="LIS3MDL_">
+      <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)"/>
+    </section>
+  </doc>
+  <header>
+    <file name="mag_lis3mdl.h"/>
+  </header>
+  <init fun="mag_lis3mdl_module_init()"/>
+  <periodic fun="mag_lis3mdl_module_periodic()" freq="60"/>
+  <periodic fun="mag_lis3mdl_report()" freq="10" autorun="FALSE"/>
+  <event fun="mag_lis3mdl_module_event()"/>
+  <makefile target="ap">
+    <file name="mag_lis3mdl.c"/>
+    <file name="lis3mdl.c" dir="peripherals"/>
+    <raw>
+      ifeq ($(MAG_LIS3MDL_I2C_DEV),)
+        $(error mag_lis3mdl module error: please configure MAG_LIS3MDL_I2C_DEV)
+      endif
+    </raw>
+    <configure name="MAG_LIS3MDL_I2C_DEV" default="TRUE" case="upper|lower"/>
+    <define name="USE_$(MAG_LIS3MDL_I2C_DEV_UPPER)"/>
+    <define name="MAG_LIS3MDL_I2C_DEV" value="$(MAG_LIS3MDL_I2C_DEV_LOWER)"/>
+  </makefile>
+</module>

sw/airborne/modules/sensors/mag_lis3mdl.c

@@ -0,0 +1,137 @@
+/*
+ * Copyright (C) 2019 Gautier Hattenberger <gautier.hattenberger@enac.fr>
+ *
+ * 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_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);
+}

sw/airborne/modules/sensors/mag_lis3mdl.h

@@ -0,0 +1,39 @@
+/*
+ * Copyright (C) 2019 Gautier Hattenberger <gautier.hattenberger@enac.fr>
+ *
+ * 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_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

sw/airborne/peripherals/lis3mdl.c

@@ -0,0 +1,160 @@
+/*
+ * Copyright (C) 2019 Gautier Hattenberger <gautier.hattenberger@enac.fr>
+ *
+ * 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/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;
+  }
+}
+