-
Notifications
You must be signed in to change notification settings - Fork 1.1k
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
1 parent
fcfac87
commit 0f1bd11
Showing
6 changed files
with
479 additions
and
0 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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="i2cX" description="I2C device to use"/> | ||
<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" 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> |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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); | ||
} |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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; | ||
} | ||
} | ||
|
Oops, something went wrong.