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;
}
