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);
}
Ejemplo n.º 2
0
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
}
Ejemplo n.º 3
0
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);
}
Ejemplo n.º 4
0
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);
  }
}
Ejemplo n.º 5
0
/**
 * @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);
}
Ejemplo n.º 6
0
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;
}
Ejemplo n.º 7
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();
}
Ejemplo n.º 8
0
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;
}
Ejemplo n.º 9
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 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;
}
Ejemplo n.º 10
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;
}
Ejemplo n.º 11
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;
}
Ejemplo n.º 12
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;
}
Ejemplo n.º 13
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.               */


}