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

implement ST-like API for EM7180 

pared down EM7180 to just enough to set some config parameters and get back real data
LIS2MDL, LPS22HB, and LSM6DSM are non-functional
master
Daniel Peter Chokola 4 years ago
parent
c79ee256a8
commit
d0f01e53a4
7 changed files with 421 additions and 653 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 119
      Drivers/EM7180/Inc/em7180.h
  2. 38
      Drivers/EM7180/Inc/em7180_common.h
  3. 794
      Drivers/EM7180/Src/em7180.c
  4. 36
      Drivers/EM7180/Src/em7180_common.c
  5. 32
      Drivers/EM7180/Src/lis2mdl.c
  6. 15
      Drivers/EM7180/Src/lps22hb.c
  7. 40
      Drivers/EM7180/Src/lsm6dsm.c

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

@ -17,92 +17,17 @@ @@ -17,92 +17,17 @@
/* Includes */
#include <stdint.h>
#include "i2c.h"
#include "lsm6dsm.h"
#include "lps22hb.h"
#include "lis2mdl.h"
#include "em7180_common.h"
/* Definitions */
/*
* EM7180 SENtral register map
* see http://www.emdeveloper.com/downloads/7180/EMSentral_EM7180_Register_Map_v1_3.pdf
*/
#define EM7180_QX 0x00 // this is a 32-bit normalized floating point number read from registers 0x00-03
#define EM7180_QY 0x04 // this is a 32-bit normalized floating point number read from registers 0x04-07
#define EM7180_QZ 0x08 // this is a 32-bit normalized floating point number read from registers 0x08-0B
#define EM7180_QW 0x0C // this is a 32-bit normalized floating point number read from registers 0x0C-0F
#define EM7180_QTIME 0x10 // this is a 16-bit unsigned integer read from registers 0x10-11
#define EM7180_MX 0x12 // int16_t from registers 0x12-13
#define EM7180_MY 0x14 // int16_t from registers 0x14-15
#define EM7180_MZ 0x16 // int16_t from registers 0x16-17
#define EM7180_MTIME 0x18 // uint16_t from registers 0x18-19
#define EM7180_AX 0x1A // int16_t from registers 0x1A-1B
#define EM7180_AY 0x1C // int16_t from registers 0x1C-1D
#define EM7180_AZ 0x1E // int16_t from registers 0x1E-1F
#define EM7180_ATIME 0x20 // uint16_t from registers 0x20-21
#define EM7180_GX 0x22 // int16_t from registers 0x22-23
#define EM7180_GY 0x24 // int16_t from registers 0x24-25
#define EM7180_GZ 0x26 // int16_t from registers 0x26-27
#define EM7180_GTIME 0x28 // uint16_t from registers 0x28-29
#define EM7180_Baro 0x2A // start of two-byte MS5637 pressure data, 16-bit signed interger
#define EM7180_BaroTIME 0x2C // start of two-byte MS5637 pressure timestamp, 16-bit unsigned
#define EM7180_Temp 0x2E // start of two-byte MS5637 temperature data, 16-bit signed interger
#define EM7180_TempTIME 0x30 // start of two-byte MS5637 temperature timestamp, 16-bit unsigned
#define EM7180_QRateDivisor 0x32 // uint8_t
#define EM7180_EnableEvents 0x33
#define EM7180_HostControl 0x34
#define EM7180_EventStatus 0x35
#define EM7180_SensorStatus 0x36
#define EM7180_SentralStatus 0x37
#define EM7180_AlgorithmStatus 0x38
#define EM7180_FeatureFlags 0x39
#define EM7180_ParamAcknowledge 0x3A
#define EM7180_SavedParamByte0 0x3B
#define EM7180_SavedParamByte1 0x3C
#define EM7180_SavedParamByte2 0x3D
#define EM7180_SavedParamByte3 0x3E
#define EM7180_ActualMagRate 0x45
#define EM7180_ActualAccelRate 0x46
#define EM7180_ActualGyroRate 0x47
#define EM7180_ActualBaroRate 0x48
#define EM7180_ActualTempRate 0x49
#define EM7180_ErrorRegister 0x50
#define EM7180_AlgorithmControl 0x54
#define EM7180_MagRate 0x55
#define EM7180_AccelRate 0x56
#define EM7180_GyroRate 0x57
#define EM7180_BaroRate 0x58
#define EM7180_TempRate 0x59
#define EM7180_LoadParamByte0 0x60
#define EM7180_LoadParamByte1 0x61
#define EM7180_LoadParamByte2 0x62
#define EM7180_LoadParamByte3 0x63
#define EM7180_ParamRequest 0x64
#define EM7180_ROMVersion1 0x70
#define EM7180_ROMVersion2 0x71
#define EM7180_RAMVersion1 0x72
#define EM7180_RAMVersion2 0x73
#define EM7180_ProductID 0x90
#define EM7180_RevisionID 0x91
#define EM7180_RunStatus 0x92
#define EM7180_UploadAddress 0x94 // uint16_t registers 0x94 (MSB)-5(LSB)
#define EM7180_UploadData 0x96
#define EM7180_CRCHost 0x97 // uint32_t from registers 0x97-9A
#define EM7180_ResetRequest 0x9B
#define EM7180_PassThruStatus 0x9E
#define EM7180_PassThruControl 0xA0
#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 EM7180_OK (0)
#define EM7180_BAD_ARG (1 << 0)
#define EM7180_BAD_COMM (1 << 1)
/* Data Structures */
typedef struct em7180_s
typedef int32_t em7180_status_t;
typedef struct em7180_init_s
{
I2C_HandleTypeDef *hi2c;
lsm6dsm_t *lsm6dsm;
lis2mdl_t *lis2mdl;
lps22hb_t *lps22hb;
uint16_t acc_fs;
uint16_t gyro_fs;
uint16_t mag_fs;
@ -111,18 +36,30 @@ typedef struct em7180_s @@ -111,18 +36,30 @@ typedef struct em7180_s
uint8_t acc_rate;
uint8_t gyro_rate;
uint8_t baro_rate;
} em7180_init_t;
typedef struct em7180_s
{
em7180_init_t *init;
uint8_t i2c_addr;
delay_func_t delay_func;
i2c_read_func_t i2c_read_func;
i2c_write_func_t i2c_write_func;
} em7180_t;
/* Function Prototypes */
void em7180_init(em7180_t *em7180, I2C_HandleTypeDef *hi2c, lsm6dsm_t *lsm6dsm,
lis2mdl_t *lis2mdl, lps22hb_t *lps22hb, 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(em7180_t *em7180, uint16_t gyro_fs);
void em7180_mag_acc_set_fs(em7180_t *em7180, uint16_t mag_fs, uint16_t acc_fs);
void em7180_set_integer_param(em7180_t *em7180, uint8_t param,
uint32_t param_val);
em7180_status_t em7180_init(em7180_t *em7180, em7180_init_t *init);
void em7180_set_delay_cb(em7180_t *em7180, delay_func_t delay_func);
void em7180_set_i2c_cbs(em7180_t *em7180, i2c_read_func_t i2c_read_func,
i2c_write_func_t i2c_write_func, uint8_t dev_addr);
em7180_status_t em7180_config(em7180_t *em7180);
void em7180_reset(em7180_t *em7180);
void em7180_passthrough_enable(em7180_t *em7180);
void em7180_passthrough_disable(em7180_t *em7180);
em7180_status_t em7180_quatdata_get(em7180_t *em7180, float *destination);
em7180_status_t em7180_acceldata_get(em7180_t *em7180, int16_t *destination);
em7180_status_t em7180_gyrodata_get(em7180_t *em7180, int16_t *destination);
em7180_status_t em7180_magdata_get(em7180_t *em7180, int16_t *destination);
em7180_status_t em7180_barodata_get(em7180_t *em7180, int16_t *destination);
em7180_status_t em7180_tempdata_get(em7180_t *em7180, int16_t *destination);
#endif /* EM7180_H_ */

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

@ -12,7 +12,6 @@ @@ -12,7 +12,6 @@
/* Includes */
#include <stdint.h>
#include "i2c.h"
/* Definitions */
#define return_if_fail(cond) \
@ -26,28 +25,21 @@ @@ -26,28 +25,21 @@
return (val); \
}
/* Function Prototypes */
inline __attribute__((always_inline)) HAL_StatusTypeDef i2c_write_byte(
I2C_HandleTypeDef *hi2c, uint16_t addr, uint16_t sub_addr, uint8_t data)
{
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);
/* Data Structures */
typedef void (*delay_func_t)(uint32_t);
typedef int32_t (*i2c_write_func_t)(uint16_t, uint16_t, uint16_t, uint8_t*,
uint16_t);
typedef int32_t (*i2c_read_func_t)(uint16_t, uint16_t, uint16_t, uint8_t*,
uint16_t);
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);
}
/* Function Prototypes */
int32_t i2c_read_byte(i2c_read_func_t read, uint16_t dev_addr, uint16_t addr,
uint8_t *byte);
int32_t i2c_write_byte(i2c_write_func_t write, uint16_t dev_addr, uint16_t addr,
uint8_t byte);
int32_t i2c_read(i2c_read_func_t read, uint16_t dev_addr, uint16_t addr,
uint8_t *data, uint16_t len);
int32_t i2c_write(i2c_write_func_t write, uint16_t dev_addr, uint16_t addr,
uint8_t *data, uint16_t len);
#endif /* EM7180_COMMON_h */

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

