static msg_t lsm303_acc_update_thread(void *p) {
I2CDriver *i2cp = (I2CDriver *)p;
while (TRUE) {
msg_t msg;
/* Waiting for the IRQ to happen.*/
chSysLock();
acctp = chThdSelf();
chSchGoSleepS(THD_STATE_SUSPENDED);
msg = chThdSelf()->p_u.rdymsg;
chSysUnlock();
/* If data ready, update all axis.*/
if (msg == LSM303_ACC_DATA_READY) {
lsm303_acc_update(i2cp);
while (status_a != 0x00) {
lsm303_acc_update(i2cp);
}
// chprintf((BaseChannel *)&SERIAL_DRIVER, "ACC T: %d X: %d Y: %d Z: %d\r\n",
// acc_update_time, acc_data.x, acc_data.y, acc_data.z);
}
}
return RDY_OK;
}
Thread *accRun(I2CDriver *i2cp, tprio_t prio) {
Thread *tp;
lsm303_acc_init(i2cp);
chThdSleepMilliseconds(200);
lsm303_acc_update(i2cp);
tp = chThdCreateFromHeap(NULL, ACC_WA_SIZE, prio, lsm303_acc_update_thread,
(void*)i2cp);
extChannelEnable(&EXTD1, AM_INT1);
return tp;
}
Thread *accRun(SPIDriver *spip, tprio_t prio) {
Thread *tp;
if (lsm303_init(spip) != 0) chSysHalt();
chThdSleepMilliseconds(200);
lsm303_acc_update(spip);
tp = chThdCreateFromHeap(NULL, ACC_WA_SIZE, prio, lsm303_acc_update_thread, (void*) spip);
extChannelEnable(&EXTD1, GPIOA_AM_INT1);
return tp;
}
static msg_t lsm303_acc_update_thread(void *p) {
SPIDriver *spip = (SPIDriver *) p;
while (TRUE) {
msg_t msg;
/* Waiting for the IRQ to happen.*/
chSysLock();
acctp = chThdSelf();
chSchGoSleepS(THD_STATE_SUSPENDED);
msg = chThdSelf()->p_u.rdymsg;
chSysUnlock();
/* If data ready, update all axis.*/
if (msg == LSM303D_ACC_DATA_READY) {
lsm303_acc_update(spip);
}
}
return RDY_OK;
}
