KM3NeT CLB  2.0
KM3NeT CLB v2 Embedded Software
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Groups Pages
sub_ins.c
1 /*
2  * KM3NeT CLB v2 Firmware
3  * ----------------------
4  *
5  * Copyright 2013 KM3NeT Collaboration
6  *
7  * All Rights Reserved.
8  *
9  *
10  * File : sub_ins.c
11  * Created : 6 jan. 2014
12  * Author : Vincent van Beveren
13  */
14 
15 #include "errorcode.h"
16 #include "cfg_board.h"
17 
18 #include <string.h>
19 #include <stdio.h>
20 #include "util/macro.h"
21 #include "util/float.h"
22 #include "util/log.h"
23 
24 #include "kernel/err.h"
25 #include "kernel/tm.h"
26 #include "kernel/scheduler.h"
27 #include "drv/i2c/sht21.h"
28 #include "drv/i2c/ahrs.h"
29 #include "drv/i2c/lsm303d.h"
30 #include "drv/i2c/lsm303agr.h"
31 
32 #include "pv/vars.h"
33 
34 #include "errtrack.h"
35 #include "clbstate.h"
36 
37 
38 LOG_DEF(Ins);
39 
40 static enum CompassDriver {
41  COMPASS_NONE,
42  COMPASS_AHRS,
43  COMPASS_LSM303D,
44  COMPASS_LSM303AGR
45 } _compassDriver;
46 
47 static const char * COMPASS_DRIVER_NAMES[] = {
48  [COMPASS_NONE] = "None",
49  [COMPASS_AHRS] = "AHRS",
50  [COMPASS_LSM303D] = "LSM303D",
51  [COMPASS_LSM303AGR] = "LSM303AGR"
52 };
53 
54 
55 static uint8_t _compassVersion;
56 uint8_t AHRS_I2C_ADDR = 0;
57 
59 
60 int _detectAhrsTaskId = -1;
61 
62 static bool insInitLsm303D();
63 
64 static void detectCompass()
65 {
66 
67  bool isLSM303 = false;
68  // i2cDebug(true);
69 
70  for (int i = 0; i < 5; ++i)
71  {
72  if (lsm303DValid(INS_I2C_DEV, LSM303D_I2C_ADDR, &isLSM303)) {
73  if (isLSM303) {
74  _compassDriver = COMPASS_LSM303D;
75  _compassVersion = 0xFF;
76  i2cDebug(false);
77  return;
78  }
79  }
80  errClear();
81 
82  if (lsm303AgrValid(INS_I2C_DEV, &isLSM303)) {
83  if (isLSM303) {
84  _compassDriver = COMPASS_LSM303AGR;
85  _compassVersion = 0xFE;
86  i2cDebug(false);
87  return;
88  }
89  }
90 
91  errClear();
92  timeDelay(2);
93  AHRS_I2C_ADDR = AHRS_I2C_PRIM_ADDR;
94  if (ahrsGetVersion(INS_I2C_DEV, AHRS_I2C_ADDR, &_compassVersion)) {
95  _compassDriver = COMPASS_AHRS;
96  i2cDebug(false);
97  return;
98  }
99  errClear();
100  timeDelay(2);
101  AHRS_I2C_ADDR = AHRS_I2C_SEC_ADDR;
102  if (ahrsGetVersion(INS_I2C_DEV, AHRS_I2C_ADDR, &_compassVersion)) {
103  _compassDriver = COMPASS_AHRS;
104  i2cDebug(false);
105  return;
106  }
107  errClear();
108  }
109  //i2cDebug(false);
110  _compassVersion = 0;
111  _compassDriver = COMPASS_NONE;
112 }
113 
114 static bool insInitLsm303D()
115 {
116 
117  // configure magnet at 12.5Hz
118  Lsm303DMagCfg magCfg = {
119  .magScale = LSM303D_MAGSCALE_2GAUSS,
120  .magRate = LSM303D_MAGRATE_12HZ5,
121  .magMode = LSM303D_MAGMODE_CONTINUOUS,
122  .lowPower = false,
123  .hiRes = true
124  };
125 
126  if (!lsm303DMagnetCfg(INS_I2C_DEV, LSM303D_I2C_ADDR, magCfg)) return false;
127 
128  // configure accelerometer at same rate.
129  Lsm303DAccCfg accelCfg = {
130  .accRate = LSM303D_ACCRATE_12HZ5,
131  .accAAF = LSM303D_ACCAAF_773HZ,
132  .accHPF = LSM303D_ACCHPF_NORMAL,
133  .accScale = LSM303D_ACCSCALE_2G,
134  .selfTest = false,
135  .filterAcc = false
136  };
137 
138  if (!lsm303DAccelCfg(INS_I2C_DEV, LSM303D_I2C_ADDR, accelCfg)) return false;
139 
140 
141  Lsm303DCfg cfg = {
142  .bdu = true,
143  .temp = false,
144  .tempOnly = false
145  };
146 
147  return lsm303DCfg(INS_I2C_DEV, LSM303D_I2C_ADDR, cfg);
148 }
149 
150 static bool insInitLsm303Agr()
151 {
153  accCfg.temp = true;
154  accCfg.bdu = true;
155  accCfg.rate = LSM303AGR_ACC_RATE_10HZ;
156 
157  if (!lsm303AgrAccCfg(INS_I2C_DEV, accCfg)) return false;
158 
160 
161  magCfg.rate = LSM303AGR_MAG_RATE_10HZ;
162  magCfg.mode = LSM303AGR_MAG_MODE_CONTINUOUS;
163  magCfg.bdu = true;
164  magCfg.offCanc = true;
165  magCfg.tempCompEn = true;
166 
167  return lsm303AgrMagCfg(INS_I2C_DEV, magCfg);
168 }
169 
170 
171 static void detectAndInitCompass()
172 {
173  detectCompass();
174  ins.compass_version = _compassVersion;
175  ins.ahrs_valid = false;
176 
177  switch (_compassDriver)
178  {
179  case COMPASS_LSM303D: if (!insInitLsm303D()) goto err; break;
180  case COMPASS_AHRS: break;
181  case COMPASS_LSM303AGR: if (!insInitLsm303Agr()) goto err; break;
182  default:
183  break;
184  }
185 
186  logInfo("Compass Driver=%s, address (AHRS only)=%02x, version=%d", COMPASS_DRIVER_NAMES[_compassDriver], AHRS_I2C_ADDR, _compassVersion);
187 
188 
189 
190  _clbStateUpdate(clbSys2Idx[CLB_SUB_INS], clbEventInit, 0);
191  return;
192 err:
193  _clbStateModErr(clbSys2Idx[CLB_SUB_INS]);
194  _clbStateUpdate(clbSys2Idx[CLB_SUB_INS], clbEventInit, 0);
195 }
196 
197 void _subsInsExecEvent (ClbEvent event)
198 {
199  switch (event)
200  {
201  // implement events here. See clbstate.h for possible events
202  case clbEventReset:
203  case clbEventBoot:
204  ahrsOff();
205  break;
206  case clbEventInit:
207  if (!etIsDisabled(SYS_SYS_DISABLE_AHRS))
208  {
209  ahrsOn();
210 
211  // asynchronous checking of AHRS.
212  if (_detectAhrsTaskId < 0)
213  schdRegister(detectAndInitCompass, false, &_detectAhrsTaskId);
214 
215  schdRunDelay(_detectAhrsTaskId, 750);
216  return;
217  }
218  break;
219  case clbEventConfigure:
220  break;
221  case clbEventStart:
222  break;
223  case clbEventStop:
224  break;
225  default:
226  break;
227  }
228  _clbStateUpdate(clbSys2Idx[CLB_SUB_INS], event, 0);
229 
230 }
231 
232 
233 
234 
235 static bool insReadAHRS()
236 {
237  if (AHRS_I2C_ADDR == 0) return true;
238 
239  return ahrsRead(INS_I2C_DEV, AHRS_I2C_ADDR, &insCompassData);
240 }
241 
242 static bool insReadLsm303D()
243 {
244  Lsm303Axis axis;
245 
246  // clear data
247 
248 
249  if (!lsm303DMagnetRead(INS_I2C_DEV, LSM303D_I2C_ADDR, &axis)) return false;
250 
251  insCompassData.hx = fltFromI16(axis.x, 14);
252  insCompassData.hy = fltFromI16(axis.y, 14);
253  insCompassData.hz = fltFromI16(axis.z, 14);
254 
255 
256  if (!lsm303DAccelRead(INS_I2C_DEV, LSM303D_I2C_ADDR, &axis)) return false;
257 
258  insCompassData.ax = fltFromI16(axis.x, 14);
259  insCompassData.ay = fltFromI16(axis.y, 14);
260  insCompassData.az = fltFromI16(axis.z, 14);
261 
262  return true;
263 }
264 
265 
266 static bool insReadLsm303Agr()
267 {
268  Lsm303Axis axis;
269 
270  // clear data
271 
272 
273  if (!lsm303AgrMagRead(INS_I2C_DEV, &axis)) return false;
274 
275  insCompassData.hx = fltFromI16((axis.x*1536)/1000, 10);
276  insCompassData.hy = fltFromI16((axis.y*1536)/1000, 10);
277  insCompassData.hz = fltFromI16((axis.z*1536)/1000, 10);
278 
279 
280  if (!lsm303AgrAccRead(INS_I2C_DEV, &axis)) return false;
281 
282  insCompassData.ax = fltFromI16(axis.x, 14);
283  insCompassData.ay = fltFromI16(axis.y, 14);
284  insCompassData.az = fltFromI16(axis.z, 14);
285 
286 
287  return true;
288 }
289 
290 
291 static bool insReadCompass()
292 {
293  switch (_compassDriver)
294  {
295  case COMPASS_LSM303D: return insReadLsm303D();
296  case COMPASS_LSM303AGR: return insReadLsm303Agr();
297  case COMPASS_AHRS: return insReadAHRS();
298  default:
299  return false;
300  }
301 }
302 
303 
304 bool _subsInsUpdate(ClbState state, uint32_t time)
305 {
306  int humid, temp;
307  static char rstate = 0;
308  static int ahrs_validcnt = 0;
309 
310  // ----------------------------------------------
311  // Read AHRS
312  // ----------------------------------------------
313 
314  if ( (state > clbStateIdle) && !etIsDisabled(SYS_SYS_DISABLE_AHRS) && _compassDriver != COMPASS_NONE)
315  {
316  memset(&insCompassData, 0, sizeof(CompassData));
317  //i2cDebug(true);
318  if (insReadCompass())
319  {
320 
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;
333 
334  if (_compassDriver == COMPASS_AHRS)
335  {
336  if(ahrs_validcnt < AHRS_VALID_CNT_NUM)
337  {
338  // data not yet valid
339  ahrs_validcnt++;
340  ins.ahrs_valid = false;
341  } else {
342  ins.ahrs_valid = true;
343  }
344  }
345  else
346  {
347  ins.ahrs_valid = true;
348  }
349  }
350  else
351  {
352  etCheckFailure(SYS_SYS_FAIL_AHRS);
353  ahrs_validcnt = 0;
354  ins.ahrs_valid = false;
355  }
356  // i2cDebug(false);
357  }
358  if (!etIsDisabled(SYS_SYS_DISABLE_TEMP_HUM))
359  {
360  bool result;
361  if(rstate == 0)
362  {
363 
364  // temperature reading
365  result = sht21ReadTemp( INS_I2C_DEV, SHT21_I2C_ADDR, &temp);
366  result &= sht21StartHumidMeas(INS_I2C_DEV, SHT21_I2C_ADDR);
367  rstate = 1;
368  ins.temp = result ? temp : 0;
369  }
370  else
371  {
372  // humidity reading
373  result = sht21ReadHumid( INS_I2C_DEV, SHT21_I2C_ADDR, &humid);
374  result &= sht21StartTempMeas( INS_I2C_DEV, SHT21_I2C_ADDR);
375  rstate = 0;
376  ins.humid = result ? humid : 0;
377  }
378  etCheckFailure(SYS_SYS_FAIL_TEMP_HUM);
379  }
380 
381  return !errHas();
382 }
f32_t az
Az in g (Float)
Definition: types.h:28
Axis structure for 3D information.
Definition: lsm303.h:31
bool errHas()
Returns whether there is an error pending.
Definition: err.c:52
void ahrsOff()
Switch off the AHRS.
Definition: ahrs.c:46
bool tempCompEn
Temperature compensation enabled.
Definition: lsm303agr.h:93
Accelerator configuration structure.
Definition: lsm303agr.h:114
This driver is to read and configure the AHRS I2C sensor.
f32_t ahrs_roll
Roll in degrees.
Definition: vars.h:419
Idle state.
Definition: clbstate.h:62
void ahrsOn()
Switch on the AHRS.
Definition: ahrs.c:41
bool lsm303AgrAccCfg(I2C_Device *dev, Lsm303AgrAccCfg config)
Set the accelerometer configuration.
Definition: lsm303agr.c:223
ClbEvent
All state change events.
Definition: clbstate.h:88
bool lsm303DMagnetCfg(I2C_Device *dev, uint8_t addr, Lsm303DMagCfg config)
Set the Magnet configuration.
Definition: lsm303d.c:130
f32_t ahrs_pitch
Pitch in degrees.
Definition: vars.h:416
Magnet configuration.
Definition: lsm303agr.h:90
bool ahrsGetVersion(I2C_Device *dev, uint8_t addr, uint8_t *version)
Reads the AHRS version.
Definition: ahrs.c:86
Generic LSM303D configuration.
Definition: lsm303d.h:82
static void _clbStateModErr(int idx)
Invoked by subsystem to indicate an error happened.
Definition: clbstate.h:189
uint16_t humid
Humidity in 1/100th RH.
Definition: vars.h:437
#define SYS_SYS_FAIL_TEMP_HUM
Bit which disables Temperature / Humidity.
Definition: vars.h:100
uint16_t accRate
Accelerometer rate , one of LSM303D_ACCRATE_*.
Definition: lsm303d.h:118
Provides access to all variables of the various subsystems.
bool schdRegister(SchdTaskF task, bool priority, int *taskId)
Register a task with the scheduler.
Definition: scheduler.c:95
Accelerator configuration structure.
Definition: lsm303d.h:117
bool lsm303DAccelRead(I2C_Device *dev, uint8_t addr, Lsm303Axis *result)
Read the accelerometer information.
Definition: lsm303d.c:118
f32_t hx
Hx in gauss (Float)
Definition: types.h:32
bool lsm303AgrAccRead(I2C_Device *dev, Lsm303Axis *result)
Read the accelerometer information.
Definition: lsm303agr.c:188
uint16_t mode
one of LSM303AGR_MAG_MODE_*
Definition: lsm303agr.h:91
Ready =&gt; Running.
Definition: clbstate.h:94
f32_t ahrs_g[3]
Angular velocity projection, (Index 0=X, 1=Y, 2=Z)
Definition: vars.h:428
bool lsm303AgrMagCfg(I2C_Device *dev, Lsm303AgrMagCfg config)
Set the Magnet configuration.
Definition: lsm303agr.c:200
f32_t pitch
Pitch in deg (Float)
Definition: types.h:24
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.
Definition: lsm303agr.c:163
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.
Definition: tm.c:18
CompassData insCompassData
Provides the AHRS data.
Definition: sub_ins.c:58
f32_t hz
Hz in gauss (Float)
Definition: types.h:34
uint8_t clbSys2Idx[6]
Mapping from subsystem ID to index.
Definition: clbstate.c:59
Magnet configuration.
Definition: lsm303d.h:98
#define LSM303AGR_MAG_CFG_DEFAULT
DEfault magnet configuration.
Definition: lsm303agr.h:101
bool sht21ReadTemp(I2C_Device *dev, uint8_t addr, int *answer)
Reads the temperatures from SHT21 at I2C address &#39;addr&#39;; puts the result in answer.
Definition: sht21.c:61
Structure defines data from a compass/tilt/gyro sensor.
Definition: types.h:22
f32_t gx
Gx in deg/sec (Float)
Definition: types.h:29
f32_t gy
Gy in deg/sec (Float)
Definition: types.h:30
void schdRunDelay(int taskId, int interval)
Schedule a task to run after a delay.
Definition: scheduler.c:201
f32_t ahrs_h[3]
Magnetic field projection, (Index 0=X, 1=Y, 2=Z)
Definition: vars.h:431
ClbState
Various states.
Definition: clbstate.h:59
bool sht21ReadHumid(I2C_Device *dev, uint8_t addr, int *answer)
Reads the relative humidity from SHT21 at I2C address &#39;addr&#39;; puts the result in answer.
Definition: sht21.c:124
#define LSM303D_I2C_ADDR
LSM303D Address.
Definition: cfg_board.h:57
Manages the global system error.
uint8_t compass_version
Compass version: 0 if not available or very old version.
Definition: vars.h:449
bool offCanc
Offset cancellation.
Definition: lsm303agr.h:95
int16_t temp
Temperature in 1/100th of a degree.
Definition: vars.h:434
Undefined =&gt; Idle, for internal use only.
Definition: clbstate.h:91
bool ahrs_valid
Compass data are valid.
Definition: vars.h:443
uint16_t rate
One of LSM303AGR_MAG_RATE_*.
Definition: lsm303agr.h:92
bool lsm303DValid(I2C_Device *dev, uint8_t addr, bool *isLSM303)
Checks whether or not the LSM303D is valid.
Definition: lsm303d.c:92
StandBy =&gt; Idle.
Definition: clbstate.h:99
Simple timer functions.
bool ahrsRead(I2C_Device *dev, uint8_t addr, CompassData *data)
Reads all data from the AHRS.
Definition: ahrs.c:99
void errClear()
Clears the current error.
Definition: err.c:46
uint16_t rate
Accelerometer rate , one of LSM303AG_ACC_RATE_*.
Definition: lsm303agr.h:116
f32_t ahrs_a[3]
Acceleration projection, (Index 0=X, 1=Y, 2=Z)
Definition: vars.h:425
f32_t ax
Ax in g (Float)
Definition: types.h:26
f32_t fltFromI16(int16_t val, int fBits)
Takes an integer value and converts it to a float.
Definition: float.c:122
This module is responsible for distributing error codes.
#define SHT21_I2C_ADDR
SHT21 I2C Address.
Definition: cfg_board.h:43
#define SYS_SYS_DISABLE_AHRS
Bit which disables AHRS (Compass) periodic readout.
Definition: vars.h:83
bool lsm303DMagnetRead(I2C_Device *dev, uint8_t addr, Lsm303Axis *result)
Read the magnet information.
Definition: lsm303d.c:106
bool temp
Temperature sensor enable.
Definition: lsm303agr.h:121
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.
Definition: ahrs.h:56
bool bdu
Block data update.
Definition: lsm303agr.h:120
#define AHRS_I2C_SEC_ADDR
AHRS I2C Address.
Definition: cfg_board.h:46
#define LOG_DEF(NAME,...)
Define a logger for a module.
Definition: log.h:129
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.
Definition: lsm303d.c:154
#define SYS_SYS_FAIL_AHRS
Bit which disables AHRS (Compass) periodic readout.
Definition: vars.h:99
bool lsm303DCfg(I2C_Device *dev, uint8_t addr, Lsm303DCfg config)
Sets the generic device configuration parameters.
Definition: lsm303d.c:179
ins_t ins
Provides access to all process variables of subsystem Instrumentation.
Definition: vars.c:212
Idle =&gt; StandBy.
Definition: clbstate.h:92
f32_t gz
Gz in deg/sec (Float)
Definition: types.h:31
Driver for the ST LSM303Dx Compass sensor.
StandBy =&gt; Ready.
Definition: clbstate.h:93
f32_t hy
Hy in gauss (Float)
Definition: types.h:33
Provides common macros.
bool sht21StartTempMeas(I2C_Device *dev, uint8_t addr)
Starts the temperature measurement in the SHT21 module at I2C address &#39;addr&#39;.
Definition: sht21.c:37
Implements a generic logger facility.
Paused =&gt; StandBy.
Definition: clbstate.h:97
#define LSM303AGR_ACC_CFG_DEFAULT
Default accelerometer configuration.
Definition: lsm303agr.h:127
#define SYS_SYS_DISABLE_TEMP_HUM
Bit which disables Temperature / Humidity.
Definition: vars.h:84
f32_t ahrs_yaw
Yaw in degrees.
Definition: vars.h:422
bool bdu
Block date update enable.
Definition: lsm303agr.h:97
f32_t yaw
Yaw in deg (Float)
Definition: types.h:23
#define logInfo(MSG,...)
Write a log message with formatting on info level.
Definition: log.h:202
f32_t ay
Ay in g (Float)
Definition: types.h:27
void _clbStateUpdate(int idx, ClbEvent event, uint8_t status)
Invoked by the subsystem to indicate a state change has happened.
Definition: clbstate.c:204
bool lsm303AgrMagRead(I2C_Device *dev, Lsm303Axis *result)
Read the magnet information.
Definition: lsm303agr.c:176
bool sht21StartHumidMeas(I2C_Device *dev, uint8_t addr)
Starts the relative humidity measurement in the SHT21 module at I2C address &#39;addr&#39;.
Definition: sht21.c:100
Configures the board-specific peripherals, like I2C, SPI etc...
f32_t roll
Roll in deg (Float)
Definition: types.h:25
uint8_t magScale
Magnet scale, use one of LSM303D_MAGSCALEE_*.
Definition: lsm303d.h:99