dan
/
EM7180_STM32
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

flesh out API; allow instantiation of multiple devices

master
Daniel Peter Chokola 4 years ago
parent
16d2932478
commit
6f195cc05b
9 changed files with 525 additions and 476 deletions
Split View
  1. 38
      Drivers/EM7180/Inc/em7180.h
  2. 22
      Drivers/EM7180/Inc/em7180_common.h
  3. 15
      Drivers/EM7180/Inc/lis2mdl.h
  4. 28
      Drivers/EM7180/Inc/lps22hb.h
  5. 22
      Drivers/EM7180/Inc/lsm6dsm.h
  6. 556
      Drivers/EM7180/Src/em7180.c
  7. 60
      Drivers/EM7180/Src/lis2mdl.c
  8. 70
      Drivers/EM7180/Src/lps22hb.c
  9. 190
      Drivers/EM7180/Src/lsm6dsm.c

38
Drivers/EM7180/Inc/em7180.h
Unescape View File

@ -17,6 +17,10 @@ @@ -17,6 +17,10 @@
/* Includes */
#include <stdint.h>
#include "i2c.h"
#include "lsm6dsm.h"
#include "lps22hb.h"
#include "lis2mdl.h"
/* Definitions */
/*
@ -87,28 +91,32 @@ @@ -87,28 +91,32 @@
#define EM7180_ResetRequest 0x9B
#define EM7180_PassThruStatus 0x9E
#define EM7180_PassThruControl 0xA0
#define EM7180_ACC_LPF_BW 0x5B //Register GP36
#define EM7180_GYRO_LPF_BW 0x5C //Register GP37
#define EM7180_BARO_LPF_BW 0x5D //Register GP38
#define EM7180_ADDRESS 0x28 // Address of the EM7180 SENtral sensor hub
#define M24512DFM_DATA_ADDRESS 0x50 // Address of the 500 page M24512DFM EEPROM data buffer, 1024 bits (128 8-bit bytes) per page
#define M24512DFM_IDPAGE_ADDRESS 0x58 // Address of the single M24512DFM lockable EEPROM ID page
#define AFS_2G 0
#define AFS_4G 1
#define AFS_8G 2
#define AFS_16G 3
#define GFS_250DPS 0
#define GFS_500DPS 1
#define GFS_1000DPS 2
#define GFS_2000DPS 3
#define MFS_14BITS 0 // 0.6 mG per LSB
#define MFS_16BITS 1 // 0.15 mG per LSB
/* Data Structures */
typedef struct em7180_s
{
I2C_HandleTypeDef *hi2c;
lsm6dsm_t *lsm6dsm;
uint16_t acc_fs;
uint16_t gyro_fs;
uint16_t mag_fs;
uint8_t q_rate_div;
uint8_t mag_rate;
uint8_t acc_rate;
uint8_t gyro_rate;
uint8_t baro_rate;
} em7180_t;
/* Function Prototypes */
void em7180_init(em7180_t *em7180, lsm6dsm_t *lsm6dsm, I2C_HandleTypeDef *hi2c1,
uint16_t acc_fs, uint16_t gyro_fs, uint16_t mag_fs,
uint8_t q_rate_div, uint8_t mag_rate, uint8_t acc_rate,
uint8_t gyro_rate, uint8_t baro_rate);
void em7180_config(em7180_t *em7180);
void em7180_gyro_set_fs(uint16_t gyro_fs);
void em7180_mag_acc_set_fs(uint16_t mag_fs, uint16_t acc_fs);
void em7180_set_integer_param(uint8_t param, uint32_t param_val);

22
Drivers/EM7180/Inc/em7180_common.h
Unescape View File

@ -0,0 +1,22 @@ @@ -0,0 +1,22 @@
/*
* em7180_common.h
*
* Created on: Jan 18, 2021
* Author: Daniel Peter Chokola
*
* Library may be used freely and without limit with attribution.
*/
#ifndef EM7180_COMMON_h
#define EM7180_COMMON_h
/* Includes */
#include <stdint.h>
/* Function Prototypes */
void lsm6dsm_write_byte(uint8_t address, uint8_t subAddress, uint8_t data);
uint8_t lsm6dsm_read_byte(uint8_t address, uint8_t subAddress);
void lsm6dsm_read(uint8_t address, uint8_t subAddress, uint8_t count,
uint8_t *dest);
#endif /* EM7180_COMMON_h */

15
Drivers/EM7180/Inc/lis2mdl.h
Unescape View File

@ -19,6 +19,7 @@ @@ -19,6 +19,7 @@
/* Includes */
#include <stdint.h>
#include "i2c.h"
/* Definitions */
//Register map for LIS2MDL'
@ -54,4 +55,18 @@ @@ -54,4 +55,18 @@
#define MODR_50Hz 0x02
#define MODR_100Hz 0x03
#define MFS_14BITS 0 // 0.6 mG per LSB
#define MFS_16BITS 1 // 0.15 mG per LSB
/* Data Structures */
typedef struct lis2mdl_s
{
I2C_HandleTypeDef *hi2c;
uint8_t m_odr;
} lis2mdl_t;
/* Function Prototypes */
void lis2mdl_init(lis2mdl_t *lis2mdl, I2C_HandleTypeDef *hi2c, uint8_t m_odr);
void lis2mdl_config(lis2mdl_t *lis2mdl);
#endif

28
Drivers/EM7180/Inc/lps22hb.h
Unescape View File

@ -19,6 +19,7 @@ @@ -19,6 +19,7 @@
/* Includes */
#include <stdint.h>
#include "i2c.h"
// See LPS22H "MEMS pressure sensor: 260-1260 hPa absolute digital output barometer" Data Sheet
// http://www.st.com/content/ccc/resource/technical/document/datasheet/bf/c1/4f/23/61/17/44/8a/DM00140895.pdf/files/DM00140895.pdf/jcr:content/translations/en.DM00140895.pdf
@ -49,11 +50,22 @@ @@ -49,11 +50,22 @@
#define LPS22H_ADDRESS 0x5C // Address of altimeter
// Altimeter output data rate
#define P_1shot 0x00;
#define P_1Hz 0x01;
#define P_10Hz 0x02;
#define P_25Hz 0x03; // 25 Hz output data rate
#define P_50Hz 0x04;
#define P_75Hz 0x05;
#endif
#define P_1shot 0x00
#define P_1Hz 0x01
#define P_10Hz 0x02
#define P_25Hz 0x03 // 25 Hz output data rate
#define P_50Hz 0x04
#define P_75Hz 0x05
/* Data Structures */
typedef struct lps22hb_s
{
I2C_HandleTypeDef *hi2c;
uint8_t p_odr;
} lps22hb_t;
/* Function Prototypes */
void lps22hb_init(lps22hb_t *lps22hb, I2C_HandleTypeDef *hi2c, uint8_t p_odr);
void lps22hb_config(lps22hb_t *lps22hb);
#endif

22
Drivers/EM7180/Inc/lsm6dsm.h
Unescape View File

@ -19,7 +19,9 @@ @@ -19,7 +19,9 @@
/* Includes */
#include <stdint.h>
#include "i2c.h"
/* Definitions */
/*
* LSM6DSM registers
* http://www.st.com/content/ccc/resource/technical/document/datasheet/76/27/cf/88/c5/03/42/6b/DM00218116.pdf/files/DM00218116.pdf/jcr:content/translations/en.DM00218116.pdf
@ -125,13 +127,12 @@ @@ -125,13 +127,12 @@
#define LSM6DSM_ADDRESS 0x6A // Address of LSM6DSM accel/gyro when ADO = 0
#define AFS_2G 0x00
#define AFS_4G 0x02
#define AFS_8G 0x03
#define AFS_16G 0x01
#define GFS_245DPS 0x00
#define GFS_250DPS 0x00
#define GFS_500DPS 0x01
#define GFS_1000DPS 0x02
#define GFS_2000DPS 0x03
@ -158,4 +159,19 @@ @@ -158,4 +159,19 @@
#define GODR_3330Hz 0x09
#define GODR_6660Hz 0x0A
#endif
/* Data Structures */
typedef struct lsm6dsm_s
{
I2C_HandleTypeDef *hi2c;
uint8_t ascale;
uint8_t gscale;
uint8_t a_odr;
uint8_t g_odr;
} lsm6dsm_t;
/* Function Prototypes */
void lsm6dsm_init(lsm6dsm_t *lsm6dsm, I2C_HandleTypeDef *hi2c, uint8_t ascale,
uint8_t gscale, uint8_t a_odr, uint8_t g_odr);
void lsm6dsm_config(lsm6dsm_t *lsm6dsm);
#endif

