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 5 years ago
parent
16d2932478
commit
6f195cc05b
9 changed files with 525 additions and 476 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 38
      Drivers/EM7180/Inc/em7180.h
  2. 22
      Drivers/EM7180/Inc/em7180_common.h
  3. 15
      Drivers/EM7180/Inc/lis2mdl.h
  4. 24
      Drivers/EM7180/Inc/lps22hb.h
  5. 20
      Drivers/EM7180/Inc/lsm6dsm.h
  6. 490
      Drivers/EM7180/Src/em7180.c
  7. 60
      Drivers/EM7180/Src/lis2mdl.c
  8. 66
      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 @@
/* Includes */ /* Includes */
#include <stdint.h> #include <stdint.h>
#include "i2c.h"
#include "lsm6dsm.h"
#include "lps22hb.h"
#include "lis2mdl.h"
/* Definitions */ /* Definitions */
/* /*
@ -87,28 +91,32 @@
#define EM7180_ResetRequest 0x9B #define EM7180_ResetRequest 0x9B
#define EM7180_PassThruStatus 0x9E #define EM7180_PassThruStatus 0x9E
#define EM7180_PassThruControl 0xA0 #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 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_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 M24512DFM_IDPAGE_ADDRESS 0x58 // Address of the single M24512DFM lockable EEPROM ID page
#define AFS_2G 0 /* Data Structures */
#define AFS_4G 1 typedef struct em7180_s
#define AFS_8G 2 {
#define AFS_16G 3 I2C_HandleTypeDef *hi2c;
lsm6dsm_t *lsm6dsm;
#define GFS_250DPS 0 uint16_t acc_fs;
#define GFS_500DPS 1 uint16_t gyro_fs;
#define GFS_1000DPS 2 uint16_t mag_fs;
#define GFS_2000DPS 3 uint8_t q_rate_div;
uint8_t mag_rate;
#define MFS_14BITS 0 // 0.6 mG per LSB uint8_t acc_rate;
#define MFS_16BITS 1 // 0.15 mG per LSB uint8_t gyro_rate;
uint8_t baro_rate;
} em7180_t;
/* Function Prototypes */ /* 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_gyro_set_fs(uint16_t gyro_fs);
void em7180_mag_acc_set_fs(uint16_t mag_fs, uint16_t acc_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); 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 @@
/*
* 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 @@
/* Includes */ /* Includes */
#include <stdint.h> #include <stdint.h>
#include "i2c.h"
/* Definitions */ /* Definitions */
//Register map for LIS2MDL' //Register map for LIS2MDL'
@ -54,4 +55,18 @@
#define MODR_50Hz 0x02 #define MODR_50Hz 0x02
#define MODR_100Hz 0x03 #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 #endif

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

@ -19,6 +19,7 @@
/* Includes */ /* Includes */
#include <stdint.h> #include <stdint.h>
#include "i2c.h"
// See LPS22H "MEMS pressure sensor: 260-1260 hPa absolute digital output barometer" Data Sheet // 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 // 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 @@
#define LPS22H_ADDRESS 0x5C // Address of altimeter #define LPS22H_ADDRESS 0x5C // Address of altimeter
// Altimeter output data rate // Altimeter output data rate
#define P_1shot 0x00; #define P_1shot 0x00
#define P_1Hz 0x01; #define P_1Hz 0x01
#define P_10Hz 0x02; #define P_10Hz 0x02
#define P_25Hz 0x03; // 25 Hz output data rate #define P_25Hz 0x03 // 25 Hz output data rate
#define P_50Hz 0x04; #define P_50Hz 0x04
#define P_75Hz 0x05; #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 #endif

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

