void f_accelerometer(frame_t *buffer, uint16_t frame) { int32_t x, y; static int8_t xbuf[4], ybuf[4]; /* Last accelerometer data.*/ unsigned i; (void) frame; /* Keeping an history of the latest four accelerometer readings.*/ for (i = 3; i > 0; i--) { xbuf[i] = xbuf[i - 1]; ybuf[i] = ybuf[i - 1]; } /* Reading MEMS accelerometer X and Y registers.*/ xbuf[0] = (int8_t) lis302dlReadRegister(&SPID1, LIS302DL_OUTX); ybuf[0] = (int8_t) lis302dlReadRegister(&SPID1, LIS302DL_OUTY); /* Calculating average of the latest four accelerometer readings.*/ x = ((int32_t) xbuf[0] + (int32_t) xbuf[1] + (int32_t) xbuf[2] + (int32_t) xbuf[3]) / 4; y = ((int32_t) ybuf[0] + (int32_t) ybuf[1] + (int32_t) ybuf[2] + (int32_t) ybuf[3]) / 4; memset(buffer, 0, sizeof(frame_t) * CUBE_HEIGHT); draw_plane(buffer, PLANE_XY, ROT_CENTER); rotate_axis(buffer, -((int16_t)map(y, -128, 128, 180, -180))-90, AXIS_X); rotate_axis(buffer, ((int16_t)map(x, -128, 128, 180, -180))-90, AXIS_Y); }
static THD_FUNCTION(AcceleroThd, arg) { systime_t time; (void)arg; chRegSetThreadName("accelerometer"); /* Reader thread loop.*/ while (TRUE) { unsigned i; time = chVTGetSystemTime(); /* Reading MEMS accelerometer X, Y and Z registers.*/ acc_sample.acceleration[0] = (int8_t)lis302dlReadRegister(&SPID1, LIS302DL_OUTX); acc_sample.acceleration[1] = (int8_t)lis302dlReadRegister(&SPID1, LIS302DL_OUTY); acc_sample.acceleration[2] = (int8_t)lis302dlReadRegister(&SPID1, LIS302DL_OUTZ); acc_callback(); /* Waiting until the next 100 milliseconds time interval.*/ chThdSleepUntilWindowed(time, time + MS2ST(100)); } }
/* * Read accelerometer data. */ void readAccel(void) { // Keeping an history of the latest four accelerometer readings. unsigned i; for (i = 3; i > 0; i--) { accel_x[i] = accel_x[i - 1]; accel_y[i] = accel_y[i - 1]; accel_z[i] = accel_z[i - 1]; } // Reading MEMS accelerometer X, Y, and Z registers. accel_x[0] = (int8_t)lis302dlReadRegister(&SPID1, LIS302DL_OUTX); accel_y[0] = (int8_t)lis302dlReadRegister(&SPID1, LIS302DL_OUTY); accel_z[0] = (int8_t)lis302dlReadRegister(&SPID1, LIS302DL_OUTZ); // Calculating average of the latest four accelerometer readings. chMtxLock(&accelMtx); accel_x[4] = ((int32_t)accel_x[0] + (int32_t)accel_x[1] + (int32_t)accel_x[2] + (int32_t)accel_x[3]) / 4; accel_y[4] = ((int32_t)accel_y[0] + (int32_t)accel_y[1] + (int32_t)accel_y[2] + (int32_t)accel_y[3]) / 4; accel_z[4] = ((int32_t)accel_z[0] + (int32_t)accel_z[1] + (int32_t)accel_z[2] + (int32_t)accel_z[3]) / 4; chMtxUnlock(); }
/* * Application entry point. */ int main(void) { /* * System initializations. * - HAL initialization, this also initializes the configured device drivers * and performs the board-specific initializations. * - Kernel initialization, the main() function becomes a thread and the * RTOS is active. */ halInit(); chSysInit(); /* * Activates the serial driver 2 using the driver default configuration. * PA2(TX) and PA3(RX) are routed to USART2. */ sdStart(&SD2, NULL); palSetPadMode(GPIOA, 2, PAL_MODE_ALTERNATE(7)); palSetPadMode(GPIOA, 3, PAL_MODE_ALTERNATE(7)); /* * If the user button is pressed after the reset then the test suite is * executed immediately before activating the various device drivers in * order to not alter the benchmark scores. */ if (palReadPad(GPIOA, GPIOA_BUTTON)) TestThread(&SD2); /* * Initializes the SPI driver 2. The SPI2 signals are routed as follow: * PB12 - NSS. * PB13 - SCK. * PB14 - MISO. * PB15 - MOSI. */ spiStart(&SPID2, &spi2cfg); palSetPad(GPIOB, 12); palSetPadMode(GPIOB, 12, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST); /* NSS. */ palSetPadMode(GPIOB, 13, PAL_MODE_ALTERNATE(5) | PAL_STM32_OSPEED_HIGHEST); /* SCK. */ palSetPadMode(GPIOB, 14, PAL_MODE_ALTERNATE(5)); /* MISO. */ palSetPadMode(GPIOB, 15, PAL_MODE_ALTERNATE(5) | PAL_STM32_OSPEED_HIGHEST); /* MOSI. */ /* * Initializes the ADC driver 1 and enable the thermal sensor. * The pin PC0 on the port GPIOC is programmed as analog input. */ adcStart(&ADCD1, NULL); adcSTM32EnableTSVREFE(); palSetPadMode(GPIOC, 1, PAL_MODE_INPUT_ANALOG); /* * Initializes the PWM driver 4, routes the TIM4 outputs to the board LEDs. */ pwmStart(&PWMD4, &pwmcfg); palSetPadMode(GPIOD, GPIOD_LED4, PAL_MODE_ALTERNATE(2)); /* Green. */ palSetPadMode(GPIOD, GPIOD_LED6, PAL_MODE_ALTERNATE(2)); /* Blue. */ /* * Creates the example thread. */ chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL); /* * Initializes the SPI driver 1 in order to access the MEMS. The signals * are initialized in the board file. * Several LIS302DL registers are then initialized. */ spiStart(&SPID1, &spi1cfg); lis302dlWriteRegister(&SPID1, LIS302DL_CTRL_REG1, 0x43); lis302dlWriteRegister(&SPID1, LIS302DL_CTRL_REG2, 0x00); lis302dlWriteRegister(&SPID1, LIS302DL_CTRL_REG3, 0x00); /* * Normal main() thread activity, in this demo it does nothing except * sleeping in a loop and check the button state, when the button is * pressed the test procedure is launched with output on the serial * driver 2. */ while (TRUE) { int8_t x, y, z; if (palReadPad(GPIOA, GPIOA_BUTTON)) TestThread(&SD2); x = (int8_t)lis302dlReadRegister(&SPID1, LIS302DL_OUTX); y = (int8_t)lis302dlReadRegister(&SPID1, LIS302DL_OUTY); z = (int8_t)lis302dlReadRegister(&SPID1, LIS302DL_OUTZ); chprintf((BaseChannel *)&SD2, "%d, %d, %d\r\n", x, y, z); chThdSleepMilliseconds(500); } }
static THD_FUNCTION(Thread1, arg) { static int8_t xbuf[4], ybuf[4]; /* Last accelerometer data.*/ systime_t time; /* Next deadline.*/ (void)arg; chRegSetThreadName("reader"); /* LIS302DL initialization.*/ lis302dlWriteRegister(&SPID1, LIS302DL_CTRL_REG1, 0x43); lis302dlWriteRegister(&SPID1, LIS302DL_CTRL_REG2, 0x00); lis302dlWriteRegister(&SPID1, LIS302DL_CTRL_REG3, 0x00); /* Reader thread loop.*/ time = chVTGetSystemTime(); while (TRUE) { int32_t x, y; unsigned i; /* Keeping an history of the latest four accelerometer readings.*/ for (i = 3; i > 0; i--) { xbuf[i] = xbuf[i - 1]; ybuf[i] = ybuf[i - 1]; } /* Reading MEMS accelerometer X and Y registers.*/ xbuf[0] = (int8_t)lis302dlReadRegister(&SPID1, LIS302DL_OUTX); ybuf[0] = (int8_t)lis302dlReadRegister(&SPID1, LIS302DL_OUTY); /* Transmitting accelerometer the data over SPI2.*/ spiSelect(&SPID2); spiSend(&SPID2, 4, xbuf); spiSend(&SPID2, 4, ybuf); spiUnselect(&SPID2); /* Calculating average of the latest four accelerometer readings.*/ x = ((int32_t)xbuf[0] + (int32_t)xbuf[1] + (int32_t)xbuf[2] + (int32_t)xbuf[3]) / 4; y = ((int32_t)ybuf[0] + (int32_t)ybuf[1] + (int32_t)ybuf[2] + (int32_t)ybuf[3]) / 4; /* Reprogramming the four PWM channels using the accelerometer data.*/ if (y < 0) { pwmEnableChannel(&PWMD4, 0, (pwmcnt_t)-y); pwmEnableChannel(&PWMD4, 2, (pwmcnt_t)0); } else { pwmEnableChannel(&PWMD4, 2, (pwmcnt_t)y); pwmEnableChannel(&PWMD4, 0, (pwmcnt_t)0); } if (x < 0) { pwmEnableChannel(&PWMD4, 1, (pwmcnt_t)-x); pwmEnableChannel(&PWMD4, 3, (pwmcnt_t)0); } else { pwmEnableChannel(&PWMD4, 3, (pwmcnt_t)x); pwmEnableChannel(&PWMD4, 1, (pwmcnt_t)0); } /* Waiting until the next 250 milliseconds time interval.*/ chThdSleepUntil(time += MS2ST(100)); } }
int memsGetRegValues() { X_a = (int8_t)lis302dlReadRegister(&SPID1, LIS302DL_OUTX); Y_a = (int8_t)lis302dlReadRegister(&SPID1, LIS302DL_OUTY); Z_a = (int8_t)lis302dlReadRegister(&SPID1, LIS302DL_OUTZ); return 0; }