556
Drivers/EM7180/Src/em7180.c
Unescape View File

@ -16,16 +16,16 @@ @@ -16,16 +16,16 @@
#include <stdint.h>
#include <stdbool.h>
#include <math.h>
#include "em7180_common.h"
#include "em7180.h"
#include "i2c.h"
/* Data Structures */
/* Private Global Variables */
static uint8_t _intPin;
static bool _passThru;
static float _aRes;
static float _gRes;
static float _mRes;
/* Function Prototypes */
static void em7180_float_to_bytes(float param_val, uint8_t *buf);
static void m24512dfm_write_byte(uint8_t device_address, uint8_t data_address1,
uint8_t data_address2, uint8_t data);
static void m24512dfm_write(uint8_t device_address, uint8_t data_address1,
@ -39,150 +39,38 @@ static void em7180_read(uint8_t address, uint8_t subAddress, uint8_t count, @@ -39,150 +39,38 @@ static void em7180_read(uint8_t address, uint8_t subAddress, uint8_t count,
uint8_t *dest);
/* Function Definitions */
em7180_new(uint8_t pin, bool passthru)
{
/* pinMode(pin, INPUT); */
_intPin = pin;
_passThru = passthru;
}
void em7180_chip_id_get()
{
// Read SENtral device information
uint16_t ROM1 = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ROMVersion1);
uint16_t ROM2 = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ROMVersion2);
/* Serial.print("EM7180 ROM Version: 0x"); */
/* Serial.print(ROM1, HEX); */
/* Serial.println(ROM2, HEX); */
/* Serial.println("Should be: 0xE609"); */
uint16_t RAM1 = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_RAMVersion1);
uint16_t RAM2 = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_RAMVersion2);
/* Serial.print("EM7180 RAM Version: 0x"); */
/* Serial.print(RAM1); */
/* Serial.println(RAM2); */
uint8_t PID = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ProductID);
/* Serial.print("EM7180 ProductID: 0x"); */
/* Serial.print(PID, HEX); */
/* Serial.println(" Should be: 0x80"); */
uint8_t RID = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_RevisionID);
/* Serial.print("EM7180 RevisionID: 0x"); */
/* Serial.print(RID, HEX); */
/* Serial.println(" Should be: 0x02"); */
}
void em7180_load_fw_from_eeprom()
void em7180_init(em7180_t *em7180, lsm6dsm_t *lsm6dsm, I2C_HandleTypeDef *hi2c1,
uint16_t acc_fs, uint16_t gyro_fs, uint16_t mag_fs,
uint8_t q_rate_div, uint8_t mag_rate, uint8_t acc_rate,
uint8_t gyro_rate, uint8_t baro_rate)
{
// Check which sensors can be detected by the EM7180
uint8_t featureflag = lsm6dsm_read_byte(EM7180_ADDRESS,
EM7180_FeatureFlags);
if(featureflag & 0x01)
{
/* Serial.println("A barometer is installed"); */
}
if(featureflag & 0x02)
{
/* Serial.println("A humidity sensor is installed"); */
}
if(featureflag & 0x04)
{
/* Serial.println("A temperature sensor is installed"); */
}
if(featureflag & 0x08)
{
/* Serial.println("A custom sensor is installed"); */
}
if(featureflag & 0x10)
{
/* Serial.println("A second custom sensor is installed"); */
}
if(featureflag & 0x20)
if(!em7180)
{
/* Serial.println("A third custom sensor is installed"); */
return;
}
HAL_Delay(1000); // give some time to read the screen
em7180->lsm6dsm = lsm6dsm;
em7180->acc_fs = acc_fs;
em7180->gyro_fs = gyro_fs;
em7180->mag_fs = mag_fs;
em7180->q_rate_div = q_rate_div;
em7180->mag_rate = mag_rate;
em7180->acc_rate = acc_rate;
em7180->gyro_rate = gyro_rate;
em7180->baro_rate = baro_rate;
// Check SENtral status, make sure EEPROM upload of firmware was accomplished
uint8_t STAT = (lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus)
& 0x01);
if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x01)
{
/* Serial.println("EEPROM detected on the sensor bus!"); */
}
if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x02)
{
/* Serial.println("EEPROM uploaded config file!"); */
}
if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x04)
{
/* Serial.println("EEPROM CRC incorrect!"); */
}
if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x08)
{
/* Serial.println("EM7180 in initialized state!"); */
}
if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x10)
{
/* Serial.println("No EEPROM detected!"); */
}
int count = 0;
while(!STAT)
{
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ResetRequest, 0x01);
HAL_Delay(500);
count++;
STAT = (lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x01);
if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x01)
{
/* Serial.println("EEPROM detected on the sensor bus!"); */
}
if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x02)
{
/* Serial.println("EEPROM uploaded config file!"); */
}
if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x04)
{
/* Serial.println("EEPROM CRC incorrect!"); */
}
if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x08)
{
/* Serial.println("EM7180 in initialized state!"); */
}
if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x10)
{
/* Serial.println("No EEPROM detected!"); */
}
if(count > 10)
{
break;
}
}
if(!(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x04))
{
/* Serial.println("EEPROM upload successful!"); */
}
em7180_config(em7180);
}
uint8_t em7180_status()
void em7180_config(em7180_t *em7180)
{
// Check event status register, way to check data ready by polling rather than interrupt
uint8_t c = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_EventStatus); // reading clears the register and interrupt
return c;
}
uint8_t em7180_errors()
{
uint8_t c = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ErrorRegister); // check error register
return c;
}
void em7180_init(uint8_t accBW, uint8_t gyroBW, uint16_t accFS, uint16_t gyroFS,
uint16_t magFS, uint8_t QRtDiv, uint8_t magRt, uint8_t accRt,
uint8_t gyroRt, uint8_t baroRt)
{
uint16_t EM7180_mag_fs, EM7180_acc_fs, EM7180_gyro_fs; // EM7180 sensor full scale ranges
uint8_t param[4];
uint8_t param_xfer;
uint8_t runStatus;
uint8_t algoStatus;
uint8_t passthruStatus;
uint8_t eventStatus;
uint8_t sensorStatus;
// Enter EM7180 initialized state
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_HostControl, 0x00); // set SENtral in initialized state to configure registers
@ -190,15 +78,17 @@ void em7180_init(uint8_t accBW, uint8_t gyroBW, uint16_t accFS, uint16_t gyroFS, @@ -190,15 +78,17 @@ void em7180_init(uint8_t accBW, uint8_t gyroBW, uint16_t accFS, uint16_t gyroFS,
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_HostControl, 0x01); // Force initialize
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_HostControl, 0x00); // set SENtral in initialized state to configure registers
//Setup LPF bandwidth (BEFORE setting ODR's)
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ACC_LPF_BW, accBW); // accBW = 3 = 41Hz
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_GYRO_LPF_BW, gyroBW); // gyroBW = 3 = 41Hz
/* Legacy MPU6250 stuff, it seems
// Setup LPF bandwidth (BEFORE setting ODR's)
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ACC_LPF_BW, accBW); // accBW = 3 = 41Hz
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_GYRO_LPF_BW, gyroBW); // gyroBW = 3 = 41Hz */
// Set accel/gyro/mag desired ODR rates
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_QRateDivisor, QRtDiv); // quat rate = gyroRt/(1 QRTDiv)
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_MagRate, magRt); // 0x64 = 100 Hz
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AccelRate, accRt); // 200/10 Hz, 0x14 = 200 Hz
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_GyroRate, gyroRt); // 200/10 Hz, 0x14 = 200 Hz
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_BaroRate, 0x80 | baroRt); // set enable bit and set Baro rate to 25 Hz, rate = baroRt/2, 0x32 = 25 Hz
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_QRateDivisor, em7180->q_rate_div); // quat rate = gyroRt/(1 QRTDiv)
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_MagRate, em7180->mag_rate); // 0x64 = 100 Hz
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AccelRate, em7180->acc_rate); // 200/10 Hz, 0x14 = 200 Hz
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_GyroRate, em7180->gyro_rate); // 200/10 Hz, 0x14 = 200 Hz
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_BaroRate,
0x80 | em7180->baro_rate); // set enable bit and set Baro rate to 25 Hz, rate = baroRt/2, 0x32 = 25 Hz
// Configure operating mode
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // read scale sensor data
@ -213,95 +103,20 @@ void em7180_init(uint8_t accBW, uint8_t gyroBW, uint16_t accFS, uint16_t gyroFS, @@ -213,95 +103,20 @@ void em7180_init(uint8_t accBW, uint8_t gyroBW, uint16_t accFS, uint16_t gyroFS,
// EM7180 parameter adjustments
/* Serial.println("Beginning Parameter Adjustments"); */
// Read sensor default FS values from parameter space
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0x4A); // Request to read parameter 74
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x80); // Request parameter transfer process
uint8_t param_xfer = lsm6dsm_read_byte(EM7180_ADDRESS,
EM7180_ParamAcknowledge);
while(!(param_xfer == 0x4A))
{
param_xfer = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
}
param[0] = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SavedParamByte0);
param[1] = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SavedParamByte1);
param[2] = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SavedParamByte2);
param[3] = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SavedParamByte3);
EM7180_mag_fs = ((int16_t) (param[1] << 8) | param[0]);
EM7180_acc_fs = ((int16_t) (param[3] << 8) | param[2]);
/* Serial.print("Magnetometer Default Full Scale Range: +/-"); */
/* Serial.print(EM7180_mag_fs); */
/* Serial.println("uT"); */
/* Serial.print("Accelerometer Default Full Scale Range: +/-"); */
/* Serial.print(EM7180_acc_fs); */
/* Serial.println("g"); */
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0x4B); // Request to read parameter 75
param_xfer = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
while(!(param_xfer == 0x4B))
{
param_xfer = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
}
param[0] = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SavedParamByte0);
param[1] = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SavedParamByte1);
param[2] = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SavedParamByte2);
param[3] = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SavedParamByte3);
EM7180_gyro_fs = ((int16_t) (param[1] << 8) | param[0]);
/* Serial.print("Gyroscope Default Full Scale Range: +/-"); */
/* Serial.print(EM7180_gyro_fs); */
/* Serial.println("dps"); */
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0x00); //End parameter transfer
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // re-enable algorithm
//Disable stillness mode for balancing robot application
EM7180_set_integer_param(0x49, 0x00);
//Write desired sensor full scale ranges to the EM7180
EM7180_set_mag_acc_FS(magFS, accFS); // 1000 uT == 0x3E8, 8 g == 0x08
EM7180_set_gyro_FS(gyroFS); // 2000 dps == 0x7D0
// Read sensor new FS values from parameter space
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0x4A); // Request to read parameter 74
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x80); // Request parameter transfer process
param_xfer = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
while(!(param_xfer == 0x4A))
{
param_xfer = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
}
param[0] = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SavedParamByte0);
param[1] = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SavedParamByte1);
param[2] = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SavedParamByte2);
param[3] = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SavedParamByte3);
EM7180_mag_fs = ((int16_t) (param[1] << 8) | param[0]);
EM7180_acc_fs = ((int16_t) (param[3] << 8) | param[2]);
/* Serial.print("Magnetometer New Full Scale Range: +/-"); */
/* Serial.print(EM7180_mag_fs); */
/* Serial.println("uT"); */
/* Serial.print("Accelerometer New Full Scale Range: +/-"); */
/* Serial.print(EM7180_acc_fs); */
/* Serial.println("g"); */
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0x4B); // Request to read parameter 75
param_xfer = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
while(!(param_xfer == 0x4B))
{
param_xfer = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
}
param[0] = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SavedParamByte0);
param[1] = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SavedParamByte1);
param[2] = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SavedParamByte2);
param[3] = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SavedParamByte3);
EM7180_gyro_fs = ((int16_t) (param[1] << 8) | param[0]);
/* Serial.print("Gyroscope New Full Scale Range: +/-"); */
/* Serial.print(EM7180_gyro_fs); */
/* Serial.println("dps"); */
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0x00); //End parameter transfer
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // re-enable algorithm
// Disable stillness mode for balancing robot application
em7180_set_integer_param(0x49, 0x00);
// Write desired sensor full scale ranges to the EM7180
em7180_mag_acc_set_fs(em7180->mag_fs, em7180->acc_fs); // 1000 uT == 0x3E8, 8 g == 0x08
em7180_gyro_set_fs(em7180->gyro_fs); // 2000 dps == 0x7D0
// Read EM7180 status
uint8_t runStatus = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_RunStatus);
runStatus = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_RunStatus);
if(runStatus & 0x01)
{
/* Serial.println(" EM7180 run status = normal mode"); */
}
uint8_t algoStatus = lsm6dsm_read_byte(EM7180_ADDRESS,
algoStatus = lsm6dsm_read_byte(EM7180_ADDRESS,
EM7180_AlgorithmStatus);
if(algoStatus & 0x01)
{
@ -327,13 +142,13 @@ void em7180_init(uint8_t accBW, uint8_t gyroBW, uint16_t accFS, uint16_t gyroFS, @@ -327,13 +142,13 @@ void em7180_init(uint8_t accBW, uint8_t gyroBW, uint16_t accFS, uint16_t gyroFS,
{
/* Serial.println(" EM7180 unreliable sensor data"); */
}
uint8_t passthruStatus = lsm6dsm_read_byte(EM7180_ADDRESS,
passthruStatus = lsm6dsm_read_byte(EM7180_ADDRESS,
EM7180_PassThruStatus);
if(passthruStatus & 0x01)
{
/* Serial.print(" EM7180 in passthru mode!"); */
}
uint8_t eventStatus = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_EventStatus);
eventStatus = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_EventStatus);
if(eventStatus & 0x01)
{
/* Serial.println(" EM7180 CPU reset"); */
@ -362,8 +177,7 @@ void em7180_init(uint8_t accBW, uint8_t gyroBW, uint16_t accFS, uint16_t gyroFS, @@ -362,8 +177,7 @@ void em7180_init(uint8_t accBW, uint8_t gyroBW, uint16_t accFS, uint16_t gyroFS,
HAL_Delay(1000); // give some time to read the screen
// Check sensor status
uint8_t sensorStatus = lsm6dsm_read_byte(EM7180_ADDRESS,
EM7180_SensorStatus);
sensorStatus = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SensorStatus);
/* Serial.print(" EM7180 sensor status = "); */
/* Serial.println(sensorStatus); */
if(sensorStatus & 0x01)
@ -405,6 +219,137 @@ void em7180_init(uint8_t accBW, uint8_t gyroBW, uint16_t accFS, uint16_t gyroFS, @@ -405,6 +219,137 @@ void em7180_init(uint8_t accBW, uint8_t gyroBW, uint16_t accFS, uint16_t gyroFS,
/* Serial.println(" Hz"); */
}
void em7180_chip_id_get()
{
// Read SENtral device information
uint16_t ROM1 = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ROMVersion1);
uint16_t ROM2 = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ROMVersion2);
/* Serial.print("EM7180 ROM Version: 0x"); */
/* Serial.print(ROM1, HEX); */
/* Serial.println(ROM2, HEX); */
/* Serial.println("Should be: 0xE609"); */
uint16_t RAM1 = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_RAMVersion1);
uint16_t RAM2 = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_RAMVersion2);
/* Serial.print("EM7180 RAM Version: 0x"); */
/* Serial.print(RAM1); */
/* Serial.println(RAM2); */
uint8_t PID = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ProductID);
/* Serial.print("EM7180 ProductID: 0x"); */
/* Serial.print(PID, HEX); */
/* Serial.println(" Should be: 0x80"); */
uint8_t RID = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_RevisionID);
/* Serial.print("EM7180 RevisionID: 0x"); */
/* Serial.print(RID, HEX); */
/* Serial.println(" Should be: 0x02"); */
}
void em7180_load_fw_from_eeprom()
{
// Check which sensors can be detected by the EM7180
uint8_t featureflag = lsm6dsm_read_byte(EM7180_ADDRESS,
EM7180_FeatureFlags);
if(featureflag & 0x01)
{
/* Serial.println("A barometer is installed"); */
}
if(featureflag & 0x02)
{
/* Serial.println("A humidity sensor is installed"); */
}
if(featureflag & 0x04)
{
/* Serial.println("A temperature sensor is installed"); */
}
if(featureflag & 0x08)
{
/* Serial.println("A custom sensor is installed"); */
}
if(featureflag & 0x10)
{
/* Serial.println("A second custom sensor is installed"); */
}
if(featureflag & 0x20)
{
/* Serial.println("A third custom sensor is installed"); */
}
HAL_Delay(1000); // give some time to read the screen
// Check SENtral status, make sure EEPROM upload of firmware was accomplished
uint8_t STAT = (lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus)
& 0x01);
if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x01)
{
/* Serial.println("EEPROM detected on the sensor bus!"); */
}
if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x02)
{
/* Serial.println("EEPROM uploaded config file!"); */
}
if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x04)
{
/* Serial.println("EEPROM CRC incorrect!"); */
}
if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x08)
{
/* Serial.println("EM7180 in initialized state!"); */
}
if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x10)
{
/* Serial.println("No EEPROM detected!"); */
}
int count = 0;
while(!STAT)
{
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ResetRequest, 0x01);
HAL_Delay(500);
count++;
STAT = (lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x01);
if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x01)
{
/* Serial.println("EEPROM detected on the sensor bus!"); */
}
if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x02)
{
/* Serial.println("EEPROM uploaded config file!"); */
}
if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x04)
{
/* Serial.println("EEPROM CRC incorrect!"); */
}
if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x08)
{
/* Serial.println("EM7180 in initialized state!"); */
}
if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x10)
{
/* Serial.println("No EEPROM detected!"); */
}
if(count > 10)
{
break;
}
}
if(!(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x04))
{
/* Serial.println("EEPROM upload successful!"); */
}
}
uint8_t em7180_status()
{
// Check event status register, way to check data ready by polling rather than interrupt
uint8_t c = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_EventStatus); // reading clears the register and interrupt
return c;
}
uint8_t em7180_errors()
{
uint8_t c = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ErrorRegister); // check error register
return c;
}
float em7180_uint32_reg_to_float(uint8_t *buf)
{
union
@ -431,19 +376,21 @@ float em7180_int32_reg_to_float(uint8_t *buf) @@ -431,19 +376,21 @@ float em7180_int32_reg_to_float(uint8_t *buf)
return u.f;
}
void em7180_float_to_bytes(float param_val, uint8_t *buf)
static void em7180_float_to_bytes(float param_val, uint8_t *buf)
{
union
{
float f;
uint8_t comp[sizeof(float)];
uint8_t u8[sizeof(float)];
} u;
u.f = param_val;
for(uint8_t i = 0; i < sizeof(float); i++)
{
buf[i] = u.comp[i];
buf[i] = u.u8[i];
}
//Convert to LITTLE ENDIAN
// Convert to LITTLE ENDIAN
/* FIXME: What the hell? */
for(uint8_t i = 0; i < sizeof(float); i++)
{
buf[i] = buf[(sizeof(float) - 1) - i];
@ -457,18 +404,18 @@ void em7180_gyro_set_fs(uint16_t gyro_fs) @@ -457,18 +404,18 @@ void em7180_gyro_set_fs(uint16_t gyro_fs)
bytes[1] = (gyro_fs >> 8) & (0xFF);
bytes[2] = 0x00;
bytes[3] = 0x00;
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte0, bytes[0]); //Gyro LSB
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte1, bytes[1]); //Gyro MSB
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte2, bytes[2]); //Unused
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte3, bytes[3]); //Unused
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0xCB); //Parameter 75; 0xCB is 75 decimal with the MSB set high to indicate a paramter write processs
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x80); //Request parameter transfer procedure
STAT = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge); //Check the parameter acknowledge register and loop until the result matches parameter request byte
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte0, bytes[0]); // Gyro LSB
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte1, bytes[1]); // Gyro MSB
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte2, bytes[2]); // Unused
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte3, bytes[3]); // Unused
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0xCB); // Parameter 75; 0xCB is 75 decimal with the MSB set high to indicate a parameter write process
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x80); // Request parameter transfer procedure
STAT = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge); // Check the parameter acknowledge register and loop until the result matches parameter request byte
while(!(STAT == 0xCB))
{
STAT = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
}
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0x00); //Parameter request = 0 to end parameter transfer process
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0x00); // Parameter request = 0 to end parameter transfer process
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // Re-start algorithm
}
@ -479,18 +426,18 @@ void em7180_mag_acc_set_fs(uint16_t mag_fs, uint16_t acc_fs) @@ -479,18 +426,18 @@ void em7180_mag_acc_set_fs(uint16_t mag_fs, uint16_t acc_fs)
bytes[1] = (mag_fs >> 8) & (0xFF);
bytes[2] = acc_fs & (0xFF);
bytes[3] = (acc_fs >> 8) & (0xFF);
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte0, bytes[0]); //Mag LSB
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte1, bytes[1]); //Mag MSB
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte2, bytes[2]); //Acc LSB
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte3, bytes[3]); //Acc MSB
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0xCA); //Parameter 74; 0xCA is 74 decimal with the MSB set high to indicate a paramter write processs
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x80); //Request parameter transfer procedure
STAT = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge); //Check the parameter acknowledge register and loop until the result matches parameter request byte
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte0, bytes[0]); // Mag LSB
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte1, bytes[1]); // Mag MSB
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte2, bytes[2]); // Acc LSB
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte3, bytes[3]); // Acc MSB
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0xCA); // Parameter 74; 0xCA is 74 decimal with the MSB set high to indicate a paramter write processs
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x80); // Request parameter transfer procedure
STAT = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge); // Check the parameter acknowledge register and loop until the result matches parameter request byte
while(!(STAT == 0xCA))
{
STAT = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
}
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0x00); //Parameter request = 0 to end parameter transfer process
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0x00); // Parameter request = 0 to end parameter transfer process
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // Re-start algorithm
}
@ -501,39 +448,39 @@ void em7180_set_integer_param(uint8_t param, uint32_t param_val) @@ -501,39 +448,39 @@ void em7180_set_integer_param(uint8_t param, uint32_t param_val)
bytes[1] = (param_val >> 8) & (0xFF);
bytes[2] = (param_val >> 16) & (0xFF);
bytes[3] = (param_val >> 24) & (0xFF);
param = param | 0x80; //Parameter is the decimal value with the MSB set high to indicate a paramter write processs
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte0, bytes[0]); //Param LSB
param = param | 0x80; // Parameter is the decimal value with the MSB set high to indicate a paramter write processs
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte0, bytes[0]); // Param LSB
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte1, bytes[1]);
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte2, bytes[2]);
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte3, bytes[3]); //Param MSB
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte3, bytes[3]); // Param MSB
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, param);
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x80); //Request parameter transfer procedure
STAT = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge); //Check the parameter acknowledge register and loop until the result matches parameter request byte
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x80); // Request parameter transfer procedure
STAT = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge); // Check the parameter acknowledge register and loop until the result matches parameter request byte
while(!(STAT == param))
{
STAT = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
}
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0x00); //Parameter request = 0 to end parameter transfer process
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0x00); // Parameter request = 0 to end parameter transfer process
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // Re-start algorithm
}
void em7180_param_set_float(uint8_t param, float param_val)
{
uint8_t bytes[4], STAT;
float_to_bytes(param_val, &bytes[0]);
param = param | 0x80; //Parameter is the decimal value with the MSB set high to indicate a paramter write processs
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte0, bytes[0]); //Param LSB
em7180_float_to_bytes(param_val, &bytes[0]);
param = param | 0x80; // Parameter is the decimal value with the MSB set high to indicate a paramter write processs
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte0, bytes[0]); // Param LSB
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte1, bytes[1]);
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte2, bytes[2]);
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte3, bytes[3]); //Param MSB
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte3, bytes[3]); // Param MSB
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, param);
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x80); //Request parameter transfer procedure
STAT = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge); //Check the parameter acknowledge register and loop until the result matches parameter request byte
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x80); // Request parameter transfer procedure
STAT = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge); // Check the parameter acknowledge register and loop until the result matches parameter request byte
while(!(STAT == param))
{
STAT = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
}
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0x00); //Parameter request = 0 to end parameter transfer process
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0x00); // Parameter request = 0 to end parameter transfer process
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // Re-start algorithm
}
@ -577,71 +524,88 @@ void em7180_magdata_get(int16_t *destination) @@ -577,71 +524,88 @@ void em7180_magdata_get(int16_t *destination)
float em7180_mres_get(uint8_t Mscale)
{
float m_res;
switch(Mscale)
{
// Possible magnetometer scales (and their register bit settings) are:
// 14 bit resolution (0) and 16 bit resolution (1)
/*
* Possible magnetometer scales (and their register bit settings) are:
* 14 bit resolution (0) and 16 bit resolution (1)
*/
case MFS_14BITS:
_mRes = 10. * 4912. / 8190.; // Proper scale to return milliGauss
return _mRes;
m_res = 10. * 4912. / 8190.; // Proper scale to return milliGauss
break;
case MFS_16BITS:
_mRes = 10. * 4912. / 32760.0; // Proper scale to return milliGauss
return _mRes;
m_res = 10. * 4912. / 32760.0; // Proper scale to return milliGauss
break;
default:
m_res = NAN;
break;
}
return m_res;
}
float em7180_gres_get(uint8_t gscale)
{
float g_res;
switch(gscale)
{
// Possible gyro scales (and their register bit settings) are:
// 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS (11).
// Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value:
/*
* Possible gyro scales (and their register bit settings) are:
* 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS (11).
* Here's a bit of an algorithm to calculate DPS/(ADC tick) based on that 2-bit value:
*/
case GFS_250DPS:
_gRes = 250.0 / 32768.0;
return _gRes;
g_res = 250.0 / 32768.0;
break;
case GFS_500DPS:
_gRes = 500.0 / 32768.0;
return _gRes;
g_res = 500.0 / 32768.0;
break;
case GFS_1000DPS:
_gRes = 1000.0 / 32768.0;
return _gRes;
g_res = 1000.0 / 32768.0;
break;
case GFS_2000DPS:
_gRes = 2000.0 / 32768.0;
return _gRes;
g_res = 2000.0 / 32768.0;
break;
default:
g_res = NAN;
break;
}
return g_res;
}
float em7180_ares_get(uint8_t ascale)
{
float a_res;
switch(ascale)
{
// Possible accelerometer scales (and their register bit settings) are:
// 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs (11).
// Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value:
/*
* Possible accelerometer scales (and their register bit settings) are:
* 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs (11).
* Here's a bit of an algorithm to calculate DPS/(ADC tick) based on that 2-bit value:
*/
case AFS_2G:
_aRes = 2.0 / 32768.0;
return _aRes;
a_res = 2.0 / 32768.0;
break;
case AFS_4G:
_aRes = 4.0 / 32768.0;
return _aRes;
a_res = 4.0 / 32768.0;
break;
case AFS_8G:
_aRes = 8.0 / 32768.0;
return _aRes;
a_res = 8.0 / 32768.0;
break;
case AFS_16G:
_aRes = 16.0 / 32768.0;
return _aRes;
a_res = 16.0 / 32768.0;
break;
default:
a_res = NAN;
break;
}
return a_res;
}
int16_t em7180_baro_get()

