void initializeWheelEncoder(void) {
// Initializes the ICU drivers in order to access the PWM values from the wheel encoder ICU sensor.
icuStart(&ICUD4, &icuLeftWheelFirstSensor);
icuStart(&ICUD12, &icuLeftWheelSecondSensor);
icuStart(&ICUD5, &icuRightWheelFirstSensor);
icuStart(&ICUD9, &icuRightWheelSecondSensor);
// Wheel encoder ICU sensor left wheel first sensor is connected to PB7 and is monitored by timer 4/channel 2.
palSetPadMode(GPIOB, 7, PAL_MODE_ALTERNATE(STM32F4GPIO_AF_TIM4));
// Wheel encoder ICU sensor left wheel second sensor is connected to PB14 and is monitored by timer 12/channel 1.
palSetPadMode(GPIOB, 14, PAL_MODE_ALTERNATE(STM32F4GPIO_AF_TIM12));
// Wheel encoder ICU sensor right wheel first sensor is connected to PA1 and is monitored by timer 5/channel 2.
palSetPadMode(GPIOA, 1, PAL_MODE_ALTERNATE(STM32F4GPIO_AF_TIM5));
// Wheel encoder ICU sensor right wheel second sensor is connected to PE5 and is monitored by timer 14/channel 1.
palSetPadMode(GPIOE, 5, PAL_MODE_ALTERNATE(STM32F4GPIO_AF_TIM9));
// Enable ICU reading in background.
icuEnable(&ICUD4);
icuEnable(&ICUD12);
icuEnable(&ICUD5);
icuEnable(&ICUD9);
// Start speed reading thread.
ThreadSpeed = chThdCreateStatic(workingAreaThread_Speed,
sizeof(workingAreaThread_Speed),
NORMALPRIO + 15, Thread_Speed, NULL);
}
void initializeRCReceiver(void) {
// Initializes the ICU drivers in order to access the PWM values from the RC-receiver ICU sensor.
icuStart(&ICUD1, &icuRCReceiverCH0);
icuStart(&ICUD8, &icuRCReceiverCH1);
#if !USE_ONBOARD_ACCELEROMETER
icuStart(&ICUD14, &icuRCReceiverCH2);
#endif
// RC-receiver ICU sensor channel 0 is connected to PA8 and is monitored by timer 1/channel 1.
palSetPadMode(GPIOA, 8, PAL_MODE_ALTERNATE(STM32F4GPIO_AF_TIM1));
// RC-receiver ICU sensor channel 1 is connected to PC6 and is monitored by timer 8/channel 1.
palSetPadMode(GPIOC, 6, PAL_MODE_ALTERNATE(STM32F4GPIO_AF_TIM8));
#if !USE_ONBOARD_ACCELEROMETER
// RC-receiver ICU sensor channel 2 is connected to PA7 and is monitored by timer 14/channel 1.
palSetPadMode(GPIOA, 7, PAL_MODE_ALTERNATE(STM32F4GPIO_AF_TIM14));
#endif
// Enable ICU reading in background.
icuEnable(&ICUD1);
icuEnable(&ICUD8);
#if !USE_ONBOARD_ACCELEROMETER
icuEnable(&ICUD14);
#endif
}
void ICUinit(void)
{
icuStart(&ICUD5, &icucfg);
icuStart(&ICUD2, &icucfg2);
palSetPadMode(GPIOA, 0, PAL_MODE_ALTERNATE(2));
palSetPadMode(GPIOB, 3, PAL_MODE_ALTERNATE(2)); //TIM2_CH2
icuEnable(&ICUD5);
icuEnable(&ICUD2);
chThdSleepMilliseconds(500);
}
int main(void) {
halInit();
chSysInit();
static ICUConfig icu3cfg = {
ICU_INPUT_ACTIVE_HIGH,
0, /* bogus frequency */
width_cb,
period_cb,
overflow_cb,
};
palSetPadMode(IOPORT2, LED, PAL_MODE_OUTPUT_PUSHPULL);
palSetPadMode(IOPORT4, 4, PAL_MODE_OUTPUT_PUSHPULL);
palClearPad(IOPORT2, LED);
palClearPad(IOPORT4, 4);
sdStart(&SD1, NULL);
icuStart(&ICUD3, &icu3cfg);
thread_main = chThdSelf();
chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL);
chThdCreateStatic(waThread2, sizeof(waThread2), NORMALPRIO, Thread2, NULL);
while (1) {
BaseSequentialStream *serp = (BaseSequentialStream *) &SD1;
chprintf(serp, "Testing 50 duty cycle\r\n");
icuEnable(&ICUD3);
output_single_cycle(500, 500);
icuDisable(&ICUD3);
chEvtWaitAny((eventmask_t) 3);
chprintf(serp, "Testing 25 duty cycle\r\n");
icuEnable(&ICUD3);
output_single_cycle(250, 750);
icuDisable(&ICUD3);
chEvtWaitAny((eventmask_t) 3);
chprintf(serp, "Testing 75 duty cycle\r\n");
icuEnable(&ICUD3);
output_single_cycle(750, 250);
icuDisable(&ICUD3);
chEvtWaitAny((eventmask_t) 3);
chprintf(serp, "Testing overflow\r\n");
icuEnable(&ICUD3);
chEvtWaitAny((eventmask_t) 3);
icuDisable(&ICUD3);
}
}
/**
* @brief Starts the ADC and ICU input drivers.
* @note ICU drivers used in the firmware are modified ChibiOS
* drivers for extended input capture functionality.
* @return none.
*/
void mixedInputStart(void) {
/* Activates the ICU2 and ICU3 drivers. */
icuStart(&ICUD2, &icucfg2);
icuStart(&ICUD3, &icucfg3);
/* Starts continuous pulse width measurements. */
icuEnable(&ICUD2);
icuEnable(&ICUD3);
/* Activates the ADC1 driver. */
adcStart(&ADCD1, NULL);
/* Starts an ADC continuous conversion. */
adcStartConversion(&ADCD1, &adcgrpcfg, adcBuf, ADC_GRP_BUF_DEPTH);
}
static msg_t RCThread(void *arg) {
char frame_flag = 0;
int i,k;
chRegSetThreadName("RadioController");
(void)arg;
i=0;
k=0;
icuStart(&ICUD1, &icucfg);
icuEnable(&ICUD1);
while (TRUE)
{
if(frame_flag = 0)
if(last_width > 30000)
{
// if((k++ % 1000) == 0) palTogglePad(IOPORT3,11);
frame_flag = 1;
break;
}
if(frame_flag = 1)
for(i=0;i<=8;i++)
{
icutemp[i] = last_width;
}
frame_flag = 0;
chThdSleepMilliseconds(5);
}
return 0;
}
int main(void) {
halInit();
chSysInit();
chThdSleepMilliseconds(200);
// input capture & high-res timer
const ICUConfig icuConfig = { ICU_INPUT_ACTIVE_HIGH, 1000000, nullptr, nullptr, nullptr, nullptr, nullptr };
icuStart(&TIMING_ICU, &icuConfig);
icuEnable(&TIMING_ICU);
// serial setup
const SerialConfig btSerialConfig = { 921600, 0, USART_CR2_STOP1_BITS, USART_CR3_CTSE | USART_CR3_RTSE };
sdStart(&BT_SERIAL, &btSerialConfig);
// PWM setup
const PWMConfig mPWMConfig = { STM32_TIMCLK1, PWM_PERIOD, nullptr, {
{ PWM_OUTPUT_DISABLED, nullptr },
{ PWM_OUTPUT_DISABLED, nullptr },
{ PWM_OUTPUT_ACTIVE_HIGH, nullptr },
{ PWM_OUTPUT_ACTIVE_HIGH, nullptr } }, 0, };
pwmStart(&M1_PWM, &mPWMConfig);
// SPI setup
// speed = pclk/8 = 5.25MHz
const SPIConfig m1SPIConfig = { NULL, GPIOC, GPIOC_M1_NSS, SPI_CR1_DFF | SPI_CR1_BR_1 };
const SPIConfig m2SPIConfig = { NULL, GPIOD, GPIOD_M2_NSS, SPI_CR1_DFF | SPI_CR1_BR_1 };
const SPIConfig adcSPIConfig = { NULL, GPIOA, GPIOA_ADC_NSS, SPI_CR1_BR_2 | SPI_CR1_CPHA };
spiStart(&M1_SPI, &m1SPIConfig);
spiStart(&M2_SPI, &m2SPIConfig);
spiStart(&ADC_SPI, &adcSPIConfig);
// motor setup
A4960 m1(&M1_SPI, &M1_PWM, M1_PWM_CHAN);
A4960 m2(&M2_SPI, &M2_PWM, M2_PWM_CHAN);
// ADC setup
ADS1259 adc(&ADC_SPI);
// initialize control structure
Tortilla tortilla(m1, m2, adc, &TIMING_ICU, &BT_SERIAL);
// start slave threads
// chThdCreateStatic(waHeartbeat, sizeof(waHeartbeat), IDLEPRIO, tfunc_t(threadHeartbeat), nullptr);
chThdCreateStatic(waIO, sizeof(waIO), LOWPRIO, tfunc_t(threadIO), &tortilla);
// done with setup
palClearPad(GPIOC, GPIOC_LEDB);
palClearPad(GPIOB, GPIOB_LED2);
tortilla.fastLoop();
}
static ICUDriver *turnOnTriggerInputPin(brain_pin_e hwPin) {
// configure pin
turnOnCapturePin(hwPin);
shaft_icucfg.channel = ICU_CHANNEL_1;
ICUDriver *driver = getInputCaptureDriver(hwPin);
scheduleMsg(logger, "turnOnTriggerInputPin %s", hwPortname(hwPin));
// todo: reuse 'setWaveReaderMode' method here?
if (driver != NULL) {
bool_t needWidthCallback = !CONFIG(useOnlyFrontForTrigger) || TRIGGER_SHAPE(useRiseEdge);
shaft_icucfg.width_cb = needWidthCallback ? shaft_icu_width_callback : NULL;
bool_t needPeriodCallback = !CONFIG(useOnlyFrontForTrigger) || !TRIGGER_SHAPE(useRiseEdge);
shaft_icucfg.period_cb = needPeriodCallback ? shaft_icu_period_callback : NULL;
efiIcuStart(driver, &shaft_icucfg);
icuEnable(driver);
}
return driver;
}
/*
* 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();
/*
* Initializes the PWM driver 1 and ICU driver 1.
* GPIOD10 is the PWM output.
* GPIOA0 is the ICU input.
* The two pins have to be externally connected together.
*/
icuStart(&ICUD1, &icucfg);
icuEnable(&ICUD1);
/* Sets A0 alternative function.*/
SIU.PCR[0].R = 0b0100010100000100;
pwmStart(&PWMD1, &pwmcfg);
/* Sets D10 alternative function.*/
SIU.PCR[58].R = 0b0100010100000100;
chThdSleepMilliseconds(2000);
/*
* Starts the PWM channel 0 using 75% duty cycle.
*/
pwmEnableChannel(&PWMD1, 0, PWM_PERCENTAGE_TO_WIDTH(&PWMD1, 7500));
chThdSleepMilliseconds(5000);
/*
* Changes the PWM channel 0 to 50% duty cycle.
*/
pwmEnableChannel(&PWMD1, 0, PWM_PERCENTAGE_TO_WIDTH(&PWMD1, 5000));
chThdSleepMilliseconds(5000);
/*
* Changes the PWM channel 0 to 25% duty cycle.
*/
pwmEnableChannel(&PWMD1, 0, PWM_PERCENTAGE_TO_WIDTH(&PWMD1, 2500));
chThdSleepMilliseconds(5000);
/*
* Changes PWM period and the PWM channel 0 to 50% duty cycle.
*/
pwmChangePeriod(&PWMD1, 25000);
pwmEnableChannel(&PWMD1, 0, PWM_PERCENTAGE_TO_WIDTH(&PWMD1, 5000));
chThdSleepMilliseconds(5000);
/*
* Disables channel 0 and stops the drivers.
*/
pwmDisableChannel(&PWMD1, 0);
pwmStop(&PWMD1);
icuDisable(&ICUD1);
icuStop(&ICUD1);
palClearPad(PORT_D, PD_LED3);
palClearPad(PORT_D, PD_LED4);
/*
* Normal main() thread activity, in this demo it does nothing.
*/
while (TRUE) {
chThdSleepMilliseconds(500);
}
return 0;
}
/*
* 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();
/*
* LED initially off.
*/
palSetPad(IOPORT3, GPIOC_LED);
/*
* Initializes the PWM driver 1 and ICU driver 4.
*/
pwmStart(&PWMD1, &pwmcfg);
palSetPadMode(IOPORT1, 8, PAL_MODE_STM32_ALTERNATE_PUSHPULL);
icuStart(&ICUD4, &icucfg);
icuEnable(&ICUD4);
chThdSleepMilliseconds(2000);
/*
* Starts the PWM channel 0 using 75% duty cycle.
*/
pwmEnableChannel(&PWMD1, 0, PWM_PERCENTAGE_TO_WIDTH(&PWMD1, 7500));
chThdSleepMilliseconds(5000);
/*
* Changes the PWM channel 0 to 50% duty cycle.
*/
pwmEnableChannel(&PWMD1, 0, PWM_PERCENTAGE_TO_WIDTH(&PWMD1, 5000));
chThdSleepMilliseconds(5000);
/*
* Changes the PWM channel 0 to 25% duty cycle.
*/
pwmEnableChannel(&PWMD1, 0, PWM_PERCENTAGE_TO_WIDTH(&PWMD1, 2500));
chThdSleepMilliseconds(5000);
/*
* Changes PWM period to half second the duty cycle becomes 50%
* implicitly.
*/
pwmChangePeriod(&PWMD1, 5000);
chThdSleepMilliseconds(5000);
/*
* Disables channel 0 and stops the drivers.
*/
pwmDisableChannel(&PWMD1, 0);
pwmStop(&PWMD1);
icuDisable(&ICUD4);
icuStop(&ICUD4);
palSetPad(IOPORT3, GPIOC_LED);
/*
* Normal main() thread activity, in this demo it does nothing.
*/
while (TRUE) {
chThdSleepMilliseconds(500);
}
return 0;
}
/*
* 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();
/*
* Initializes the PWM driver 2 and ICU driver 3.
* GPIOA15 is the PWM output.
* GPIOC6 is the ICU input.
* The two pins have to be externally connected together.
*/
pwmStart(&PWMD2, &pwmcfg);
palSetPadMode(GPIOA, 15, PAL_MODE_ALTERNATE(1));
icuStart(&ICUD3, &icucfg);
palSetPadMode(GPIOC, 6, PAL_MODE_ALTERNATE(2));
icuEnable(&ICUD3);
chThdSleepMilliseconds(2000);
/*
* Starts the PWM channel 0 using 75% duty cycle.
*/
pwmEnableChannel(&PWMD2, 0, PWM_PERCENTAGE_TO_WIDTH(&PWMD2, 7500));
chThdSleepMilliseconds(5000);
/*
* Changes the PWM channel 0 to 50% duty cycle.
*/
pwmEnableChannel(&PWMD2, 0, PWM_PERCENTAGE_TO_WIDTH(&PWMD2, 5000));
chThdSleepMilliseconds(5000);
/*
* Changes the PWM channel 0 to 25% duty cycle.
*/
pwmEnableChannel(&PWMD2, 0, PWM_PERCENTAGE_TO_WIDTH(&PWMD2, 2500));
chThdSleepMilliseconds(5000);
/*
* Changes PWM period to half second the duty cycle becomes 50%
* implicitly.
*/
pwmChangePeriod(&PWMD2, 5000);
chThdSleepMilliseconds(5000);
/*
* Disables channel 0 and stops the drivers.
*/
pwmDisableChannel(&PWMD2, 0);
pwmStop(&PWMD2);
icuDisable(&ICUD3);
icuStop(&ICUD3);
palClearPad(GPIOE, GPIOE_LED4_BLUE);
palClearPad(GPIOE, GPIOE_LED9_BLUE);
/*
* Normal main() thread activity, in this demo it does nothing.
*/
while (TRUE) {
chThdSleepMilliseconds(500);
}
return 0;
}
/*
* Application entry point.
*/
int main(void) {
/* Initialization of all the imported components in the order specified in
the application wizard. The function is generated automatically.*/
componentsInit();
palClearPad(PORT11, P11_LED4);
/*
* Initializes the PWM driver 8 and ICU driver 1.
* PIN80 is the PWM output.
* PIN63 is the ICU input.
* The two pins have to be externally connected together.
*/
/* Sets PIN63 alternative function.*/
SIU.PCR[179].R = 0b0000011000001100;
/* Sets PIN65 alternative function.*/
SIU.PCR[181].R = 0b0000010100001100;
icuStart(&ICUD2, &icucfg);
icuEnable(&ICUD2);
pwmStart(&PWMD1, &pwmcfg);
chThdSleepMilliseconds(2000);
/*
* Starts the PWM channel 0 using 75% duty cycle.
*/
pwmEnableChannel(&PWMD1, 0, PWM_PERCENTAGE_TO_WIDTH(&PWMD1, 7500));
chThdSleepMilliseconds(5000);
/*
* Changes the PWM channel 0 to 50% duty cycle.
*/
pwmEnableChannel(&PWMD1, 0, PWM_PERCENTAGE_TO_WIDTH(&PWMD1, 5000));
chThdSleepMilliseconds(5000);
/*
* Changes the PWM channel 0 to 25% duty cycle.
*/
pwmEnableChannel(&PWMD1, 0, PWM_PERCENTAGE_TO_WIDTH(&PWMD1, 2500));
chThdSleepMilliseconds(5000);
/*
* Changes PWM period and the PWM channel 0 to 50% duty cycle.
*/
pwmChangePeriod(&PWMD1, 25000);
pwmEnableChannel(&PWMD1, 0, PWM_PERCENTAGE_TO_WIDTH(&PWMD1, 5000));
chThdSleepMilliseconds(5000);
/*
* Disables PWM channel 0 and stops the drivers.
*/
pwmDisableChannel(&PWMD1, 0);
pwmStop(&PWMD1);
/*
* Disables and stops the ICU drivers.
*/
icuDisable(&ICUD2);
icuStop(&ICUD2);
palClearPad(PORT11, P11_LED3);
palClearPad(PORT11, P11_LED4);
/*
* Normal main() thread activity, in this demo it does nothing.
*/
while (TRUE) {
chThdSleepMilliseconds(500);
}
return 0;
}
/*
* 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.
*/
uint32_t tempR, tempP, tempY;
float Roll,Pitch,Yaw;
halInit();
chSysInit();
i2c_setup();
palSetPadMode(IOPORT1, 8, PAL_MODE_INPUT); // PA8 - RADIO INPUT
palSetPadMode(IOPORT2, 13, PAL_MODE_STM32_ALTERNATE_OPENDRAIN); /* SCK. */
palSetPadMode(IOPORT2, 14, PAL_MODE_STM32_ALTERNATE_PUSHPULL); /* MISO.*/
palSetPadMode(IOPORT2, 15, PAL_MODE_STM32_ALTERNATE_OPENDRAIN); /* MOSI.*/
palSetPadMode(IOPORT2, 12, PAL_MODE_STM32_ALTERNATE_OPENDRAIN);
palSetPadMode(IOPORT3, 11, PAL_MODE_OUTPUT_PUSHPULL);
chMtxInit(&mtx_imu);
chMtxInit(&mutex_motors);
palClearPad(IOPORT3,10);
//palClearPad(IOPORT3,11);
/*
* Activates the USB driver and then the USB bus pull-up on D+.
*/
sduObjectInit(&SDU1);
sduStart(&SDU1, &serusbcfg);
usbConnectBus(serusbcfg.usbp);
//palClearPad(GPIOC, GPIOC_USB_DISC);
icuStart(&ICUD1, &icucfg);
icuEnable(&ICUD1);
chThdSleepSeconds(1);
chThdCreateStatic(waThread1, sizeof(waThread1), HIGHPRIO, Thread1, NULL);
//chThdCreateStatic(waMotorsThread, sizeof(waMotorsThread), NORMALPRIO, MotorsThread, NULL);
spiAcquireBus(&SPID2); /* Acquire ownership of the bus. */
spiStart(&SPID2, &ls_spicfg); /* Setup transfer parameters. */
spiSelect(&SPID2);
KP = 1;
KI = 1;
KD = 0;
while (TRUE) {
while(!(SPID2.spi->SR & SPI_SR_RXNE));
spiReceive(&SPID2,24,rxbuf_16bit);
// Floating point calculation of Roll/Pitch/Yaw inside the microcontroller. Decomment next 6 lines if you want to implement
// the control Law.
tempR = (uint32_t)((rxbuf_16bit[0] << 31) | (rxbuf_16bit[1] << 23) | (rxbuf_16bit[2] << 11) | rxbuf_16bit[3]);
tempP = (uint32_t)((rxbuf_16bit[4] << 31) | (rxbuf_16bit[5] << 23) | (rxbuf_16bit[6] << 11) | rxbuf_16bit[7]);
tempY = (uint32_t)((rxbuf_16bit[8] << 31) | (rxbuf_16bit[9] << 23) | (rxbuf_16bit[10] << 11) | rxbuf_16bit[11]);
Roll = (*(float*)&tempR);
Pitch = (*(float*)&tempP);
Yaw = (*(float*)&tempY);
// CONTROL LAW HERE
//++ (ROLL,PITCH,YAW ERRORS) -----> [CONTROLLER] -----> (MOTORS INPUT)
// PID Control Law
roll_error = 0 - Roll;
pitch_error = 0 - Pitch;
yaw_error = 0 - Yaw;
roll_I = roll_I + roll_error*0.01;
pitch_I = pitch_I + pitch_error*0.01;
roll_D = (roll_error - roll_prev_error)/0.01;
pitch_D = (pitch_error - pitch_prev_error)/0.01;
roll_controller_output = (int8_t)(KP*roll_error + KI*roll_I + KD*roll_D);
pitch_controller_output = (int8_t)(KP*pitch_error + KI*pitch_I + KD*pitch_D);
//roll_controller_output = (int8_t)roll_error;
//pitch_controller_output = (int8_t)pitch_error;
roll_prev_error = roll_error;
pitch_prev_error = pitch_error;
//-------------------------------------------------------------------
blctrl20_set_velocity();
palSetPad(IOPORT3,11);
/* SPI FLOATING POINT TEST PACKET (sending 5.6F)
rxbuf_16bit[0] = 0;
rxbuf_16bit[1] = 129;
rxbuf_16bit[2] = 1638;
rxbuf_16bit[3] = 819;
rxbuf_16bit[4] = 0;
rxbuf_16bit[5] = 129;
rxbuf_16bit[6] = 1638;
rxbuf_16bit[7] = 819;
rxbuf_16bit[8] = 1;
rxbuf_16bit[9] = 129;
rxbuf_16bit[10] = 1638;
rxbuf_16bit[11] = 819;
*/
if(SDU1.config->usbp->state==USB_ACTIVE)
{
// chprintf((BaseChannel *)&SDU1,"S:%6d:%6d:%6d:%6d:%6d:%6d:%6d:%6d:%6d:%6d:%6d:%6d:E\r\n",rxbuf_16bit[0],rxbuf_16bit[1],rxbuf_16bit[2],rxbuf_16bit[3],rxbuf_16bit[4],rxbuf_16bit[5],rxbuf_16bit[6],rxbuf_16bit[7],rxbuf_16bit[8],rxbuf_16bit[9],rxbuf_16bit[10],rxbuf_16bit[11]);
chprintf((BaseChannel *)&SDU1, "S:%6D:%6D:%6D:%6D:%6D:%6D:%6D:%6D:%6D:%6D:%6D:%6D:%6D:%6D:%6D:%6D:%6D:%6D:%6D:%6D:E\r\n",rxbuf_16bit[0],rxbuf_16bit[1],rxbuf_16bit[2],rxbuf_16bit[3],rxbuf_16bit[4],rxbuf_16bit[5],rxbuf_16bit[6],rxbuf_16bit[7],rxbuf_16bit[8],rxbuf_16bit[9],rxbuf_16bit[10],rxbuf_16bit[11],(int8_t)Roll,(int8_t)Pitch,(int8_t)Yaw,icu_ch[3],icu_ch[4],roll_controller_output,pitch_controller_output,yaw_controller_output);
}
chThdSleepMilliseconds(10);
}
spiUnselect(&SPID2);
spiReleaseBus(&SPID2); /* Ownership release. */
}