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

STM-ify I2C and get just enough communication working to configure the devices

master
Daniel Peter Chokola 4 years ago
parent
6f195cc05b
commit
c79ee256a8
9 changed files with 399 additions and 470 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. 18
      Drivers/EM7180/Inc/em7180.h
  2. 39
      Drivers/EM7180/Inc/em7180_common.h
  3. 5
      Drivers/EM7180/Inc/lis2mdl.h
  4. 4
      Drivers/EM7180/Inc/lps22hb.h
  5. 6
      Drivers/EM7180/Inc/lsm6dsm.h
  6. 496
      Drivers/EM7180/Src/em7180.c
  7. 95
      Drivers/EM7180/Src/lis2mdl.c
  8. 79
      Drivers/EM7180/Src/lps22hb.c
  9. 127
      Drivers/EM7180/Src/lsm6dsm.c

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

@ -101,6 +101,8 @@ typedef struct em7180_s
{ {
I2C_HandleTypeDef *hi2c; I2C_HandleTypeDef *hi2c;
lsm6dsm_t *lsm6dsm; lsm6dsm_t *lsm6dsm;
lis2mdl_t *lis2mdl;
lps22hb_t *lps22hb;
uint16_t acc_fs; uint16_t acc_fs;
uint16_t gyro_fs; uint16_t gyro_fs;
uint16_t mag_fs; uint16_t mag_fs;
@ -112,13 +114,15 @@ typedef struct em7180_s
} em7180_t; } em7180_t;
/* Function Prototypes */ /* Function Prototypes */
void em7180_init(em7180_t *em7180, lsm6dsm_t *lsm6dsm, I2C_HandleTypeDef *hi2c1, void em7180_init(em7180_t *em7180, I2C_HandleTypeDef *hi2c, lsm6dsm_t *lsm6dsm,
uint16_t acc_fs, uint16_t gyro_fs, uint16_t mag_fs, lis2mdl_t *lis2mdl, lps22hb_t *lps22hb, uint16_t acc_fs,
uint8_t q_rate_div, uint8_t mag_rate, uint8_t acc_rate, uint16_t gyro_fs, uint16_t mag_fs, uint8_t q_rate_div,
uint8_t gyro_rate, uint8_t baro_rate); uint8_t mag_rate, uint8_t acc_rate, uint8_t gyro_rate,
uint8_t baro_rate);
void em7180_config(em7180_t *em7180); void em7180_config(em7180_t *em7180);
void em7180_gyro_set_fs(uint16_t gyro_fs); void em7180_gyro_set_fs(em7180_t *em7180, uint16_t gyro_fs);
void em7180_mag_acc_set_fs(uint16_t mag_fs, uint16_t acc_fs); void em7180_mag_acc_set_fs(em7180_t *em7180, 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(em7180_t *em7180, uint8_t param,
uint32_t param_val);
#endif /* EM7180_H_ */ #endif /* EM7180_H_ */

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

@ -12,11 +12,42 @@
/* Includes */ /* Includes */
#include <stdint.h> #include <stdint.h>
#include "i2c.h"
/* Definitions */
#define return_if_fail(cond) \
if(!(cond)) \
{ \
return; \
}
#define return_val_if_fail(cond, val) \
if(!(cond)) \
{ \
return (val); \
}
/* Function Prototypes */ /* Function Prototypes */
void lsm6dsm_write_byte(uint8_t address, uint8_t subAddress, uint8_t data); inline __attribute__((always_inline)) HAL_StatusTypeDef i2c_write_byte(
uint8_t lsm6dsm_read_byte(uint8_t address, uint8_t subAddress); I2C_HandleTypeDef *hi2c, uint16_t addr, uint16_t sub_addr, uint8_t data)
void lsm6dsm_read(uint8_t address, uint8_t subAddress, uint8_t count, {
uint8_t *dest); return HAL_I2C_Mem_Write(hi2c, addr << 1, sub_addr, 1, &data, 1,
HAL_MAX_DELAY);
}
inline __attribute__((always_inline)) uint8_t i2c_read_byte(
I2C_HandleTypeDef *hi2c, uint16_t addr, uint16_t sub_addr)
{
uint8_t temp;
HAL_I2C_Mem_Read(hi2c, addr << 1, sub_addr, 1, &temp, 1, HAL_MAX_DELAY);
return temp;
}
inline __attribute__((always_inline)) HAL_StatusTypeDef i2c_read(
I2C_HandleTypeDef *hi2c, uint16_t addr, uint16_t sub_addr, uint8_t *data,
uint16_t len)
{
return HAL_I2C_Mem_Read(hi2c, addr << 1, sub_addr, 1, data, len,
HAL_MAX_DELAY);
}
#endif /* EM7180_COMMON_h */ #endif /* EM7180_COMMON_h */

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

@ -61,12 +61,11 @@
/* Data Structures */ /* Data Structures */
typedef struct lis2mdl_s typedef struct lis2mdl_s
{ {
I2C_HandleTypeDef *hi2c;
uint8_t m_odr; uint8_t m_odr;
} lis2mdl_t; } lis2mdl_t;
/* Function Prototypes */ /* Function Prototypes */
void lis2mdl_init(lis2mdl_t *lis2mdl, I2C_HandleTypeDef *hi2c, uint8_t m_odr); void lis2mdl_init(lis2mdl_t *lis2mdl, uint8_t m_odr);
void lis2mdl_config(lis2mdl_t *lis2mdl); void lis2mdl_config(lis2mdl_t *lis2mdl, I2C_HandleTypeDef *hi2c);
#endif #endif

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

@ -65,7 +65,7 @@ typedef struct lps22hb_s
} lps22hb_t; } lps22hb_t;
/* Function Prototypes */ /* Function Prototypes */
void lps22hb_init(lps22hb_t *lps22hb, I2C_HandleTypeDef *hi2c, uint8_t p_odr); void lps22hb_init(lps22hb_t *lps22hb, uint8_t p_odr);
void lps22hb_config(lps22hb_t *lps22hb); void lps22hb_config(lps22hb_t *lps22hb, I2C_HandleTypeDef *hi2c);
#endif #endif

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

@ -170,8 +170,8 @@ typedef struct lsm6dsm_s
} lsm6dsm_t; } lsm6dsm_t;
/* Function Prototypes */ /* Function Prototypes */
void lsm6dsm_init(lsm6dsm_t *lsm6dsm, I2C_HandleTypeDef *hi2c, uint8_t ascale, void lsm6dsm_init(lsm6dsm_t *lsm6dsm, uint8_t ascale, uint8_t gscale,
uint8_t gscale, uint8_t a_odr, uint8_t g_odr); uint8_t a_odr, uint8_t g_odr);
void lsm6dsm_config(lsm6dsm_t *lsm6dsm); void lsm6dsm_config(lsm6dsm_t *lsm6dsm, I2C_HandleTypeDef *hi2c);
#endif #endif

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