60
Drivers/EM7180/Src/lis2mdl.c
Unescape View File

@ -16,11 +16,10 @@ @@ -16,11 +16,10 @@
/* Includes */
#include <stdint.h>
#include "em7180_common.h"
#include "lis2mdl.h"
/* Private Global Variables */
static uint8_t _intPin;
static float _mRes;
/* Function Prototypes */
static void lis2mdl_write_byte(uint8_t address, uint8_t subAddress,
@ -30,10 +29,29 @@ static void lis2mdl_read(uint8_t address, uint8_t subAddress, uint8_t count, @@ -30,10 +29,29 @@ static void lis2mdl_read(uint8_t address, uint8_t subAddress, uint8_t count,
uint8_t *dest);
/* Function Definitions */
lis2mdl_new(uint8_t pin)
void lis2mdl_init(lis2mdl_t *lis2mdl, I2C_HandleTypeDef *hi2c, uint8_t m_odr)
{
/* pinMode(pin, INPUT); */
_intPin = pin;
if(!lis2mdl)
{
return;
}
lis2mdl->hi2c = hi2c;
lis2mdl->m_odr = m_odr;
}
void lis2mdl_config(lis2mdl_t *lis2mdl)
{
// enable temperature compensation (bit 7 == 1), continuous mode (bits 0:1 == 00)
lis2mdl_write_byte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A,
0x80 | lis2mdl->m_odr << 2);
// enable low pass filter (bit 0 == 1), set to ODR/4
lis2mdl_write_byte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_B, 0x01);
// enable data ready on interrupt pin (bit 0 == 1), enable block data read (bit 4 == 1)
lis2mdl_write_byte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C, 0x01 | 0x10);
}
uint8_t lis2mdl_chip_id_get()
@ -52,20 +70,6 @@ void lis2mdl_reset() @@ -52,20 +70,6 @@ void lis2mdl_reset()
HAL_Delay(100); // Wait for all registers to reset
}
void lis2mdl_init(uint8_t MODR)
{
// enable temperature compensation (bit 7 == 1), continuous mode (bits 0:1 == 00)
lis2mdl_write_byte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A, 0x80 | MODR << 2);
// enable low pass filter (bit 0 == 1), set to ODR/4
lis2mdl_write_byte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_B, 0x01);
// enable data ready on interrupt pin (bit 0 == 1), enable block data read (bit 4 == 1)
lis2mdl_write_byte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C, 0x01 | 0x10);
}
uint8_t lis2mdl_status()
{
// Read the status register of the altimeter
@ -99,7 +103,7 @@ void lis2mdl_offset_bias(float *dest1, float *dest2) @@ -99,7 +103,7 @@ void lis2mdl_offset_bias(float *dest1, float *dest2)
int32_t mag_bias[3] = { 0, 0, 0 }, mag_scale[3] = { 0, 0, 0 };
int16_t mag_max[3] = { -32767, -32767, -32767 }, mag_min[3] =
{ 32767, 32767, 32767 }, mag_temp[3] = { 0, 0, 0 };
float _mRes = 0.0015f;
float m_res = 0.0015f;
/* Serial.println("Calculate mag offset bias: move all around to sample the complete response surface!"); */
HAL_Delay(4000);
@ -121,15 +125,15 @@ void lis2mdl_offset_bias(float *dest1, float *dest2) @@ -121,15 +125,15 @@ void lis2mdl_offset_bias(float *dest1, float *dest2)
HAL_Delay(12);
}
_mRes = 0.0015f; // fixed sensitivity
m_res = 0.0015f; // fixed sensitivity
// Get hard iron correction
mag_bias[0] = (mag_max[0] + mag_min[0]) / 2; // get average x mag bias in counts
mag_bias[1] = (mag_max[1] + mag_min[1]) / 2; // get average y mag bias in counts
mag_bias[2] = (mag_max[2] + mag_min[2]) / 2; // get average z mag bias in counts
dest1[0] = (float) mag_bias[0] * _mRes; // save mag biases in G for main program
dest1[1] = (float) mag_bias[1] * _mRes;
dest1[2] = (float) mag_bias[2] * _mRes;
dest1[0] = (float) mag_bias[0] * m_res; // save mag biases in G for main program
dest1[1] = (float) mag_bias[1] * m_res;
dest1[2] = (float) mag_bias[2] * m_res;
// Get soft iron correction estimate
mag_scale[0] = (mag_max[0] - mag_min[0]) / 2; // get average x axis max chord length in counts
@ -152,7 +156,7 @@ void lis2mdl_self_test() @@ -152,7 +156,7 @@ void lis2mdl_self_test()
float magTest[3] = { 0., 0., 0. };
float magNom[3] = { 0., 0., 0. };
int32_t sum[3] = { 0, 0, 0 };
float _mRes = 0.0015f;
float m_res = 0.0015f;
// first, get average response with self test disabled
for(int ii = 0; ii < 50; ii++)
@ -193,12 +197,12 @@ void lis2mdl_self_test() @@ -193,12 +197,12 @@ void lis2mdl_self_test()
/* Serial.println("Mag Self Test:"); */
/* Serial.print("Mx results:"); */
/* Serial.print((magTest[0] - magNom[0]) * _mRes * 1000.0); */
/* Serial.print((magTest[0] - magNom[0]) * m_res * 1000.0); */
/* Serial.println(" mG"); */
/* Serial.print("My results:"); */
/* Serial.println((magTest[0] - magNom[0]) * _mRes * 1000.0); */
/* Serial.println((magTest[0] - magNom[0]) * m_res * 1000.0); */
/* Serial.print("Mz results:"); */
/* Serial.println((magTest[1] - magNom[1]) * _mRes * 1000.0); */
/* Serial.println((magTest[1] - magNom[1]) * m_res * 1000.0); */
/* Serial.println("Should be between 15 and 500 mG"); */
HAL_Delay(2000); // give some time to read the screen
}

