From 6f195cc05b33fa061485ac139c0e093a7ab9e4c3 Mon Sep 17 00:00:00 2001 From: Daniel Peter Chokola Date: Tue, 19 Jan 2021 18:17:47 -0500 Subject: [PATCH] flesh out API; allow instantiation of multiple devices --- Drivers/EM7180/Inc/em7180.h | 38 +- Drivers/EM7180/Inc/em7180_common.h | 22 ++ Drivers/EM7180/Inc/lis2mdl.h | 15 + Drivers/EM7180/Inc/lps22hb.h | 28 +- Drivers/EM7180/Inc/lsm6dsm.h | 22 +- Drivers/EM7180/Src/em7180.c | 556 ++++++++++++++--------------- Drivers/EM7180/Src/lis2mdl.c | 60 ++-- Drivers/EM7180/Src/lps22hb.c | 70 ++-- Drivers/EM7180/Src/lsm6dsm.c | 190 +++++----- 9 files changed, 525 insertions(+), 476 deletions(-) create mode 100644 Drivers/EM7180/Inc/em7180_common.h diff --git a/Drivers/EM7180/Inc/em7180.h b/Drivers/EM7180/Inc/em7180.h index f5c396b..1b5247c 100644 --- a/Drivers/EM7180/Inc/em7180.h +++ b/Drivers/EM7180/Inc/em7180.h @@ -17,6 +17,10 @@ /* Includes */ #include +#include "i2c.h" +#include "lsm6dsm.h" +#include "lps22hb.h" +#include "lis2mdl.h" /* Definitions */ /* @@ -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); diff --git a/Drivers/EM7180/Inc/em7180_common.h b/Drivers/EM7180/Inc/em7180_common.h new file mode 100644 index 0000000..0c0d7e1 --- /dev/null +++ b/Drivers/EM7180/Inc/em7180_common.h @@ -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 + +/* 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 */ diff --git a/Drivers/EM7180/Inc/lis2mdl.h b/Drivers/EM7180/Inc/lis2mdl.h index 98a1042..411028e 100644 --- a/Drivers/EM7180/Inc/lis2mdl.h +++ b/Drivers/EM7180/Inc/lis2mdl.h @@ -19,6 +19,7 @@ /* Includes */ #include +#include "i2c.h" /* Definitions */ //Register map for LIS2MDL' @@ -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 diff --git a/Drivers/EM7180/Inc/lps22hb.h b/Drivers/EM7180/Inc/lps22hb.h index 86c3615..0ef1649 100644 --- a/Drivers/EM7180/Inc/lps22hb.h +++ b/Drivers/EM7180/Inc/lps22hb.h @@ -19,6 +19,7 @@ /* Includes */ #include +#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 @@ #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 diff --git a/Drivers/EM7180/Inc/lsm6dsm.h b/Drivers/EM7180/Inc/lsm6dsm.h index 8fbcbc4..46716fd 100644 --- a/Drivers/EM7180/Inc/lsm6dsm.h +++ b/Drivers/EM7180/Inc/lsm6dsm.h @@ -19,7 +19,9 @@ /* Includes */ #include +#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 @@ #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 @@ #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 diff --git a/Drivers/EM7180/Src/em7180.c b/Drivers/EM7180/Src/em7180.c index d86aa93..b7c195a 100644 --- a/Drivers/EM7180/Src/em7180.c +++ b/Drivers/EM7180/Src/em7180.c @@ -16,16 +16,16 @@ #include #include #include +#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, 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, 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, // 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, { /* 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, 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, /* 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) 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) 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) 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) 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) 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() diff --git a/Drivers/EM7180/Src/lis2mdl.c b/Drivers/EM7180/Src/lis2mdl.c index bf62b3a..5c90503 100644 --- a/Drivers/EM7180/Src/lis2mdl.c +++ b/Drivers/EM7180/Src/lis2mdl.c @@ -16,11 +16,10 @@ /* Includes */ #include +#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, 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() 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) 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) 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() 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() /* 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 } diff --git a/Drivers/EM7180/Src/lps22hb.c b/Drivers/EM7180/Src/lps22hb.c index 8e9bd36..96c0a2e 100644 --- a/Drivers/EM7180/Src/lps22hb.c +++ b/Drivers/EM7180/Src/lps22hb.c @@ -16,65 +16,67 @@ /* Includes */ #include +#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) /* 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) /* 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, /* 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 */ } diff --git a/Drivers/EM7180/Src/lsm6dsm.c b/Drivers/EM7180/Src/lsm6dsm.c index bf2a377..4ae57cc 100644 --- a/Drivers/EM7180/Src/lsm6dsm.c +++ b/Drivers/EM7180/Src/lsm6dsm.c @@ -16,28 +16,51 @@ /* Includes */ #include +#include +#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() 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() 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() 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() 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() /* 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) 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) } // 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); */ }