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()
174 ins.compass_version = _compassVersion;
175 ins.ahrs_valid =
false;
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)
207 if (!etIsDisabled(SYS_SYS_DISABLE_AHRS))
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;
314 if ( (state >
clbStateIdle) && !etIsDisabled(SYS_SYS_DISABLE_AHRS) && _compassDriver != COMPASS_NONE)
318 if (insReadCompass())
321 ins.ahrs_a[0] = insCompassData.
ax;
322 ins.ahrs_a[1] = insCompassData.
ay;
323 ins.ahrs_a[2] = insCompassData.
az;
324 ins.ahrs_g[0] = insCompassData.
gx;
325 ins.ahrs_g[1] = insCompassData.
gy;
326 ins.ahrs_g[2] = insCompassData.
gz;
327 ins.ahrs_h[0] = insCompassData.
hx;
328 ins.ahrs_h[1] = insCompassData.
hy;
329 ins.ahrs_h[2] = insCompassData.
hz;
330 ins.ahrs_pitch = insCompassData.
pitch;
331 ins.ahrs_roll = insCompassData.
roll;
332 ins.ahrs_yaw = insCompassData.
yaw;
334 if (_compassDriver == COMPASS_AHRS)
340 ins.ahrs_valid =
false;
342 ins.ahrs_valid =
true;
347 ins.ahrs_valid =
true;
352 etCheckFailure(SYS_SYS_FAIL_AHRS);
354 ins.ahrs_valid =
false;
358 if (!etIsDisabled(SYS_SYS_DISABLE_TEMP_HUM))
368 ins.temp = result ? temp : 0;
376 ins.humid = result ? humid : 0;
378 etCheckFailure(SYS_SYS_FAIL_TEMP_HUM);
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.
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.
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 accRate
Accelerometer rate , one of LSM303D_ACCRATE_*.
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_*
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.
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.
bool offCanc
Offset cancellation.
Undefined => Idle, for internal use only.
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 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.
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.
bool lsm303DCfg(I2C_Device *dev, uint8_t addr, Lsm303DCfg config)
Sets the generic device configuration parameters.
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.
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_*.