@ -14,510 +14,366 @@ @@ -14,510 +14,366 @@
/* Includes */
#include <stdint.h>
#include <stdbool.h>
#include <math.h>
#include <stddef.h>
#include "em7180_common.h"
#include "em7180.h"
#include "i2c.h"
/* Definitions */
/*
* EM7180 SENtral register map
* see http://www.emdeveloper.com/downloads/7180/EMSentral_EM7180_Register_Map_v1_3.pdf
*/
#define EM7180_QX 0x00 /* this is a 32-bit normalized floating point number read from registers 0x00-03 */
#define EM7180_QY 0x04 /* this is a 32-bit normalized floating point number read from registers 0x04-07 */
#define EM7180_QZ 0x08 /* this is a 32-bit normalized floating point number read from registers 0x08-0B */
#define EM7180_QW 0x0C /* this is a 32-bit normalized floating point number read from registers 0x0C-0F */
#define EM7180_QTIME 0x10 /* uint16_t from registers 0x10-11 */
#define EM7180_MX 0x12 /* int16_t from registers 0x12-13 */
#define EM7180_MY 0x14 /* int16_t from registers 0x14-15 */
#define EM7180_MZ 0x16 /* int16_t from registers 0x16-17 */
#define EM7180_MTIME 0x18 /* uint16_t from registers 0x18-19 */
#define EM7180_AX 0x1A /* int16_t from registers 0x1A-1B */
#define EM7180_AY 0x1C /* int16_t from registers 0x1C-1D */
#define EM7180_AZ 0x1E /* int16_t from registers 0x1E-1F */
#define EM7180_ATIME 0x20 /* uint16_t from registers 0x20-21 */
#define EM7180_GX 0x22 /* int16_t from registers 0x22-23 */
#define EM7180_GY 0x24 /* int16_t from registers 0x24-25 */
#define EM7180_GZ 0x26 /* int16_t from registers 0x26-27 */
#define EM7180_GTIME 0x28 /* uint16_t from registers 0x28-29 */
#define EM7180_Baro 0x2A /* start of two-byte MS5637 pressure data, 16-bit signed interger */
#define EM7180_BaroTIME 0x2C /* start of two-byte MS5637 pressure timestamp, 16-bit unsigned */
#define EM7180_Temp 0x2E /* start of two-byte MS5637 temperature data, 16-bit signed interger */
#define EM7180_TempTIME 0x30 /* start of two-byte MS5637 temperature timestamp, 16-bit unsigned */
#define EM7180_QRateDivisor 0x32 /* uint8_t */
#define EM7180_EnableEvents 0x33
#define EM7180_HostControl 0x34
#define EM7180_EventStatus 0x35
#define EM7180_SensorStatus 0x36
#define EM7180_SentralStatus 0x37
#define EM7180_AlgorithmStatus 0x38
#define EM7180_FeatureFlags 0x39
#define EM7180_ParamAcknowledge 0x3A
#define EM7180_SavedParamByte0 0x3B
#define EM7180_SavedParamByte1 0x3C
#define EM7180_SavedParamByte2 0x3D
#define EM7180_SavedParamByte3 0x3E
#define EM7180_ActualMagRate 0x45
#define EM7180_ActualAccelRate 0x46
#define EM7180_ActualGyroRate 0x47
#define EM7180_ActualBaroRate 0x48
#define EM7180_ActualTempRate 0x49
#define EM7180_ErrorRegister 0x50
#define EM7180_AlgorithmControl 0x54
#define EM7180_MagRate 0x55
#define EM7180_AccelRate 0x56
#define EM7180_GyroRate 0x57
#define EM7180_BaroRate 0x58
#define EM7180_TempRate 0x59
#define EM7180_LoadParamByte0 0x60
#define EM7180_LoadParamByte1 0x61
#define EM7180_LoadParamByte2 0x62
#define EM7180_LoadParamByte3 0x63
#define EM7180_ParamRequest 0x64
#define EM7180_ROMVersion1 0x70
#define EM7180_ROMVersion2 0x71
#define EM7180_RAMVersion1 0x72
#define EM7180_RAMVersion2 0x73
#define EM7180_ProductID 0x90
#define EM7180_RevisionID 0x91
#define EM7180_RunStatus 0x92
#define EM7180_UploadAddress 0x94 /* uint16_t registers 0x94 (MSB)-5(LSB) */
#define EM7180_UploadData 0x96
#define EM7180_CRCHost 0x97 /* uint32_t from registers 0x97-9A */
#define EM7180_ResetRequest 0x9B
#define EM7180_PassThroughStatus 0x9E
#define EM7180_PassThroughControl 0xA0
/* Macros */
#define em7180_read_byte(addr, byte) i2c_read_byte((em7180->i2c_read_func), (em7180->i2c_addr), (addr), (byte))
#define em7180_write_byte(addr, byte) i2c_write_byte((em7180->i2c_write_func), (em7180->i2c_addr), (addr), (byte))
#define em7180_read(addr, data, len) i2c_read((em7180->i2c_read_func), (em7180->i2c_addr), (addr), (data), (len))
#define em7180_write(addr, data, len) i2c_write((em7180->i2c_write_func), (em7180->i2c_addr), (addr), (data), (len))
/* Data Structures */
/* Private Global Variables */
/* Function Prototypes */
static void em7180_passthrough(em7180_t *em7180);
static float uint32_reg_to_float(uint8_t *buf);
static void float_to_bytes(float param_val, uint8_t *buf);
/* Function Definitions */
void em7180_init(em7180_t *em7180, I2C_HandleTypeDef *hi2c, lsm6dsm_t *lsm6dsm,
lis2mdl_t *lis2mdl, lps22hb_t *lps22hb, 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_status_t em7180_init(em7180_t *em7180, em7180_init_t *init)
{
return_if_fail(em7180);
int8_t *ptr = (int8_t*) em7180;
size_t i;
em7180->hi2c = hi2c;
em7180->lsm6dsm = lsm6dsm;
em7180->lis2mdl = lis2mdl;
em7180->lps22hb = lps22hb;
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;
/* configure the 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);
}
}
return_val_if_fail(em7180, EM7180_BAD_ARG);
return_val_if_fail(init, EM7180_BAD_ARG);
void em7180_config(em7180_t *em7180)
{
uint8_t runStatus;
uint8_t algoStatus;
uint8_t passthruStatus;
uint8_t eventStatus;
uint8_t sensorStatus;
// Enter EM7180 initialized state
i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_HostControl, 0x00); // set SENtral in initialized state to configure registers
i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_PassThruControl, 0x00); // make sure pass through mode is off
i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_HostControl, 0x01); // Force initialize
i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_HostControl, 0x00); // set SENtral in initialized state to configure registers
/* Legacy MPU6250 stuff, it seems
// Setup LPF bandwidth (BEFORE setting ODR's)
i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_ACC_LPF_BW, accBW); // accBW = 3 = 41Hz
i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_GYRO_LPF_BW, gyroBW); // gyroBW = 3 = 41Hz */
// Set accel/gyro/mag desired ODR rates
i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_QRateDivisor,
em7180->q_rate_div); // quat rate = gyroRt/(1 QRTDiv)
i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_MagRate,
em7180->mag_rate); // 0x64 = 100 Hz
i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_AccelRate,
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
i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // read scale sensor data
// Enable interrupt to host upon certain events
// 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)
i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_EnableEvents, 0x07);
// Enable EM7180 run mode
i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_HostControl, 0x01); // set SENtral in normal run mode
HAL_Delay(100);
// EM7180 parameter adjustments
/* Serial.println("Beginning Parameter Adjustments"); */
// Disable stillness mode for balancing robot application
em7180_set_integer_param(em7180, 0x49, 0x00);
// Write desired sensor full scale ranges to the EM7180
em7180_mag_acc_set_fs(em7180, em7180->mag_fs, em7180->acc_fs); // 1000 uT == 0x3E8, 8 g == 0x08
em7180_gyro_set_fs(em7180, em7180->gyro_fs); // 2000 dps == 0x7D0
// Read EM7180 status
runStatus = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_RunStatus);
if(runStatus & 0x01)
{
/* Serial.println(" EM7180 run status = normal mode"); */
}
algoStatus = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS,
EM7180_AlgorithmStatus);
if(algoStatus & 0x01)
{
/* Serial.println(" EM7180 standby status"); */
}
if(algoStatus & 0x02)
{
/* Serial.println(" EM7180 algorithm slow"); */
}
if(algoStatus & 0x04)
{
/* Serial.println(" EM7180 in stillness mode"); */
}
if(algoStatus & 0x08)
{
/* Serial.println(" EM7180 mag calibration completed"); */
}
if(algoStatus & 0x10)
/* zero em7180_t struct */
for(i = 0; i < sizeof(em7180_t); i++)
{
/* Serial.println(" EM7180 magnetic anomaly detected"); */
}
if(algoStatus & 0x20)
{
/* Serial.println(" EM7180 unreliable sensor data"); */
}
passthruStatus = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS,
EM7180_PassThruStatus);
if(passthruStatus & 0x01)
{
/* Serial.print(" EM7180 in passthru mode!"); */
}
eventStatus = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS,
EM7180_EventStatus);
if(eventStatus & 0x01)
{
/* Serial.println(" EM7180 CPU reset"); */
}
if(eventStatus & 0x02)
{
/* Serial.println(" EM7180 Error"); */
}
if(eventStatus & 0x04)
{
/* Serial.println(" EM7180 new quaternion result"); */
}
if(eventStatus & 0x08)
{
/* Serial.println(" EM7180 new mag result"); */
}
if(eventStatus & 0x10)
{
/* Serial.println(" EM7180 new accel result"); */
}
if(eventStatus & 0x20)
{
/* Serial.println(" EM7180 new gyro result"); */
*ptr++ = 0;
}
HAL_Delay(1000); // give some time to read the screen
em7180->init = init;
// Check sensor status
sensorStatus = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS,
EM7180_SensorStatus);
/* Serial.print(" EM7180 sensor status = "); */
/* Serial.println(sensorStatus); */
if(sensorStatus & 0x01)
{
/* Serial.print("Magnetometer not acknowledging!"); */
}
if(sensorStatus & 0x02)
{
/* Serial.print("Accelerometer not acknowledging!"); */
}
if(sensorStatus & 0x04)
{
/* Serial.print("Gyro not acknowledging!"); */
}
if(sensorStatus & 0x10)
{
/* Serial.print("Magnetometer ID not recognized!"); */
}
if(sensorStatus & 0x20)
{
/* Serial.print("Accelerometer ID not recognized!"); */
}
if(sensorStatus & 0x40)
{
/* Serial.print("Gyro ID not recognized!"); */
}
/* Serial.print("Actual MagRate = "); */
/* Serial.print(i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_ActualMagRate)); */
/* Serial.println(" Hz"); */
/* Serial.print("Actual AccelRate = "); */
/* Serial.print(10 * i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_ActualAccelRate)); */
/* Serial.println(" Hz"); */
/* Serial.print("Actual GyroRate = "); */
/* Serial.print(10 * i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_ActualGyroRate)); */
/* Serial.println(" Hz"); */
/* Serial.print("Actual BaroRate = "); */
/* Serial.print(i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_ActualBaroRate)); */
/* Serial.println(" Hz"); */
return EM7180_OK;
}
#if(0)
void em7180_chip_id_get(em7180_t *em7180)
void em7180_set_delay_cb(em7180_t *em7180, delay_func_t delay_func)
{
// Read SENtral device information
uint16_t ROM1 = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_ROMVersion1);
uint16_t ROM2 = i2c_read_byte(em7180->hi2c, 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 = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_RAMVersion1);
uint16_t RAM2 = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_RAMVersion2);
/* Serial.print("EM7180 RAM Version: 0x"); */
/* Serial.print(RAM1); */
/* Serial.println(RAM2); */
uint8_t PID = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_ProductID);
/* Serial.print("EM7180 ProductID: 0x"); */
/* Serial.print(PID, HEX); */
/* Serial.println(" Should be: 0x80"); */
uint8_t RID = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_RevisionID);
/* Serial.print("EM7180 RevisionID: 0x"); */
/* Serial.print(RID, HEX); */
/* Serial.println(" Should be: 0x02"); */
return_if_fail(em7180);
em7180->delay_func = delay_func;
}
#endif
void em7180_load_fw_from_eeprom(em7180_t *em7180)
void em7180_set_i2c_cbs(em7180_t *em7180, i2c_read_func_t i2c_read_func,
i2c_write_func_t i2c_write_func, uint8_t dev_addr)
{
// Check which sensors can be detected by the EM7180
uint8_t featureflag = i2c_read_byte(em7180->hi2c, 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"); */
}
return_if_fail(em7180);
HAL_Delay(1000); // give some time to read the screen
em7180->i2c_read_func = i2c_read_func;
em7180->i2c_write_func = i2c_write_func;
em7180->i2c_addr = dev_addr;
}
// Check SENtral status, make sure EEPROM upload of firmware was accomplished
uint8_t STAT = (i2c_read_byte(em7180->hi2c, EM7180_ADDRESS,
EM7180_SentralStatus)
& 0x01);
if(i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_SentralStatus) & 0x01)
{
/* Serial.println("EEPROM detected on the sensor bus!"); */
}
if(i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_SentralStatus) & 0x02)
{
/* Serial.println("EEPROM uploaded config file!"); */
}
if(i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_SentralStatus) & 0x04)
{
/* Serial.println("EEPROM CRC incorrect!"); */
}
if(i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_SentralStatus) & 0x08)
{
/* Serial.println("EM7180 in initialized state!"); */
}
if(i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_SentralStatus) & 0x10)
{
/* Serial.println("No EEPROM detected!"); */
}
int count = 0;
while(!STAT)
{
i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_ResetRequest, 0x01);
HAL_Delay(500);
count++;
STAT = (i2c_read_byte(em7180->hi2c, EM7180_ADDRESS,
EM7180_SentralStatus)
& 0x01);
if(i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_SentralStatus) & 0x01)
{
/* Serial.println("EEPROM detected on the sensor bus!"); */
}
if(i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_SentralStatus) & 0x02)
{
/* Serial.println("EEPROM uploaded config file!"); */
}
if(i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_SentralStatus) & 0x04)
{
/* Serial.println("EEPROM CRC incorrect!"); */
}
if(i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_SentralStatus) & 0x08)
{
/* Serial.println("EM7180 in initialized state!"); */
}
if(i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_SentralStatus) & 0x10)
{
/* Serial.println("No EEPROM detected!"); */
}
if(count > 10)
{
break;
}
}
em7180_status_t em7180_config(em7180_t *em7180)
{
uint8_t resp;
int32_t ret = 0;
if(!(i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_SentralStatus)
& 0x04))
/* wait for EM7180 to enter unprogrammed or initialized state */
do
{
/* Serial.println("EEPROM upload successful!"); */
em7180_read_byte(EM7180_SentralStatus, &resp);
}
}
while(!(resp & 0x08));
uint8_t em7180_status(em7180_t *em7180)
{
// Check event status register, way to check data ready by polling rather than interrupt
uint8_t c = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_EventStatus); // reading clears the register and interrupt
/* configure EM7180 to accept initial register set-up */
ret |= em7180_write_byte(EM7180_HostControl, 0x00); /* put EM7180 in initialized state to configure registers */
ret |= em7180_write_byte(EM7180_PassThroughControl, 0x00); /* make sure pass through mode is off */
ret |= em7180_write_byte(EM7180_HostControl, 0x01); /* force initialize */
ret |= em7180_write_byte(EM7180_HostControl, 0x00); /* put EM7180 in initialized state to configure registers */
return c;
}
/* set output data rates */
ret |= em7180_write_byte(EM7180_MagRate, em7180->init->mag_rate);
ret |= em7180_write_byte(EM7180_AccelRate, em7180->init->acc_rate);
ret |= em7180_write_byte(EM7180_GyroRate, em7180->init->gyro_rate);
ret |= em7180_write_byte(EM7180_BaroRate, 0x80 | em7180->init->baro_rate);
ret |= em7180_write_byte(EM7180_QRateDivisor, em7180->init->q_rate_div);
uint8_t em7180_errors(em7180_t *em7180)
{
uint8_t c = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS,
EM7180_ErrorRegister); // check error register
return c;
}
/* configure operating mode */
ret |= em7180_write_byte(EM7180_AlgorithmControl, 0x00);
/* enable interrupts for CPUReset, Error, QuaternionResult, MagResult, AccelResult, GyroResult */
ret |= em7180_write_byte(EM7180_EnableEvents, 0x07);
/* run EM7180 */
ret |= em7180_write_byte(EM7180_HostControl, 0x01);
void em7180_gyro_set_fs(em7180_t *em7180, uint16_t gyro_fs)
{
uint8_t bytes[4], STAT;
bytes[0] = gyro_fs & (0xFF);
bytes[1] = (gyro_fs >> 8) & (0xFF);
bytes[2] = 0x00;
bytes[3] = 0x00;
i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_LoadParamByte0,
bytes[0]); // Gyro LSB
i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_LoadParamByte1,
bytes[1]); // Gyro MSB
i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_LoadParamByte2,
bytes[2]); // Unused
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))
{
STAT = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS,
EM7180_ParamAcknowledge);
}
i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_ParamRequest, 0x00); // Parameter request = 0 to end parameter transfer process
i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // Re-start algorithm
return ret ? EM7180_BAD_COMM : EM7180_OK;
}
void em7180_mag_acc_set_fs(em7180_t *em7180, uint16_t mag_fs, uint16_t acc_fs)
void em7180_reset(em7180_t *em7180)
{
uint8_t bytes[4], STAT;
bytes[0] = mag_fs & (0xFF);
bytes[1] = (mag_fs >> 8) & (0xFF);
bytes[2] = acc_fs & (0xFF);
bytes[3] = (acc_fs >> 8) & (0xFF);
i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_LoadParamByte0,
bytes[0]); // Mag LSB
i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_LoadParamByte1,
bytes[1]); // Mag MSB
i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_LoadParamByte2,
bytes[2]); // Acc LSB
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))
{
STAT = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS,
EM7180_ParamAcknowledge);
}
i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_ParamRequest, 0x00); // Parameter request = 0 to end parameter transfer process
i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // Re-start algorithm
/* reset EM7180 */
em7180_write_byte(EM7180_ResetRequest, 0x01);
}
void em7180_set_integer_param(em7180_t *em7180, uint8_t param,
uint32_t param_val)
void em7180_passthrough_enable(em7180_t *em7180)
{
uint8_t bytes[4], STAT;
bytes[0] = param_val & (0xFF);
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
i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_LoadParamByte0,
bytes[0]); // Param LSB
i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_LoadParamByte1,
bytes[1]);
i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_LoadParamByte2,
bytes[2]);
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))
{
STAT = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS,
EM7180_ParamAcknowledge);
}
i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_ParamRequest, 0x00); // Parameter request = 0 to end parameter transfer process
i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // Re-start algorithm
/* put EM7180 in standby mode */
em7180_write_byte(EM7180_AlgorithmControl, 0x01);
/* put EM7180 in passthrough mode */
em7180_write_byte(EM7180_PassThroughControl, 0x01);
}
void em7180_param_set_float(em7180_t *em7180, uint8_t param, float param_val)
void em7180_passthrough_disable(em7180_t *em7180)
{
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
i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_LoadParamByte0,
bytes[0]); // Param LSB
i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_LoadParamByte1,
bytes[1]);
i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_LoadParamByte2,
bytes[2]);
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))
{
STAT = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS,
EM7180_ParamAcknowledge);
}
i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_ParamRequest, 0x00); // Parameter request = 0 to end parameter transfer process
i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // Re-start algorithm
/* put EM7180 in standby mode */
em7180_write_byte(EM7180_PassThroughControl, 0x00);
/* put EM7180 in normal operation */
em7180_write_byte(EM7180_AlgorithmControl, 0x00);
}
void em7180_quatdata_get(em7180_t *em7180, float *destination)
em7180_status_t em7180_quatdata_get(em7180_t *em7180, float *destination)
{
uint8_t data[16]; // x/y/z quaternion register data stored here
int32_t ret = 0;
uint8_t data[16];
i2c_read(em7180->hi2c, EM7180_ADDRESS, EM7180_QX, data, 16); // Read the sixteen raw data registers into data array
ret |= em7180_read(EM7180_QX, data, 16);
/* EM7180 quaternion is xi + yj + zk + s */
destination[0] = uint32_reg_to_float(&data[12]);
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!
return ret ? EM7180_BAD_COMM : EM7180_OK;
}
void em7180_acceldata_get(em7180_t *em7180, int16_t *destination)
em7180_status_t em7180_acceldata_get(em7180_t *em7180, int16_t *destination)
{
uint8_t data[6]; // x/y/z accel register data stored here
int32_t ret = 0;
uint8_t data[6];
i2c_read(em7180->hi2c, EM7180_ADDRESS, EM7180_AX, data, 6); // Read the six raw data registers into data array
destination[0] = (int16_t) (((int16_t) data[1] << 8) | data[0]); // Turn the MSB and LSB into a signed 16-bit value
ret |= em7180_read(EM7180_AX, data, 6);
destination[0] = (int16_t) (((int16_t) data[1] << 8) | data[0]);
destination[1] = (int16_t) (((int16_t) data[3] << 8) | data[2]);
destination[2] = (int16_t) (((int16_t) data[5] << 8) | data[4]);
return ret ? EM7180_BAD_COMM : EM7180_OK;
}
void em7180_gyrodata_get(em7180_t *em7180, int16_t *destination)
em7180_status_t em7180_gyrodata_get(em7180_t *em7180, int16_t *destination)
{
uint8_t data[6]; // x/y/z gyro register data stored here
int32_t ret = 0;
uint8_t data[6];
i2c_read(em7180->hi2c, EM7180_ADDRESS, EM7180_GX, data, 6); // Read the six raw data registers sequentially into data array
destination[0] = (int16_t) (((int16_t) data[1] << 8) | data[0]); // Turn the MSB and LSB into a signed 16-bit value
ret |= em7180_read(EM7180_GX, data, 6);
destination[0] = (int16_t) (((int16_t) data[1] << 8) | data[0]);
destination[1] = (int16_t) (((int16_t) data[3] << 8) | data[2]);
destination[2] = (int16_t) (((int16_t) data[5] << 8) | data[4]);
return ret ? EM7180_BAD_COMM : EM7180_OK;
}
void em7180_magdata_get(em7180_t *em7180, int16_t *destination)
em7180_status_t em7180_magdata_get(em7180_t *em7180, int16_t *destination)
{
uint8_t data[6]; // x/y/z mag register data stored here
int32_t ret = 0;
uint8_t data[6];
i2c_read(em7180->hi2c, EM7180_ADDRESS, EM7180_MX, data, 6); // Read the six raw data registers sequentially into data array
destination[0] = (int16_t) (((int16_t) data[1] << 8) | data[0]); // Turn the MSB and LSB into a signed 16-bit value
ret |= em7180_read(EM7180_MX, data, 6);
destination[0] = (int16_t) (((int16_t) data[1] << 8) | data[0]);
destination[1] = (int16_t) (((int16_t) data[3] << 8) | data[2]);
destination[2] = (int16_t) (((int16_t) data[5] << 8) | data[4]);
return ret ? EM7180_BAD_COMM : EM7180_OK;
}
em7180_status_t em7180_barodata_get(em7180_t *em7180, int16_t *destination)
{
int32_t ret = 0;
uint8_t data[2];
ret |= em7180_read(EM7180_Baro, data, 2);
*destination = (((int16_t) data[1] << 8) | data[0]);
return ret ? EM7180_BAD_COMM : EM7180_OK;
}
em7180_status_t em7180_tempdata_get(em7180_t *em7180, int16_t *destination)
{
int32_t ret = 0;
uint8_t data[2];
ret |= em7180_read(EM7180_Temp, data, 2);
*destination = (((int16_t) data[1] << 8) | data[0]);
return ret ? EM7180_BAD_COMM : EM7180_OK;
}
/* FIXME: haven't explored the usagge/usefulness of these yet: */
#if(0)
uint8_t em7180_status(em7180_t *em7180)
{
// Check event status register, way to check data ready by polling rather than interrupt
uint8_t c;
em7180_read_byte(EM7180_EventStatus, &c); // reading clears the register and interrupt
return c;
}
uint8_t em7180_errors(em7180_t *em7180)
{
uint8_t c;
em7180_read_byte(EM7180_ErrorRegister, &c); // check error register
return c;
}
void em7180_gyro_set_fs(em7180_t *em7180, uint16_t gyro_fs)
{
uint8_t resp;
em7180_write_byte(EM7180_LoadParamByte0, gyro_fs & (0xFF)); // Gyro LSB
em7180_write_byte(EM7180_LoadParamByte1, (gyro_fs >> 8) & (0xFF)); // Gyro MSB
em7180_write_byte(EM7180_LoadParamByte2, 0x00); // Unused
em7180_write_byte(EM7180_LoadParamByte3, 0x00); // Unused
em7180_write_byte(EM7180_ParamRequest, 0xcb); // Parameter 75; 0xCB is 75 decimal with the MSB set high to indicate a parameter write process
em7180_write_byte(EM7180_AlgorithmControl, 0x80); // Request parameter transfer procedure
/* Check the parameter acknowledge register and loop until the result matches parameter request byte */
do
{
em7180_read_byte(EM7180_ParamAcknowledge, &resp);
}
while(resp != 0xCB);
em7180_write_byte(EM7180_ParamRequest, 0x00); // Parameter request = 0 to end parameter transfer process
em7180_write_byte(EM7180_AlgorithmControl, 0x00); // Re-start algorithm
}
void em7180_mag_acc_set_fs(em7180_t *em7180, uint16_t mag_fs, uint16_t acc_fs)
{
uint8_t resp;
em7180_write_byte(EM7180_LoadParamByte0, mag_fs & (0xFF)); // Mag LSB
em7180_write_byte(EM7180_LoadParamByte1, (mag_fs >> 8) & (0xFF)); // Mag MSB
em7180_write_byte(EM7180_LoadParamByte2, acc_fs & (0xFF)); // Acc LSB
em7180_write_byte(EM7180_LoadParamByte3, (acc_fs >> 8) & (0xFF)); // Acc MSB
em7180_write_byte(EM7180_ParamRequest, 0xca); // Parameter 74; 0xCA is 74 decimal with the MSB set high to indicate a parameter write process
em7180_write_byte(EM7180_AlgorithmControl, 0x80); // Request parameter transfer procedure
/* Check the parameter acknowledge register and loop until the result matches parameter request byte */
do
{
em7180_read_byte(EM7180_ParamAcknowledge, &resp);
}
while(!(resp == 0xCA));
em7180_write_byte(EM7180_ParamRequest, 0x00); // Parameter request = 0 to end parameter transfer process
em7180_write_byte(EM7180_AlgorithmControl, 0x00); // Re-start algorithm
}
void em7180_set_integer_param(em7180_t *em7180, uint8_t param,
uint32_t param_val)
{
uint8_t resp;
em7180_write_byte(EM7180_LoadParamByte0, param_val & (0xFF)); // Param LSB
em7180_write_byte(EM7180_LoadParamByte1, (param_val >> 8) & (0xFF));
em7180_write_byte(EM7180_LoadParamByte2, (param_val >> 16) & (0xFF));
em7180_write_byte(EM7180_LoadParamByte3, (param_val >> 24) & (0xFF)); // Param MSB
em7180_write_byte(EM7180_ParamRequest, param | 0x80); // Parameter is the decimal value with the MSB set high to indicate a parameter write process
em7180_write_byte(EM7180_AlgorithmControl, 0x80); // Request parameter transfer procedure
/* Check the parameter acknowledge register and loop until the result matches parameter request byte */
do
{
em7180_read_byte(EM7180_ParamAcknowledge, &resp);
}
while(resp != param);
em7180_write_byte(EM7180_ParamRequest, 0x00); // Parameter request = 0 to end parameter transfer process
em7180_write_byte(EM7180_AlgorithmControl, 0x00); // Re-start algorithm
}
void em7180_param_set_float(em7180_t *em7180, uint8_t param, float param_val)
{
uint8_t resp;
uint8_t bytes[4];
float_to_bytes(param_val, bytes);
em7180_write_byte(EM7180_LoadParamByte0, bytes[0]); // Param LSB
em7180_write_byte(EM7180_LoadParamByte1, bytes[1]);
em7180_write_byte(EM7180_LoadParamByte2, bytes[2]);
em7180_write_byte(EM7180_LoadParamByte3, bytes[3]); // Param MSB
em7180_write_byte(EM7180_ParamRequest, param | 0x80); // Parameter is the decimal value with the MSB set high to indicate a parameter write process
em7180_write_byte(EM7180_AlgorithmControl, 0x80); // Request parameter transfer procedure
/* Check the parameter acknowledge register and loop until the result matches parameter request byte */
do
{
em7180_read_byte(EM7180_ParamAcknowledge, &resp);
}
while(resp != param);
em7180_write_byte(EM7180_ParamRequest, 0x00); // Parameter request = 0 to end parameter transfer process
em7180_write_byte(EM7180_AlgorithmControl, 0x00); // Re-start algorithm
}
float em7180_mres_get(uint8_t Mscale)
@ -605,48 +461,7 @@ float em7180_ares_get(uint8_t ascale) @@ -605,48 +461,7 @@ float em7180_ares_get(uint8_t ascale)
return a_res;
}
int16_t em7180_baro_get(em7180_t *em7180)
{
uint8_t data[2]; // baro register data stored here
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(em7180_t *em7180)
{
uint8_t data[2]; // temp register data stored here
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
}
static void em7180_passthrough(em7180_t *em7180)
{
// First put SENtral in standby mode
uint8_t c = i2c_read_byte(em7180->hi2c, EM7180_ADDRESS,
EM7180_AlgorithmControl);
i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_AlgorithmControl,
c | 0x01);
// c = readByte(EM7180_ADDRESS, EM7180_AlgorithmStatus);
/* // Serial.print("c = "); Serial.println(c); */
// Verify standby status
// if(readByte(EM7180_ADDRESS, EM7180_AlgorithmStatus) & 0x01) {
/* Serial.println("SENtral in standby mode"); */
// Place SENtral in pass-through mode
i2c_write_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_PassThruControl, 0x01);
if(i2c_read_byte(em7180->hi2c, EM7180_ADDRESS, EM7180_PassThruStatus) & 0x01)
{
/* Serial.println("SENtral in pass-through mode"); */
}
else
{
/* Serial.println("ERROR! SENtral not in pass-through mode!"); */
}
}
#endif
static float uint32_reg_to_float(uint8_t *buf)
{
@ -661,24 +476,3 @@ static float uint32_reg_to_float(uint8_t *buf) @@ -661,24 +476,3 @@ static float uint32_reg_to_float(uint8_t *buf)
return u.f;
}
static void float_to_bytes(float param_val, uint8_t *buf)
{
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];
}
}