@ -19,7 +19,9 @@
/* Includes */ /* Includes */
#include <stdint.h> #include <stdint.h>
#include "i2c.h"
/* Definitions */
/* /*
* LSM6DSM registers * 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 * 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 @@
#define LSM6DSM_ADDRESS 0x6A // Address of LSM6DSM accel/gyro when ADO = 0 #define LSM6DSM_ADDRESS 0x6A // Address of LSM6DSM accel/gyro when ADO = 0
#define AFS_2G 0x00 #define AFS_2G 0x00
#define AFS_4G 0x02 #define AFS_4G 0x02
#define AFS_8G 0x03 #define AFS_8G 0x03
#define AFS_16G 0x01 #define AFS_16G 0x01
#define GFS_245DPS 0x00 #define GFS_250DPS 0x00
#define GFS_500DPS 0x01 #define GFS_500DPS 0x01
#define GFS_1000DPS 0x02 #define GFS_1000DPS 0x02
#define GFS_2000DPS 0x03 #define GFS_2000DPS 0x03
@ -158,4 +159,19 @@
#define GODR_3330Hz 0x09 #define GODR_3330Hz 0x09
#define GODR_6660Hz 0x0A #define GODR_6660Hz 0x0A
/* 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 #endif

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

@ -16,16 +16,16 @@
#include <stdint.h> #include <stdint.h>
#include <stdbool.h> #include <stdbool.h>
#include <math.h> #include <math.h>
#include "em7180_common.h"
#include "em7180.h" #include "em7180.h"
#include "i2c.h"
/* Data Structures */
/* Private Global Variables */ /* Private Global Variables */
static uint8_t _intPin;
static bool _passThru;
static float _aRes;
static float _gRes;
static float _mRes;
/* Function Prototypes */ /* 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, static void m24512dfm_write_byte(uint8_t device_address, uint8_t data_address1,
uint8_t data_address2, uint8_t data); uint8_t data_address2, uint8_t data);
static void m24512dfm_write(uint8_t device_address, uint8_t data_address1, 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,
uint8_t *dest); uint8_t *dest);
/* Function Definitions */ /* Function Definitions */
em7180_new(uint8_t pin, bool passthru) void em7180_init(em7180_t *em7180, lsm6dsm_t *lsm6dsm, I2C_HandleTypeDef *hi2c1,
{ uint16_t acc_fs, uint16_t gyro_fs, uint16_t mag_fs,
/* pinMode(pin, INPUT); */ uint8_t q_rate_div, uint8_t mag_rate, uint8_t acc_rate,
_intPin = pin; uint8_t gyro_rate, uint8_t baro_rate)
_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()
{
// 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(!em7180)
}
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!"); */ return;
}
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)) em7180->lsm6dsm = lsm6dsm;
{ em7180->acc_fs = acc_fs;
/* Serial.println("EEPROM upload successful!"); */ em7180->gyro_fs = gyro_fs;
} em7180->mag_fs = mag_fs;
} em7180->q_rate_div = q_rate_div;
em7180->mag_rate = mag_rate;
uint8_t em7180_status() em7180->acc_rate = acc_rate;
{ em7180->gyro_rate = gyro_rate;
// Check event status register, way to check data ready by polling rather than interrupt em7180->baro_rate = baro_rate;
uint8_t c = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_EventStatus); // reading clears the register and interrupt
return c;
}
uint8_t em7180_errors() em7180_config(em7180);
{
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, void em7180_config(em7180_t *em7180)
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[4];
uint8_t param_xfer;
uint8_t runStatus;
uint8_t algoStatus;
uint8_t passthruStatus;
uint8_t eventStatus;
uint8_t sensorStatus;
// Enter EM7180 initialized state // Enter EM7180 initialized state
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_HostControl, 0x00); // set SENtral in initialized state to configure registers 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,
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_HostControl, 0x01); // Force initialize 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 lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_HostControl, 0x00); // set SENtral in initialized state to configure registers
/* Legacy MPU6250 stuff, it seems
// Setup LPF bandwidth (BEFORE setting ODR's) // 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_ACC_LPF_BW, accBW); // accBW = 3 = 41Hz
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_GYRO_LPF_BW, gyroBW); // gyroBW = 3 = 41Hz lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_GYRO_LPF_BW, gyroBW); // gyroBW = 3 = 41Hz */
// Set accel/gyro/mag desired ODR rates // 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_QRateDivisor, em7180->q_rate_div); // quat rate = gyroRt/(1 QRTDiv)
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_MagRate, magRt); // 0x64 = 100 Hz lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_MagRate, em7180->mag_rate); // 0x64 = 100 Hz
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AccelRate, accRt); // 200/10 Hz, 0x14 = 200 Hz lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AccelRate, em7180->acc_rate); // 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_GyroRate, em7180->gyro_rate); // 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_BaroRate,
0x80 | em7180->baro_rate); // set enable bit and set Baro rate to 25 Hz, rate = baroRt/2, 0x32 = 25 Hz
// Configure operating mode // Configure operating mode
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // read scale sensor data 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,
// EM7180 parameter adjustments // EM7180 parameter adjustments
/* Serial.println("Beginning 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 // Disable stillness mode for balancing robot application
EM7180_set_integer_param(0x49, 0x00); em7180_set_integer_param(0x49, 0x00);
// Write desired sensor full scale ranges to the EM7180 // Write desired sensor full scale ranges to the EM7180
EM7180_set_mag_acc_FS(magFS, accFS); // 1000 uT == 0x3E8, 8 g == 0x08 em7180_mag_acc_set_fs(em7180->mag_fs, em7180->acc_fs); // 1000 uT == 0x3E8, 8 g == 0x08
EM7180_set_gyro_FS(gyroFS); // 2000 dps == 0x7D0 em7180_gyro_set_fs(em7180->gyro_fs); // 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
// Read EM7180 status // Read EM7180 status
uint8_t runStatus = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_RunStatus); runStatus = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_RunStatus);
if(runStatus & 0x01) if(runStatus & 0x01)
{ {
/* Serial.println(" EM7180 run status = normal mode"); */ /* Serial.println(" EM7180 run status = normal mode"); */
} }
uint8_t algoStatus = lsm6dsm_read_byte(EM7180_ADDRESS, algoStatus = lsm6dsm_read_byte(EM7180_ADDRESS,
EM7180_AlgorithmStatus); EM7180_AlgorithmStatus);
if(algoStatus & 0x01) if(algoStatus & 0x01)
{ {
@ -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"); */ /* Serial.println(" EM7180 unreliable sensor data"); */
} }
uint8_t passthruStatus = lsm6dsm_read_byte(EM7180_ADDRESS, passthruStatus = lsm6dsm_read_byte(EM7180_ADDRESS,
EM7180_PassThruStatus); EM7180_PassThruStatus);
if(passthruStatus & 0x01) if(passthruStatus & 0x01)
{ {
/* Serial.print(" EM7180 in passthru mode!"); */ /* 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) if(eventStatus & 0x01)
{ {
/* Serial.println(" EM7180 CPU reset"); */ /* Serial.println(" EM7180 CPU reset"); */
@ -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 HAL_Delay(1000); // give some time to read the screen
// Check sensor status // Check sensor status
uint8_t sensorStatus = lsm6dsm_read_byte(EM7180_ADDRESS, sensorStatus = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SensorStatus);
EM7180_SensorStatus);
/* Serial.print(" EM7180 sensor status = "); */ /* Serial.print(" EM7180 sensor status = "); */
/* Serial.println(sensorStatus); */ /* Serial.println(sensorStatus); */
if(sensorStatus & 0x01) if(sensorStatus & 0x01)
@ -405,6 +219,137 @@ void em7180_init(uint8_t accBW, uint8_t gyroBW, uint16_t accFS, uint16_t gyroFS,
/* Serial.println(" Hz"); */ /* 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) float em7180_uint32_reg_to_float(uint8_t *buf)
{ {
union union
@ -431,19 +376,21 @@ float em7180_int32_reg_to_float(uint8_t *buf)
return u.f; 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 union
{ {
float f; float f;
uint8_t comp[sizeof(float)]; uint8_t u8[sizeof(float)];
} u; } u;
u.f = param_val; u.f = param_val;
for(uint8_t i = 0; i < sizeof(float); i++) 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++) for(uint8_t i = 0; i < sizeof(float); i++)
{ {
buf[i] = buf[(sizeof(float) - 1) - i]; buf[i] = buf[(sizeof(float) - 1) - i];
@ -461,7 +408,7 @@ void em7180_gyro_set_fs(uint16_t gyro_fs)
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte1, bytes[1]); // Gyro MSB 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_LoadParamByte2, bytes[2]); // Unused
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte3, bytes[3]); // 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_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 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 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)) while(!(STAT == 0xCB))
@ -520,7 +467,7 @@ void em7180_set_integer_param(uint8_t param, uint32_t param_val)
void em7180_param_set_float(uint8_t param, float param_val) void em7180_param_set_float(uint8_t param, float param_val)
{ {
uint8_t bytes[4], STAT; uint8_t bytes[4], STAT;
float_to_bytes(param_val, &bytes[0]); 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 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_LoadParamByte0, bytes[0]); // Param LSB
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte1, bytes[1]); lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte1, bytes[1]);
@ -577,71 +524,88 @@ void em7180_magdata_get(int16_t *destination)
float em7180_mres_get(uint8_t Mscale) float em7180_mres_get(uint8_t Mscale)
{ {
float m_res;
switch(Mscale) 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: case MFS_14BITS:
_mRes = 10. * 4912. / 8190.; // Proper scale to return milliGauss m_res = 10. * 4912. / 8190.; // Proper scale to return milliGauss
return _mRes;
break; break;
case MFS_16BITS: case MFS_16BITS:
_mRes = 10. * 4912. / 32760.0; // Proper scale to return milliGauss m_res = 10. * 4912. / 32760.0; // Proper scale to return milliGauss
return _mRes; break;
default:
m_res = NAN;
break; break;
} }
return m_res;
} }
float em7180_gres_get(uint8_t gscale) float em7180_gres_get(uint8_t gscale)
{ {
float g_res;
switch(gscale) switch(gscale)
{ {
// Possible gyro scales (and their register bit settings) are: /*
// 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS (11). * Possible gyro scales (and their register bit settings) are:
// Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value: * 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: case GFS_250DPS:
_gRes = 250.0 / 32768.0; g_res = 250.0 / 32768.0;
return _gRes;
break; break;
case GFS_500DPS: case GFS_500DPS:
_gRes = 500.0 / 32768.0; g_res = 500.0 / 32768.0;
return _gRes;
break; break;
case GFS_1000DPS: case GFS_1000DPS:
_gRes = 1000.0 / 32768.0; g_res = 1000.0 / 32768.0;
return _gRes;
break; break;
case GFS_2000DPS: case GFS_2000DPS:
_gRes = 2000.0 / 32768.0; g_res = 2000.0 / 32768.0;
return _gRes; break;
default:
g_res = NAN;
break; break;
} }
return g_res;
} }
float em7180_ares_get(uint8_t ascale) float em7180_ares_get(uint8_t ascale)
{ {
float a_res;
switch(ascale) switch(ascale)
{ {
// Possible accelerometer scales (and their register bit settings) are: /*
// 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs (11). * Possible accelerometer scales (and their register bit settings) are:
// Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value: * 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: case AFS_2G:
_aRes = 2.0 / 32768.0; a_res = 2.0 / 32768.0;
return _aRes;
break; break;
case AFS_4G: case AFS_4G:
_aRes = 4.0 / 32768.0; a_res = 4.0 / 32768.0;
return _aRes;
break; break;
case AFS_8G: case AFS_8G:
_aRes = 8.0 / 32768.0; a_res = 8.0 / 32768.0;
return _aRes;
break; break;
case AFS_16G: case AFS_16G:
_aRes = 16.0 / 32768.0; a_res = 16.0 / 32768.0;
return _aRes; break;
default:
a_res = NAN;
break; break;
} }
return a_res;
} }
int16_t em7180_baro_get() int16_t em7180_baro_get()

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

@ -16,11 +16,10 @@
/* Includes */ /* Includes */
#include <stdint.h> #include <stdint.h>
#include "em7180_common.h"
#include "lis2mdl.h" #include "lis2mdl.h"
/* Private Global Variables */ /* Private Global Variables */
static uint8_t _intPin;
static float _mRes;
/* Function Prototypes */ /* Function Prototypes */
static void lis2mdl_write_byte(uint8_t address, uint8_t subAddress, 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,
uint8_t *dest); uint8_t *dest);
/* Function Definitions */ /* Function Definitions */
lis2mdl_new(uint8_t pin) void lis2mdl_init(lis2mdl_t *lis2mdl, I2C_HandleTypeDef *hi2c, uint8_t m_odr)
{ {
/* pinMode(pin, INPUT); */ if(!lis2mdl)
_intPin = pin; {
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() uint8_t lis2mdl_chip_id_get()
@ -52,20 +70,6 @@ void lis2mdl_reset()
HAL_Delay(100); // Wait for all registers to 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() uint8_t lis2mdl_status()
{ {
// Read the status register of the altimeter // Read the status register of the altimeter
@ -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 }; 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] = int16_t mag_max[3] = { -32767, -32767, -32767 }, mag_min[3] =
{ 32767, 32767, 32767 }, mag_temp[3] = { 0, 0, 0 }; { 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!"); */ /* Serial.println("Calculate mag offset bias: move all around to sample the complete response surface!"); */
HAL_Delay(4000); HAL_Delay(4000);
@ -121,15 +125,15 @@ void lis2mdl_offset_bias(float *dest1, float *dest2)
HAL_Delay(12); HAL_Delay(12);
} }
_mRes = 0.0015f; // fixed sensitivity m_res = 0.0015f; // fixed sensitivity
// Get hard iron correction // Get hard iron correction
mag_bias[0] = (mag_max[0] + mag_min[0]) / 2; // get average x mag bias in counts 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[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 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[0] = (float) mag_bias[0] * m_res; // save mag biases in G for main program
dest1[1] = (float) mag_bias[1] * _mRes; dest1[1] = (float) mag_bias[1] * m_res;
dest1[2] = (float) mag_bias[2] * _mRes; dest1[2] = (float) mag_bias[2] * m_res;
// Get soft iron correction estimate // Get soft iron correction estimate
mag_scale[0] = (mag_max[0] - mag_min[0]) / 2; // get average x axis max chord length in counts 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()
float magTest[3] = { 0., 0., 0. }; float magTest[3] = { 0., 0., 0. };
float magNom[3] = { 0., 0., 0. }; float magNom[3] = { 0., 0., 0. };
int32_t sum[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 // first, get average response with self test disabled
for(int ii = 0; ii < 50; ii++) for(int ii = 0; ii < 50; ii++)
@ -193,12 +197,12 @@ void lis2mdl_self_test()
/* Serial.println("Mag Self Test:"); */ /* Serial.println("Mag Self Test:"); */
/* Serial.print("Mx results:"); */ /* 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.println(" mG"); */
/* Serial.print("My results:"); */ /* 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.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"); */ /* Serial.println("Should be between 15 and 500 mG"); */
HAL_Delay(2000); // give some time to read the screen HAL_Delay(2000); // give some time to read the screen
} }

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

@ -16,65 +16,67 @@
/* Includes */ /* Includes */
#include <stdint.h> #include <stdint.h>
#include "em7180_common.h"
#include "lps22hb.h" #include "lps22hb.h"
/* Private Global Variables */ /* Private Global Variables */
static uint8_t _intPin;
/* Function Prototypes */ /* Function Prototypes */
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,
static uint8_t lps22h_read_byte(uint8_t address, uint8_t subAddress); uint8_t data);
static void lps22h_read(uint8_t address, uint8_t subAddress, uint8_t count, 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); uint8_t *dest);
/* Function Definitions */ /* Function Definitions */
lps22h_new(uint8_t pin) void lps22hb_init(lps22hb_t *lps22hb, I2C_HandleTypeDef *hi2c, uint8_t p_odr)
{ {
/* pinMode(pin, INPUT); */ lps22hb->hi2c = hi2c;
_intPin = pin; 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 // 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; return temp;
} }
uint8_t lps22h_status() uint8_t lps22hb_status()
{ {
// Read the status register of the altimeter // 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; return temp;
} }
int32_t lps22h_pressure_get() int32_t lps22hb_pressure_get()
{ {
uint8_t rawData[3]; // 24-bit pressure register data stored here 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 return (int32_t) ((int32_t) rawData[2] << 16 | (int32_t) rawData[1] << 8
| rawData[0]); | rawData[0]);
} }
int16_t lps22h_temp_get() int16_t lps22hb_temp_get()
{ {
uint8_t rawData[2]; // 16-bit pressure register data stored here 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]); return (int16_t) ((int16_t) rawData[1] << 8 | rawData[0]);
} }
void lps22h_init(uint8_t PODR) static void lps22hb_write_byte(uint8_t address, uint8_t subAddress,
{ uint8_t data)
// 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)
{ {
/* Wire.beginTransmission(address); // Initialize the Tx buffer */ /* Wire.beginTransmission(address); // Initialize the Tx buffer */
/* Wire.write(subAddress); // Put slave register address in 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)
/* Wire.endTransmission(); // Send the Tx buffer */ /* 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 uint8_t data; // `data` will store the register data
/* Wire.beginTransmission(address); // Initialize the Tx buffer */ /* Wire.beginTransmission(address); // Initialize the Tx buffer */
@ -90,10 +92,10 @@ 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.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 */ /* Wire.requestFrom(address, (size_t) 1); // Read one uint8_t from slave register address */
/* data = Wire.read(); // Fill Rx buffer with result */ /* 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, static void lps22hb_read(uint8_t address, uint8_t subAddress, uint8_t count,
uint8_t *dest) uint8_t *dest)
{ {
/* Wire.beginTransmission(address); // Initialize the Tx buffer */ /* Wire.beginTransmission(address); // Initialize the Tx buffer */
@ -104,7 +106,7 @@ static void lps22h_read(uint8_t address, uint8_t subAddress, uint8_t count,
/* /*
while(Wire.available()) while(Wire.available())
{ {
dest[i++] = Wire.lps22h_read(); dest[i++] = Wire.lps22hb_read();
} // Put lps22h_read results in the Rx buffer } // Put lps22hb_read results in the Rx buffer
*/ */
} }

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

@ -16,28 +16,51 @@
/* Includes */ /* Includes */
#include <stdint.h> #include <stdint.h>
#include <math.h>
#include "em7180_common.h"
#include "lsm6dsm.h" #include "lsm6dsm.h"
/* Private Global Variables */ /* Private Global Variables */
static uint8_t _intPin1;
static uint8_t _intPin2;
static float _aRes;
static float _gRes;
/* Function Prototypes */ /* 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 */ /* 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); */ if(!lsm6dsm)
_intPin1 = pin1; {
/* pinMode(pin2, INPUT); */ return;
_intPin2 = pin2; }
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() uint8_t lsm6dsm_chip_id_get()
@ -46,55 +69,61 @@ uint8_t lsm6dsm_chip_id_get()
return c; 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: // Possible accelerometer scales (and their register bit settings) are:
// 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs (11). // 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: // Here's a bit of an algorithm to calculate DPS/(ADC tick) based on that 2-bit value:
case AFS_2G: case AFS_2G:
_aRes = 2.0f / 32768.0f; a_res = 2.0f / 32768.0f;
return _aRes;
break; break;
case AFS_4G: case AFS_4G:
_aRes = 4.0f / 32768.0f; a_res = 4.0f / 32768.0f;
return _aRes;
break; break;
case AFS_8G: case AFS_8G:
_aRes = 8.0f / 32768.0f; a_res = 8.0f / 32768.0f;
return _aRes;
break; break;
case AFS_16G: case AFS_16G:
_aRes = 16.0f / 32768.0f; a_res = 16.0f / 32768.0f;
return _aRes; break;
default:
a_res = NAN;
break; 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: // Possible gyro scales (and their register bit settings) are:
// 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS (11). // 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS (11).
case GFS_245DPS: case GFS_250DPS:
_gRes = 245.0f / 32768.0f; g_res = 250.0f / 32768.0f;
return _gRes;
break; break;
case GFS_500DPS: case GFS_500DPS:
_gRes = 500.0f / 32768.0f; g_res = 500.0f / 32768.0f;
return _gRes;
break; break;
case GFS_1000DPS: case GFS_1000DPS:
_gRes = 1000.0f / 32768.0f; g_res = 1000.0f / 32768.0f;
return _gRes;
break; break;
case GFS_2000DPS: case GFS_2000DPS:
_gRes = 2000.0f / 32768.0f; g_res = 2000.0f / 32768.0f;
return _gRes; break;
default:
g_res = NAN;
break; break;
} }
return g_res;
} }
void lsm6dsm_reset() void lsm6dsm_reset()
@ -105,29 +134,6 @@ void lsm6dsm_reset()
HAL_Delay(100); // Wait for all registers to 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() void lsm6dsm_selfTest()
{ {
int16_t temp[7] = { 0, 0, 0, 0, 0, 0, 0 }; int16_t temp[7] = { 0, 0, 0, 0, 0, 0, 0 };
@ -135,7 +141,7 @@ void lsm6dsm_selfTest()
gyroPTest[3] = { 0, 0, 0 }, gyroNTest[3] = { 0, 0, 0 }; gyroPTest[3] = { 0, 0, 0 }, gyroNTest[3] = { 0, 0, 0 };
int16_t accelNom[3] = { 0, 0, 0 }, gyroNom[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[0] = temp[4];
accelNom[1] = temp[5]; accelNom[1] = temp[5];
accelNom[2] = temp[6]; accelNom[2] = temp[6];
@ -145,28 +151,28 @@ void lsm6dsm_selfTest()
lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x01); // positive accel self test lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x01); // positive accel self test
HAL_Delay(100); // let accel respond HAL_Delay(100); // let accel respond
readData(temp); lsm6dsm_read_data(temp);
accelPTest[0] = temp[4]; accelPTest[0] = temp[4];
accelPTest[1] = temp[5]; accelPTest[1] = temp[5];
accelPTest[2] = temp[6]; accelPTest[2] = temp[6];
lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x03); // negative accel self test lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x03); // negative accel self test
HAL_Delay(100); // let accel respond HAL_Delay(100); // let accel respond
readData(temp); lsm6dsm_read_data(temp);
accelNTest[0] = temp[4]; accelNTest[0] = temp[4];
accelNTest[1] = temp[5]; accelNTest[1] = temp[5];
accelNTest[2] = temp[6]; accelNTest[2] = temp[6];
lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x04); // positive gyro self test lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x04); // positive gyro self test
HAL_Delay(100); // let gyro respond HAL_Delay(100); // let gyro respond
readData(temp); lsm6dsm_read_data(temp);
gyroPTest[0] = temp[1]; gyroPTest[0] = temp[1];
gyroPTest[1] = temp[2]; gyroPTest[1] = temp[2];
gyroPTest[2] = temp[3]; gyroPTest[2] = temp[3];
lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x0C); // negative gyro self test lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x0C); // negative gyro self test
HAL_Delay(100); // let gyro respond HAL_Delay(100); // let gyro respond
readData(temp); lsm6dsm_read_data(temp);
gyroNTest[0] = temp[1]; gyroNTest[0] = temp[1];
gyroNTest[1] = temp[2]; gyroNTest[1] = temp[2];
gyroNTest[2] = temp[3]; gyroNTest[2] = temp[3];
@ -176,50 +182,52 @@ void lsm6dsm_selfTest()
/* Serial.println("Accel Self Test:"); */ /* Serial.println("Accel Self Test:"); */
/* Serial.print("+Ax results:"); */ /* 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.println(" mg"); */
/* Serial.print("-Ax results:"); */ /* 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.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.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.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.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("Should be between 90 and 1700 mg"); */
/* Serial.println("Gyro Self Test:"); */ /* Serial.println("Gyro Self Test:"); */
/* Serial.print("+Gx results:"); */ /* Serial.print("+Gx results:"); */
/* Serial.print((gyroPTest[0] - gyroNom[0]) * _gRes); */ /* Serial.print((gyroPTest[0] - gyroNom[0]) * g_res); */
/* Serial.println(" dps"); */ /* Serial.println(" dps"); */
/* Serial.print("-Gx results:"); */ /* Serial.print("-Gx results:"); */
/* Serial.println((gyroNTest[0] - gyroNom[0]) * _gRes); */ /* Serial.println((gyroNTest[0] - gyroNom[0]) * g_res); */
/* Serial.print("+Gy results:"); */ /* Serial.print("+Gy results:"); */
/* Serial.println((gyroPTest[1] - gyroNom[1]) * _gRes); */ /* Serial.println((gyroPTest[1] - gyroNom[1]) * g_res); */
/* Serial.print("-Gy results:"); */ /* Serial.print("-Gy results:"); */
/* Serial.println((gyroNTest[1] - gyroNom[1]) * _gRes); */ /* Serial.println((gyroNTest[1] - gyroNom[1]) * g_res); */
/* Serial.print("+Gz results:"); */ /* Serial.print("+Gz results:"); */
/* Serial.println((gyroPTest[2] - gyroNom[2]) * _gRes); */ /* Serial.println((gyroPTest[2] - gyroNom[2]) * g_res); */
/* Serial.print("-Gz results:"); */ /* 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"); */ /* Serial.println("Should be between 20 and 80 dps"); */
HAL_Delay(2000); 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 }; int16_t temp[7] = { 0, 0, 0, 0, 0, 0, 0 };
int32_t sum[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!"); */ /* Serial.println("Calculate accel and gyro offset biases: keep sensor flat and motionless!"); */
HAL_Delay(4000); HAL_Delay(4000);
for(int ii = 0; ii < 128; ii++) for(int ii = 0; ii < 128; ii++)
{ {
readData(temp); lsm6dsm_read_data(temp);
sum[1] += temp[1]; sum[1] += temp[1];
sum[2] += temp[2]; sum[2] += temp[2];
sum[3] += temp[3]; sum[3] += temp[3];
@ -229,12 +237,12 @@ void lsm6dsm_offsetBias(float *dest1, float *dest2)
HAL_Delay(50); HAL_Delay(50);
} }
dest1[0] = sum[1] * _gRes / 128.0f; dest1[0] = sum[1] * g_res / 128.0f;
dest1[1] = sum[2] * _gRes / 128.0f; dest1[1] = sum[2] * g_res / 128.0f;
dest1[2] = sum[3] * _gRes / 128.0f; dest1[2] = sum[3] * g_res / 128.0f;
dest2[0] = sum[4] * _aRes / 128.0f; dest2[0] = sum[4] * a_res / 128.0f;
dest2[1] = sum[5] * _aRes / 128.0f; dest2[1] = sum[5] * a_res / 128.0f;
dest2[2] = sum[6] * _aRes / 128.0f; dest2[2] = sum[6] * a_res / 128.0f;
if(dest2[0] > 0.8f) if(dest2[0] > 0.8f)
{ {
@ -277,24 +285,22 @@ void lsm6dsm_read_data(int16_t *destination)
} }
// I2C read/write functions for the LSM6DSM // I2C read/write functions for the LSM6DSM
static void lsm6dsm_write_byte(uint8_t address, uint8_t subAddress, void lsm6dsm_write_byte(uint8_t addr, uint8_t sub_addr, uint8_t data)
uint8_t data)
{ {
uint8_t temp[2]; uint8_t temp[2];
temp[0] = subAddress; temp[0] = sub_addr;
temp[1] = data; 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]; uint8_t temp[1];
/* Wire.transfer(address, &subAddress, 1, &temp[0], 1); */ /* Wire.transfer(addr, &sub_addr, 1, &temp[0], 1); */
return temp[0]; return temp[0];
} }
static void lsm6dsm_read(uint8_t address, uint8_t subAddress, uint8_t count, void lsm6dsm_read(uint8_t addr, uint8_t sub_addr, uint8_t count, uint8_t *dest)
uint8_t *dest)
{ {
/* Wire.transfer(address, &subAddress, 1, dest, count); */ /* Wire.transfer(addr, &sub_addr, 1, dest, count); */
} }

Write Preview
Loading…
Cancel
Save