40 static enum CompassDriver {
47 static const char * COMPASS_DRIVER_NAMES[] = {
48 [COMPASS_NONE] =
"None",
49 [COMPASS_AHRS] =
"AHRS",
50 [COMPASS_LSM303D] =
"LSM303D",
51 [COMPASS_LSM303AGR] =
"LSM303AGR"
55 static uint8_t _compassVersion;
56 uint8_t AHRS_I2C_ADDR = 0;
60 int _detectAhrsTaskId = -1;
62 static bool insInitLsm303D();
64 static void detectCompass()
67 bool isLSM303 =
false;
70 for (
int i = 0; i < 5; ++i)
74 _compassDriver = COMPASS_LSM303D;
75 _compassVersion = 0xFF;
84 _compassDriver = COMPASS_LSM303AGR;
85 _compassVersion = 0xFE;
93 AHRS_I2C_ADDR = AHRS_I2C_PRIM_ADDR;
94 if (
ahrsGetVersion(INS_I2C_DEV, AHRS_I2C_ADDR, &_compassVersion)) {
95 _compassDriver = COMPASS_AHRS;
102 if (
ahrsGetVersion(INS_I2C_DEV, AHRS_I2C_ADDR, &_compassVersion)) {
103 _compassDriver = COMPASS_AHRS;
111 _compassDriver = COMPASS_NONE;
114 static bool insInitLsm303D()
119 .
magScale = LSM303D_MAGSCALE_2GAUSS,
120 .magRate = LSM303D_MAGRATE_12HZ5,
121 .magMode = LSM303D_MAGMODE_CONTINUOUS,
130 .
accRate = LSM303D_ACCRATE_12HZ5,
131 .accAAF = LSM303D_ACCAAF_773HZ,
132 .accHPF = LSM303D_ACCHPF_NORMAL,
133 .accScale = LSM303D_ACCSCALE_2G,
150 static bool insInitLsm303Agr()
155 accCfg.
rate = LSM303AGR_ACC_RATE_10HZ;
161 magCfg.
rate = LSM303AGR_MAG_RATE_10HZ;
162 magCfg.
mode = LSM303AGR_MAG_MODE_CONTINUOUS;
171 static void detectAndInitCompass()
177 switch (_compassDriver)
179 case COMPASS_LSM303D:
if (!insInitLsm303D())
goto err;
break;
180 case COMPASS_AHRS:
break;
181 case COMPASS_LSM303AGR:
if (!insInitLsm303Agr())
goto err;
break;
186 logInfo(
"Compass Driver=%s, address (AHRS only)=%02x, version=%d", COMPASS_DRIVER_NAMES[_compassDriver], AHRS_I2C_ADDR, _compassVersion);
197 void _subsInsExecEvent (
ClbEvent event)
212 if (_detectAhrsTaskId < 0)
213 schdRegister(detectAndInitCompass,
false, &_detectAhrsTaskId);
235 static bool insReadAHRS()
237 if (AHRS_I2C_ADDR == 0)
return true;
239 return ahrsRead(INS_I2C_DEV, AHRS_I2C_ADDR, &insCompassData);
242 static bool insReadLsm303D()
266 static bool insReadLsm303Agr()
291 static bool insReadCompass()
293 switch (_compassDriver)
295 case COMPASS_LSM303D:
return insReadLsm303D();
296 case COMPASS_LSM303AGR:
return insReadLsm303Agr();
297 case COMPASS_AHRS:
return insReadAHRS();
304 bool _subsInsUpdate(
ClbState state, uint32_t time)
307 static char rstate = 0;
308 static int ahrs_validcnt = 0;
318 if (insReadCompass())
334 if (_compassDriver == COMPASS_AHRS)
Axis structure for 3D information.
bool errHas()
Returns whether there is an error pending.
void ahrsOff()
Switch off the AHRS.
bool tempCompEn
Temperature compensation enabled.
Accelerator configuration structure.
This driver is to read and configure the AHRS I2C sensor.
f32_t ahrs_roll
Roll in degrees.
void ahrsOn()
Switch on the AHRS.
bool lsm303AgrAccCfg(I2C_Device *dev, Lsm303AgrAccCfg config)
Set the accelerometer configuration.
ClbEvent
All state change events.
bool lsm303DMagnetCfg(I2C_Device *dev, uint8_t addr, Lsm303DMagCfg config)
Set the Magnet configuration.
f32_t ahrs_pitch
Pitch in degrees.
bool ahrsGetVersion(I2C_Device *dev, uint8_t addr, uint8_t *version)
Reads the AHRS version.
Generic LSM303D configuration.
static void _clbStateModErr(int idx)
Invoked by subsystem to indicate an error happened.
uint16_t humid
Humidity in 1/100th RH.
#define SYS_SYS_FAIL_TEMP_HUM
Bit which disables Temperature / Humidity.
uint16_t accRate
Accelerometer rate , one of LSM303D_ACCRATE_*.
Provides access to all variables of the various subsystems.
bool schdRegister(SchdTaskF task, bool priority, int *taskId)
Register a task with the scheduler.
Accelerator configuration structure.
bool lsm303DAccelRead(I2C_Device *dev, uint8_t addr, Lsm303Axis *result)
Read the accelerometer information.
f32_t hx
Hx in gauss (Float)
bool lsm303AgrAccRead(I2C_Device *dev, Lsm303Axis *result)
Read the accelerometer information.
uint16_t mode
one of LSM303AGR_MAG_MODE_*
f32_t ahrs_g[3]
Angular velocity projection, (Index 0=X, 1=Y, 2=Z)
bool lsm303AgrMagCfg(I2C_Device *dev, Lsm303AgrMagCfg config)
Set the Magnet configuration.
f32_t pitch
Pitch in deg (Float)
Special library for primitive IEEE 754 floating point handling without dragging all float support alo...
Simple task scheduler for tasks.
bool lsm303AgrValid(I2C_Device *dev, bool *isLSM303Agr)
Checks whether or not the lsm303Agr is valid.
Driver for the ST lsm303Agr Compass sensor, which unlike the name suggests quite incompable to the LS...
void timeDelay(uint32_t msec)
Simple busy-wait delay.
CompassData insCompassData
Provides the AHRS data.
f32_t hz
Hz in gauss (Float)
uint8_t clbSys2Idx[6]
Mapping from subsystem ID to index.
#define LSM303AGR_MAG_CFG_DEFAULT
DEfault magnet configuration.
bool sht21ReadTemp(I2C_Device *dev, uint8_t addr, int *answer)
Reads the temperatures from SHT21 at I2C address 'addr'; puts the result in answer.
Structure defines data from a compass/tilt/gyro sensor.
f32_t gx
Gx in deg/sec (Float)
f32_t gy
Gy in deg/sec (Float)
void schdRunDelay(int taskId, int interval)
Schedule a task to run after a delay.
f32_t ahrs_h[3]
Magnetic field projection, (Index 0=X, 1=Y, 2=Z)
bool sht21ReadHumid(I2C_Device *dev, uint8_t addr, int *answer)
Reads the relative humidity from SHT21 at I2C address 'addr'; puts the result in answer.
#define LSM303D_I2C_ADDR
LSM303D Address.
Manages the global system error.
uint8_t compass_version
Compass version: 0 if not available or very old version.
bool offCanc
Offset cancellation.
int16_t temp
Temperature in 1/100th of a degree.
Undefined => Idle, for internal use only.
bool ahrs_valid
Compass data are valid.
uint16_t rate
One of LSM303AGR_MAG_RATE_*.
bool lsm303DValid(I2C_Device *dev, uint8_t addr, bool *isLSM303)
Checks whether or not the LSM303D is valid.
bool ahrsRead(I2C_Device *dev, uint8_t addr, CompassData *data)
Reads all data from the AHRS.
void errClear()
Clears the current error.
uint16_t rate
Accelerometer rate , one of LSM303AG_ACC_RATE_*.
f32_t ahrs_a[3]
Acceleration projection, (Index 0=X, 1=Y, 2=Z)
f32_t fltFromI16(int16_t val, int fBits)
Takes an integer value and converts it to a float.
This module is responsible for distributing error codes.
#define SHT21_I2C_ADDR
SHT21 I2C Address.
#define SYS_SYS_DISABLE_AHRS
Bit which disables AHRS (Compass) periodic readout.
bool lsm303DMagnetRead(I2C_Device *dev, uint8_t addr, Lsm303Axis *result)
Read the magnet information.
bool temp
Temperature sensor enable.
This driver is to read the SHT21 I2C temperature and humidity sensor.
#define AHRS_VALID_CNT_NUM
Number of measure to do before receiving accurate results.
bool bdu
Block data update.
#define AHRS_I2C_SEC_ADDR
AHRS I2C Address.
#define LOG_DEF(NAME,...)
Define a logger for a module.
The CLB stare module tracks is responsible for state management of the various sub-systems on the CLB...
bool lsm303DAccelCfg(I2C_Device *dev, uint8_t addr, Lsm303DAccCfg config)
Set the accelerometer configuration.
#define SYS_SYS_FAIL_AHRS
Bit which disables AHRS (Compass) periodic readout.
bool lsm303DCfg(I2C_Device *dev, uint8_t addr, Lsm303DCfg config)
Sets the generic device configuration parameters.
ins_t ins
Provides access to all process variables of subsystem Instrumentation.
f32_t gz
Gz in deg/sec (Float)
Driver for the ST LSM303Dx Compass sensor.
f32_t hy
Hy in gauss (Float)
bool sht21StartTempMeas(I2C_Device *dev, uint8_t addr)
Starts the temperature measurement in the SHT21 module at I2C address 'addr'.
Implements a generic logger facility.
#define LSM303AGR_ACC_CFG_DEFAULT
Default accelerometer configuration.
#define SYS_SYS_DISABLE_TEMP_HUM
Bit which disables Temperature / Humidity.
f32_t ahrs_yaw
Yaw in degrees.
bool bdu
Block date update enable.
f32_t yaw
Yaw in deg (Float)
#define logInfo(MSG,...)
Write a log message with formatting on info level.
void _clbStateUpdate(int idx, ClbEvent event, uint8_t status)
Invoked by the subsystem to indicate a state change has happened.
bool lsm303AgrMagRead(I2C_Device *dev, Lsm303Axis *result)
Read the magnet information.
bool sht21StartHumidMeas(I2C_Device *dev, uint8_t addr)
Starts the relative humidity measurement in the SHT21 module at I2C address 'addr'.
Configures the board-specific peripherals, like I2C, SPI etc...
f32_t roll
Roll in deg (Float)
uint8_t magScale
Magnet scale, use one of LSM303D_MAGSCALEE_*.