36
Drivers/EM7180/Src/em7180_common.c
Unescape View File

@ -0,0 +1,36 @@ @@ -0,0 +1,36 @@
/*
* em7180_common.c
*
* Created on: Jan 22, 2021
* Author: dan
*/
/* Includes */
#include <stdint.h>
#include "em7180_common.h"
/* Function Definitions */
int32_t i2c_read_byte(i2c_read_func_t read, uint16_t dev_addr, uint16_t addr,
uint8_t *byte)
{
return read(dev_addr, addr, 1, byte, 1);
}
int32_t i2c_write_byte(i2c_write_func_t write, uint16_t dev_addr, uint16_t addr,
uint8_t byte)
{
return write(dev_addr, addr, 1, &byte, 1);
}
int32_t i2c_read(i2c_read_func_t read, uint16_t dev_addr, uint16_t addr,
uint8_t *data, uint16_t len)
{
return read(dev_addr, addr, 1, data, len);
}
int32_t i2c_write(i2c_write_func_t write, uint16_t dev_addr, uint16_t addr,
uint8_t *data, uint16_t len)
{
return write(dev_addr, addr, 1, data, len);
}

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

@ -37,20 +37,21 @@ void lis2mdl_init(lis2mdl_t *lis2mdl, uint8_t m_odr) @@ -37,20 +37,21 @@ void lis2mdl_init(lis2mdl_t *lis2mdl, uint8_t m_odr)
void lis2mdl_config(lis2mdl_t *lis2mdl, I2C_HandleTypeDef *hi2c)
{
// enable temperature compensation (bit 7 == 1), continuous mode (bits 0:1 == 00)
i2c_write_byte(hi2c, LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A,
0x80 | lis2mdl->m_odr << 2);
broken_i2c_write_byte(hi2c, LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A,
0x80 | lis2mdl->m_odr << 2);
// enable low pass filter (bit 0 == 1), set to ODR/4
i2c_write_byte(hi2c, LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_B, 0x01);
broken_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)
i2c_write_byte(hi2c, LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C, 0x01 | 0x10);
broken_i2c_write_byte(hi2c, LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C,
0x01 | 0x10);
}
uint8_t lis2mdl_chip_id_get(I2C_HandleTypeDef *hi2c)
{
uint8_t c = i2c_read_byte(hi2c, LIS2MDL_ADDRESS, LIS2MDL_WHO_AM_I);
uint8_t c = broken_i2c_read_byte(hi2c, LIS2MDL_ADDRESS, LIS2MDL_WHO_AM_I);
return c;
}
@ -58,25 +59,30 @@ uint8_t lis2mdl_chip_id_get(I2C_HandleTypeDef *hi2c) @@ -58,25 +59,30 @@ uint8_t lis2mdl_chip_id_get(I2C_HandleTypeDef *hi2c)
void lis2mdl_reset(I2C_HandleTypeDef *hi2c)
{
// reset device
uint8_t temp = i2c_read_byte(hi2c, LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A);
uint8_t temp = broken_i2c_read_byte(hi2c, LIS2MDL_ADDRESS,
LIS2MDL_CFG_REG_A);
i2c_write_byte(hi2c, LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A, temp | 0x20); // Set bit 5 to 1 to reset LIS2MDL
broken_i2c_write_byte(hi2c, LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A,
temp | 0x20); // Set bit 5 to 1 to reset LIS2MDL
HAL_Delay(1);
i2c_write_byte(hi2c, LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A, temp | 0x40); // Set bit 6 to 1 to boot LIS2MDL
broken_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
}
uint8_t lis2mdl_status(I2C_HandleTypeDef *hi2c)
{
// Read the status register of the altimeter
uint8_t temp = i2c_read_byte(hi2c, LIS2MDL_ADDRESS, LIS2MDL_STATUS_REG);
uint8_t temp = broken_i2c_read_byte(hi2c, LIS2MDL_ADDRESS,
LIS2MDL_STATUS_REG);
return temp;
}
void lis2mdl_data_get(I2C_HandleTypeDef *hi2c, int16_t *destination)
{
uint8_t data[6]; // x/y/z mag register data stored here
i2c_read(hi2c, LIS2MDL_ADDRESS, (0x80 | LIS2MDL_OUTX_L_REG), data, 8); // Read the 6 raw data registers into data array
broken_i2c_read(hi2c, LIS2MDL_ADDRESS, (0x80 | LIS2MDL_OUTX_L_REG), data,
8); // Read the 6 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];
@ -167,8 +173,8 @@ void lis2mdl_self_test(I2C_HandleTypeDef *hi2c) @@ -167,8 +173,8 @@ void lis2mdl_self_test(I2C_HandleTypeDef *hi2c)
magNom[1] = (float) sum[1] / 50.0f;
magNom[2] = (float) sum[2] / 50.0f;
uint8_t c = i2c_read_byte(hi2c, LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C);
i2c_write_byte(hi2c, LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C, c | 0x02); // enable self test
uint8_t c = broken_i2c_read_byte(hi2c, LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C);
broken_i2c_write_byte(hi2c, LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C, c | 0x02); // enable self test
HAL_Delay(100); // let mag respond
sum[0] = 0;
@ -187,7 +193,7 @@ void lis2mdl_self_test(I2C_HandleTypeDef *hi2c) @@ -187,7 +193,7 @@ void lis2mdl_self_test(I2C_HandleTypeDef *hi2c)
magTest[1] = (float) sum[1] / 50.0f;
magTest[2] = (float) sum[2] / 50.0f;
i2c_write_byte(hi2c, LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C, c); // return to previous settings/normal mode
broken_i2c_write_byte(hi2c, LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C, c); // return to previous settings/normal mode
HAL_Delay(100); // let mag respond
/* Serial.println("Mag Self Test:"); */

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

@ -37,15 +37,15 @@ void lps22hb_config(lps22hb_t *lps22hb, I2C_HandleTypeDef *hi2c) @@ -37,15 +37,15 @@ void lps22hb_config(lps22hb_t *lps22hb, I2C_HandleTypeDef *hi2c)
// 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
i2c_write_byte(hi2c, LPS22H_ADDRESS, LPS22H_CTRL_REG1,
lps22hb->p_odr << 4 | 0x08 | 0x02);
i2c_write_byte(hi2c, LPS22H_ADDRESS, LPS22H_CTRL_REG3, 0x04); // enable data ready as interrupt source
broken_i2c_write_byte(hi2c, LPS22H_ADDRESS, LPS22H_CTRL_REG1,
lps22hb->p_odr << 4 | 0x08 | 0x02);
broken_i2c_write_byte(hi2c, LPS22H_ADDRESS, LPS22H_CTRL_REG3, 0x04); // enable data ready as interrupt source
}
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
uint8_t temp = i2c_read_byte(hi2c, LPS22H_ADDRESS, LPS22H_WHOAMI); // Read WHO_AM_I register for LPS22H
uint8_t temp = broken_i2c_read_byte(hi2c, LPS22H_ADDRESS, LPS22H_WHOAMI); // Read WHO_AM_I register for LPS22H
return temp;
}
@ -53,7 +53,7 @@ uint8_t lps22hb_chip_id_get(I2C_HandleTypeDef *hi2c) @@ -53,7 +53,7 @@ uint8_t lps22hb_chip_id_get(I2C_HandleTypeDef *hi2c)
uint8_t lps22hb_status(I2C_HandleTypeDef *hi2c)
{
// Read the status register of the altimeter
uint8_t temp = i2c_read_byte(hi2c, LPS22H_ADDRESS, LPS22H_STATUS);
uint8_t temp = broken_i2c_read_byte(hi2c, LPS22H_ADDRESS, LPS22H_STATUS);
return temp;
}
@ -62,7 +62,8 @@ int32_t lps22hb_pressure_get(I2C_HandleTypeDef *hi2c) @@ -62,7 +62,8 @@ int32_t lps22hb_pressure_get(I2C_HandleTypeDef *hi2c)
{
uint8_t data[3]; // 24-bit pressure register data stored here
i2c_read(hi2c, LPS22H_ADDRESS, (LPS22H_PRESS_OUT_XL | 0x80), data, 3); // bit 7 must be one to read multiple bytes
broken_i2c_read(hi2c, LPS22H_ADDRESS, (LPS22H_PRESS_OUT_XL | 0x80), data,
3); // bit 7 must be one to read multiple bytes
return ((int32_t) data[2] << 16 | (int32_t) data[1] << 8 | data[0]);
}
@ -71,7 +72,7 @@ int16_t lps22hb_temp_get(I2C_HandleTypeDef *hi2c) @@ -71,7 +72,7 @@ int16_t lps22hb_temp_get(I2C_HandleTypeDef *hi2c)
{
uint8_t data[2]; // 16-bit pressure register data stored here
i2c_read(hi2c, LPS22H_ADDRESS, (LPS22H_TEMP_OUT_L | 0x80), data, 2); // bit 7 must be one to read multiple bytes
broken_i2c_read(hi2c, LPS22H_ADDRESS, (LPS22H_TEMP_OUT_L | 0x80), data, 2); // bit 7 must be one to read multiple bytes
return ((int16_t) data[1] << 8 | data[0]);
}

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

