//fungsi utama
int main (void) {
	
	//inisialisasi HAL (Hardware Abstraction Layer)
	halInit();
	
	//inisialisasi kernel
	chSysInit();
	
	Adc_Init();
	
	process_running();
	
	//inisialisasi CAN
	CAN_Config();
	
	// serial
	sdStart(&SD2,NULL);  
    palSetPadMode(GPIOB,3, PAL_MODE_ALTERNATE(7)); //TX
    palSetPadMode(GPIOB,4, PAL_MODE_ALTERNATE(7)); //RX

    sdStart(&SD1,NULL);
    palSetPadMode(GPIOA,9, PAL_MODE_ALTERNATE(7)); //TX
    palSetPadMode(GPIOA,10, PAL_MODE_ALTERNATE(7)); //RX

    sdStart(&SD3,NULL);
    palSetPadMode(GPIOB,10, PAL_MODE_ALTERNATE(7)); //TX
    palSetPadMode(GPIOE,15, PAL_MODE_ALTERNATE(7)); //RX
    
	//inisialisasi USB
	sduObjectInit(&SDU1);
    sduStart(&SDU1, &serusbcfg);
    
    //aktifasi USB
    usbDisconnectBus(serusbcfg.usbp);
    chThdSleepMilliseconds(200);
    usbStart(serusbcfg.usbp, &usbcfg);
    usbConnectBus(serusbcfg.usbp);
	
	//thread 1
	chThdCreateStatic(waBacavolt, sizeof(waBacavolt), NORMALPRIO, Bacavolt, NULL);
	chThdCreateStatic(waBacacurrent, sizeof(waBacacurrent), NORMALPRIO, Bacacurrent, NULL);
	chThdCreateStatic(waThreadBMS, sizeof(waThreadBMS), NORMALPRIO, ThreadBMS, NULL);
	//chThdCreateStatic(waThreadkirimjoule, sizeof(waThreadkirimjoule), NORMALPRIO, Threadkirimjoule, NULL);
	//chThdCreateStatic(waThreadkirimjoule2, sizeof(waThreadkirimjoule2), NORMALPRIO, Threadkirimjoule2, NULL);


while(TRUE) {
	kirim_data_usart1();
    chprintf((BaseSequentialStream *)&SD1,"%d,%f,%d,%d,%f,%f,%d,%d,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%d,%d,%d,%d \n",avgtemp,packsoc,packDCL,packCCL,lowvoltcell,highvoltcell,hightemp,lowtemp,packcurrent,packvolt,amphours,supply12v, deltacellvolt,avgcellvolt,mppt1_v,mppt2_v,mppt3_v,mppt4_v,mppt5_v,mppt6_v,mppt7_v,mppt8_v,mppt9_v,mppt10_v,mppt11_v,mppt12_v,mppt13_v,mppt15_v,mppt16_v, mppt1_i, mppt2_i, mppt3_i, mppt4_i, mppt5_i, mppt6_i, mppt7_i,mppt8_i, mppt9_i, mppt10_i, mppt11_i, mppt12_i, mppt13_i, mppt14_i, mppt15_i,current1,current2,current3,voltage1,voltage2,voltage3,milidetik,detik,menit,jam);
    //chprintf((BaseSequentialStream *)&SDU1,"%d,%f,%d,%d,%f,%f,%d,%d,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f \n",avgtemp,packsoc,packDCL,packCCL,lowvoltcell,highvoltcell,hightemp,lowtemp,packcurrent,packvolt,amphours,supply12v, deltacellvolt,avgcellvolt,mppt1_v,mppt2_v,mppt3_v,mppt4_v,mppt5_v,mppt6_v,mppt7_v,mppt8_v,mppt9_v,mppt10_v,mppt11_v,mppt12_v,mppt13_v,mppt15_v,mppt16_v, mppt1_i, mppt2_i, mppt3_i, mppt4_i, mppt5_i, mppt6_i, mppt7_i,mppt8_i, mppt9_i, mppt10_i, mppt11_i, mppt12_i, mppt13_i, mppt14_i, mppt15_i);
    //chprintf((BaseSequentialStream *)&SDU1,"%x %x %x %x %x %x %x %x \n",RxMessage33.StdId, RxMessage44.StdId, RxMessage55.StdId, RxMessage66.StdId, RxMessage77.StdId, RxMessage88.StdId, RxMessage99.StdId, RxMessage11.StdId); 
    //chprintf((BaseSequentialStream *)&SD1,"tes \n");
    chThdSleepMilliseconds(500);
}
   return 0 ;
}	

