void demo_acc_start(accelerometer_callback callback)
{
static const SPIConfig spi1cfg = {
NULL,
/* HW dependent part.*/
GPIOE,
GPIOE_CS_SPI,
SPI_CR1_BR_0 | SPI_CR1_BR_1 | SPI_CR1_CPOL | SPI_CR1_CPHA
};
spiStart(&SPID1, &spi1cfg);
acc_callback = callback;
chThdSleepMilliseconds(500);
/* LIS302DL initialization.*/
lis302dlWriteRegister(&SPID1, LIS302DL_CTRL_REG1, 0x43);
lis302dlWriteRegister(&SPID1, LIS302DL_CTRL_REG2, 0x00);
lis302dlWriteRegister(&SPID1, LIS302DL_CTRL_REG3, 0x00);
chThdCreateStatic(waAcceleroThd,
sizeof(waAcceleroThd),
NORMALPRIO + 10,
AcceleroThd,
NULL);
}
void mySPIinit(void){
chMtxInit(&accelMtx);
// Initializes the SPI driver 1 in order to access the MEMS. The signals are already initialized in the board file.
spiStart(&SPID1, &spi1cfg);
// LIS302DL initialization.
lis302dlWriteRegister(&SPID1, LIS302DL_CTRL_REG1, 0x43);
lis302dlWriteRegister(&SPID1, LIS302DL_CTRL_REG2, 0x00);
lis302dlWriteRegister(&SPID1, LIS302DL_CTRL_REG3, 0x00);
// Start accelerator reading thread.
chThdCreateStatic(waThread1, sizeof(waThread1),
NORMALPRIO + 10, Thread1, NULL);
}
/*
* 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 memsSetRegister(uint8_t reg1, uint8_t reg2, uint8_t reg3) {
lis302dlWriteRegister(sdriver, LIS302DL_CTRL_REG1, reg1);
lis302dlWriteRegister(sdriver, LIS302DL_CTRL_REG2, reg2);
lis302dlWriteRegister(sdriver, LIS302DL_CTRL_REG3, reg3);
return 0;
}
