KM3NeT CLB  2.0
KM3NeT CLB v2 Embedded Software
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Groups Pages
shell_ahrs.c
1 /*
2  * KM3NeT CLB v2 Firmware
3  * ----------------------
4  *
5  * Copyright 2013 National Institute for Subatomic Physics Nikhef
6  *
7  * All Rights Reserved.
8  *
9  *
10  * File : ahrs.c
11  * Created : 21 jun. 2013
12  * Author : Antonio Orzelli
13  */
14 
15 
16 #include "cfg_board.h"
17 
18 #include <stdio.h>
19 #include <stdlib.h>
20 
21 #include "util/float.h"
22 
23 #include "drv/i2c/ahrs.h"
24 #include "drv/i2c/lsm303d.h"
25 #include "drv/i2c/lsm303agr.h"
26 #include "kernel/scheduler.h"
27 #include "kernel/tm.h"
28 
29 
30 static int _ahrsTestId = -1;
31 static int _ahrsTestCnt = 0;
32 
33 //const char cmd_ahrs_help[] = "AHRS test: ahrs on, reads data; ahrs init, initialize ahrs; ahrs reg <n>, reads reg n";
34 const char cmd_ahrs_help[] = "AHRS test: call ahrs no args for more help";
35 
36 // Read data from AHRS module and prints the results on shell
37 // Run this task 10 times, each second; then stops
38 void ahrsReadTask()
39 {
40 
41  CompassData data;
42  if (ahrsRead(I2C3, AHRS_I2C_ADDR, &data)) {
43 
44  printf("\n\nYaw: %d.%03u \n", FLT2DU(data.yaw,3));
45  printf("Pitch: %d.%03u \n", FLT2DU(data.pitch,3));
46  printf("Roll: %d.%03u \n", FLT2DU(data.roll,3));
47  printf("Ax: %d.%03u \n", FLT2DU(data.ax,3));
48  printf("Ay: %d.%03u \n", FLT2DU(data.ay,3));
49  printf("Az: %d.%03u \n", FLT2DU(data.az,3));
50  printf("Gx: %d.%03u \n", FLT2DU(data.gx,3));
51  printf("Gy: %d.%03u \n", FLT2DU(data.gy,3));
52  printf("Gz: %d.%03u \n", FLT2DU(data.gz,3));
53  printf("Hx: %d.%03u \n", FLT2DU(data.hx,3));
54  printf("Hy: %d.%03u \n", FLT2DU(data.hy,3));
55  printf("Hz: %d.%03u \n", FLT2DU(data.hz,3));
56  }else{
57  schdSetEnable(_ahrsTestId, false);
58 
59  //print the error
60  printf("Error reading AHRS:\n");
61  errPrint(true);
62 
63  }
64 
65  if (++_ahrsTestCnt == 10){
66  schdSetEnable(_ahrsTestId, false);
67  }
68 
69 }
70 
71 bool cmd_ahrs_exec(int argc, const char *args[])
72 {
73  // set default value (using register 0x43)
74 // uint8_t writeinit[5] = {0x43, 0, 0, 0, 0};
75 // AHRSRegRead answer;
76 
77  if (argc == 0){
78  puts("\nahrs on: enable ahrs \nahrs off: disable ahrs \nahrs meas: start measure");
79  return true;
80  }
81 
82  if (_ahrsTestId == -1) {
83  if (!schdRegister(ahrsReadTask, false, &_ahrsTestId))
84  errPrint(true);
85 
86  }
87 
88  if ( (argc == 1) && (strcmp(args[0], "on") == 0))
89  {
90  ahrsOn();
91  }
92 
93  else if ( (argc == 1) && (strcmp(args[0], "off") == 0))
94  {
95  ahrsOff();
96  }
97 
98  else if ( (argc == 1) && (strcmp(args[0], "meas") == 0))
99  {
100  schdSetEnable(_ahrsTestId, true); // just in case it was disabled
101  schdRunPeriodic(_ahrsTestId, 1000);
102  _ahrsTestCnt = 0;
103  }
104 
105 /* else if ( (argc == 2) && (strcmp(args[0], "reg") == 0))
106  {
107  if (!ahrsRead(I2C2, AHRS_I2C_ADDR, atoi(args[1]), (uint8_t *)&answer, sizeof(answer)))
108  errPrint(true);
109  else
110  printf("\n\nReg: %d.%u \n", answer.regAddr, answer.val);
111  }
112 */
113  else
114  {
115  schdSetEnable(_ahrsTestId, false);
116  }
117 
118  return true;
119 }
120 
121 
122 //const char cmd_ahrs_help[] = "AHRS test: ahrs on, reads data; ahrs init, initialize ahrs; ahrs reg <n>, reads reg n";
123 const char cmd_lsmd_help[] = "LSM303D test: call lsmd no args for more help";
124 
125 
126 bool cmd_lsmd_exec(int argc, const char *args[])
127 {
128  // set default value (using register 0x43)
129 // uint8_t writeinit[5] = {0x43, 0, 0, 0, 0};
130 // AHRSRegRead answer;
131 
132  if (argc == 0) {
133  puts("lsmd init : enable and init lsm \n"
134  "lsmd off : disable LSM303D\n"
135  "lsmd check : check LSM\n"
136  "lsmd meas : singe measure");
137  return true;
138  }
139  else if (argc == 1)
140  {
141  if (strcmp(args[0], "init") == 0) {
142  ahrsOn();
143  timeDelay(100);
144  bool isLSMD303 = false;
145  if (!lsm303DValid(INS_I2C_DEV, LSM303D_I2C_ADDR, &isLSMD303))
146  {
147  puts("LSM303 not detected!");
148  errClear();
149  return false;
150  }
151  if (isLSMD303) {
152  puts("LSM303 found and valid");
153  } else {
154  puts("I2C device present, but not an LSM303D");
155  return false;
156  }
157 
158 
160  if (!lsm303DCfg(INS_I2C_DEV, LSM303D_I2C_ADDR, cfg))
161  return false;
162 
164  magCfg.hiRes = true;
165  magCfg.lowPower = false;
166  magCfg.magRate = LSM303D_MAGRATE_3HZ125;
167  magCfg.magScale = LSM303D_MAGSCALE_2GAUSS;
168  magCfg.magMode = LSM303D_MAGMODE_CONTINUOUS;
169  if (!lsm303DMagnetCfg(INS_I2C_DEV, LSM303D_I2C_ADDR, magCfg))
170  return false;
171 
173  accCfg.accRate = LSM303D_ACCRATE_3HZ125;
174  accCfg.accScale = LSM303D_ACCSCALE_2G;
175  if (!lsm303DAccelCfg(INS_I2C_DEV, LSM303D_I2C_ADDR, accCfg))
176  return false;
177 
178  }
179  else if (strcmp(args[0], "off") == 0)
180  ahrsOff();
181  else if (strcmp(args[0], "check") == 0)
182  {
183  }
184  else if (strcmp(args[0], "meas") == 0) {
185  Lsm303Axis magAxis, accAxis;
186  if (!lsm303DMagnetRead(INS_I2C_DEV, LSM303D_I2C_ADDR, &magAxis))
187  return false;
188  if (!lsm303DAccelRead(INS_I2C_DEV, LSM303D_I2C_ADDR, &accAxis))
189  return false;
190 
191  printf("Magnet: X = %d, Y = %d, Z = %d\n", magAxis.x, magAxis.y, magAxis.z);
192  printf("Accell: X = %d, Y = %d, Z = %d\n", accAxis.x, accAxis.y, accAxis.z);
193 
194  f32_t x = fltFromI16(accAxis.x, 14);
195  f32_t y = fltFromI16(accAxis.y, 14);
196  f32_t z = fltFromI16(accAxis.z, 14);
197 
198  printf("X: %d.%03u Y: %d.%03u Z: %d.%03u\n", FLT2DU(x,3), FLT2DU(y,3), FLT2DU(z,3));
199 
200  }
201  else return false;
202  }
203  else return false;
204 
205  return true;
206 }
207 
208 
209 
210 
211 //const char cmd_ahrs_help[] = "AHRS test: ahrs on, reads data; ahrs init, initialize ahrs; ahrs reg <n>, reads reg n";
212 const char cmd_lsmagr_help[] = "LSM303AGR test: call lsmagr no args for more help";
213 
214 
215 bool cmd_lsmagr_exec(int argc, const char *args[])
216 {
217  // set default value (using register 0x43)
218 // uint8_t writeinit[5] = {0x43, 0, 0, 0, 0};
219 // AHRSRegRead answer;
220 
221  if (argc == 0) {
222  puts("lsmagr init : enable and init lsm \n");
223  puts("lsmagr temp : read temperature sensor 10 times\n");
224  puts("lsmagr acc : read accelerometer\n");
225  puts("lsmagr mag : read magnet\n");
226 
227  return true;
228  }
229  else if (argc == 1)
230  {
231  if (strcmp(args[0], "init") == 0) {
232  ahrsOn();
233  timeDelay(100);
234  bool isLSM303Agr = false;
235  if (!lsm303AgrValid(INS_I2C_DEV, &isLSM303Agr))
236  {
237  puts("LSM303AGR not detected!");
238  errClear();
239  return false;
240  }
241  if (isLSM303Agr) {
242  puts("LSM303AGR found and valid");
243  } else {
244  puts("I2C device present, but not an LSM303AGR");
245  return false;
246  }
247 
248  //i2cDebug(true);
249 
251  accCfg.temp = true;
252  accCfg.bdu = true;
253  accCfg.rate = LSM303AGR_ACC_RATE_10HZ;
254 
255  lsm303AgrAccCfg(INS_I2C_DEV, accCfg);
256 
258 
259  magCfg.rate = LSM303AGR_MAG_RATE_10HZ;
260  magCfg.mode = LSM303AGR_MAG_MODE_CONTINUOUS;
261  magCfg.bdu = true;
262 
263  lsm303AgrMagCfg(INS_I2C_DEV, magCfg);
264  return true;
265  }
266  else if (strcmp(args[0], "temp") == 0)
267  {
268 
269  int16_t temp;
270  for (int i = 0; i < 10; ++i) {
271  bool hasData = false;
272  while (!hasData) {
273  if (!lsm303AgrHasTemp(INS_I2C_DEV, &hasData)) return false;
274  }
275  lsm303AgrTempRead(INS_I2C_DEV, &temp);
276  printf("Temperature: %04x (%d)\n", 0xFFFF & temp, temp);
277  }
278 
279  // i2cDebug(false);
280  return true;
281  }
282  else if (strcmp(args[0], "acc") == 0)
283  {
284 
285  Lsm303Axis accAxis;
286  lsm303AgrAccRead(INS_I2C_DEV, &accAxis);
287  printf("XL: X = %d, Y = %d, Z = %d\n", accAxis.x, accAxis.y, accAxis.z);
288  f32_t x = fltFromI16(accAxis.x, 14);
289  f32_t y = fltFromI16(accAxis.y, 14);
290  f32_t z = fltFromI16(accAxis.z, 14);
291  printf("X: %d.%03u Y: %d.%03u Z: %d.%03u\n", FLT2DU(x,3), FLT2DU(y,3), FLT2DU(z,3));
292  return true;
293  }
294  else if (strcmp(args[0], "mag") == 0)
295  {
296 
297  Lsm303Axis magAxis;
298  lsm303AgrMagRead(INS_I2C_DEV, &magAxis);
299  printf("XL: X = %d, Y = %d, Z = %d\n", magAxis.x, magAxis.y, magAxis.z);
300 
301  // q/d mag -> gauss
302  f32_t x = fltFromI16((magAxis.x*1536)/1000, 10);
303  f32_t y = fltFromI16((magAxis.y*1536)/1000, 10);
304  f32_t z = fltFromI16((magAxis.z*1536)/1000, 10);
305  printf("X: %d.%03u Y: %d.%03u Z: %d.%03u\n", FLT2DU(x,3), FLT2DU(y,3), FLT2DU(z,3));
306 
307  return true;
308  }
309 
310  }
311 
312  return false;
313 }
f32_t az
Az in g (Float)
Definition: types.h:28
Axis structure for 3D information.
Definition: lsm303.h:31
void ahrsOff()
Switch off the AHRS.
Definition: ahrs.c:46
Accelerator configuration structure.
Definition: lsm303agr.h:114
This driver is to read and configure the AHRS I2C sensor.
uint8_t magMode
Magnetic sensor mode selection, LSM303D_MAGMODE_*.
Definition: lsm303d.h:101
void ahrsOn()
Switch on the AHRS.
Definition: ahrs.c:41
uint16_t accScale
Accelerometer scale, one of LSM303D_ACCSCALE_*.
Definition: lsm303d.h:121
bool lsm303AgrAccCfg(I2C_Device *dev, Lsm303AgrAccCfg config)
Set the accelerometer configuration.
Definition: lsm303agr.c:223
void schdRunPeriodic(int taskId, int interval)
Schedule a task to run periodically.
Definition: scheduler.c:212
bool lsm303DMagnetCfg(I2C_Device *dev, uint8_t addr, Lsm303DMagCfg config)
Set the Magnet configuration.
Definition: lsm303d.c:130
Magnet configuration.
Definition: lsm303agr.h:90
Generic LSM303D configuration.
Definition: lsm303d.h:82
bool hiRes
High resolution.
Definition: lsm303d.h:103
uint16_t accRate
Accelerometer rate , one of LSM303D_ACCRATE_*.
Definition: lsm303d.h:118
uint8_t magRate
Magnet rate, use one of LSM303D_MAGRATE_*.
Definition: lsm303d.h:100
bool schdRegister(SchdTaskF task, bool priority, int *taskId)
Register a task with the scheduler.
Definition: scheduler.c:95
#define LSM303D_MAGCFG_DEFAULT
DEfault magnet configuration.
Definition: lsm303d.h:107
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
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 lowPower
Magnet in low power mode.
Definition: lsm303d.h:102
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
#define I2C3
Virtual OpenCores I2C.
Definition: dev_soc.h:66
f32_t hz
Hz in gauss (Float)
Definition: types.h:34
Magnet configuration.
Definition: lsm303d.h:98
#define LSM303AGR_MAG_CFG_DEFAULT
DEfault magnet configuration.
Definition: lsm303agr.h:101
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
#define LSM303D_I2C_ADDR
LSM303D Address.
Definition: cfg_board.h:57
bool lsm303AgrTempRead(I2C_Device *dev, int16_t *result)
Reads the temperature.
Definition: lsm303agr.c:146
void errPrint(bool clear)
Prints the last error.
Definition: err.c:79
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
Simple timer functions.
bool ahrsRead(I2C_Device *dev, uint8_t addr, CompassData *data)
Reads all data from the AHRS.
Definition: ahrs.c:99
uint32_t f32_t
32 bit representation for float.
Definition: float.h:30
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 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
bool temp
Temperature sensor enable.
Definition: lsm303agr.h:121
bool lsm303DMagnetRead(I2C_Device *dev, uint8_t addr, Lsm303Axis *result)
Read the magnet information.
Definition: lsm303d.c:106
bool bdu
Block data update.
Definition: lsm303agr.h:120
bool lsm303DAccelCfg(I2C_Device *dev, uint8_t addr, Lsm303DAccCfg config)
Set the accelerometer configuration.
Definition: lsm303d.c:154
bool lsm303DCfg(I2C_Device *dev, uint8_t addr, Lsm303DCfg config)
Sets the generic device configuration parameters.
Definition: lsm303d.c:179
#define LSM303D_ACCCFG_DEFAULT
Default accelerometer configuration.
Definition: lsm303d.h:128
f32_t gz
Gz in deg/sec (Float)
Definition: types.h:31
Driver for the ST LSM303Dx Compass sensor.
f32_t hy
Hy in gauss (Float)
Definition: types.h:33
void schdSetEnable(int taskId, bool enabled)
Enable of disable a task.
Definition: scheduler.c:248
#define LSM303D_CFG_DEFAULT
Default generic configuration.
Definition: lsm303d.h:89
#define LSM303AGR_ACC_CFG_DEFAULT
Default accelerometer configuration.
Definition: lsm303agr.h:127
bool bdu
Block date update enable.
Definition: lsm303agr.h:97
f32_t yaw
Yaw in deg (Float)
Definition: types.h:23
f32_t ay
Ay in g (Float)
Definition: types.h:27
bool lsm303AgrMagRead(I2C_Device *dev, Lsm303Axis *result)
Read the magnet information.
Definition: lsm303agr.c:176
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