static void setPower( bool_t en )
{
   if ( en )
       palSetPad( PWR_PORT, PWR_PIN );
       // initUsb();
   else
   {
       palClearPad( PWR_PORT, PWR_PIN );
       setLight( 0 );
       adcCfg( 0 );
       motoConfig( 0, 3 );
       // finitUsb();
   }
   palSetPadMode( PWR_PORT, PWR_PIN, PAL_MODE_OUTPUT_PUSHPULL );
}

void i2cUtilsStart(I2CDriver * driver, const I2CConfig * config, const I2CPins * pins){
    chDbgCheck(driver && config && pins, utils_i2cStart);
    chDbgAssert(driver->state != I2C_UNINIT,
                DBG_PREFIX "I2C hal driver must be enabled",
                NULL);

    palSetPadMode(pins->SDA.port, pins->SDA.pad, I2C_PINMODE);
    palSetPadMode(pins->SCL.port, pins->SCL.pad, I2C_PINMODE);

    if(driver->state == I2C_STOP){
        i2cStart(driver, config);
    }else{
        //Verify I2C configs match.
        chDbgAssert(driver->config->op_mode == config->op_mode,
                DBG_PREFIX "requested opmode does not match previously configured",
                NULL);
        chDbgAssert(driver->config->clock_speed == config->clock_speed,
                DBG_PREFIX "requested clock speed does not match previously configured",
                NULL);
        chDbgAssert(driver->config->duty_cycle == config->duty_cycle,
                DBG_PREFIX "requested duty cycle does not match previously configured",
                NULL);
    }
}

