mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-22 04:13:39 +08:00
LSM6DS33 (gyro/accel) and LPS25H (baro) drivers (#2437)
This commit is contained in:
Alexis Cornard
committed by
Gautier Hattenberger
Gautier Hattenberger
parent
0391b4df26
commit
281116f797
@@ -0,0 +1,52 @@
|
||||
/*
|
||||
* Copyright (C) 2019 Alexis Cornard <alexiscornard@gmail.com>
|
||||
*
|
||||
* 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, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*/
|
||||
/**
|
||||
* @file peripherals/lps25h.h
|
||||
*
|
||||
* LPS25H barometer driver interface.
|
||||
*/
|
||||
|
||||
#ifndef LPS25H_H
|
||||
#define LPS25H_H
|
||||
|
||||
|
||||
#include "std.h"
|
||||
|
||||
/* Include address and register definition */
|
||||
#include "peripherals/lps25h_regs.h"
|
||||
|
||||
enum Lps25hConfStatus {
|
||||
LPS25H_CONF_UNINIT,
|
||||
LPS25H_CONF_CTRL1,
|
||||
LPS25H_CONF_DONE
|
||||
};
|
||||
|
||||
struct Lps25hConfig {
|
||||
uint8_t ctrl1;
|
||||
};
|
||||
|
||||
static inline void lps25h_set_default_config(struct Lps25hConfig *c)
|
||||
{
|
||||
c->ctrl1 = 0xB0;
|
||||
}
|
||||
|
||||
|
||||
#endif // LPS25H_H
|
||||
@@ -0,0 +1,135 @@
|
||||
/*
|
||||
* Copyright (C) 2019 Alexis Cornard <alexiscornard@gmail.com>
|
||||
*
|
||||
* 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 publpshed 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, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file peripherals/lps25h_i2c.c
|
||||
*
|
||||
* Driver for LPS25H barometer I2C.
|
||||
*/
|
||||
|
||||
#include "peripherals/lps25h_i2c.h"
|
||||
#include "std.h"
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
|
||||
void lps25h_i2c_init(struct Lps25h_I2c *lps, struct i2c_periph *i2c_p, uint8_t addr)
|
||||
{
|
||||
/* set i2c_peripheral */
|
||||
lps->i2c_p = i2c_p;
|
||||
/* set i2c address */
|
||||
lps->i2c_trans.slave_addr = addr;
|
||||
lps->i2c_trans.status = I2CTransDone;
|
||||
/* set default config options */
|
||||
lps25h_set_default_config(&(lps->config));
|
||||
lps->initialized = false;
|
||||
lps->data_available = false;
|
||||
lps->init_status = LPS25H_CONF_UNINIT;
|
||||
}
|
||||
|
||||
|
||||
static void lps25h_i2c_tx_reg(struct Lps25h_I2c *lps, uint8_t reg, uint8_t val)
|
||||
{
|
||||
lps->i2c_trans.type = I2CTransTx;
|
||||
lps->i2c_trans.buf[0] = reg;
|
||||
lps->i2c_trans.buf[1] = val;
|
||||
lps->i2c_trans.len_r = 0;
|
||||
lps->i2c_trans.len_w = 2;
|
||||
i2c_submit(lps->i2c_p, &(lps->i2c_trans));
|
||||
}
|
||||
|
||||
// Configuration function called once before normal use
|
||||
static void lps25h_i2c_send_config(struct Lps25h_I2c *lps)
|
||||
{
|
||||
switch (lps->init_status) {
|
||||
case LPS25H_CONF_CTRL1:
|
||||
lps25h_i2c_tx_reg(lps, LPS25H_CTRL_REG1, lps->config.ctrl1);
|
||||
lps->init_status++;
|
||||
break;
|
||||
case LPS25H_CONF_DONE:
|
||||
lps->initialized = true;
|
||||
lps->i2c_trans.status = I2CTransDone;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// Start configuration if not already done
|
||||
void lps25h_i2c_start_configure(struct Lps25h_I2c *lps)
|
||||
{
|
||||
if (lps->init_status == LPS25H_CONF_UNINIT) {
|
||||
lps->init_status++;
|
||||
if (lps->i2c_trans.status == I2CTransSuccess || lps->i2c_trans.status == I2CTransDone) {
|
||||
lps25h_i2c_send_config(lps);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Normal reading
|
||||
void lps25h_i2c_read(struct Lps25h_I2c *lps)
|
||||
{
|
||||
if (lps->initialized && lps->i2c_trans.status == I2CTransDone) {
|
||||
lps->i2c_trans.buf[0] = LPS25H_REG_OUT_XL | (1 << 7);
|
||||
lps->i2c_trans.type = I2CTransTxRx;
|
||||
lps->i2c_trans.len_r = 3;
|
||||
lps->i2c_trans.len_w = 1;
|
||||
i2c_submit(lps->i2c_p, &(lps->i2c_trans));
|
||||
}
|
||||
}
|
||||
|
||||
#define Int32FromBuf(buf, idx) (int32_t)(int8_t)buf[idx+2] << 16 | (uint16_t)buf[idx+1] << 8 | buf[idx];
|
||||
|
||||
// The two following functions can be used to check data
|
||||
float lps25h_readPressureMillibars(int32_t press)
|
||||
{
|
||||
return (float)press / 4096;
|
||||
}
|
||||
|
||||
float pressureToAltMeters(float pressure_mbar, float altimeter_setting_mbar){
|
||||
return (1-pow((pressure_mbar/altimeter_setting_mbar), 0.190263)) * 4430.8;
|
||||
}
|
||||
|
||||
void lps25h_i2c_event(struct Lps25h_I2c *lps)
|
||||
{
|
||||
if (lps->initialized) {
|
||||
if (lps->i2c_trans.status == I2CTransFailed) {
|
||||
lps->i2c_trans.status = I2CTransDone;
|
||||
} else if (lps->i2c_trans.status == I2CTransSuccess) {
|
||||
lps->data = Int32FromBuf(lps->i2c_trans.buf, 0)
|
||||
lps->data_available = true;
|
||||
lps->i2c_trans.status = I2CTransDone;
|
||||
}
|
||||
} else if (lps->init_status != LPS25H_CONF_UNINIT) { // Configuring but not yet initialized
|
||||
switch(lps->i2c_trans.status){
|
||||
case I2CTransFailed:
|
||||
lps->init_status--;
|
||||
case I2CTransSuccess:
|
||||
case I2CTransDone:
|
||||
lps25h_i2c_send_config(lps);
|
||||
if(lps->initialized)
|
||||
lps->i2c_trans.status = I2CTransDone;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,66 @@
|
||||
/*
|
||||
* Copyright (C) 2019 Alexis Cornard <alexiscornard@gmail.com>
|
||||
*
|
||||
* 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, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file peripherals/lps25h_i2c.h
|
||||
*
|
||||
* I2C interface for LPS25H barometer.
|
||||
*/
|
||||
|
||||
#ifndef LPS25H_I2C_H
|
||||
#define LPS25H_I2C_H
|
||||
|
||||
#include "std.h"
|
||||
#include "math/pprz_algebra_int.h"
|
||||
#include "mcu_periph/i2c.h"
|
||||
|
||||
/* Include common LPS25H options and definitions */
|
||||
#include "peripherals/lps25h.h"
|
||||
|
||||
struct Lps25h_I2c {
|
||||
struct i2c_periph *i2c_p;
|
||||
struct i2c_transaction i2c_trans;
|
||||
enum Lps25hConfStatus init_status; ///< init status
|
||||
bool initialized; ///< config done flag
|
||||
volatile bool data_available; ///< data ready flag
|
||||
int32_t data;
|
||||
struct Lps25hConfig config;
|
||||
};
|
||||
|
||||
// Functions
|
||||
extern void lps25h_i2c_init(struct Lps25h_I2c *lps, struct i2c_periph *i2c_p, uint8_t addr);
|
||||
extern void lps25h_i2c_start_configure(struct Lps25h_I2c *lps);
|
||||
extern void lps25h_i2c_read(struct Lps25h_I2c *lps);
|
||||
extern void lps25h_i2c_event(struct Lps25h_I2c *lps);
|
||||
extern float lps25h_readPressureMillibars(int32_t press);
|
||||
extern float pressureToAltMeters(float pressure_mbar, float altimeter_setting_mbar);
|
||||
|
||||
// convenience function: read or start configuration if not already initialized
|
||||
static inline void lps25h_i2c_periodic(struct Lps25h_I2c *lps)
|
||||
{
|
||||
if (lps->initialized) {
|
||||
lps25h_i2c_read(lps);
|
||||
} else {
|
||||
lps25h_i2c_start_configure(lps);
|
||||
}
|
||||
}
|
||||
|
||||
#endif // LPS25H_I2C_H
|
||||
@@ -0,0 +1,41 @@
|
||||
/*
|
||||
* Copyright (C) 2019 Alexis Cornard <alexiscornard@gmail.com>
|
||||
*
|
||||
* 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, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*/
|
||||
/**
|
||||
* @file peripherals/lp25h_regs.h
|
||||
* Register defs for ST LPS25H barometer.
|
||||
*
|
||||
* Has an I2C and SPI interface.
|
||||
*/
|
||||
|
||||
#ifndef LPS25H_REGS_H
|
||||
#define LPS25H_REGS_H
|
||||
|
||||
// I2C Address
|
||||
#define LPS25H_ADDR 0xBA
|
||||
|
||||
/* Registers */
|
||||
#define LPS25H_CTRL_REG1 0x20
|
||||
|
||||
#define LPS25H_REG_OUT_XL 0x28
|
||||
#define LPS25H_REG_OUT_L 0x29
|
||||
#define LPS25H_REG_OUT_H 0x30
|
||||
|
||||
#endif // LPS25H_REGS_H
|
||||
@@ -0,0 +1,128 @@
|
||||
/*
|
||||
* Copyright (C) 2019 Alexis Cornard <alexiscornard@gmail.com>
|
||||
*
|
||||
* 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, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*/
|
||||
/**
|
||||
* @file peripherals/lsm6ds33.h
|
||||
*
|
||||
* LSM6DS33 accelerometer and gyrometer driver I2C interface.
|
||||
*/
|
||||
|
||||
#ifndef LSM6_H
|
||||
#define LSM6_H
|
||||
|
||||
#ifndef LSM6_XL_DEFAULT_ODR
|
||||
#define LSM6_XL_DEFAULT_ODR (LSM6_ODR_XL_1_6KHZ << 4)
|
||||
#endif
|
||||
|
||||
#ifndef LSM6_XL_DEFAULT_FS
|
||||
#define LSM6_XL_DEFAULT_FS (LSM6_FS_XL_16G << 2)
|
||||
#endif
|
||||
|
||||
|
||||
#ifndef LSM6_G_DEFAULT_ODR
|
||||
#define LSM6_G_DEFAULT_ODR (LSM6_ODR_G_1_6KHZ << 4)
|
||||
#endif
|
||||
|
||||
#ifndef LSM6_G_DEFAULT_FS
|
||||
#define LSM6_G_DEFAULT_FS (LSM6_FS_G_1000 << 2)
|
||||
#endif
|
||||
|
||||
#ifndef LSM6_C_DEFAULT
|
||||
#define LSM6_C_DEFAULT (0x04)
|
||||
#endif
|
||||
|
||||
#ifndef LSM6_ORIENT
|
||||
#define LSM6_ORIENT (0x02)
|
||||
#endif
|
||||
|
||||
#ifndef GYRO_SENS_H
|
||||
#define GYRO_SENS_H
|
||||
/** default gyro sensitivy from the datasheet
|
||||
* sens = [(deg/s)/LSB] * pi/180 * 2^INT32_RATE_FRAC
|
||||
* ex: LSM6 with 1000 deg/s has 0.035 (deg/s)/LSB
|
||||
* sens = 0.035 * pi/180 * 4096 = 2.5021
|
||||
*/
|
||||
|
||||
#define LSM6_GYRO_SENS_245 0.3127
|
||||
#define LSM6_GYRO_SENS_245_NUM 3127
|
||||
#define LSM6_GYRO_SENS_245_DEN 1000
|
||||
#define LSM6_GYRO_SENS_500 0.6255
|
||||
#define LSM6_GYRO_SENS_500_NUM 6255
|
||||
#define LSM6_GYRO_SENS_500_DEN 1000
|
||||
#define LSM6_GYRO_SENS_1000 2.5021
|
||||
#define LSM6_GYRO_SENS_1000_NUM 25021
|
||||
#define LSM6_GYRO_SENS_1000_DEN 1000
|
||||
#define LSM6_GYRO_SENS_2000 5.0042
|
||||
#define LSM6_GYRO_SENS_2000_NUM 50042
|
||||
#define LSM6_GYRO_SENS_2000_DEN 1000
|
||||
|
||||
#endif
|
||||
|
||||
#ifndef ACCEL_SENS_H
|
||||
#define ACCEL_SENS_H
|
||||
|
||||
#define LSM6_ACCEL_SENS_2G 0.613125
|
||||
#define LSM6_ACCEL_SENS_2G_NUM 981
|
||||
#define LSM6_ACCEL_SENS_2G_DEN 1600
|
||||
#define LSM6_ACCEL_SENS_4G 1.22625
|
||||
#define LSM6_ACCEL_SENS_4G_NUM 981
|
||||
#define LSM6_ACCEL_SENS_4G_DEN 800
|
||||
#define LSM6_ACCEL_SENS_8G 2.4525
|
||||
#define LSM6_ACCEL_SENS_8G_NUM 981
|
||||
#define LSM6_ACCEL_SENS_8G_DEN 400
|
||||
#define LSM6_ACCEL_SENS_16G 4.905
|
||||
#define LSM6_ACCEL_SENS_16G_NUM 981
|
||||
#define LSM6_ACCEL_SENS_16G_DEN 200
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
#include "std.h"
|
||||
|
||||
/* Include address and register definition */
|
||||
#include "peripherals/lsm6ds33_regs.h"
|
||||
|
||||
enum Lsm6ConfStatus {
|
||||
LSM6_CONF_UNINIT,
|
||||
LSM6_CONF_CTRL1_XL,
|
||||
LSM6_CONF_CTRL2_G,
|
||||
LSM6_CONF_CTRL3_C,
|
||||
LSM6_CONF_CTRL3_ORIENT,
|
||||
LSM6_CONF_DONE
|
||||
};
|
||||
|
||||
struct Lsm6Config {
|
||||
uint8_t xl; // Accelerometer configuration
|
||||
uint8_t g; // Gyroscope configuration
|
||||
uint8_t c; // Common configuration
|
||||
uint8_t orient; // Axes orientation
|
||||
|
||||
};
|
||||
|
||||
static inline void lsm6_set_default_config(struct Lsm6Config *c)
|
||||
{
|
||||
c->xl = LSM6_XL_DEFAULT_ODR | LSM6_XL_DEFAULT_FS;
|
||||
c->g = LSM6_G_DEFAULT_ODR | LSM6_G_DEFAULT_FS;
|
||||
c->c = LSM6_C_DEFAULT;
|
||||
c->orient = LSM6_ORIENT;
|
||||
}
|
||||
|
||||
|
||||
#endif // LSM6_H
|
||||
@@ -0,0 +1,145 @@
|
||||
/*
|
||||
* Copyright (C) 2019 Alexis Cornard <alexiscornard@gmail.com>
|
||||
*
|
||||
* 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, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file peripherals/lsm6ds33_i2c.c
|
||||
*
|
||||
* Driver for LSM6DS33 accelerometer and gyrometer using I2C.
|
||||
*/
|
||||
|
||||
#include "peripherals/lsm6ds33_i2c.h"
|
||||
#include "std.h"
|
||||
#include <stdio.h>
|
||||
|
||||
|
||||
void lsm6_i2c_init(struct Lsm6_I2c *lsm, struct i2c_periph *i2c_p, uint8_t addr)
|
||||
{
|
||||
/* set i2c_peripheral */
|
||||
lsm->i2c_p = i2c_p;
|
||||
/* set i2c address */
|
||||
lsm->i2c_trans.slave_addr = addr;
|
||||
lsm->i2c_trans.status = I2CTransDone;
|
||||
/* set default config options */
|
||||
lsm6_set_default_config(&(lsm->config));
|
||||
lsm->initialized = false;
|
||||
lsm->data_available = false;
|
||||
lsm->init_status = LSM6_CONF_UNINIT;
|
||||
}
|
||||
|
||||
|
||||
static void lsm6_i2c_tx_reg(struct Lsm6_I2c *lsm, uint8_t reg, uint8_t val)
|
||||
{
|
||||
lsm->i2c_trans.type = I2CTransTx;
|
||||
lsm->i2c_trans.buf[0] = reg;
|
||||
lsm->i2c_trans.buf[1] = val;
|
||||
lsm->i2c_trans.len_r = 0;
|
||||
lsm->i2c_trans.len_w = 2;
|
||||
i2c_submit(lsm->i2c_p, &(lsm->i2c_trans));
|
||||
}
|
||||
|
||||
// Configuration function called once before normal use
|
||||
static void lsm6_i2c_send_config(struct Lsm6_I2c *lsm)
|
||||
{
|
||||
switch (lsm->init_status) {
|
||||
case LSM6_CONF_CTRL1_XL:
|
||||
lsm6_i2c_tx_reg(lsm, LSM6_REG_CTRL1_XL, lsm->config.xl);
|
||||
lsm->init_status++;
|
||||
break;
|
||||
case LSM6_CONF_CTRL2_G:
|
||||
lsm6_i2c_tx_reg(lsm, LSM6_REG_CTRL2_G, lsm->config.g);
|
||||
lsm->init_status++;
|
||||
break;
|
||||
case LSM6_CONF_CTRL3_C:
|
||||
lsm6_i2c_tx_reg(lsm, LSM6_REG_CTRL3_C, lsm->config.c);
|
||||
lsm->init_status++;
|
||||
break;
|
||||
case LSM6_CONF_CTRL3_ORIENT:
|
||||
lsm6_i2c_tx_reg(lsm, LSM6_REG_ORIENT_CFG_G, lsm->config.orient);
|
||||
lsm->init_status++;
|
||||
break;
|
||||
case LSM6_CONF_DONE:
|
||||
lsm->initialized = true;
|
||||
lsm->i2c_trans.status = I2CTransDone;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// Start configuration if not already done
|
||||
void lsm6_i2c_start_configure(struct Lsm6_I2c *lsm)
|
||||
{
|
||||
if (lsm->init_status == LSM6_CONF_UNINIT) {
|
||||
lsm->init_status++;
|
||||
if (lsm->i2c_trans.status == I2CTransSuccess || lsm->i2c_trans.status == I2CTransDone) {
|
||||
lsm6_i2c_send_config(lsm);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Normal reading
|
||||
void lsm6_i2c_read(struct Lsm6_I2c *lsm)
|
||||
{
|
||||
if (lsm->initialized && lsm->i2c_trans.status == I2CTransDone) {
|
||||
lsm->i2c_trans.buf[0] = LSM6_REG_OUTX_L_G;
|
||||
lsm->i2c_trans.type = I2CTransTxRx;
|
||||
lsm->i2c_trans.len_r = 12;
|
||||
lsm->i2c_trans.len_w = 1;
|
||||
i2c_submit(lsm->i2c_p, &(lsm->i2c_trans));
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
#define Int16FromBuf(_buf,_idx) ((int16_t)((_buf[_idx+1]<<8) | _buf[_idx]))
|
||||
|
||||
void lsm6_i2c_event(struct Lsm6_I2c *lsm)
|
||||
{
|
||||
if (lsm->initialized) {
|
||||
if (lsm->i2c_trans.status == I2CTransFailed) {
|
||||
lsm->i2c_trans.status = I2CTransDone;
|
||||
} else if (lsm->i2c_trans.status == I2CTransSuccess) {
|
||||
// Successfull reading
|
||||
lsm->data_xl.vect.x = Int16FromBuf(lsm->i2c_trans.buf, 6);
|
||||
lsm->data_xl.vect.y = Int16FromBuf(lsm->i2c_trans.buf, 8);
|
||||
lsm->data_xl.vect.z = Int16FromBuf(lsm->i2c_trans.buf, 10);
|
||||
lsm->data_g.rates.p = Int16FromBuf(lsm->i2c_trans.buf, 0);
|
||||
lsm->data_g.rates.q = Int16FromBuf(lsm->i2c_trans.buf, 2);
|
||||
lsm->data_g.rates.r = Int16FromBuf(lsm->i2c_trans.buf, 4);
|
||||
lsm->data_available = true;
|
||||
lsm->i2c_trans.status = I2CTransDone;
|
||||
}
|
||||
} else if (lsm->init_status != LSM6_CONF_UNINIT) { // Configuring but not yet initialized
|
||||
switch(lsm->i2c_trans.status){
|
||||
case I2CTransFailed:
|
||||
lsm->init_status--;
|
||||
case I2CTransSuccess:
|
||||
case I2CTransDone:
|
||||
lsm6_i2c_send_config(lsm);
|
||||
if(lsm->initialized)
|
||||
lsm->i2c_trans.status = I2CTransDone;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,71 @@
|
||||
/*
|
||||
* Copyright (C) 2019 Alexis Cornard <alexiscornard@gmail.com>
|
||||
*
|
||||
* 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, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file peripherals/lsm6ds33_i2c.h
|
||||
*
|
||||
* Driver for the accelerometer and gyrometer LSM6DS33.
|
||||
*/
|
||||
|
||||
#ifndef LSM6_I2C_H
|
||||
#define LSM6_I2C_H
|
||||
|
||||
#include "std.h"
|
||||
#include "math/pprz_algebra_int.h"
|
||||
#include "mcu_periph/i2c.h"
|
||||
|
||||
/* Include common LSM6DS33 options and definitions */
|
||||
#include "peripherals/lsm6ds33.h"
|
||||
|
||||
struct Lsm6_I2c {
|
||||
struct i2c_periph *i2c_p;
|
||||
struct i2c_transaction i2c_trans;
|
||||
enum Lsm6ConfStatus init_status; ///< init status
|
||||
bool initialized; ///< config done flag
|
||||
volatile bool data_available; ///< data ready flag
|
||||
union {
|
||||
struct Int16Vect3 vect; ///< data vector in accel coordinate system
|
||||
int16_t value[3]; ///< data values accessible by channel index
|
||||
} data_xl;
|
||||
union {
|
||||
struct Int16Rates rates; ///< data as angular rates in gyroscop coordinate system
|
||||
int16_t value[3]; ///< data values accessible by channel index
|
||||
} data_g;
|
||||
struct Lsm6Config config;
|
||||
};
|
||||
|
||||
// Functions
|
||||
extern void lsm6_i2c_init(struct Lsm6_I2c *lsm, struct i2c_periph *i2c_p, uint8_t addr);
|
||||
extern void lsm6_i2c_start_configure(struct Lsm6_I2c *lsm);
|
||||
extern void lsm6_i2c_read(struct Lsm6_I2c *lsm);
|
||||
extern void lsm6_i2c_event(struct Lsm6_I2c *lsm);
|
||||
|
||||
/// convenience function: read or start configuration if not already initialized
|
||||
static inline void lsm6_i2c_periodic(struct Lsm6_I2c *lsm)
|
||||
{
|
||||
if (lsm->initialized) {
|
||||
lsm6_i2c_read(lsm);
|
||||
} else {
|
||||
lsm6_i2c_start_configure(lsm);
|
||||
}
|
||||
}
|
||||
|
||||
#endif // LSM6_I2C_H
|
||||
@@ -0,0 +1,175 @@
|
||||
/*
|
||||
* Copyright (C) 2019 Alexis Cornard <alexiscornard@gmail.com>
|
||||
*
|
||||
* 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, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*/
|
||||
/**
|
||||
* @file peripherals/lsm6ds33_regs.h
|
||||
* Register defs for ST LSM6DS33 3D accelerometer and gyroscope.
|
||||
*
|
||||
* Has an I2C interface.
|
||||
* The LSM6DS33 has linear acceleration full scales of ±2g / ±4g / ±8g / ±16g
|
||||
* and an angular rate range of ±245 / ±500 / ±1000 / ±2000 dps.
|
||||
*/
|
||||
|
||||
#ifndef LSM6_REGS_H
|
||||
#define LSM6_REGS_H
|
||||
|
||||
// I2C Address
|
||||
#define LSM6_ADDR 0xD6
|
||||
|
||||
/* Registers */
|
||||
#define LSM6_REG_FUNC_CFG_ACCESS 0x01
|
||||
|
||||
#define LSM6_REG_FIFO_CTRL1 0x06
|
||||
#define LSM6_REG_FIFO_CTRL2 0x07
|
||||
#define LSM6_REG_FIFO_CTRL3 0x08
|
||||
#define LSM6_REG_FIFO_CTRL4 0x09
|
||||
#define LSM6_REG_FIFO_CTRL5 0x0A
|
||||
#define LSM6_REG_ORIENT_CFG_G 0x0B
|
||||
|
||||
#define LSM6_REG_INT1_CTRL 0x0D
|
||||
#define LSM6_REG_INT2_CTRL 0x0E
|
||||
#define LSM6_REG_WHO_AM_I 0x0F
|
||||
#define LSM6_REG_CTRL1_XL 0x10
|
||||
#define LSM6_REG_CTRL2_G 0x11
|
||||
#define LSM6_REG_CTRL3_C 0x12
|
||||
#define LSM6_REG_CTRL4_C 0x13
|
||||
#define LSM6_REG_CTRL5_C 0x14
|
||||
#define LSM6_REG_CTRL6_C 0x15
|
||||
#define LSM6_REG_CTRL7_G 0x16
|
||||
#define LSM6_REG_CTRL8_XL 0x17
|
||||
#define LSM6_REG_CTRL9_XL 0x18
|
||||
#define LSM6_REG_CTRL10_C 0x19
|
||||
|
||||
#define LSM6_REG_WAKE_UP_SRC 0x1B
|
||||
#define LSM6_REG_TAP_SRC 0x1C
|
||||
#define LSM6_REG_D6D_SRC 0x1D
|
||||
#define LSM6_REG_STATUS_REG 0x1E
|
||||
|
||||
#define LSM6_REG_OUT_TEMP_L 0x20
|
||||
#define LSM6_REG_OUT_TEMP_H 0x21
|
||||
#define LSM6_REG_OUTX_L_G 0x22
|
||||
#define LSM6_REG_OUTX_H_G 0x23
|
||||
#define LSM6_REG_OUTY_L_G 0x24
|
||||
#define LSM6_REG_OUTY_H_G 0x25
|
||||
#define LSM6_REG_OUTZ_L_G 0x26
|
||||
#define LSM6_REG_OUTZ_H_G 0x27
|
||||
#define LSM6_REG_OUTX_L_XL 0x28
|
||||
#define LSM6_REG_OUTX_H_XL 0x29
|
||||
#define LSM6_REG_OUTY_L_XL 0x2A
|
||||
#define LSM6_REG_OUTY_H_XL 0x2B
|
||||
#define LSM6_REG_OUTZ_L_XL 0x2C
|
||||
#define LSM6_REG_OUTZ_H_XL 0x2D
|
||||
|
||||
|
||||
#define LSM6_REG_FIFO_STATUS1 0x3A
|
||||
#define LSM6_REG_FIFO_STATUS2 0x3B
|
||||
#define LSM6_REG_FIFO_STATUS3 0x3C
|
||||
#define LSM6_REG_FIFO_STATUS4 0x3D
|
||||
#define LSM6_REG_FIFO_DATA_OUTL 0x3E
|
||||
#define LSM6_REG_FIFO_DATA_OUTX 0x3F
|
||||
#define LSM6_REG_TIMESTAMP0_REG 0x40
|
||||
#define LSM6_REG_TIMESTAMP1_REG 0x41
|
||||
#define LSM6_REG_TIMESTAMP2_REG 0x42
|
||||
|
||||
#define LSM6_REG_STEP_TIMESTAMP_L 0x49
|
||||
#define LSM6_REG_STEP_TIMESTAMP_H 0x4A
|
||||
#define LSM6_REG_STEP_COUNTER_L 0x4B
|
||||
#define LSM6_REG_STEP_COUNTER_H 0x4C
|
||||
|
||||
#define LSM6_REG_FUNC_SRC 0x53
|
||||
|
||||
#define LSM6_REG_TAP_CFG 0x58
|
||||
#define LSM6_REG_TAP_THS_6D 0x59
|
||||
#define LSM6_REG_INT_DUR2 0x5A
|
||||
#define LSM6_REG_WAKE_UP_THS 0x5B
|
||||
#define LSM6_REG_WAKE_UP_DUR 0x5C
|
||||
#define LSM6_REG_FREE_FALL 0x5D
|
||||
#define LSM6_REG_MD1_CFG 0x5E
|
||||
#define LSM6_REG_MD2_CFG 0x5F
|
||||
|
||||
|
||||
/**
|
||||
* Selectable gyro range
|
||||
*/
|
||||
enum Lsm6GyroRanges {
|
||||
LSM6_FS_G_245 = 0x00,
|
||||
LSM6_FS_G_500 = 0x01,
|
||||
LSM6_FS_G_1000 = 0x02,
|
||||
LSM6_FS_G_2000 = 0x03
|
||||
};
|
||||
|
||||
/**
|
||||
* Selectable gyro ODR
|
||||
*/
|
||||
enum Lsm6GyroODR {
|
||||
LSM6_ODR_G_PWR_DWN = 0x00,
|
||||
LSM6_ODR_G_13HZ = 0x01,
|
||||
LSM6_ODR_G_26HZ = 0x02,
|
||||
LSM6_ODR_G_52HZ = 0x03,
|
||||
LSM6_ODR_G_104HZ = 0x04,
|
||||
LSM6_ODR_G_208HZ = 0x05,
|
||||
LSM6_ODR_G_416HZ = 0x06,
|
||||
LSM6_ODR_G_833HZ = 0x07,
|
||||
LSM6_ODR_G_1_6KHZ = 0x08
|
||||
};
|
||||
|
||||
|
||||
/**
|
||||
* Selectable accel range
|
||||
*/
|
||||
enum Lsm6AccelRanges {
|
||||
LSM6_FS_XL_2G = 0x00,
|
||||
LSM6_FS_XL_4G = 0x02,
|
||||
LSM6_FS_XL_8G = 0x03,
|
||||
LSM6_FS_XL_16G = 0x01
|
||||
};
|
||||
|
||||
/**
|
||||
* Selectable accel ODR
|
||||
*/
|
||||
enum Lsm6AccelODR {
|
||||
LSM6_ODR_XL_PWR_DWN = 0x00,
|
||||
LSM6_ODR_XL_13HZ = 0x01,
|
||||
LSM6_ODR_XL_26HZ = 0x02,
|
||||
LSM6_ODR_XL_52HZ = 0x03,
|
||||
LSM6_ODR_XL_104HZ = 0x04,
|
||||
LSM6_ODR_XL_208HZ = 0x05,
|
||||
LSM6_ODR_XL_416HZ = 0x06,
|
||||
LSM6_ODR_XL_833HZ = 0x07,
|
||||
LSM6_ODR_XL_1_6KHZ = 0x08,
|
||||
LSM6_ODR_XL_3_3KHZ = 0x09,
|
||||
LSM6_ODR_XL_6_6KHZ = 0x0A
|
||||
};
|
||||
|
||||
/**
|
||||
* Anti-aliasing filter bandwith
|
||||
*/
|
||||
enum Lsm6AccelBw {
|
||||
LSM6_BW_XL_400HZ = 0x00,
|
||||
LSM6_BW_XL_200HZ = 0x01,
|
||||
LSM6_BW_XL_100HZ = 0x02,
|
||||
LSM6_BW_XL_50HZ = 0x03
|
||||
};
|
||||
|
||||
|
||||
/** LSM6 device identifier in LSM6_REG_WHO_AM_I */
|
||||
#define LSM6_WHO_I_AM 0x69
|
||||
|
||||
#endif // LSM6_REGS_H
|
||||
Reference in New Issue
Block a user