@ -25,31 +25,23 @@
/* Private Global Variables */ /* Private Global Variables */
/* Function Prototypes */ /* Function Prototypes */
static void em7180_float_to_bytes(float param_val, uint8_t *buf); static void em7180_passthrough(em7180_t *em7180);
static void m24512dfm_write_byte(uint8_t device_address, uint8_t data_address1, static float uint32_reg_to_float(uint8_t *buf);
uint8_t data_address2, uint8_t data); static void float_to_bytes(float param_val, uint8_t *buf);
static void m24512dfm_write(uint8_t device_address, uint8_t data_address1,
uint8_t data_address2, uint8_t count, uint8_t *dest);
static uint8_t m24512dfm_read_byte(uint8_t device_address,
uint8_t data_address1, uint8_t data_address2);
static void m24512dfm_read(uint8_t device_address, uint8_t data_address1,
uint8_t data_address2, uint8_t count, uint8_t *dest);
static uint8_t em7180_read_byte(uint8_t address, uint8_t subAddress);
static void em7180_read(uint8_t address, uint8_t subAddress, uint8_t count,
uint8_t *dest);
/* Function Definitions */ /* Function Definitions */
void em7180_init(em7180_t *em7180, lsm6dsm_t *lsm6dsm, I2C_HandleTypeDef *hi2c1, void em7180_init(em7180_t *em7180, I2C_HandleTypeDef *hi2c, lsm6dsm_t *lsm6dsm,
uint16_t acc_fs, uint16_t gyro_fs, uint16_t mag_fs, lis2mdl_t *lis2mdl, lps22hb_t *lps22hb, uint16_t acc_fs,
uint8_t q_rate_div, uint8_t mag_rate, uint8_t acc_rate, uint16_t gyro_fs, uint16_t mag_fs, uint8_t q_rate_div,
uint8_t gyro_rate, uint8_t baro_rate) uint8_t mag_rate, uint8_t acc_rate, uint8_t gyro_rate,
uint8_t baro_rate)
{ {
if(!em7180) return_if_fail(em7180);
{
return;
}
em7180->hi2c = hi2c;
em7180->lsm6dsm = lsm6dsm; em7180->lsm6dsm = lsm6dsm;
em7180->lis2mdl = lis2mdl;
em7180->lps22hb = lps22hb;
em7180->acc_fs = acc_fs; em7180->acc_fs = acc_fs;
em7180->gyro_fs = gyro_fs; em7180->gyro_fs = gyro_fs;
em7180->mag_fs = mag_fs; em7180->mag_fs = mag_fs;
@ -59,13 +51,27 @@ void em7180_init(em7180_t *em7180, lsm6dsm_t *lsm6dsm, I2C_HandleTypeDef *hi2c1,
em7180->gyro_rate = gyro_rate; em7180->gyro_rate = gyro_rate;
em7180->baro_rate = baro_rate; em7180->baro_rate = baro_rate;
/* configure the EM7180 */
em7180_config(em7180); em7180_config(em7180);
/* enter passthrough mode */
em7180_passthrough(em7180);
/* and configure the devices on the slave bus */
if(em7180->lsm6dsm)
{
lsm6dsm_config(em7180->lsm6dsm, em7180->hi2c);
}
if(em7180->lis2mdl)
{
lis2mdl_config(em7180->lis2mdl, em7180->hi2c);
}
if(em7180->lps22hb)
{
lps22hb_config(em7180->lps22hb, em7180->hi2c);
}
} }
void em7180_config(em7180_t *em7180) void em7180_config(em7180_t *em7180)
{ {
uint8_t param[4];
uint8_t param_xfer;
uint8_t runStatus; uint8_t runStatus;
uint8_t algoStatus; uint8_t algoStatus;
uint8_t passthruStatus; uint8_t passthruStatus;
@ -73,50 +79,54 @@ void em7180_config(em7180_t *em7180)
uint8_t sensorStatus; 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 i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_HostControl, 0x00); // set SENtral in initialized state to configure registers
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_PassThruControl, 0x00); // make sure pass through mode is off i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_PassThruControl, 0x00); // make sure pass through mode is off
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_HostControl, 0x01); // Force initialize i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_HostControl, 0x01); // Force initialize
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_HostControl, 0x00); // set SENtral in initialized state to configure registers i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_HostControl, 0x00); // set SENtral in initialized state to configure registers
/* Legacy MPU6250 stuff, it seems /* 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 i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_ACC_LPF_BW, accBW); // accBW = 3 = 41Hz
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_GYRO_LPF_BW, gyroBW); // gyroBW = 3 = 41Hz */ i2c_write_byte(em7180->hi2c, 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, em7180->q_rate_div); // quat rate = gyroRt/(1 QRTDiv) i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_QRateDivisor,
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_MagRate, em7180->mag_rate); // 0x64 = 100 Hz em7180->q_rate_div); // quat rate = gyroRt/(1 QRTDiv)
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AccelRate, em7180->acc_rate); // 200/10 Hz, 0x14 = 200 Hz i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_MagRate,
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_GyroRate, em7180->gyro_rate); // 200/10 Hz, 0x14 = 200 Hz em7180->mag_rate); // 0x64 = 100 Hz
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_BaroRate, i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_AccelRate,
0x80 | em7180->baro_rate); // set enable bit and set Baro rate to 25 Hz, rate = baroRt/2, 0x32 = 25 Hz em7180->acc_rate); // 200/10 Hz, 0x14 = 200 Hz
i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_GyroRate,
em7180->gyro_rate); // 200/10 Hz, 0x14 = 200 Hz
i2c_write_byte(em7180->hi2c, 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 i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // read scale sensor data
// Enable interrupt to host upon certain events // Enable interrupt to host upon certain events
// choose host interrupts when any sensor updated (0x40), new gyro data (0x20), new accel data (0x10), // choose host interrupts when any sensor updated (0x40), new gyro data (0x20), new accel data (0x10),
// new mag data (0x08), quaternions updated (0x04), an error occurs (0x02), or the SENtral needs to be reset(0x01) // new mag data (0x08), quaternions updated (0x04), an error occurs (0x02), or the SENtral needs to be reset(0x01)
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_EnableEvents, 0x07); i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_EnableEvents, 0x07);
// Enable EM7180 run mode // Enable EM7180 run mode
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_HostControl, 0x01); // set SENtral in normal run mode i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_HostControl, 0x01); // set SENtral in normal run mode
HAL_Delay(100); HAL_Delay(100);
// EM7180 parameter adjustments // EM7180 parameter adjustments
/* Serial.println("Beginning Parameter Adjustments"); */ /* Serial.println("Beginning Parameter Adjustments"); */
// Disable stillness mode for balancing robot application // Disable stillness mode for balancing robot application
em7180_set_integer_param(0x49, 0x00); em7180_set_integer_param(em7180, 0x49, 0x00);
// Write desired sensor full scale ranges to the EM7180 // 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_mag_acc_set_fs(em7180, em7180->mag_fs, em7180->acc_fs); // 1000 uT == 0x3E8, 8 g == 0x08
em7180_gyro_set_fs(em7180->gyro_fs); // 2000 dps == 0x7D0 em7180_gyro_set_fs(em7180, em7180->gyro_fs); // 2000 dps == 0x7D0
// Read EM7180 status // Read EM7180 status
runStatus = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_RunStatus); runStatus = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_RunStatus);
if(runStatus & 0x01) if(runStatus & 0x01)
{ {
/* Serial.println(" EM7180 run status = normal mode"); */ /* Serial.println(" EM7180 run status = normal mode"); */
} }
algoStatus = lsm6dsm_read_byte(EM7180_ADDRESS, algoStatus = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS,
EM7180_AlgorithmStatus); EM7180_AlgorithmStatus);
if(algoStatus & 0x01) if(algoStatus & 0x01)
{ {
@ -142,13 +152,14 @@ void em7180_config(em7180_t *em7180)
{ {
/* Serial.println(" EM7180 unreliable sensor data"); */ /* Serial.println(" EM7180 unreliable sensor data"); */
} }
passthruStatus = lsm6dsm_read_byte(EM7180_ADDRESS, passthruStatus = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS,
EM7180_PassThruStatus); EM7180_PassThruStatus);
if(passthruStatus & 0x01) if(passthruStatus & 0x01)
{ {
/* Serial.print(" EM7180 in passthru mode!"); */ /* Serial.print(" EM7180 in passthru mode!"); */
} }
eventStatus = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_EventStatus); eventStatus = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS,
EM7180_EventStatus);
if(eventStatus & 0x01) if(eventStatus & 0x01)
{ {
/* Serial.println(" EM7180 CPU reset"); */ /* Serial.println(" EM7180 CPU reset"); */
@ -177,7 +188,8 @@ void em7180_config(em7180_t *em7180)
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
sensorStatus = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SensorStatus); sensorStatus = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS,
EM7180_SensorStatus);
/* Serial.print(" EM7180 sensor status = "); */ /* Serial.print(" EM7180 sensor status = "); */
/* Serial.println(sensorStatus); */ /* Serial.println(sensorStatus); */
if(sensorStatus & 0x01) if(sensorStatus & 0x01)
@ -206,47 +218,49 @@ void em7180_config(em7180_t *em7180)
} }
/* Serial.print("Actual MagRate = "); */ /* Serial.print("Actual MagRate = "); */
/* Serial.print(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ActualMagRate)); */ /* Serial.print(i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_ActualMagRate)); */
/* Serial.println(" Hz"); */ /* Serial.println(" Hz"); */
/* Serial.print("Actual AccelRate = "); */ /* Serial.print("Actual AccelRate = "); */
/* Serial.print(10 * lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ActualAccelRate)); */ /* Serial.print(10 * i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_ActualAccelRate)); */
/* Serial.println(" Hz"); */ /* Serial.println(" Hz"); */
/* Serial.print("Actual GyroRate = "); */ /* Serial.print("Actual GyroRate = "); */
/* Serial.print(10 * lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ActualGyroRate)); */ /* Serial.print(10 * i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_ActualGyroRate)); */
/* Serial.println(" Hz"); */ /* Serial.println(" Hz"); */
/* Serial.print("Actual BaroRate = "); */ /* Serial.print("Actual BaroRate = "); */
/* Serial.print(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ActualBaroRate)); */ /* Serial.print(i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_ActualBaroRate)); */
/* Serial.println(" Hz"); */ /* Serial.println(" Hz"); */
} }
void em7180_chip_id_get() #if(0)
void em7180_chip_id_get(em7180_t *em7180)
{ {
// Read SENtral device information // Read SENtral device information
uint16_t ROM1 = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ROMVersion1); uint16_t ROM1 = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_ROMVersion1);
uint16_t ROM2 = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ROMVersion2); uint16_t ROM2 = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_ROMVersion2);
/* Serial.print("EM7180 ROM Version: 0x"); */ /* Serial.print("EM7180 ROM Version: 0x"); */
/* Serial.print(ROM1, HEX); */ /* Serial.print(ROM1, HEX); */
/* Serial.println(ROM2, HEX); */ /* Serial.println(ROM2, HEX); */
/* Serial.println("Should be: 0xE609"); */ /* Serial.println("Should be: 0xE609"); */
uint16_t RAM1 = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_RAMVersion1); uint16_t RAM1 = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_RAMVersion1);
uint16_t RAM2 = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_RAMVersion2); uint16_t RAM2 = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_RAMVersion2);
/* Serial.print("EM7180 RAM Version: 0x"); */ /* Serial.print("EM7180 RAM Version: 0x"); */
/* Serial.print(RAM1); */ /* Serial.print(RAM1); */
/* Serial.println(RAM2); */ /* Serial.println(RAM2); */
uint8_t PID = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ProductID); uint8_t PID = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_ProductID);
/* Serial.print("EM7180 ProductID: 0x"); */ /* Serial.print("EM7180 ProductID: 0x"); */
/* Serial.print(PID, HEX); */ /* Serial.print(PID, HEX); */
/* Serial.println(" Should be: 0x80"); */ /* Serial.println(" Should be: 0x80"); */
uint8_t RID = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_RevisionID); uint8_t RID = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_RevisionID);
/* Serial.print("EM7180 RevisionID: 0x"); */ /* Serial.print("EM7180 RevisionID: 0x"); */
/* Serial.print(RID, HEX); */ /* Serial.print(RID, HEX); */
/* Serial.println(" Should be: 0x02"); */ /* Serial.println(" Should be: 0x02"); */
} }
#endif
void em7180_load_fw_from_eeprom() void em7180_load_fw_from_eeprom(em7180_t *em7180)
{ {
// Check which sensors can be detected by the EM7180 // Check which sensors can be detected by the EM7180
uint8_t featureflag = lsm6dsm_read_byte(EM7180_ADDRESS, uint8_t featureflag = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS,
EM7180_FeatureFlags); EM7180_FeatureFlags);
if(featureflag & 0x01) if(featureflag & 0x01)
{ {
@ -276,52 +290,55 @@ void em7180_load_fw_from_eeprom()
HAL_Delay(1000); // give some time to read the screen HAL_Delay(1000); // give some time to read the screen
// Check SENtral status, make sure EEPROM upload of firmware was accomplished // Check SENtral status, make sure EEPROM upload of firmware was accomplished
uint8_t STAT = (lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) uint8_t STAT = (i2c_read_byte(em7180->hi2c, EM7180_ADDRESS,
& 0x01); EM7180_SentralStatus)
if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x01) & 0x01);
if(i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_SentralStatus) & 0x01)
{ {
/* Serial.println("EEPROM detected on the sensor bus!"); */ /* Serial.println("EEPROM detected on the sensor bus!"); */
} }
if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x02) if(i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_SentralStatus) & 0x02)
{ {
/* Serial.println("EEPROM uploaded config file!"); */ /* Serial.println("EEPROM uploaded config file!"); */
} }
if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x04) if(i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_SentralStatus) & 0x04)
{ {
/* Serial.println("EEPROM CRC incorrect!"); */ /* Serial.println("EEPROM CRC incorrect!"); */
} }
if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x08) if(i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_SentralStatus) & 0x08)
{ {
/* Serial.println("EM7180 in initialized state!"); */ /* Serial.println("EM7180 in initialized state!"); */
} }
if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x10) if(i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_SentralStatus) & 0x10)
{ {
/* Serial.println("No EEPROM detected!"); */ /* Serial.println("No EEPROM detected!"); */
} }
int count = 0; int count = 0;
while(!STAT) while(!STAT)
{ {
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ResetRequest, 0x01); i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_ResetRequest, 0x01);
HAL_Delay(500); HAL_Delay(500);
count++; count++;
STAT = (lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x01); STAT = (i2c_read_byte(em7180->hi2c, EM7180_ADDRESS,
if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x01) EM7180_SentralStatus)
& 0x01);
if(i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_SentralStatus) & 0x01)
{ {
/* Serial.println("EEPROM detected on the sensor bus!"); */ /* Serial.println("EEPROM detected on the sensor bus!"); */
} }
if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x02) if(i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_SentralStatus) & 0x02)
{ {
/* Serial.println("EEPROM uploaded config file!"); */ /* Serial.println("EEPROM uploaded config file!"); */
} }
if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x04) if(i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_SentralStatus) & 0x04)
{ {
/* Serial.println("EEPROM CRC incorrect!"); */ /* Serial.println("EEPROM CRC incorrect!"); */
} }
if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x08) if(i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_SentralStatus) & 0x08)
{ {
/* Serial.println("EM7180 in initialized state!"); */ /* Serial.println("EM7180 in initialized state!"); */
} }
if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x10) if(i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_SentralStatus) & 0x10)
{ {
/* Serial.println("No EEPROM detected!"); */ /* Serial.println("No EEPROM detected!"); */
} }
@ -331,117 +348,85 @@ void em7180_load_fw_from_eeprom()
} }
} }
if(!(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x04)) if(!(i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_SentralStatus)
& 0x04))
{ {
/* Serial.println("EEPROM upload successful!"); */ /* Serial.println("EEPROM upload successful!"); */
} }
} }
uint8_t em7180_status() uint8_t em7180_status(em7180_t *em7180)
{ {
// Check event status register, way to check data ready by polling rather than interrupt // 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 uint8_t c = i2c_read_byte(em7180->hi2c, 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; return c;
} }
float em7180_uint32_reg_to_float(uint8_t *buf) uint8_t em7180_errors(em7180_t *em7180)
{
union
{
uint32_t ui32;
float f;
} u;
u.ui32 = (((uint32_t) buf[0]) + (((uint32_t) buf[1]) << 8)
+ (((uint32_t) buf[2]) << 16) + (((uint32_t) buf[3]) << 24));
return u.f;
}
float em7180_int32_reg_to_float(uint8_t *buf)
{ {
union uint8_t c = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS,
{ EM7180_ErrorRegister); // check error register
int32_t i32;
float f;
} u;
u.i32 = (((int32_t) buf[0]) + (((int32_t) buf[1]) << 8)
+ (((int32_t) buf[2]) << 16) + (((int32_t) buf[3]) << 24));
return u.f;
}
static void em7180_float_to_bytes(float param_val, uint8_t *buf) return c;
{
union
{
float f;
uint8_t u8[sizeof(float)];
} u;
u.f = param_val;
for(uint8_t i = 0; i < sizeof(float); i++)
{
buf[i] = u.u8[i];
}
// Convert to LITTLE ENDIAN
/* FIXME: What the hell? */
for(uint8_t i = 0; i < sizeof(float); i++)
{
buf[i] = buf[(sizeof(float) - 1) - i];
}
} }
void em7180_gyro_set_fs(uint16_t gyro_fs) void em7180_gyro_set_fs(em7180_t *em7180, uint16_t gyro_fs)
{ {
uint8_t bytes[4], STAT; uint8_t bytes[4], STAT;
bytes[0] = gyro_fs & (0xFF); bytes[0] = gyro_fs & (0xFF);
bytes[1] = (gyro_fs >> 8) & (0xFF); bytes[1] = (gyro_fs >> 8) & (0xFF);
bytes[2] = 0x00; bytes[2] = 0x00;
bytes[3] = 0x00; bytes[3] = 0x00;
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte0, bytes[0]); // Gyro LSB i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_LoadParamByte0,
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte1, bytes[1]); // Gyro MSB bytes[0]); // Gyro LSB
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte2, bytes[2]); // Unused i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_LoadParamByte1,
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte3, bytes[3]); // Unused bytes[1]); // Gyro MSB
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 i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_LoadParamByte2,
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x80); // Request parameter transfer procedure bytes[2]); // Unused
STAT = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge); // Check the parameter acknowledge register and loop until the result matches parameter request byte i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_LoadParamByte3,
bytes[3]); // Unused
i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_ParamRequest, 0xCB); // Parameter 75; 0xCB is 75 decimal with the MSB set high to indicate a parameter write process
i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_AlgorithmControl, 0x80); // Request parameter transfer procedure
STAT = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_ParamAcknowledge); // Check the parameter acknowledge register and loop until the result matches parameter request byte
while(!(STAT == 0xCB)) while(!(STAT == 0xCB))
{ {
STAT = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge); STAT = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS,
EM7180_ParamAcknowledge);
} }
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0x00); // Parameter request = 0 to end parameter transfer process i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_ParamRequest, 0x00); // Parameter request = 0 to end parameter transfer process
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // Re-start algorithm i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // Re-start algorithm
} }
void em7180_mag_acc_set_fs(uint16_t mag_fs, uint16_t acc_fs) void em7180_mag_acc_set_fs(em7180_t *em7180, uint16_t mag_fs, uint16_t acc_fs)
{ {
uint8_t bytes[4], STAT; uint8_t bytes[4], STAT;
bytes[0] = mag_fs & (0xFF); bytes[0] = mag_fs & (0xFF);
bytes[1] = (mag_fs >> 8) & (0xFF); bytes[1] = (mag_fs >> 8) & (0xFF);
bytes[2] = acc_fs & (0xFF); bytes[2] = acc_fs & (0xFF);
bytes[3] = (acc_fs >> 8) & (0xFF); bytes[3] = (acc_fs >> 8) & (0xFF);
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte0, bytes[0]); // Mag LSB i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_LoadParamByte0,
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte1, bytes[1]); // Mag MSB bytes[0]); // Mag LSB
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte2, bytes[2]); // Acc LSB i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_LoadParamByte1,
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte3, bytes[3]); // Acc MSB bytes[1]); // Mag 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 i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_LoadParamByte2,
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x80); // Request parameter transfer procedure bytes[2]); // Acc LSB
STAT = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge); // Check the parameter acknowledge register and loop until the result matches parameter request byte i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_LoadParamByte3,
bytes[3]); // Acc MSB
i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_ParamRequest, 0xCA); // Parameter 74; 0xCA is 74 decimal with the MSB set high to indicate a paramter write processs
i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_AlgorithmControl, 0x80); // Request parameter transfer procedure
STAT = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_ParamAcknowledge); // Check the parameter acknowledge register and loop until the result matches parameter request byte
while(!(STAT == 0xCA)) while(!(STAT == 0xCA))
{ {
STAT = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge); STAT = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS,
EM7180_ParamAcknowledge);
} }
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0x00); // Parameter request = 0 to end parameter transfer process i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_ParamRequest, 0x00); // Parameter request = 0 to end parameter transfer process
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // Re-start algorithm i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // Re-start algorithm
} }
void em7180_set_integer_param(uint8_t param, uint32_t param_val) void em7180_set_integer_param(em7180_t *em7180, uint8_t param,
uint32_t param_val)
{ {
uint8_t bytes[4], STAT; uint8_t bytes[4], STAT;
bytes[0] = param_val & (0xFF); bytes[0] = param_val & (0xFF);
@ -449,77 +434,90 @@ void em7180_set_integer_param(uint8_t param, uint32_t param_val)
bytes[2] = (param_val >> 16) & (0xFF); bytes[2] = (param_val >> 16) & (0xFF);
bytes[3] = (param_val >> 24) & (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 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 i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_LoadParamByte0,
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte1, bytes[1]); bytes[0]); // Param LSB
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte2, bytes[2]); i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_LoadParamByte1,
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte3, bytes[3]); // Param MSB bytes[1]);
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, param); i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_LoadParamByte2,
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x80); // Request parameter transfer procedure bytes[2]);
STAT = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge); // Check the parameter acknowledge register and loop until the result matches parameter request byte i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_LoadParamByte3,
bytes[3]); // Param MSB
i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_ParamRequest, param);
i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_AlgorithmControl, 0x80); // Request parameter transfer procedure
STAT = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_ParamAcknowledge); // Check the parameter acknowledge register and loop until the result matches parameter request byte
while(!(STAT == param)) while(!(STAT == param))
{ {
STAT = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge); STAT = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS,
EM7180_ParamAcknowledge);
} }
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0x00); // Parameter request = 0 to end parameter transfer process i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_ParamRequest, 0x00); // Parameter request = 0 to end parameter transfer process
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // Re-start algorithm i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // Re-start algorithm
} }
void em7180_param_set_float(uint8_t param, float param_val) void em7180_param_set_float(em7180_t *em7180, uint8_t param, float param_val)
{ {
uint8_t bytes[4], STAT; uint8_t bytes[4], STAT;
em7180_float_to_bytes(param_val, &bytes[0]); 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 i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_LoadParamByte0,
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte1, bytes[1]); bytes[0]); // Param LSB
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte2, bytes[2]); i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_LoadParamByte1,
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte3, bytes[3]); // Param MSB bytes[1]);
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, param); i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_LoadParamByte2,
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x80); // Request parameter transfer procedure bytes[2]);
STAT = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge); // Check the parameter acknowledge register and loop until the result matches parameter request byte i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_LoadParamByte3,
bytes[3]); // Param MSB
i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_ParamRequest, param);
i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_AlgorithmControl, 0x80); // Request parameter transfer procedure
STAT = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_ParamAcknowledge); // Check the parameter acknowledge register and loop until the result matches parameter request byte
while(!(STAT == param)) while(!(STAT == param))
{ {
STAT = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge); STAT = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS,
EM7180_ParamAcknowledge);
} }
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0x00); // Parameter request = 0 to end parameter transfer process i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_ParamRequest, 0x00); // Parameter request = 0 to end parameter transfer process
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // Re-start algorithm i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // Re-start algorithm
} }
void em7180_quatdata_get(float *destination) void em7180_quatdata_get(em7180_t *em7180, float *destination)
{ {
uint8_t rawData[16]; // x/y/z quaternion register data stored here uint8_t data[16]; // x/y/z quaternion register data stored here
em7180_read(EM7180_ADDRESS, EM7180_QX, 16, &rawData[0]); // Read the sixteen raw data registers into data array
destination[1] = uint32_reg_to_float(&rawData[0]);
destination[2] = uint32_reg_to_float(&rawData[4]);
destination[3] = uint32_reg_to_float(&rawData[8]);
destination[0] = uint32_reg_to_float(&rawData[12]); // SENtral stores quats as qx, qy, qz, q0!
i2c_read(em7180->hi2c, EM7180_ADDRESS, EM7180_QX, data, 16); // Read the sixteen raw data registers into data array
destination[1] = uint32_reg_to_float(&data[0]);
destination[2] = uint32_reg_to_float(&data[4]);
destination[3] = uint32_reg_to_float(&data[8]);
destination[0] = uint32_reg_to_float(&data[12]); // SENtral stores quats as qx, qy, qz, q0!
} }
void em7180_acceldata_get(int16_t *destination) void em7180_acceldata_get(em7180_t *em7180, int16_t *destination)
{ {
uint8_t rawData[6]; // x/y/z accel register data stored here uint8_t data[6]; // x/y/z accel register data stored here
em7180_read(EM7180_ADDRESS, EM7180_AX, 6, &rawData[0]); // Read the six raw data registers into data array
destination[0] = (int16_t) (((int16_t) rawData[1] << 8) | rawData[0]); // Turn the MSB and LSB into a signed 16-bit value i2c_read(em7180->hi2c, EM7180_ADDRESS, EM7180_AX, data, 6); // Read the six raw data registers into data array
destination[1] = (int16_t) (((int16_t) rawData[3] << 8) | rawData[2]); destination[0] = (int16_t) (((int16_t) data[1] << 8) | data[0]); // Turn the MSB and LSB into a signed 16-bit value
destination[2] = (int16_t) (((int16_t) rawData[5] << 8) | rawData[4]); destination[1] = (int16_t) (((int16_t) data[3] << 8) | data[2]);
destination[2] = (int16_t) (((int16_t) data[5] << 8) | data[4]);
} }
void em7180_gyrodata_get(int16_t *destination) void em7180_gyrodata_get(em7180_t *em7180, int16_t *destination)
{ {
uint8_t rawData[6]; // x/y/z gyro register data stored here uint8_t data[6]; // x/y/z gyro register data stored here
em7180_read(EM7180_ADDRESS, EM7180_GX, 6, &rawData[0]); // Read the six raw data registers sequentially into data array
destination[0] = (int16_t) (((int16_t) rawData[1] << 8) | rawData[0]); // Turn the MSB and LSB into a signed 16-bit value i2c_read(em7180->hi2c, EM7180_ADDRESS, EM7180_GX, data, 6); // Read the six raw data registers sequentially into data array
destination[1] = (int16_t) (((int16_t) rawData[3] << 8) | rawData[2]); destination[0] = (int16_t) (((int16_t) data[1] << 8) | data[0]); // Turn the MSB and LSB into a signed 16-bit value
destination[2] = (int16_t) (((int16_t) rawData[5] << 8) | rawData[4]); destination[1] = (int16_t) (((int16_t) data[3] << 8) | data[2]);
destination[2] = (int16_t) (((int16_t) data[5] << 8) | data[4]);
} }
void em7180_magdata_get(int16_t *destination) void em7180_magdata_get(em7180_t *em7180, int16_t *destination)
{ {
uint8_t rawData[6]; // x/y/z gyro register data stored here uint8_t data[6]; // x/y/z mag register data stored here
em7180_read(EM7180_ADDRESS, EM7180_MX, 6, &rawData[0]); // Read the six raw data registers sequentially into data array
destination[0] = (int16_t) (((int16_t) rawData[1] << 8) | rawData[0]); // Turn the MSB and LSB into a signed 16-bit value i2c_read(em7180->hi2c, EM7180_ADDRESS, EM7180_MX, data, 6); // Read the six raw data registers sequentially into data array
destination[1] = (int16_t) (((int16_t) rawData[3] << 8) | rawData[2]); destination[0] = (int16_t) (((int16_t) data[1] << 8) | data[0]); // Turn the MSB and LSB into a signed 16-bit value
destination[2] = (int16_t) (((int16_t) rawData[5] << 8) | rawData[4]); destination[1] = (int16_t) (((int16_t) data[3] << 8) | data[2]);
destination[2] = (int16_t) (((int16_t) data[5] << 8) | data[4]);
} }
float em7180_mres_get(uint8_t Mscale) float em7180_mres_get(uint8_t Mscale)
@ -608,33 +606,39 @@ float em7180_ares_get(uint8_t ascale)
return a_res; return a_res;
} }
int16_t em7180_baro_get() int16_t em7180_baro_get(em7180_t *em7180)
{ {
uint8_t rawData[2]; // x/y/z gyro register data stored here uint8_t data[2]; // baro register data stored here
em7180_read(EM7180_ADDRESS, EM7180_Baro, 2, &rawData[0]); // Read the two raw data registers sequentially into data array
return (int16_t) (((int16_t) rawData[1] << 8) | rawData[0]); // Turn the MSB and LSB into a signed 16-bit value i2c_read(em7180->hi2c, EM7180_ADDRESS, EM7180_Baro, data, 2); // Read the two raw data registers sequentially into data array
return (((int16_t) data[1] << 8) | data[0]); // Turn the MSB and LSB into a signed 16-bit value
} }
int16_t em7180_temp_get() int16_t em7180_temp_get(em7180_t *em7180)
{ {
uint8_t rawData[2]; // x/y/z gyro register data stored here uint8_t data[2]; // temp register data stored here
em7180_read(EM7180_ADDRESS, EM7180_Temp, 2, &rawData[0]); // Read the two raw data registers sequentially into data array
return (int16_t) (((int16_t) rawData[1] << 8) | rawData[0]); // Turn the MSB and LSB into a signed 16-bit value i2c_read(em7180->hi2c, EM7180_ADDRESS, EM7180_Temp, data, 2); // Read the two raw data registers sequentially into data array
return (((int16_t) data[1] << 8) | data[0]); // Turn the MSB and LSB into a signed 16-bit value
} }
void em7180_passthrough() static void em7180_passthrough(em7180_t *em7180)
{ {
// First put SENtral in standby mode // First put SENtral in standby mode
uint8_t c = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_AlgorithmControl); uint8_t c = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS,
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, c | 0x01); EM7180_AlgorithmControl);
i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_AlgorithmControl,
c | 0x01);
// c = readByte(EM7180_ADDRESS, EM7180_AlgorithmStatus); // c = readByte(EM7180_ADDRESS, EM7180_AlgorithmStatus);
/* // Serial.print("c = "); Serial.println(c); */ /* // Serial.print("c = "); Serial.println(c); */
// Verify standby status // Verify standby status
// if(readByte(EM7180_ADDRESS, EM7180_AlgorithmStatus) & 0x01) { // if(readByte(EM7180_ADDRESS, EM7180_AlgorithmStatus) & 0x01) {
/* Serial.println("SENtral in standby mode"); */ /* Serial.println("SENtral in standby mode"); */
// Place SENtral in pass-through mode // Place SENtral in pass-through mode
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_PassThruControl, 0x01); i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_PassThruControl, 0x01);
if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_PassThruStatus) & 0x01) if(i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_PassThruStatus) & 0x01)
{ {
/* Serial.println("SENtral in pass-through mode"); */ /* Serial.println("SENtral in pass-through mode"); */
} }
@ -644,67 +648,37 @@ void em7180_passthrough()
} }
} }
// I2C communication with the M24512DFM EEPROM is a little different from I2C communication with the usual motion sensor static float uint32_reg_to_float(uint8_t *buf)
// since the address is defined by two bytes
static void m24512dfm_write_byte(uint8_t device_address, uint8_t data_address1,
uint8_t data_address2, uint8_t data)
{ {
uint8_t temp[2] = { data_address1, data_address2 }; union
/* Wire.transfer(device_address, &temp[0], 2, NULL, 0); */
/* Wire.transfer(device_address, &data, 1, NULL, 0); */
}
static void m24512dfm_write(uint8_t device_address, uint8_t data_address1,
uint8_t data_address2, uint8_t count, uint8_t *dest)
{
if(count > 128)
{ {
count = 128; uint32_t ui32;
/* Serial.print("Page count cannot be more than 128 bytes!"); */ float f;
} } u;
uint8_t temp[2] = { data_address1, data_address2 };
/* Wire.transfer(device_address, &temp[0], 2, NULL, 0); */
/* Wire.transfer(device_address, &dest[0], count, NULL, 0); */
}
static uint8_t m24512dfm_read_byte(uint8_t device_address,
uint8_t data_address1, uint8_t data_address2)
{
uint8_t data; // `data` will store the register data
/* Wire.beginTransmission(device_address); // Initialize the Tx buffer */
/* Wire.write(data_address1); // Put slave register address in Tx buffer */
/* Wire.write(data_address2); // Put slave register address in Tx buffer */
/* Wire.endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive */
/* Wire.requestFrom(device_address, 1); // Read one byte from slave register address */
/* data = Wire.read(); // Fill Rx buffer with result */
return data; // Return data read from slave register
}
static void m24512dfm_read(uint8_t device_address, uint8_t data_address1, u.ui32 = (((uint32_t) buf[0]) + (((uint32_t) buf[1]) << 8)
uint8_t data_address2, uint8_t count, uint8_t *dest) + (((uint32_t) buf[2]) << 16) + (((uint32_t) buf[3]) << 24));
{
uint8_t temp[2] = { data_address1, data_address2 };
/* Wire.transfer(device_address, &temp[0], 2, dest, count); */
}
// I2C read/write functions for the EM7180 return u.f;
void em7180_write_byte(uint8_t address, uint8_t subAddress, uint8_t data)
{
uint8_t temp[2];
temp[0] = subAddress;
temp[1] = data;
/* Wire.transfer(address, &temp[0], 2, NULL, 0); */
} }
static uint8_t em7180_read_byte(uint8_t address, uint8_t subAddress) static void float_to_bytes(float param_val, uint8_t *buf)
{ {
uint8_t temp[1]; union
/* Wire.transfer(address, &subAddress, 1, &temp[0], 1); */ {
return temp[0]; float f;
} uint8_t u8[sizeof(float)];
} u;
static void em7180_read(uint8_t address, uint8_t subAddress, uint8_t count, u.f = param_val;
uint8_t *dest) for(uint8_t i = 0; i < sizeof(float); i++)
{ {
/* Wire.transfer(address, &subAddress, 1, dest, count); */ buf[i] = u.u8[i];
}
// Convert to LITTLE ENDIAN
/* FIXME: What the hell? */
for(uint8_t i = 0; i < sizeof(float); i++)
{
buf[i] = buf[(sizeof(float) - 1) - i];
}
} }

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