void encoder_init_as5047p_spi(void) {
	TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;

	palSetPadMode(SPI_SW_MISO_GPIO, SPI_SW_MISO_PIN, PAL_MODE_INPUT);
	palSetPadMode(SPI_SW_SCK_GPIO, SPI_SW_SCK_PIN, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
	palSetPadMode(SPI_SW_CS_GPIO, SPI_SW_CS_PIN, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);

	// Set MOSI to 1
#if AS5047_USE_HW_SPI_PINS
	palSetPadMode(SPI_SW_MOSI_GPIO, SPI_SW_MOSI_PIN, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
	palSetPad(SPI_SW_MOSI_GPIO, SPI_SW_MOSI_PIN);
#endif

	// Enable timer clock
	HW_ENC_TIM_CLK_EN();

	// Time Base configuration
	TIM_TimeBaseStructure.TIM_Prescaler = 0;
	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
	TIM_TimeBaseStructure.TIM_Period = ((168000000 / 2 / AS5047_SAMPLE_RATE_HZ) - 1);
	TIM_TimeBaseStructure.TIM_ClockDivision = 0;
	TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
	TIM_TimeBaseInit(HW_ENC_TIM, &TIM_TimeBaseStructure);

	// Enable overflow interrupt
	TIM_ITConfig(HW_ENC_TIM, TIM_IT_Update, ENABLE);

	// Enable timer
	TIM_Cmd(HW_ENC_TIM, ENABLE);

	nvicEnableVector(HW_ENC_TIM_ISR_CH, 6);

	mode = ENCODER_MODE_AS5047P_SPI;
	index_found = true;
	spi_error_rate = 0.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();

  /*
   * Activates the serial driver 1 using the driver default configuration.
   * PA9(TX) and PA10(RX) are routed to USART1.
   */
  sdStart(&SD1, NULL);
  palSetPadMode(GPIOA, 9, PAL_MODE_ALTERNATE(7));
  palSetPadMode(GPIOA, 10, PAL_MODE_ALTERNATE(7));

  /*
   * Creates the example thread.
   */
  chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL);

  /*
   * 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.
   */
  while (TRUE) {
    if (palReadPad(GPIOA, GPIOA_BUTTON))
      TestThread(&SD1);
    chThdSleepMilliseconds(500);
  }
}

static void initMMCSpiHw(void)
{
    /* Setup CC3000 SPI pins Chip Select*/
    palSetPad(MMC_CS_PORT, MMC_CS_PAD);
    palSetPadMode(MMC_CS_PORT, MMC_CS_PAD,
        PAL_MODE_OUTPUT_PUSHPULL |
        PAL_STM32_OSPEED_LOWEST);     /* 400 kHz */

    /* General SPI pin setting */
    palSetPadMode(MMC_SPI_PORT, MMC_SCK_PAD,
        PAL_MODE_ALTERNATE(5) |       /* SPI SCK */
        PAL_STM32_OTYPE_PUSHPULL |
        PAL_STM32_OSPEED_MID2);       /* 10 MHz */
    palSetPadMode(MMC_SPI_PORT, MMC_MISO_PAD,
        PAL_MODE_ALTERNATE(5));       /* SPI MISO */
    palSetPadMode(MMC_SPI_PORT, MMC_MOSI_PAD,
        PAL_MODE_ALTERNATE(5) |       /* SPI MOSI */
        PAL_STM32_OTYPE_PUSHPULL |
        PAL_STM32_OSPEED_MID2);       /* 10 MHz */

    mmcObjectInit(&MMCD1);
    mmcStart(&MMCD1, &mmccfg);

}

static msg_t blinkLed(void *arg)
{
  led_config_t *ledConfig = (led_config_t*) arg;

  chRegSetThreadName("Blinker");

  palSetPadMode(ledConfig->port, ledConfig->pin, PAL_MODE_OUTPUT_PUSHPULL);

  while (TRUE) {
    chThdSleepMilliseconds(ledConfig->sleep);
    palTogglePad(ledConfig->port, ledConfig->pin);
  }

  return (msg_t) 0;
}

void servoInit(Servo *servo) {
	/* create the channel configuration */
	PWMChannelConfig chcfg = { PWM_OUTPUT_ACTIVE_HIGH, NULL };
	pwmcfg.channels[servo->pwm_channel] = chcfg;

	/* configure PAL */
	#if defined(STM32F1XX)
		palSetPadMode(servo->port, servo->pin, PAL_MODE_STM32_ALTERNATE_PUSHPULL);
	#else
		#error "PAL configuration for this device not implemented yet - Please modify servoInit()"
	#endif

	/* start the PWM unit */
	pwmStart(servo->pwm_driver, &pwmcfg);
}

/*
 * Application entry point.
 */
int main(void) {
    unsigned i;

    /*
     * 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();

    /*
     * SPI2 I/O pins setup.
     */
    palSetPadMode(GPIOB, 13, PAL_MODE_STM32_ALTERNATE_PUSHPULL);     /* New SCK.     */
    palSetPadMode(GPIOB, 14, PAL_MODE_STM32_ALTERNATE_PUSHPULL);     /* New MISO.    */
    palSetPadMode(GPIOB, 15, PAL_MODE_STM32_ALTERNATE_PUSHPULL);     /* New MOSI.    */
    palSetPadMode(GPIOB, 12, PAL_MODE_OUTPUT_PUSHPULL);              /* New CS.      */

    palSetPadMode(GPIOA, 9, PAL_MODE_STM32_ALTERNATE_PUSHPULL);      /* USART1 TX.       */
    palSetPadMode(GPIOA, 10, PAL_MODE_INPUT);

    sdStart(&SD1, NULL);

    spiAcquireBus(&SPID2);              /* Acquire ownership of the bus.    */
    spiStart(&SPID2, &ls_spicfg);       /* Setup transfer parameters.       */

//  nrf_init();
    nrf_configure_sb_tx();
    nrf_dump_regs(&nrf_reg_def);

// chThdCreateStatic(blinker_wa, sizeof(blinker_wa),
//                    NORMALPRIO-1, blinker, NULL);
    static nrf_payload   p;

    int s;

    // set payload size according to the payload size configured for the RX channel
    p.size = 1;
    p.data[0] = 0;

    while (TRUE) {
        chprintf((BaseSequentialStream *) &SD1, "Sending payload: %x ", p.data[0]);
        s = nrf_send_blocking(&p);
        chprintf((BaseSequentialStream *) &SD1, " - done; bytes send: %u\n\r", s);

        chThdSleepMilliseconds(100);

        p.data[0]++;
    }

    return 0;
}

/*
 * Initialise GPIO ports to handle:
 *	- the CS43L22's I2C channel,
 *	- the CS43L22's I2S channel.
 */
static void _i2s_init_gpio( void )
{
	// Reset pin
	palSetPadMode( I2S_ORESET_PORT , I2S_ORESET_PAD ,
			PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_MID2 );

	// SDL/SDA pins
	_i2s_init_i2c_pad( I2C_OSCL_PORT , I2C_OSCL_PAD );
	_i2s_init_i2c_pad( I2C_OSDA_PORT , I2C_OSDA_PAD );

	// I2S WS/MCK/SCK/SD pins
	_i2s_init_i2s_pad( I2S_OWS_PORT , I2S_OWS_PAD );
	_i2s_init_i2s_pad( I2S_OMCK_PORT , I2S_OMCK_PAD );
	_i2s_init_i2s_pad( I2S_OSCK_PORT , I2S_OSCK_PAD );
	_i2s_init_i2s_pad( I2S_OSD_PORT , I2S_OSD_PAD );
}

/*
 * 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();

  /*
   * Setting up analog input used by the demo (P0.02, SMBA).
   */
  palSetPadMode(IOPORT1, 2, PAL_MODE_INPUT_ANALOG);

  /*
   * Creates the blinker thread.
   */
  chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL);

  /*
   * Activates the ADC1 driver and the temperature sensor.
   */
  adcStart(&ADCD1, NULL);

  /*
   * Linear conversion.
   */
  adcConvert(&ADCD1, &adcgrpcfg1, samples1, ADC_GRP1_BUF_DEPTH);
  chThdSleepMilliseconds(1000);

  /*
   * Starts an ADC continuous conversion.
   */
  adcStartConversion(&ADCD1, &adcgrpcfg2, samples2, ADC_GRP2_BUF_DEPTH);

  /*
   * Normal main() thread activity, in this demo it does nothing.
   */
  while (true) {
    if (palReadPad(IOPORT1, KEY1) == 0)
      adcStopConversion(&ADCD1);
    chThdSleepMilliseconds(500);
  }
}