70
Drivers/EM7180/Src/lps22hb.c
Unescape View File

@ -16,65 +16,67 @@ @@ -16,65 +16,67 @@
/* Includes */
#include <stdint.h>
#include "em7180_common.h"
#include "lps22hb.h"
/* Private Global Variables */
static uint8_t _intPin;
/* Function Prototypes */
static void lps22h_write_byte(uint8_t address, uint8_t subAddress, uint8_t data);
static uint8_t lps22h_read_byte(uint8_t address, uint8_t subAddress);
static void lps22h_read(uint8_t address, uint8_t subAddress, uint8_t count,
uint8_t *dest);
static void lps22hb_write_byte(uint8_t address, uint8_t subAddress,
uint8_t data);
static uint8_t lps22hb_read_byte(uint8_t address, uint8_t subAddress);
static void lps22hb_read(uint8_t address, uint8_t subAddress, uint8_t count,
uint8_t *dest);
/* Function Definitions */
lps22h_new(uint8_t pin)
void lps22hb_init(lps22hb_t *lps22hb, I2C_HandleTypeDef *hi2c, uint8_t p_odr)
{
/* pinMode(pin, INPUT); */
_intPin = pin;
lps22hb->hi2c = hi2c;
lps22hb->p_odr = p_odr;
}
uint8_t lps22h_getChipID()
void lps22hb_config(lps22hb_t *lps22hb)
{
// set sample rate by setting bits 6:4
// enable low-pass filter by setting bit 3 to one
// bit 2 == 0 means bandwidth is odr/9, bit 2 == 1 means bandwidth is odr/20
// make sure data not updated during read by setting block data udate (bit 1) to 1
lps22hb_write_byte(LPS22H_ADDRESS, LPS22H_CTRL_REG1,
lps22hb->p_odr << 4 | 0x08 | 0x02);
lps22hb_write_byte(LPS22H_ADDRESS, LPS22H_CTRL_REG3, 0x04); // enable data ready as interrupt source
}
uint8_t lps22hb_chip_id_get()
{
// Read the WHO_AM_I register of the altimeter this is a good test of communication
uint8_t temp = lps22h_read_byte(LPS22H_ADDRESS, LPS22H_WHOAMI); // Read WHO_AM_I register for LPS22H
uint8_t temp = lps22hb_read_byte(LPS22H_ADDRESS, LPS22H_WHOAMI); // Read WHO_AM_I register for LPS22H
return temp;
}
uint8_t lps22h_status()
uint8_t lps22hb_status()
{
// Read the status register of the altimeter
uint8_t temp = lps22h_read_byte(LPS22H_ADDRESS, LPS22H_STATUS);
uint8_t temp = lps22hb_read_byte(LPS22H_ADDRESS, LPS22H_STATUS);
return temp;
}
int32_t lps22h_pressure_get()
int32_t lps22hb_pressure_get()
{
uint8_t rawData[3]; // 24-bit pressure register data stored here
lps22h_read(LPS22H_ADDRESS, (LPS22H_PRESS_OUT_XL | 0x80), 3, &rawData[0]); // bit 7 must be one to read multiple bytes
lps22hb_read(LPS22H_ADDRESS, (LPS22H_PRESS_OUT_XL | 0x80), 3, &rawData[0]); // bit 7 must be one to read multiple bytes
return (int32_t) ((int32_t) rawData[2] << 16 | (int32_t) rawData[1] << 8
| rawData[0]);
}
int16_t lps22h_temp_get()
int16_t lps22hb_temp_get()
{
uint8_t rawData[2]; // 16-bit pressure register data stored here
lps22h_read(LPS22H_ADDRESS, (LPS22H_TEMP_OUT_L | 0x80), 2, &rawData[0]); // bit 7 must be one to read multiple bytes
lps22hb_read(LPS22H_ADDRESS, (LPS22H_TEMP_OUT_L | 0x80), 2, &rawData[0]); // bit 7 must be one to read multiple bytes
return (int16_t) ((int16_t) rawData[1] << 8 | rawData[0]);
}
void lps22h_init(uint8_t PODR)
{
// set sample rate by setting bits 6:4
// enable low-pass filter by setting bit 3 to one
// bit 2 == 0 means bandwidth is odr/9, bit 2 == 1 means bandwidth is odr/20
// make sure data not updated during read by setting block data udate (bit 1) to 1
lps22h_write_byte(LPS22H_ADDRESS, LPS22H_CTRL_REG1,
PODR << 4 | 0x08 | 0x02);
lps22h_write_byte(LPS22H_ADDRESS, LPS22H_CTRL_REG3, 0x04); // enable data ready as interrupt source
}
static void lps22h_write_byte(uint8_t address, uint8_t subAddress, uint8_t data)
static void lps22hb_write_byte(uint8_t address, uint8_t subAddress,
uint8_t data)
{
/* Wire.beginTransmission(address); // Initialize the Tx buffer */
/* Wire.write(subAddress); // Put slave register address in Tx buffer */
@ -82,7 +84,7 @@ static void lps22h_write_byte(uint8_t address, uint8_t subAddress, uint8_t data) @@ -82,7 +84,7 @@ static void lps22h_write_byte(uint8_t address, uint8_t subAddress, uint8_t data)
/* Wire.endTransmission(); // Send the Tx buffer */
}
static uint8_t lps22h_read_byte(uint8_t address, uint8_t subAddress)
static uint8_t lps22hb_read_byte(uint8_t address, uint8_t subAddress)
{
uint8_t data; // `data` will store the register data
/* Wire.beginTransmission(address); // Initialize the Tx buffer */
@ -90,11 +92,11 @@ static uint8_t lps22h_read_byte(uint8_t address, uint8_t subAddress) @@ -90,11 +92,11 @@ static uint8_t lps22h_read_byte(uint8_t address, uint8_t subAddress)
/* Wire.endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive */
/* Wire.requestFrom(address, (size_t) 1); // Read one uint8_t from slave register address */
/* data = Wire.read(); // Fill Rx buffer with result */
return data; // Return data lps22h_read from slave register
return data; // Return data lps22hb_read from slave register
}
static void lps22h_read(uint8_t address, uint8_t subAddress, uint8_t count,
uint8_t *dest)
static void lps22hb_read(uint8_t address, uint8_t subAddress, uint8_t count,
uint8_t *dest)
{
/* Wire.beginTransmission(address); // Initialize the Tx buffer */
/* Wire.write(subAddress); // Put slave register address in Tx buffer */
@ -104,7 +106,7 @@ static void lps22h_read(uint8_t address, uint8_t subAddress, uint8_t count, @@ -104,7 +106,7 @@ static void lps22h_read(uint8_t address, uint8_t subAddress, uint8_t count,
/*
while(Wire.available())
{
dest[i++] = Wire.lps22h_read();
} // Put lps22h_read results in the Rx buffer
dest[i++] = Wire.lps22hb_read();
} // Put lps22hb_read results in the Rx buffer
*/
}

190
Drivers/EM7180/Src/lsm6dsm.c
Unescape View File

@ -16,28 +16,51 @@ @@ -16,28 +16,51 @@
/* Includes */
#include <stdint.h>
#include <math.h>
#include "em7180_common.h"
#include "lsm6dsm.h"
/* Private Global Variables */
static uint8_t _intPin1;
static uint8_t _intPin2;
static float _aRes;
static float _gRes;
/* Function Prototypes */
static void lsm6dsm_write_byte(uint8_t address, uint8_t subAddress,
uint8_t data);
static uint8_t lsm6dsm_read_byte(uint8_t address, uint8_t subAddress);
static void lsm6dsm_read(uint8_t address, uint8_t subAddress, uint8_t count,
uint8_t *dest);
/* Function Definitions */
lsm6dsm_new(uint8_t pin1, uint8_t pin2)
void lsm6dsm_init(lsm6dsm_t *lsm6dsm, I2C_HandleTypeDef *hi2c, uint8_t ascale,
uint8_t gscale, uint8_t a_odr, uint8_t g_odr)
{
/* pinMode(pin1, INPUT); */
_intPin1 = pin1;
/* pinMode(pin2, INPUT); */
_intPin2 = pin2;
if(!lsm6dsm)
{
return;
}
lsm6dsm->hi2c = hi2c;
lsm6dsm->ascale = ascale;
lsm6dsm->gscale = gscale;
lsm6dsm->a_odr = a_odr;
lsm6dsm->g_odr = g_odr;
}
void lsm6dsm_config(lsm6dsm_t *lsm6dsm)
{
lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL1_XL,
lsm6dsm->a_odr << 4 | lsm6dsm->ascale << 2);
lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL2_G,
lsm6dsm->g_odr << 4 | lsm6dsm->gscale << 2);
uint8_t temp = lsm6dsm_read_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL3_C);
// enable block update (bit 6 = 1), auto-increment registers (bit 2 = 1)
lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL3_C, temp | 0x40 | 0x04);
// by default, interrupts active HIGH, push pull, little endian data
// (can be changed by writing to bits 5, 4, and 1, resp to above register)
// enable accel LP2 (bit 7 = 1), set LP2 tp ODR/9 (bit 6 = 1), enable input_composite (bit 3) for low noise
lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL8_XL, 0x80 | 0x40 | 0x08);
// interrupt handling
lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_DRDY_PULSE_CFG, 0x80); // latch interrupt until data read
lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_INT1_CTRL, 0x40); // enable significant motion interrupts on INT1
lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_INT2_CTRL, 0x03); // enable accel/gyro data ready interrupts on INT2
}
uint8_t lsm6dsm_chip_id_get()
@ -46,55 +69,61 @@ uint8_t lsm6dsm_chip_id_get() @@ -46,55 +69,61 @@ uint8_t lsm6dsm_chip_id_get()
return c;
}
float lsm6dsm_ares_get(uint8_t ascale)
float lsm6dsm_ares_get(lsm6dsm_t *lsm6dsm)
{
switch(ascale)
float a_res;
switch(lsm6dsm->ascale)
{
// Possible accelerometer scales (and their register bit settings) are:
// 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs (11).
// Here's a bit of an algorithm to calculate DPS/(ADC tick) based on that 2-bit value:
case AFS_2G:
_aRes = 2.0f / 32768.0f;
return _aRes;
a_res = 2.0f / 32768.0f;
break;
case AFS_4G:
_aRes = 4.0f / 32768.0f;
return _aRes;
a_res = 4.0f / 32768.0f;
break;
case AFS_8G:
_aRes = 8.0f / 32768.0f;
return _aRes;
a_res = 8.0f / 32768.0f;
break;
case AFS_16G:
_aRes = 16.0f / 32768.0f;
return _aRes;
a_res = 16.0f / 32768.0f;
break;
default:
a_res = NAN;
break;
}
return a_res;
}
float lsm6dsm_gres_get(uint8_t gscale)
float lsm6dsm_gres_get(lsm6dsm_t *lsm6dsm)
{
switch(gscale)
float g_res;
switch(lsm6dsm->gscale)
{
// Possible gyro scales (and their register bit settings) are:
// 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS (11).
case GFS_245DPS:
_gRes = 245.0f / 32768.0f;
return _gRes;
case GFS_250DPS:
g_res = 250.0f / 32768.0f;
break;
case GFS_500DPS:
_gRes = 500.0f / 32768.0f;
return _gRes;
g_res = 500.0f / 32768.0f;
break;
case GFS_1000DPS:
_gRes = 1000.0f / 32768.0f;
return _gRes;
g_res = 1000.0f / 32768.0f;
break;
case GFS_2000DPS:
_gRes = 2000.0f / 32768.0f;
return _gRes;
g_res = 2000.0f / 32768.0f;
break;
default:
g_res = NAN;
break;
}
return g_res;
}
void lsm6dsm_reset()
@ -105,29 +134,6 @@ void lsm6dsm_reset() @@ -105,29 +134,6 @@ void lsm6dsm_reset()
HAL_Delay(100); // Wait for all registers to reset
}
void lsm6dsm_init(uint8_t ascale, uint8_t gscale, uint8_t AODR, uint8_t GODR)
{
lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL1_XL,
AODR << 4 | ascale << 2);
lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL2_G,
GODR << 4 | gscale << 2);
uint8_t temp = lsm6dsm_read_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL3_C);
// enable block update (bit 6 = 1), auto-increment registers (bit 2 = 1)
lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL3_C, temp | 0x40 | 0x04);
// by default, interrupts active HIGH, push pull, little endian data
// (can be changed by writing to bits 5, 4, and 1, resp to above register)
// enable accel LP2 (bit 7 = 1), set LP2 tp ODR/9 (bit 6 = 1), enable input_composite (bit 3) for low noise
lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL8_XL, 0x80 | 0x40 | 0x08);
// interrupt handling
lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_DRDY_PULSE_CFG, 0x80); // latch interrupt until data read
lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_INT1_CTRL, 0x40); // enable significant motion interrupts on INT1
lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_INT2_CTRL, 0x03); // enable accel/gyro data ready interrupts on INT2
}
void lsm6dsm_selfTest()
{
int16_t temp[7] = { 0, 0, 0, 0, 0, 0, 0 };
@ -135,7 +141,7 @@ void lsm6dsm_selfTest() @@ -135,7 +141,7 @@ void lsm6dsm_selfTest()
gyroPTest[3] = { 0, 0, 0 }, gyroNTest[3] = { 0, 0, 0 };
int16_t accelNom[3] = { 0, 0, 0 }, gyroNom[3] = { 0, 0, 0 };
readData(temp);
lsm6dsm_read_data(temp);
accelNom[0] = temp[4];
accelNom[1] = temp[5];
accelNom[2] = temp[6];
@ -145,28 +151,28 @@ void lsm6dsm_selfTest() @@ -145,28 +151,28 @@ void lsm6dsm_selfTest()
lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x01); // positive accel self test
HAL_Delay(100); // let accel respond
readData(temp);
lsm6dsm_read_data(temp);
accelPTest[0] = temp[4];
accelPTest[1] = temp[5];
accelPTest[2] = temp[6];
lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x03); // negative accel self test
HAL_Delay(100); // let accel respond
readData(temp);
lsm6dsm_read_data(temp);
accelNTest[0] = temp[4];
accelNTest[1] = temp[5];
accelNTest[2] = temp[6];
lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x04); // positive gyro self test
HAL_Delay(100); // let gyro respond
readData(temp);
lsm6dsm_read_data(temp);
gyroPTest[0] = temp[1];
gyroPTest[1] = temp[2];
gyroPTest[2] = temp[3];
lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x0C); // negative gyro self test
HAL_Delay(100); // let gyro respond
readData(temp);
lsm6dsm_read_data(temp);
gyroNTest[0] = temp[1];
gyroNTest[1] = temp[2];
gyroNTest[2] = temp[3];
@ -176,50 +182,52 @@ void lsm6dsm_selfTest() @@ -176,50 +182,52 @@ void lsm6dsm_selfTest()
/* Serial.println("Accel Self Test:"); */
/* Serial.print("+Ax results:"); */
/* Serial.print((accelPTest[0] - accelNom[0]) * _aRes * 1000.0); */
/* Serial.print((accelPTest[0] - accelNom[0]) * a_res * 1000.0); */
/* Serial.println(" mg"); */
/* Serial.print("-Ax results:"); */
/* Serial.println((accelNTest[0] - accelNom[0]) * _aRes * 1000.0); */
/* Serial.println((accelNTest[0] - accelNom[0]) * a_res * 1000.0); */
/* Serial.print("+Ay results:"); */
/* Serial.println((accelPTest[1] - accelNom[1]) * _aRes * 1000.0); */
/* Serial.println((accelPTest[1] - accelNom[1]) * a_res * 1000.0); */
/* Serial.print("-Ay results:"); */
/* Serial.println((accelNTest[1] - accelNom[1]) * _aRes * 1000.0); */
/* Serial.println((accelNTest[1] - accelNom[1]) * a_res * 1000.0); */
/* Serial.print("+Az results:"); */
/* Serial.println((accelPTest[2] - accelNom[2]) * _aRes * 1000.0); */
/* Serial.println((accelPTest[2] - accelNom[2]) * a_res * 1000.0); */
/* Serial.print("-Az results:"); */
/* Serial.println((accelNTest[2] - accelNom[2]) * _aRes * 1000.0); */
/* Serial.println((accelNTest[2] - accelNom[2]) * a_res * 1000.0); */
/* Serial.println("Should be between 90 and 1700 mg"); */
/* Serial.println("Gyro Self Test:"); */
/* Serial.print("+Gx results:"); */
/* Serial.print((gyroPTest[0] - gyroNom[0]) * _gRes); */
/* Serial.print((gyroPTest[0] - gyroNom[0]) * g_res); */
/* Serial.println(" dps"); */
/* Serial.print("-Gx results:"); */
/* Serial.println((gyroNTest[0] - gyroNom[0]) * _gRes); */
/* Serial.println((gyroNTest[0] - gyroNom[0]) * g_res); */
/* Serial.print("+Gy results:"); */
/* Serial.println((gyroPTest[1] - gyroNom[1]) * _gRes); */
/* Serial.println((gyroPTest[1] - gyroNom[1]) * g_res); */
/* Serial.print("-Gy results:"); */
/* Serial.println((gyroNTest[1] - gyroNom[1]) * _gRes); */
/* Serial.println((gyroNTest[1] - gyroNom[1]) * g_res); */
/* Serial.print("+Gz results:"); */
/* Serial.println((gyroPTest[2] - gyroNom[2]) * _gRes); */
/* Serial.println((gyroPTest[2] - gyroNom[2]) * g_res); */
/* Serial.print("-Gz results:"); */
/* Serial.println((gyroNTest[2] - gyroNom[2]) * _gRes); */
/* Serial.println((gyroNTest[2] - gyroNom[2]) * g_res); */
/* Serial.println("Should be between 20 and 80 dps"); */
HAL_Delay(2000);
}
void lsm6dsm_offsetBias(float *dest1, float *dest2)
void lsm6dsm_offset_bias(lsm6dsm_t *lsm6dsm, float *dest1, float *dest2)
{
int16_t temp[7] = { 0, 0, 0, 0, 0, 0, 0 };
int32_t sum[7] = { 0, 0, 0, 0, 0, 0, 0 };
float a_res = lsm6dsm_ares_get(lsm6dsm);
float g_res = lsm6dsm_gres_get(lsm6dsm);
/* Serial.println("Calculate accel and gyro offset biases: keep sensor flat and motionless!"); */
HAL_Delay(4000);
for(int ii = 0; ii < 128; ii++)
{
readData(temp);
lsm6dsm_read_data(temp);
sum[1] += temp[1];
sum[2] += temp[2];
sum[3] += temp[3];
@ -229,12 +237,12 @@ void lsm6dsm_offsetBias(float *dest1, float *dest2) @@ -229,12 +237,12 @@ void lsm6dsm_offsetBias(float *dest1, float *dest2)
HAL_Delay(50);
}
dest1[0] = sum[1] * _gRes / 128.0f;
dest1[1] = sum[2] * _gRes / 128.0f;
dest1[2] = sum[3] * _gRes / 128.0f;
dest2[0] = sum[4] * _aRes / 128.0f;
dest2[1] = sum[5] * _aRes / 128.0f;
dest2[2] = sum[6] * _aRes / 128.0f;
dest1[0] = sum[1] * g_res / 128.0f;
dest1[1] = sum[2] * g_res / 128.0f;
dest1[2] = sum[3] * g_res / 128.0f;
dest2[0] = sum[4] * a_res / 128.0f;
dest2[1] = sum[5] * a_res / 128.0f;
dest2[2] = sum[6] * a_res / 128.0f;
if(dest2[0] > 0.8f)
{
@ -277,24 +285,22 @@ void lsm6dsm_read_data(int16_t *destination) @@ -277,24 +285,22 @@ void lsm6dsm_read_data(int16_t *destination)
}
// I2C read/write functions for the LSM6DSM
static void lsm6dsm_write_byte(uint8_t address, uint8_t subAddress,
uint8_t data)
void lsm6dsm_write_byte(uint8_t addr, uint8_t sub_addr, uint8_t data)
{
uint8_t temp[2];
temp[0] = subAddress;
temp[0] = sub_addr;
temp[1] = data;
/* Wire.transfer(address, &temp[0], 2, NULL, 0); */
/* Wire.transfer(addr, &temp[0], 2, NULL, 0); */
}
static uint8_t lsm6dsm_read_byte(uint8_t address, uint8_t subAddress)
uint8_t lsm6dsm_read_byte(uint8_t addr, uint8_t sub_addr)
{
uint8_t temp[1];
/* Wire.transfer(address, &subAddress, 1, &temp[0], 1); */
/* Wire.transfer(addr, &sub_addr, 1, &temp[0], 1); */
return temp[0];
}
static void lsm6dsm_read(uint8_t address, uint8_t subAddress, uint8_t count,
uint8_t *dest)
void lsm6dsm_read(uint8_t addr, uint8_t sub_addr, uint8_t count, uint8_t *dest)
{
/* Wire.transfer(address, &subAddress, 1, dest, count); */
/* Wire.transfer(addr, &sub_addr, 1, dest, count); */
}

Write Preview
Loading…
Cancel
Save