@ -40,30 +40,32 @@ void lsm6dsm_init(lsm6dsm_t *lsm6dsm, uint8_t ascale, uint8_t gscale, @@ -40,30 +40,32 @@ void lsm6dsm_init(lsm6dsm_t *lsm6dsm, uint8_t ascale, uint8_t gscale,
void lsm6dsm_config(lsm6dsm_t *lsm6dsm, I2C_HandleTypeDef *hi2c)
{
i2c_write_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_CTRL1_XL,
lsm6dsm->a_odr << 4 | lsm6dsm->ascale << 2);
broken_i2c_write_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_CTRL1_XL,
lsm6dsm->a_odr << 4 | lsm6dsm->ascale << 2);
i2c_write_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_CTRL2_G,
lsm6dsm->g_odr << 4 | lsm6dsm->gscale << 2);
broken_i2c_write_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_CTRL2_G,
lsm6dsm->g_odr << 4 | lsm6dsm->gscale << 2);
uint8_t temp = i2c_read_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_CTRL3_C);
uint8_t temp = broken_i2c_read_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_CTRL3_C);
// enable block update (bit 6 = 1), auto-increment registers (bit 2 = 1)
i2c_write_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_CTRL3_C, temp | 0x40 | 0x04);
broken_i2c_write_byte(hi2c, 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
i2c_write_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_CTRL8_XL, 0x80 | 0x40 | 0x08);
broken_i2c_write_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_CTRL8_XL,
0x80 | 0x40 | 0x08);
// interrupt handling
i2c_write_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_DRDY_PULSE_CFG, 0x80); // latch interrupt until data read
i2c_write_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_INT1_CTRL, 0x40); // enable significant motion interrupts on INT1
i2c_write_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_INT2_CTRL, 0x03); // enable accel/gyro data ready interrupts on INT2
broken_i2c_write_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_DRDY_PULSE_CFG, 0x80); // latch interrupt until data read
broken_i2c_write_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_INT1_CTRL, 0x40); // enable significant motion interrupts on INT1
broken_i2c_write_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_INT2_CTRL, 0x03); // enable accel/gyro data ready interrupts on INT2
}
uint8_t lsm6dsm_chip_id_get(lsm6dsm_t *lsm6dsm, I2C_HandleTypeDef *hi2c)
{
uint8_t c = i2c_read_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_WHO_AM_I);
uint8_t c = broken_i2c_read_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_WHO_AM_I);
return c;
}
@ -128,9 +130,9 @@ float lsm6dsm_gres_get(lsm6dsm_t *lsm6dsm) @@ -128,9 +130,9 @@ float lsm6dsm_gres_get(lsm6dsm_t *lsm6dsm)
void lsm6dsm_reset(lsm6dsm_t *lsm6dsm, I2C_HandleTypeDef *hi2c)
{
// reset device
uint8_t temp = i2c_read_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_CTRL3_C);
uint8_t temp = broken_i2c_read_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_CTRL3_C);
i2c_write_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_CTRL3_C, temp | 0x01); // Set bit 0 to 1 to reset LSM6DSM
broken_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
}
@ -152,35 +154,35 @@ void lsm6dsm_self_test(lsm6dsm_t *lsm6dsm, I2C_HandleTypeDef *hi2c) @@ -152,35 +154,35 @@ void lsm6dsm_self_test(lsm6dsm_t *lsm6dsm, I2C_HandleTypeDef *hi2c)
gyroNom[1] = temp[2];
gyroNom[2] = temp[3];
i2c_write_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x01); // positive accel self test
broken_i2c_write_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x01); // positive accel self test
HAL_Delay(100); // let accel respond
lsm6dsm_read_data(lsm6dsm, hi2c, temp);
accelPTest[0] = temp[4];
accelPTest[1] = temp[5];
accelPTest[2] = temp[6];
i2c_write_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x03); // negative accel self test
broken_i2c_write_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x03); // negative accel self test
HAL_Delay(100); // let accel respond
lsm6dsm_read_data(lsm6dsm, hi2c, temp);
accelNTest[0] = temp[4];
accelNTest[1] = temp[5];
accelNTest[2] = temp[6];
i2c_write_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x04); // positive gyro self test
broken_i2c_write_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x04); // positive gyro self test
HAL_Delay(100); // let gyro respond
lsm6dsm_read_data(lsm6dsm, hi2c, temp);
gyroPTest[0] = temp[1];
gyroPTest[1] = temp[2];
gyroPTest[2] = temp[3];
i2c_write_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x0C); // negative gyro self test
broken_i2c_write_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x0C); // negative gyro self test
HAL_Delay(100); // let gyro respond
lsm6dsm_read_data(lsm6dsm, hi2c, temp);
gyroNTest[0] = temp[1];
gyroNTest[1] = temp[2];
gyroNTest[2] = temp[3];
i2c_write_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x00); // normal mode
broken_i2c_write_byte(hi2c, LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x00); // normal mode
HAL_Delay(100); // let accel and gyro respond
/* Serial.println("Accel Self Test:"); */
@ -280,7 +282,7 @@ static void lsm6dsm_read_data(lsm6dsm_t *lsm6dsm, I2C_HandleTypeDef *hi2c, @@ -280,7 +282,7 @@ static void lsm6dsm_read_data(lsm6dsm_t *lsm6dsm, I2C_HandleTypeDef *hi2c,
{
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
broken_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];

Write Preview
Loading…
Cancel
Save