/*
 * Application entry point.
 */
int main(void) {
  halInit();
  chSysInit();

  /*
   * Serial port initialization.
   */
  sdStart(&SD1, NULL); 
  chprintf((BaseSequentialStream *)&SD1, "Main (SD1 started)\r\n");

   /*
  *Semaphore Initialization
  */
  chSemInit(&mySemaphore, 1);

  /*
   * Set mode of PINES
   */

  palSetPadMode(GPIO0_PORT, GPIO0_PAD, PAL_MODE_INPUT);   //Input T1
  palSetPadMode(GPIO1_PORT, GPIO1_PAD, PAL_MODE_INPUT);   //Input T2
  palSetPadMode(GPIO4_PORT, GPIO4_PAD, PAL_MODE_INPUT);   //Input T3
  palSetPadMode(GPIO17_PORT, GPIO17_PAD, PAL_MODE_INPUT); //Input T4
  
  palSetPadMode(GPIO18_PORT, GPIO18_PAD, PAL_MODE_OUTPUT); //Output T1
  palSetPadMode(GPIO22_PORT, GPIO22_PAD, PAL_MODE_OUTPUT); //Output T2
  palSetPadMode(GPIO23_PORT, GPIO23_PAD, PAL_MODE_OUTPUT); //Output T3
  palSetPadMode(GPIO24_PORT, GPIO24_PAD, PAL_MODE_OUTPUT); //Output T4
  
  /*
   * Creates the blinker thread.
   */
  chThdCreateStatic(waTh1, sizeof(waTh1), NORMALPRIO, Th1, NULL);
  chThdCreateStatic(waTh2, sizeof(waTh2), NORMALPRIO, Th2, NULL);
  chThdCreateStatic(waTh3, sizeof(waTh3), NORMALPRIO, Th3, NULL);
  chThdCreateStatic(waTh4, sizeof(waTh4), NORMALPRIO, Th4, NULL);
  
  /*
   * Events servicing loop.
   */
  chThdWait(chThdSelf());

  return 0;
}