@ -22,83 +22,78 @@
/* Private Global Variables */ /* Private Global Variables */
/* Function Prototypes */ /* Function Prototypes */
static void lis2mdl_write_byte(uint8_t address, uint8_t subAddress,
uint8_t data);
static uint8_t lis2mdl_read_byte(uint8_t address, uint8_t subAddress);
static void lis2mdl_read(uint8_t address, uint8_t subAddress, uint8_t count,
uint8_t *dest);
/* Function Definitions */ /* Function Definitions */
void lis2mdl_init(lis2mdl_t *lis2mdl, I2C_HandleTypeDef *hi2c, uint8_t m_odr) void lis2mdl_init(lis2mdl_t *lis2mdl, uint8_t m_odr)
{ {
if(!lis2mdl) if(!lis2mdl)
{ {
return; return;
} }
lis2mdl->hi2c = hi2c;
lis2mdl->m_odr = m_odr; lis2mdl->m_odr = m_odr;
} }
void lis2mdl_config(lis2mdl_t *lis2mdl) void lis2mdl_config(lis2mdl_t *lis2mdl, I2C_HandleTypeDef *hi2c)
{ {
// enable temperature compensation (bit 7 == 1), continuous mode (bits 0:1 == 00) // enable temperature compensation (bit 7 == 1), continuous mode (bits 0:1 == 00)
lis2mdl_write_byte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A, i2c_write_byte(hi2c, LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A,
0x80 | lis2mdl->m_odr << 2); 0x80 | lis2mdl->m_odr << 2);
// enable low pass filter (bit 0 == 1), set to ODR/4 // enable low pass filter (bit 0 == 1), set to ODR/4
lis2mdl_write_byte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_B, 0x01); i2c_write_byte(hi2c, LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_B, 0x01);
// enable data ready on interrupt pin (bit 0 == 1), enable block data read (bit 4 == 1) // 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); i2c_write_byte(hi2c, LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C, 0x01 | 0x10);
} }
uint8_t lis2mdl_chip_id_get() uint8_t lis2mdl_chip_id_get(I2C_HandleTypeDef *hi2c)
{ {
uint8_t c = lis2mdl_read_byte(LIS2MDL_ADDRESS, LIS2MDL_WHO_AM_I); uint8_t c = i2c_read_byte(hi2c, LIS2MDL_ADDRESS, LIS2MDL_WHO_AM_I);
return c; return c;
} }
void lis2mdl_reset() void lis2mdl_reset(I2C_HandleTypeDef *hi2c)
{ {
// reset device // reset device
uint8_t temp = lis2mdl_read_byte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A); uint8_t temp = i2c_read_byte(hi2c, LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A);
lis2mdl_write_byte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A, temp | 0x20); // Set bit 5 to 1 to reset LIS2MDL
i2c_write_byte(hi2c, LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A, temp | 0x20); // Set bit 5 to 1 to reset LIS2MDL
HAL_Delay(1); HAL_Delay(1);
lis2mdl_write_byte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A, temp | 0x40); // Set bit 6 to 1 to boot LIS2MDL i2c_write_byte(hi2c, LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A, temp | 0x40); // Set bit 6 to 1 to boot LIS2MDL
HAL_Delay(100); // Wait for all registers to reset HAL_Delay(100); // Wait for all registers to reset
} }
uint8_t lis2mdl_status() uint8_t lis2mdl_status(I2C_HandleTypeDef *hi2c)
{ {
// Read the status register of the altimeter // Read the status register of the altimeter
uint8_t temp = lis2mdl_read_byte(LIS2MDL_ADDRESS, LIS2MDL_STATUS_REG); uint8_t temp = i2c_read_byte(hi2c, LIS2MDL_ADDRESS, LIS2MDL_STATUS_REG);
return temp; return temp;
} }
void lis2mdl_data_get(int16_t *destination) void lis2mdl_data_get(I2C_HandleTypeDef *hi2c, int16_t *destination)
{ {
uint8_t rawData[6]; // x/y/z mag register data stored here uint8_t data[6]; // x/y/z mag register data stored here
lis2mdl_read_bytes(LIS2MDL_ADDRESS, (0x80 | LIS2MDL_OUTX_L_REG), 8, i2c_read(hi2c, LIS2MDL_ADDRESS, (0x80 | LIS2MDL_OUTX_L_REG), data, 8); // Read the 6 raw data registers into data array
&rawData[0]); // Read the 6 raw data registers into data array
destination[0] = ((int16_t) rawData[1] << 8) | rawData[0]; // Turn the MSB and LSB into a signed 16-bit value destination[0] = ((int16_t) data[1] << 8) | data[0]; // Turn the MSB and LSB into a signed 16-bit value
destination[1] = ((int16_t) rawData[3] << 8) | rawData[2]; destination[1] = ((int16_t) data[3] << 8) | data[2];
destination[2] = ((int16_t) rawData[5] << 8) | rawData[4]; destination[2] = ((int16_t) data[5] << 8) | data[4];
} }
int16_t lis2mdl_temp_get() int16_t lis2mdl_temp_get()
{ {
uint8_t rawData[2]; // x/y/z mag register data stored here uint8_t data[2]; // x/y/z mag register data stored here
lis2mdl_read_bytes(LIS2MDL_ADDRESS, (0x80 | LIS2MDL_TEMP_OUT_L_REG), 2, lis2mdl_read_bytes(LIS2MDL_ADDRESS, (0x80 | LIS2MDL_TEMP_OUT_L_REG), 2,
&rawData[0]); // Read the 8 raw data registers into data array &data[0]); // Read the 8 raw data registers into data array
int16_t temp = ((int16_t) rawData[1] << 8) | rawData[0]; // Turn the MSB and LSB into a signed 16-bit value int16_t temp = ((int16_t) data[1] << 8) | data[0]; // Turn the MSB and LSB into a signed 16-bit value
return temp; return temp;
} }
void lis2mdl_offset_bias(float *dest1, float *dest2) void lis2mdl_offset_bias(I2C_HandleTypeDef *hi2c, 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] =
@ -110,7 +105,7 @@ void lis2mdl_offset_bias(float *dest1, float *dest2)
for(int ii = 0; ii < 4000; ii++) for(int ii = 0; ii < 4000; ii++)
{ {
lis2mdl_data_get(mag_temp); lis2mdl_data_get(hi2c, mag_temp);
for(int jj = 0; jj < 3; jj++) for(int jj = 0; jj < 3; jj++)
{ {
if(mag_temp[jj] > mag_max[jj]) if(mag_temp[jj] > mag_max[jj])
@ -150,7 +145,7 @@ void lis2mdl_offset_bias(float *dest1, float *dest2)
/* Serial.println("Mag Calibration done!"); */ /* Serial.println("Mag Calibration done!"); */
} }
void lis2mdl_self_test() void lis2mdl_self_test(I2C_HandleTypeDef *hi2c)
{ {
int16_t temp[3] = { 0, 0, 0 }; int16_t temp[3] = { 0, 0, 0 };
float magTest[3] = { 0., 0., 0. }; float magTest[3] = { 0., 0., 0. };
@ -161,7 +156,7 @@ void lis2mdl_self_test()
// 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++)
{ {
lis2mdl_data_get(temp); lis2mdl_data_get(hi2c, temp);
sum[0] += temp[0]; sum[0] += temp[0];
sum[1] += temp[1]; sum[1] += temp[1];
sum[2] += temp[2]; sum[2] += temp[2];
@ -172,8 +167,8 @@ void lis2mdl_self_test()
magNom[1] = (float) sum[1] / 50.0f; magNom[1] = (float) sum[1] / 50.0f;
magNom[2] = (float) sum[2] / 50.0f; magNom[2] = (float) sum[2] / 50.0f;
uint8_t c = lis2mdl_read_byte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C); uint8_t c = i2c_read_byte(hi2c, LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C);
lis2mdl_write_byte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C, c | 0x02); // enable self test i2c_write_byte(hi2c, LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C, c | 0x02); // enable self test
HAL_Delay(100); // let mag respond HAL_Delay(100); // let mag respond
sum[0] = 0; sum[0] = 0;
@ -181,7 +176,7 @@ void lis2mdl_self_test()
sum[2] = 0; sum[2] = 0;
for(int ii = 0; ii < 50; ii++) for(int ii = 0; ii < 50; ii++)
{ {
lis2mdl_data_get(temp); lis2mdl_data_get(hi2c, temp);
sum[0] += temp[0]; sum[0] += temp[0];
sum[1] += temp[1]; sum[1] += temp[1];
sum[2] += temp[2]; sum[2] += temp[2];
@ -192,7 +187,7 @@ void lis2mdl_self_test()
magTest[1] = (float) sum[1] / 50.0f; magTest[1] = (float) sum[1] / 50.0f;
magTest[2] = (float) sum[2] / 50.0f; magTest[2] = (float) sum[2] / 50.0f;
lis2mdl_write_byte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C, c); // return to previous settings/normal mode i2c_write_byte(hi2c, LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C, c); // return to previous settings/normal mode
HAL_Delay(100); // let mag respond HAL_Delay(100); // let mag respond
/* Serial.println("Mag Self Test:"); */ /* Serial.println("Mag Self Test:"); */
@ -206,27 +201,3 @@ void lis2mdl_self_test()
/* 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
} }
// I2C read/write functions for the LIS2MDL
static void lis2mdl_write_byte(uint8_t address, uint8_t subAddress,
uint8_t data)
{
uint8_t temp[2];
temp[0] = subAddress;
temp[1] = data;
/* Wire.transfer(address, &temp[0], 2, NULL, 0); */
}
static uint8_t lis2mdl_read_byte(uint8_t address, uint8_t subAddress)
{
uint8_t temp[1];
/* Wire.transfer(address, &subAddress, 1, &temp[0], 1); */
return temp[0];
}
static void lis2mdl_read(uint8_t address, uint8_t subAddress, uint8_t count,
uint8_t *dest)
{
/* Wire.transfer(address, &subAddress, 1, dest, count); */
}

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

@ -22,91 +22,56 @@
/* Private Global Variables */ /* Private Global Variables */
/* Function Prototypes */ /* Function Prototypes */
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 */ /* Function Definitions */
void lps22hb_init(lps22hb_t *lps22hb, I2C_HandleTypeDef *hi2c, uint8_t p_odr) void lps22hb_init(lps22hb_t *lps22hb, uint8_t p_odr)
{ {
lps22hb->hi2c = hi2c; return_if_fail(lps22hb);
lps22hb->p_odr = p_odr; lps22hb->p_odr = p_odr;
} }
void lps22hb_config(lps22hb_t *lps22hb) void lps22hb_config(lps22hb_t *lps22hb, I2C_HandleTypeDef *hi2c)
{ {
// set sample rate by setting bits 6:4 // set sample rate by setting bits 6:4
// enable low-pass filter by setting bit 3 to one // 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 // 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 // make sure data not updated during read by setting block data udate (bit 1) to 1
lps22hb_write_byte(LPS22H_ADDRESS, LPS22H_CTRL_REG1, i2c_write_byte(hi2c, LPS22H_ADDRESS, LPS22H_CTRL_REG1,
lps22hb->p_odr << 4 | 0x08 | 0x02); lps22hb->p_odr << 4 | 0x08 | 0x02);
lps22hb_write_byte(LPS22H_ADDRESS, LPS22H_CTRL_REG3, 0x04); // enable data ready as interrupt source i2c_write_byte(hi2c, LPS22H_ADDRESS, LPS22H_CTRL_REG3, 0x04); // enable data ready as interrupt source
} }
uint8_t lps22hb_chip_id_get() uint8_t lps22hb_chip_id_get(I2C_HandleTypeDef *hi2c)
{ {
// 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 = lps22hb_read_byte(LPS22H_ADDRESS, LPS22H_WHOAMI); // Read WHO_AM_I register for LPS22H uint8_t temp = i2c_read_byte(hi2c, LPS22H_ADDRESS, LPS22H_WHOAMI); // Read WHO_AM_I register for LPS22H
return temp; return temp;
} }
uint8_t lps22hb_status() uint8_t lps22hb_status(I2C_HandleTypeDef *hi2c)
{ {
// Read the status register of the altimeter // Read the status register of the altimeter
uint8_t temp = lps22hb_read_byte(LPS22H_ADDRESS, LPS22H_STATUS); uint8_t temp = i2c_read_byte(hi2c, LPS22H_ADDRESS, LPS22H_STATUS);
return temp; return temp;
} }
int32_t lps22hb_pressure_get() int32_t lps22hb_pressure_get(I2C_HandleTypeDef *hi2c)
{ {
uint8_t rawData[3]; // 24-bit pressure register data stored here uint8_t data[3]; // 24-bit pressure register data stored here
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 lps22hb_temp_get() i2c_read(hi2c, LPS22H_ADDRESS, (LPS22H_PRESS_OUT_XL | 0x80), data, 3); // bit 7 must be one to read multiple bytes
{
uint8_t rawData[2]; // 16-bit pressure register data stored here
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]);
}
static void lps22hb_write_byte(uint8_t address, uint8_t subAddress, return ((int32_t) data[2] << 16 | (int32_t) data[1] << 8 | data[0]);
uint8_t data)
{
/* Wire.beginTransmission(address); // Initialize the Tx buffer */
/* Wire.write(subAddress); // Put slave register address in Tx buffer */
/* Wire.write(data); // Put data in Tx buffer */
/* Wire.endTransmission(); // Send the Tx buffer */
} }
static uint8_t lps22hb_read_byte(uint8_t address, uint8_t subAddress) int16_t lps22hb_temp_get(I2C_HandleTypeDef *hi2c)
{ {
uint8_t data; // `data` will store the register data uint8_t data[2]; // 16-bit pressure register data stored here
/* Wire.beginTransmission(address); // Initialize the Tx buffer */
/* Wire.write(subAddress); // Put slave register address in Tx buffer */
/* 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 lps22hb_read from slave register
}
static void lps22hb_read(uint8_t address, uint8_t subAddress, uint8_t count, i2c_read(hi2c, LPS22H_ADDRESS, (LPS22H_TEMP_OUT_L | 0x80), data, 2); // bit 7 must be one to read multiple bytes
uint8_t *dest)
{ return ((int16_t) data[1] << 8 | data[0]);
/* Wire.beginTransmission(address); // Initialize the Tx buffer */
/* Wire.write(subAddress); // Put slave register address in Tx buffer */
/* Wire.endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive */
uint8_t i = 0;
/* Wire.requestFrom(address, (size_t) count); // Read bytes from slave register address */
/*
while(Wire.available())
{
dest[i++] = Wire.lps22hb_read();
} // Put lps22hb_read results in the Rx buffer
*/
} }

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

@ -23,49 +23,48 @@
/* Private Global Variables */ /* Private Global Variables */
/* Function Prototypes */ /* Function Prototypes */
static void lsm6dsm_read_data(lsm6dsm_t *lsm6dsm, I2C_HandleTypeDef *hi2c,
int16_t *destination);
/* Function Definitions */ /* Function Definitions */
void lsm6dsm_init(lsm6dsm_t *lsm6dsm, I2C_HandleTypeDef *hi2c, uint8_t ascale, void lsm6dsm_init(lsm6dsm_t *lsm6dsm, uint8_t ascale, uint8_t gscale,
uint8_t gscale, uint8_t a_odr, uint8_t g_odr) uint8_t a_odr, uint8_t g_odr)
{ {
if(!lsm6dsm) return_if_fail(lsm6dsm);
{
return;
}
lsm6dsm->hi2c = hi2c;
lsm6dsm->ascale = ascale; lsm6dsm->ascale = ascale;
lsm6dsm->gscale = gscale; lsm6dsm->gscale = gscale;
lsm6dsm->a_odr = a_odr; lsm6dsm->a_odr = a_odr;
lsm6dsm->g_odr = g_odr; lsm6dsm->g_odr = g_odr;
} }
void lsm6dsm_config(lsm6dsm_t *lsm6dsm) void lsm6dsm_config(lsm6dsm_t *lsm6dsm, I2C_HandleTypeDef *hi2c)
{ {
lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL1_XL, i2c_write_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_CTRL1_XL,
lsm6dsm->a_odr << 4 | lsm6dsm->ascale << 2); lsm6dsm->a_odr << 4 | lsm6dsm->ascale << 2);
lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL2_G, i2c_write_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_CTRL2_G,
lsm6dsm->g_odr << 4 | lsm6dsm->gscale << 2); lsm6dsm->g_odr << 4 | lsm6dsm->gscale << 2);
uint8_t temp = lsm6dsm_read_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL3_C); uint8_t temp = i2c_read_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_CTRL3_C);
// enable block update (bit 6 = 1), auto-increment registers (bit 2 = 1) // enable block update (bit 6 = 1), auto-increment registers (bit 2 = 1)
lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL3_C, temp | 0x40 | 0x04); i2c_write_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_CTRL3_C, temp | 0x40 | 0x04);
// by default, interrupts active HIGH, push pull, little endian data // 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) // (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 // 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); i2c_write_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_CTRL8_XL, 0x80 | 0x40 | 0x08);
// interrupt handling // interrupt handling
lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_DRDY_PULSE_CFG, 0x80); // latch interrupt until data read i2c_write_byte(hi2c, 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 i2c_write_byte(hi2c, 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 i2c_write_byte(hi2c, 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(lsm6dsm_t *lsm6dsm, I2C_HandleTypeDef *hi2c)
{ {
uint8_t c = lsm6dsm_read_byte(LSM6DSM_ADDRESS, LSM6DSM_WHO_AM_I); uint8_t c = i2c_read_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_WHO_AM_I);
return c; return c;
} }
@ -126,22 +125,26 @@ float lsm6dsm_gres_get(lsm6dsm_t *lsm6dsm)
return g_res; return g_res;
} }
void lsm6dsm_reset() void lsm6dsm_reset(lsm6dsm_t *lsm6dsm, I2C_HandleTypeDef *hi2c)
{ {
// reset device // reset device
uint8_t temp = lsm6dsm_read_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL3_C); uint8_t temp = i2c_read_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_CTRL3_C);
lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL3_C, temp | 0x01); // Set bit 0 to 1 to reset LSM6DSM
i2c_write_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_CTRL3_C, temp | 0x01); // Set bit 0 to 1 to reset LSM6DSM
HAL_Delay(100); // Wait for all registers to reset HAL_Delay(100); // Wait for all registers to reset
} }
void lsm6dsm_selfTest() void lsm6dsm_self_test(lsm6dsm_t *lsm6dsm, I2C_HandleTypeDef *hi2c)
{ {
int16_t temp[7] = { 0, 0, 0, 0, 0, 0, 0 }; int16_t temp[7] = { 0, 0, 0, 0, 0, 0, 0 };
int16_t accelPTest[3] = { 0, 0, 0 }, accelNTest[3] = { 0, 0, 0 }, int16_t accelPTest[3] = { 0, 0, 0 };
gyroPTest[3] = { 0, 0, 0 }, gyroNTest[3] = { 0, 0, 0 }; int16_t accelNTest[3] = { 0, 0, 0 };
int16_t accelNom[3] = { 0, 0, 0 }, gyroNom[3] = { 0, 0, 0 }; int16_t gyroPTest[3] = { 0, 0, 0 };
int16_t gyroNTest[3] = { 0, 0, 0 };
lsm6dsm_read_data(temp); int16_t accelNom[3] = { 0, 0, 0 };
int16_t gyroNom[3] = { 0, 0, 0 };
lsm6dsm_read_data(lsm6dsm, hi2c, 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];
@ -149,35 +152,35 @@ void lsm6dsm_selfTest()
gyroNom[1] = temp[2]; gyroNom[1] = temp[2];
gyroNom[2] = temp[3]; gyroNom[2] = temp[3];
lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x01); // positive accel self test i2c_write_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x01); // positive accel self test
HAL_Delay(100); // let accel respond HAL_Delay(100); // let accel respond
lsm6dsm_read_data(temp); lsm6dsm_read_data(lsm6dsm, hi2c, 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 i2c_write_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x03); // negative accel self test
HAL_Delay(100); // let accel respond HAL_Delay(100); // let accel respond
lsm6dsm_read_data(temp); lsm6dsm_read_data(lsm6dsm, hi2c, 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 i2c_write_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x04); // positive gyro self test
HAL_Delay(100); // let gyro respond HAL_Delay(100); // let gyro respond
lsm6dsm_read_data(temp); lsm6dsm_read_data(lsm6dsm, hi2c, 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 i2c_write_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x0C); // negative gyro self test
HAL_Delay(100); // let gyro respond HAL_Delay(100); // let gyro respond
lsm6dsm_read_data(temp); lsm6dsm_read_data(lsm6dsm, hi2c, 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];
lsm6dsm_write_byte(LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x00); // normal mode i2c_write_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x00); // normal mode
HAL_Delay(100); // let accel and gyro respond HAL_Delay(100); // let accel and gyro respond
/* Serial.println("Accel Self Test:"); */ /* Serial.println("Accel Self Test:"); */
@ -215,7 +218,8 @@ void lsm6dsm_selfTest()
} }
void lsm6dsm_offset_bias(lsm6dsm_t *lsm6dsm, float *dest1, float *dest2) void lsm6dsm_offset_bias(lsm6dsm_t *lsm6dsm, I2C_HandleTypeDef *hi2c,
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 };
@ -227,7 +231,7 @@ void lsm6dsm_offset_bias(lsm6dsm_t *lsm6dsm, float *dest1, float *dest2)
for(int ii = 0; ii < 128; ii++) for(int ii = 0; ii < 128; ii++)
{ {
lsm6dsm_read_data(temp); lsm6dsm_read_data(lsm6dsm, hi2c, 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];
@ -271,36 +275,17 @@ void lsm6dsm_offset_bias(lsm6dsm_t *lsm6dsm, float *dest1, float *dest2)
} }
void lsm6dsm_read_data(int16_t *destination) static void lsm6dsm_read_data(lsm6dsm_t *lsm6dsm, I2C_HandleTypeDef *hi2c,
{ int16_t *destination)
uint8_t rawdata[14]; // x/y/z accel register data stored here
lsm6dsm_read(LSM6DSM_ADDRESS, LSM6DSM_OUT_TEMP_L, 14, &rawdata[0]); // Read the 14 raw data registers into data array
destination[0] = ((int16_t) rawdata[1] << 8) | rawdata[0]; // Turn the MSB and LSB into a signed 16-bit value
destination[1] = ((int16_t) rawdata[3] << 8) | rawdata[2];
destination[2] = ((int16_t) rawdata[5] << 8) | rawdata[4];
destination[3] = ((int16_t) rawdata[7] << 8) | rawdata[6];
destination[4] = ((int16_t) rawdata[9] << 8) | rawdata[8];
destination[5] = ((int16_t) rawdata[11] << 8) | rawdata[10];
destination[6] = ((int16_t) rawdata[13] << 8) | rawdata[12];
}
// I2C read/write functions for the LSM6DSM
void lsm6dsm_write_byte(uint8_t addr, uint8_t sub_addr, uint8_t data)
{
uint8_t temp[2];
temp[0] = sub_addr;
temp[1] = data;
/* Wire.transfer(addr, &temp[0], 2, NULL, 0); */
}
uint8_t lsm6dsm_read_byte(uint8_t addr, uint8_t sub_addr)
{
uint8_t temp[1];
/* Wire.transfer(addr, &sub_addr, 1, &temp[0], 1); */
return temp[0];
}
void lsm6dsm_read(uint8_t addr, uint8_t sub_addr, uint8_t count, uint8_t *dest)
{ {
/* Wire.transfer(addr, &sub_addr, 1, dest, count); */ uint8_t data[14]; // x/y/z accel register data stored here
i2c_read(hi2c, LSM6DSM_ADDRESS, LSM6DSM_OUT_TEMP_L, data, 14); // Read the 14 raw data registers into data array
destination[0] = ((int16_t) data[1] << 8) | data[0]; // Turn the MSB and LSB into a signed 16-bit value
destination[1] = ((int16_t) data[3] << 8) | data[2];
destination[2] = ((int16_t) data[5] << 8) | data[4];
destination[3] = ((int16_t) data[7] << 8) | data[6];
destination[4] = ((int16_t) data[9] << 8) | data[8];
destination[5] = ((int16_t) data[11] << 8) | data[10];
destination[6] = ((int16_t) data[13] << 8) | data[12];
} }

Write Preview
Loading…
Cancel
Save