30 static int _ahrsTestId = -1;
31 static int _ahrsTestCnt = 0;
34 const char cmd_ahrs_help[] =
"AHRS test: call ahrs no args for more help";
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));
60 printf(
"Error reading AHRS:\n");
65 if (++_ahrsTestCnt == 10){
71 bool cmd_ahrs_exec(
int argc,
const char *args[])
78 puts(
"\nahrs on: enable ahrs \nahrs off: disable ahrs \nahrs meas: start measure");
82 if (_ahrsTestId == -1) {
88 if ( (argc == 1) && (strcmp(args[0],
"on") == 0))
93 else if ( (argc == 1) && (strcmp(args[0],
"off") == 0))
98 else if ( (argc == 1) && (strcmp(args[0],
"meas") == 0))
123 const char cmd_lsmd_help[] =
"LSM303D test: call lsmd no args for more help";
126 bool cmd_lsmd_exec(
int argc,
const char *args[])
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");
141 if (strcmp(args[0],
"init") == 0) {
144 bool isLSMD303 =
false;
147 puts(
"LSM303 not detected!");
152 puts(
"LSM303 found and valid");
154 puts(
"I2C device present, but not an LSM303D");
166 magCfg.
magRate = LSM303D_MAGRATE_3HZ125;
167 magCfg.
magScale = LSM303D_MAGSCALE_2GAUSS;
168 magCfg.
magMode = LSM303D_MAGMODE_CONTINUOUS;
173 accCfg.
accRate = LSM303D_ACCRATE_3HZ125;
174 accCfg.
accScale = LSM303D_ACCSCALE_2G;
179 else if (strcmp(args[0],
"off") == 0)
181 else if (strcmp(args[0],
"check") == 0)
184 else if (strcmp(args[0],
"meas") == 0) {
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);
198 printf(
"X: %d.%03u Y: %d.%03u Z: %d.%03u\n", FLT2DU(x,3), FLT2DU(y,3), FLT2DU(z,3));
212 const char cmd_lsmagr_help[] =
"LSM303AGR test: call lsmagr no args for more help";
215 bool cmd_lsmagr_exec(
int argc,
const char *args[])
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");
231 if (strcmp(args[0],
"init") == 0) {
234 bool isLSM303Agr =
false;
237 puts(
"LSM303AGR not detected!");
242 puts(
"LSM303AGR found and valid");
244 puts(
"I2C device present, but not an LSM303AGR");
253 accCfg.
rate = LSM303AGR_ACC_RATE_10HZ;
259 magCfg.
rate = LSM303AGR_MAG_RATE_10HZ;
260 magCfg.
mode = LSM303AGR_MAG_MODE_CONTINUOUS;
266 else if (strcmp(args[0],
"temp") == 0)
270 for (
int i = 0; i < 10; ++i) {
271 bool hasData =
false;
273 if (!lsm303AgrHasTemp(INS_I2C_DEV, &hasData))
return false;
276 printf(
"Temperature: %04x (%d)\n", 0xFFFF & temp, temp);
282 else if (strcmp(args[0],
"acc") == 0)
287 printf(
"XL: X = %d, Y = %d, Z = %d\n", accAxis.x, accAxis.y, accAxis.z);
291 printf(
"X: %d.%03u Y: %d.%03u Z: %d.%03u\n", FLT2DU(x,3), FLT2DU(y,3), FLT2DU(z,3));
294 else if (strcmp(args[0],
"mag") == 0)
299 printf(
"XL: X = %d, Y = %d, Z = %d\n", magAxis.x, magAxis.y, magAxis.z);
305 printf(
"X: %d.%03u Y: %d.%03u Z: %d.%03u\n", FLT2DU(x,3), FLT2DU(y,3), FLT2DU(z,3));
Axis structure for 3D information.
void ahrsOff()
Switch off the AHRS.
Accelerator configuration structure.
This driver is to read and configure the AHRS I2C sensor.
uint8_t magMode
Magnetic sensor mode selection, LSM303D_MAGMODE_*.
void ahrsOn()
Switch on the AHRS.
uint16_t accScale
Accelerometer scale, one of LSM303D_ACCSCALE_*.
bool lsm303AgrAccCfg(I2C_Device *dev, Lsm303AgrAccCfg config)
Set the accelerometer configuration.
void schdRunPeriodic(int taskId, int interval)
Schedule a task to run periodically.
bool lsm303DMagnetCfg(I2C_Device *dev, uint8_t addr, Lsm303DMagCfg config)
Set the Magnet configuration.
Generic LSM303D configuration.
bool hiRes
High resolution.
uint16_t accRate
Accelerometer rate , one of LSM303D_ACCRATE_*.
uint8_t magRate
Magnet rate, use one of LSM303D_MAGRATE_*.
bool schdRegister(SchdTaskF task, bool priority, int *taskId)
Register a task with the scheduler.
#define LSM303D_MAGCFG_DEFAULT
DEfault magnet configuration.
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 lowPower
Magnet in low power mode.
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.
#define I2C3
Virtual OpenCores I2C.
f32_t hz
Hz in gauss (Float)
#define LSM303AGR_MAG_CFG_DEFAULT
DEfault magnet configuration.
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)
#define LSM303D_I2C_ADDR
LSM303D Address.
bool lsm303AgrTempRead(I2C_Device *dev, int16_t *result)
Reads the temperature.
void errPrint(bool clear)
Prints the last error.
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.
uint32_t f32_t
32 bit representation for float.
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.
bool temp
Temperature sensor enable.
bool lsm303DMagnetRead(I2C_Device *dev, uint8_t addr, Lsm303Axis *result)
Read the magnet information.
bool bdu
Block data update.
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.
#define LSM303D_ACCCFG_DEFAULT
Default accelerometer configuration.
f32_t gz
Gz in deg/sec (Float)
Driver for the ST LSM303Dx Compass sensor.
f32_t hy
Hy in gauss (Float)
void schdSetEnable(int taskId, bool enabled)
Enable of disable a task.
#define LSM303D_CFG_DEFAULT
Default generic configuration.
#define LSM303AGR_ACC_CFG_DEFAULT
Default accelerometer configuration.
bool bdu
Block date update enable.
f32_t yaw
Yaw in deg (Float)
bool lsm303AgrMagRead(I2C_Device *dev, Lsm303Axis *result)
Read the magnet information.
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_*.