static THD_FUNCTION(ThreadINA, arg)
{
	(void)arg;

	chRegSetThreadName("INA");
	systime_t time = chVTGetSystemTimeX();     // T0

	while (true) {
		time += US2ST(READFREQ);
		//palSetPad(GPIOG, GPIOG_LED4_RED);
		//palTogglePad(GPIOG, GPIOG_LED4_RED);
		busvoltage=ina219GetBusVoltage_V();
		current_mA=ina219GetCurrent_mA();
		shuntvoltage = ina219GetShuntVoltage_mV();
		loadvoltage = busvoltage + (shuntvoltage / 1000);
		milliwatthours += busvoltage*current_mA*READFREQ/1e6/3600; // 1 Wh = 3600 joules
		milliamphours += current_mA*READFREQ/1e6/3600;

		// Update peaks, min and avg during our serial refresh period:
		if (current_mA > rpPeakCurrent)
			rpPeakCurrent = current_mA;
		if (current_mA < rpMinCurrent)
			rpMinCurrent = current_mA;
		if (loadvoltage > rpPeakLoadVolt)
			rpPeakLoadVolt  = loadvoltage;
		if (loadvoltage < rpMinLoadVolt)
			rpMinLoadVolt = loadvoltage;
		rpAvgCurrent = (rpAvgCurrent*rpSamples + current_mA)/(rpSamples+1);
		rpAvgLoadVolt = (rpAvgLoadVolt*rpSamples + loadvoltage)/(rpSamples+1);
		rpSamples++;

		// Update absolute peaks and mins
		if (current_mA > peakCurrent) {
			peakCurrent = current_mA;
			voltageAtPeakCurrent = loadvoltage;
		}

		if (loadvoltage < minVoltage) {
			minVoltage = loadvoltage;palSetPadMode(GPIOA, 5, PAL_MODE_OUTPUT_PUSHPULL); palClearPad(GPIOA, 5);
			currentAtMinVoltage = current_mA;
		}

		//palClearPad(GPIOG, GPIOG_LED4_RED);
		chThdSleepUntil(time);
	}
}

void gps_reset() {
	palSetPadMode(GPIO_GPS_PWR_PORT, GPIO_GPS_PWR_PIN, PAL_MODE_OUTPUT_PUSHPULL);

	//! Set GPS power Off
	palClearPad(GPIO_GPS_PWR_PORT, GPIO_GPS_PWR_PIN);

	chThdSleepMilliseconds(500);

	//! Set GPS power On
	palClearPad(GPIO_GPS_PWR_PORT, GPIO_GPS_PWR_PIN);

	sdStop(&GPS_SERIAL);
	SD3_Config.sc_speed = 9600;
	sdStart(&GPS_SERIAL, &SD3_Config);

	// Wait until initialized
	while (sdReadTimeout(&GPS_SERIAL, gps_data, GPS_CMD_BUF, 100) <= 0) {}

	// Set serial config
	GPS_CMD_SEND("PSRF100,1,38400,8,1,0");
	chThdSleepMilliseconds(100);

	sdStop(&GPS_SERIAL);
	SD3_Config.sc_speed = 38400;
	sdStart(&GPS_SERIAL, &SD3_Config);

	// Disable unneeded
	GPS_CMD_SEND("PSRF103,01,00,00,01");
	chThdSleepMilliseconds(100);
	GPS_CMD_SEND("PSRF103,02,00,00,01");
	chThdSleepMilliseconds(100);
	GPS_CMD_SEND("PSRF103,03,00,00,01");
	chThdSleepMilliseconds(100);
	GPS_CMD_SEND("PSRF103,05,00,00,01");

	// Enable needed
	// GGA
	GPS_CMD_SEND("PSRF103,00,00,01,01");
	chThdSleepMilliseconds(100);
	// RMC
	GPS_CMD_SEND("PSRF103,04,00,01,01");
	chThdSleepMilliseconds(100);

	sdAsynchronousRead(&GPS_SERIAL, gps_data, GPS_CMD_BUF);
}

