aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--accel.c22
-rw-r--r--accel.h2
-rw-r--r--gyro.c49
-rw-r--r--gyro.h4
4 files changed, 37 insertions, 40 deletions
diff --git a/accel.c b/accel.c
index 9e3bda4..2a73b40 100644
--- a/accel.c
+++ b/accel.c
@@ -16,6 +16,7 @@
#define LIS302DL_WHOAMI 0xf
#define LIS302DL_CTRLREG1 0x20
#define LIS302DL_UNUSED1 0x28
+#define LIS302DL_OUTZ 0x2D
/* value for 1g with +-2g max; measured */
#define ACCEL_1G_POS (55)
@@ -28,8 +29,8 @@
/* 250ms for 100Hz data rate */
#define ACCEL_SHAKE_TIMEOUT (25)
-/* 0, 2 and 4 are zero, as they contain the dummy register’s content */
-static volatile int8_t val[6] = {0, 0, 0, 0, 0, 0};
+/* z value */
+static volatile int8_t zval = 0;
/* number of times shaken (i.e. peak level measured) */
static uint8_t shakeCount = 0;
/* if max in one direction direction has been detected give it some time to
@@ -66,11 +67,11 @@ void accelInit () {
/* XXX: make nonblocking */
void accelStart () {
/* configuration:
- * disable power-down-mode
+ * disable power-down-mode, enable z-axis
* defaults
* low active, data ready interrupt on int2
*/
- uint8_t data[] = {0b01000111, 0b0, 0b10100000};
+ uint8_t data[] = {0b01000100, 0b0, 0b10100000};
if (!twRequest (TWM_WRITE, LIS302DL, LIS302DL_CTRLREG1, data,
sizeof (data)/sizeof (*data))) {
@@ -86,7 +87,6 @@ void accelStart () {
* direction. called for every data set pulled.
*/
static void accelProcessShake () {
- const int8_t zval = val[5];
/* detect shake if:
* a) horizon is positive and accel z value is >= ACCEL_SHAKE_POS
* b) horizon is negative and accel z value is >= ACCEL_SHAKE_POS offset by
@@ -133,7 +133,6 @@ static void accelProcessShake () {
* i.e. have we been turned upside down?
*/
static void accelProcessHorizon () {
- const int8_t zval = val[5];
/* measuring approximately 1g */
if (zval > (ACCEL_1G_POS - ACCEL_1G_OFF) &&
zval < (ACCEL_1G_POS + ACCEL_1G_OFF) &&
@@ -168,10 +167,9 @@ bool accelProcess () {
}
} else {
if (!((PINC >> PINC1) & 0x1) && twr.status != TWST_WAIT) {
- /* new data available in device buffer and bus is free, we are
- * reading the registers inbetween out_x/y/z and ignore them */
- if (!twRequest (TWM_READ, LIS302DL, LIS302DL_UNUSED1,
- (uint8_t *) val, 6)) {
+ /* new data available in device buffer and bus is free */
+ if (!twRequest (TWM_READ, LIS302DL, LIS302DL_OUTZ,
+ (uint8_t *) &zval, sizeof (zval))) {
printf ("cannot start read\n");
} else {
reading = true;
@@ -182,8 +180,8 @@ bool accelProcess () {
return false;
}
-volatile const int8_t *accelGet () {
- return val;
+const int8_t accelGetZ () {
+ return zval;
}
const uint8_t accelGetShakeCount () {
diff --git a/accel.h b/accel.h
index 6c1022d..3aba602 100644
--- a/accel.h
+++ b/accel.h
@@ -9,7 +9,7 @@ typedef enum {HORIZON_NONE, HORIZON_POS, HORIZON_NEG} horizon;
void accelInit ();
void accelStart ();
bool accelProcess ();
-volatile const int8_t *accelGet ();
+const int8_t accelGetZ ();
const uint8_t accelGetShakeCount ();
const horizon accelGetHorizon ();
diff --git a/gyro.c b/gyro.c
index bc402e5..0333e86 100644
--- a/gyro.c
+++ b/gyro.c
@@ -17,11 +17,12 @@
#define L3GD20_CTRLREG1 0x20
#define L3GD20_CTRLREG3 0x22
#define L3GD20_CTRLREG4 0x23
+#define L3GD20_OUTZ 0x2c
-/* raw values */
-static volatile int16_t val[3] = {0, 0, 0};
-/* accumulated values */
-static int32_t accum[3] = {0, 0, 0};
+/* raw z value */
+static volatile int16_t zval = 0;
+/* accumulated z value */
+static int32_t zaccum = 0;
/* calculated zticks */
static int8_t zticks = 0;
/* currently reading from i2c */
@@ -46,12 +47,12 @@ void gyroInit () {
/* XXX: make nonblocking */
void gyroStart () {
/* configuration:
- * disable power-down-mode
+ * disable power-down-mode, enable z
* defaults
* low-active (does not work?), push-pull, drdy on int2
* select 500dps
*/
- uint8_t data[] = {0b00001111, 0b0, 0b00101000, 0b00010000};
+ uint8_t data[] = {0b00001100, 0b0, 0b00101000, 0b00010000};
if (!twRequest (TWM_WRITE, L3GD20, L3GD20_CTRLREG1, data,
sizeof (data)/sizeof (*data))) {
@@ -65,17 +66,16 @@ void gyroStart () {
*/
static void gyroProcessTicks () {
const uint8_t shift = 14;
- const int32_t zval = accum[2];
- if (zval > (1 << shift)) {
- const uint32_t a = abs (zval);
+ if (zaccum > (1 << shift)) {
+ const uint32_t a = abs (zaccum);
zticks += a >> shift;
/* mask shift bits */
- accum[2] -= a & (~0x3ff);
- } else if (zval < -(1 << shift)) {
- const uint32_t a = abs (zval);
+ zaccum -= a & (~0x3ff);
+ } else if (zaccum < -(1 << shift)) {
+ const uint32_t a = abs (zaccum);
zticks -= a >> shift;
- accum[2] += a & (~0x3ff);
+ zaccum += a & (~0x3ff);
}
}
@@ -85,11 +85,9 @@ bool gyroProcess () {
if (reading) {
if (twr.status == TWST_OK) {
/* new data transfered, process it */
- for (uint8_t i = 0; i < sizeof (accum)/sizeof (*accum); i++) {
- /* poor man's noise filter */
- if (abs (val[i]) > 64) {
- accum[i] += val[i];
- }
+ /* poor man's noise filter */
+ if (abs (zval) > 64) {
+ zaccum += zval;
}
gyroProcessTicks ();
reading = false;
@@ -101,7 +99,8 @@ bool gyroProcess () {
} else {
if (((PINB >> PINB1) & 0x1) && twr.status != TWST_WAIT) {
/* new data available in device buffer and bus is free */
- if (!twRequest (TWM_READ, L3GD20, 0x28, (uint8_t *) val, 6)) {
+ if (!twRequest (TWM_READ, L3GD20, L3GD20_OUTZ,
+ (uint8_t *) &zval, sizeof (zval))) {
printf ("cannot start read\n");
} else {
reading = true;
@@ -112,16 +111,16 @@ bool gyroProcess () {
return false;
}
-const int32_t *gyroGetAccum () {
- return accum;
+const int32_t gyroGetZAccum () {
+ return zaccum;
}
-void gyroResetAccum () {
- memset (accum, 0, sizeof (accum));
+void gyroResetZAccum () {
+ zaccum = 0;
}
-volatile const int16_t *gyroGetRaw () {
- return val;
+volatile const int16_t gyroGetZRaw () {
+ return zval;
}
const int8_t gyroGetZTicks () {
diff --git a/gyro.h b/gyro.h
index a3f62f7..c55d8dd 100644
--- a/gyro.h
+++ b/gyro.h
@@ -8,8 +8,8 @@ void gyroInit ();
void gyroStart ();
bool gyroProcess ();
void gyroResetAccum ();
-const int32_t *gyroGetAccum ();
-volatile const int16_t *gyroGetRaw ();
+const int32_t gyroGetZAccum ();
+volatile const int16_t gyroGetZRaw ();
const int8_t gyroGetZTicks ();
#endif /* GYROSCOPE_H */
generated by cgit v1.2.3 (git 2.39.1) at 2024-12-26 08:09:07 +0000