/**
 * @brief   Configures and activates the SPI peripheral.
 *
 * @param[in] spip      pointer to the @p SPIDriver object
 *
 * @notapi
 */
void spi_lld_start(SPIDriver *spip) {
  uint8_t scbr =
    spip->config->speed == 0 ? 0xFF :
    SystemCoreClock < spip->config->speed ? 0x01 :
    SystemCoreClock / spip->config->speed > 0xFF ? 0xFF :
    SystemCoreClock / spip->config->speed;

  if (spip->state == SPI_STOP) {
    uint32_t irq_priority =
        spip->config->irq_priority ? spip->config->irq_priority :
        SAM3XA_SPI_DEFAULT_IRQ_PRIORITY;
    chDbgCheck(CORTEX_IS_VALID_KERNEL_PRIORITY(irq_priority),
        "SPI irq_priority");

    pmc_enable_peripheral_clock(spip->peripheral_id);
    nvicEnableVector(spip->irq_id, CORTEX_PRIORITY_MASK(irq_priority));

    spip->spi->SPI_CR = SPI_CR_SPIDIS;
    spip->spi->SPI_CR = SPI_CR_SWRST;
    spip->spi->SPI_CR = SPI_CR_SWRST;
    spip->spi->SPI_MR = SPI_MR_MSTR | SPI_MR_MODFDIS;
    spip->spi->SPI_CSR[0] = SPI_CSR_BITS_8_BIT | SPI_CSR_CSAAT |
        SPI_CSR_SCBR(scbr) | (
        spip->config->spi_mode == 1 ? 0 :
        spip->config->spi_mode == 2 ? SPI_CSR_CPOL | SPI_CSR_NCPHA :
        spip->config->spi_mode == 3 ? SPI_CSR_CPOL :
        SPI_CSR_NCPHA);

    peripheral_pin_apply(&spip->config->spck_pin);
    peripheral_pin_apply(&spip->config->miso_pin);
    peripheral_pin_apply(&spip->config->mosi_pin);
    palSetPad(spip->config->cs_pin.port, spip->config->cs_pin.pin);
    palSetPadMode(spip->config->cs_pin.port, spip->config->cs_pin.pin, PAL_MODE_OUTPUT_OPENDRAIN);

    spip->spi->SPI_CR = SPI_CR_SPIEN;
  } else {
    // MMC_SD will reconfigure us without stopping
    spip->spi->SPI_CSR[0] = SPI_CSR_BITS_8_BIT | SPI_CSR_CSAAT |
        SPI_CSR_SCBR(scbr) | (
        spip->config->spi_mode == 1 ? 0 :
        spip->config->spi_mode == 2 ? SPI_CSR_CPOL | SPI_CSR_NCPHA :
        spip->config->spi_mode == 3 ? SPI_CSR_CPOL :
        SPI_CSR_NCPHA);
  }
}

/*
 * LCD set I/Os
 */
void lcd_setio(){

  palSetPadMode(GPIOE, EN, PAL_MODE_OUTPUT_PUSHPULL); //EN
  palSetPadMode(GPIOB, RS, PAL_MODE_OUTPUT_PUSHPULL); //RS

  palSetPadMode(GPIOE, DB4, PAL_MODE_OUTPUT_PUSHPULL); //DB4
  palSetPadMode(GPIOE, DB5, PAL_MODE_OUTPUT_PUSHPULL); //DB5
  palSetPadMode(GPIOE, DB6, PAL_MODE_OUTPUT_PUSHPULL); //DB6
  palSetPadMode(GPIOE, DB7, PAL_MODE_OUTPUT_PUSHPULL); //DB7

  palWritePort(GPIOE, 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();

  palSetPadMode(IOPORT3, BOARD_LED1, PAL_MODE_OUTPUT_PUSHPULL);
  palClearPad(IOPORT3, BOARD_LED1);

  /*
   * Initializes a serial-over-USB CDC driver.
   */
  sduObjectInit(&SDU1);
  sduStart(&SDU1, &serusbcfg);

  /*
   * Activates the USB driver and then the USB bus pull-up on D+.
   * Note, a delay is inserted in order to not have to disconnect the cable
   * after a reset.
   */
  usbDisconnectBus(serusbcfg.usbp);
  chThdSleepMilliseconds(1000);
  usbStart(serusbcfg.usbp, &usbcfg);
  usbConnectBus(serusbcfg.usbp);

  /*
   * Starts the LED blinker thread.
   */
  chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL);

  while (true) {
    if (SDU1.config->usbp->state == USB_ACTIVE) {
      chnWrite(&SDU1, (const uint8_t *)"Hello from Arduino Leonardo!\r\n", 30);
    }
    chThdSleepMilliseconds(2113);
  }
}

/*
 * 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();

  /*
   * Starting DAC1 driver, setting up the output pin as analog as suggested
   * by the Reference Manual.
   */
  palSetPadMode(GPIOA, 4, PAL_MODE_INPUT_ANALOG);
  dacStart(&DACD1, &dac1cfg1);

  /*
   * Starting GPT6 driver, it is used for triggering the DAC.
   */
  gptStart(&GPTD6, &gpt6cfg1);

  /*
   * Starting a continuous conversion.
   */
  dacStartConversion(&DACD1, &dacgrpcfg1, dac_buffer, DAC_BUFFER_SIZE);
  gptStartContinuous(&GPTD6, 2U);

  /*
   * Normal main() thread activity, if the button is pressed then the DAC
   * transfer is stopped.
   */
  while (true) {
    if (palReadPad(GPIOA, GPIOA_BUTTON)) {
      gptStopTimer(&GPTD6);
      dacStopConversion(&DACD1);
    }
    chThdSleepMilliseconds(500);
  }
  return 0;
}

void BoardDriverStart(void)
{
	canStart(&CAND1, &cancfg);

	/*CAN1 RX and TX*/
	palSetPadMode(GPIOB, 8, PAL_MODE_STM32_ALTERNATE_PUSHPULL);
	palSetPadMode(GPIOB, 9, PAL_MODE_STM32_ALTERNATE_PUSHPULL);

	sdStart(&SD2, &serialConfig);
	palSetPadMode(GPIOA, 0, PAL_MODE_STM32_ALTERNATE_PUSHPULL);
    palSetPadMode(GPIOA, 1, PAL_MODE_STM32_ALTERNATE_PUSHPULL);
	palSetPadMode(GPIOA, 2, PAL_MODE_STM32_ALTERNATE_PUSHPULL);
    palSetPadMode(GPIOA, 3, PAL_MODE_STM32_ALTERNATE_PUSHPULL);
}

void kirim_setup(void){

sdStart(&SD2,NULL);  
  palSetPadMode(GPIOB,3, PAL_MODE_ALTERNATE(7)); //TX
  palSetPadMode(GPIOB,4, PAL_MODE_ALTERNATE(7)); //RX

  sdStart(&SD1,NULL);
    palSetPadMode(GPIOA,9, PAL_MODE_ALTERNATE(7)); //TX
    palSetPadMode(GPIOA,10, PAL_MODE_ALTERNATE(7)); //RX

    sdStart(&SD3,NULL);
      palSetPadMode(GPIOB,10, PAL_MODE_ALTERNATE(7)); //TX
      palSetPadMode(GPIOE,15, PAL_MODE_ALTERNATE(7)); //RX
};