void gps_start()
{
GPS_UART_PWR_ON;
gps_uart.Init(GPS_UART, 9600);
gps_uart.SetInterruptPriority(MEDIUM);
GpioSetDirection(GPS_EN, OUTPUT); //active high
GpioWrite(GPS_EN, LOW);
GpioSetDirection(GPS_TIMER, INPUT); //active low, otherwise open-drain
GpioSetPull(GPS_TIMER, gpio_pull_up);
GpioSetInterrupt(GPS_TIMER, gpio_interrupt1, gpio_falling);
GpioSetDirection(GPS_RESET, OUTPUT); //active low
GpioWrite(GPS_RESET, LOW);
GpioWrite(GPS_EN, HIGH);
_delay_ms(10);
GpioWrite(GPS_RESET, LOW);
_delay_ms(20);
GpioWrite(GPS_RESET, HIGH);
gps_parser_state = GPS_IDLE;
fc.gps_data.valid = false;
fc.gps_data.fix = 0;
fc.gps_data.fix_cnt = 0;
for (uint8_t i = 0; i < GPS_SAT_CNT; i++)
{
fc.gps_data.sat_id[i] = 0;
fc.gps_data.sat_snr[i] = 0;
}
}
void bt_unknown_parser()
{
while (!bt_uart.isRxBufferEmpty())
{
uint8_t c = bt_uart.Read();
DEBUG(">> %02X %c ??\n", c, c);
}
}
void bt_step()
{
if (bt_module_state == BT_MOD_STATE_OFF)
return;
if (bt_reset_counter)
{
if (bt_reset_counter > task_get_ms_tick())
return;
DEBUG("BT RESET STEP: %d\n", bt_reset_counter_step);
switch(bt_reset_counter_step)
{
case(0):
GpioWrite(BT_EN, HIGH);
bt_reset_counter = task_get_ms_tick() + 500;
bt_reset_counter_step = 1;
break;
case(1):
//enable bt uart
BT_UART_PWR_ON;
bt_uart.Init(BT_UART, 115200);
bt_uart.SetInterruptPriority(MEDIUM);
// while(1)
// {
// _delay_ms(100);
// bt_uart.Write(0x00);
// _delay_ms(1);
// bt_uart.Write(0xAA);
// ewdt_reset();
// }
bt_reset_counter = task_get_ms_tick() + 10;
bt_reset_counter_step = 2;
break;
case(2):
GpioWrite(BT_RESET, HIGH);
bt_reset_counter_step = 0;
bt_reset_counter = 0;
break;
}
return;
}
if (bt_module_type == BT_PAN1322)
bt_pan1322.Step();
if (bt_module_type == BT_PAN1026)
bt_pan1026.Step();
if (bt_module_type == BT_UNKNOWN)
bt_unknown_parser();
}
void uart_low_speed()
{
//enable usart
DEBUG_UART_PWR_ON;
//init uart
uart.Init(DEBUG_UART, 9600);
uart.SetInterruptPriority(HIGH);
// uart.dbg = true;
SetStdIO(uart_in, uart_out);
}
void gps_set_baudrate()
{
DEBUG("set_baudrate\n");
fprintf_P(gps_out, PSTR("$PMTK251,115200*1F\r\n"));
gps_uart.FlushTxBuffer();
_delay_ms(1);
}
void bt_init()
{
DEBUG("bt_init\n");
//get module type
eeprom_busy_wait();
bt_module_type = eeprom_read_byte(&config_ro.bt_module_type);
//init bt_uart
bt_uart.InitBuffers(BUFFER_SIZE * 2, BUFFER_SIZE);
//pin init
GpioSetDirection(BT_EN, OUTPUT);
GpioSetDirection(BT_RESET, OUTPUT);
GpioSetDirection(BT_RTS, OUTPUT);
//power is off
GpioWrite(BT_RTS, LOW);
GpioWrite(BT_EN, LOW);
GpioWrite(BT_RESET, LOW);
//IRQ init
GpioSetDirection(BT_CTS, INPUT);
GpioSetPull(BT_CTS, gpio_pull_down);
GpioSetInterrupt(BT_CTS, gpio_interrupt1, gpio_rising);
}
void uart_stop()
{
uart.Stop();
//disable usart
DEBUG_UART_PWR_OFF;
}
void bt_module_deinit()
{
GpioWrite(BT_EN, LOW);
GpioWrite(BT_RESET, LOW);
bt_irgh(BT_IRQ_DEINIT, 0);
bt_uart.Stop();
BT_UART_PWR_OFF;
}
void write_time(Usart& uart, DS3231 &rtc)
{
uint8_t buffer[8] = {0};
rtc.readRaw(buffer, 8);
write_digits(uart, buffer[3]);
uart.write(':');
write_digits(uart, buffer[2]);
uart.write(':');
write_digits(uart, buffer[1]);
uart.write(' ');
write_digits(uart, buffer[5]);
uart.write('.');
write_digits(uart, buffer[6]);
uart.write(".20");
write_digits(uart, buffer[7]);
}
void uart_send(char * msg)
{
char * ptr = msg;
while (*ptr != 0)
{
uart.Write(*ptr);
ptr++;
}
}
void bt_module_reset()
{
GpioWrite(BT_EN, LOW);
GpioWrite(BT_RESET, LOW);
bt_uart.Stop();
BT_UART_PWR_OFF;
bt_reset_counter = task_get_ms_tick() + 4000;
bt_reset_counter_step = 0;
}
int main(void)
{
leds.initHW();
NVIC_SetPriorityGrouping( NVIC_PriorityGroup_4 );
leds.write( 0x0F ); delay_bad_ms( 200 );
leds.write( 0x0A ); delay_bad_ms( 200 );
leds.reset( 0x0F ); delay_bad_ms( 200 );
xTaskCreate( task_leds, "leds", 2*def_stksz, 0, 1, 0 );
xTaskCreate( task_usart2_send, "send", 2*def_stksz, 0, 1, 0 );
xTaskCreate( task_usart2_recv, "recv", 2*def_stksz, 0, 1, 0 );
xTaskCreate( task_string_send, "ss", def_stksz, 0, 1, 0 );
us2.initIRQ( configKERNEL_INTERRUPT_PRIORITY, 0 );
us2.initHW();
us2.init();
us2.itConfig( USART_IT_RXNE, ENABLE );
us2.setOnRecv( on_received_char );
us2.enable();
vTaskStartScheduler();
die4led( 0xFF );
return 0;
}
void gps_stop()
{
GpioSetDirection(GPS_EN, INPUT);
GpioSetDirection(GPS_RESET, INPUT);
fc.gps_data.valid = false;
fc.gps_data.fix = 0;
fc.gps_data.fix_cnt = 0;
GpioSetPull(GPS_TIMER, gpio_totem);
gps_init_ok = false;
gps_uart.Stop();
GPS_UART_PWR_OFF;
}
void storage_deinit()
{
DEBUG("storage_deinit\n");
if (!sd_avalible)
return;
uint8_t res;
res = f_mount(NULL, "", 1); //unmount
sd_spi_usart.Stop();
sd_avalible = false;
//power spi & sdcard
SD_EN_OFF;
SD_SPI_PWR_OFF;
}
void uart_init()
{
//enable usart
DEBUG_UART_PWR_ON;
//init uart
switch (config.connectivity.uart_function)
{
case(UART_FORWARD_DEBUG):
uart.Init(DEBUG_UART, 921600ul);
break;
case(UART_FORWARD_OFF):
DEBUG_UART_PWR_OFF;
return;
break;
case(UART_FORWARD_9600):
uart.Init(DEBUG_UART, 9600ul);
break;
case(UART_FORWARD_19200):
uart.Init(DEBUG_UART, 19200ul);
break;
case(UART_FORWARD_38400):
uart.Init(DEBUG_UART, 38400ul);
break;
case(UART_FORWARD_57600):
uart.Init(DEBUG_UART, 57600ul);
break;
case(UART_FORWARD_115200):
uart.Init(DEBUG_UART, 115200ul);
break;
}
uart.SetInterruptPriority(HIGH);
// uart.dbg = true;
SetStdIO(uart_in, uart_out);
}
void write_temp(Usart& uart, const uint8_t temp)
{
write_decimal(uart, temp);
uart.write('C');
}
void write_decimal(Usart& uart, const uint32_t dec)
{
char buffer[6] = {0};
ltoa(dec, buffer, 10);
uart.write(buffer);
}
bool storage_init()
{
uint8_t res;
GpioSetPull(SD_IN, gpio_pull_up);
GpioSetDirection(SD_IN, INPUT);
DEBUG("SD_IN %d\n", GpioRead(SD_IN));
if (!SD_CARD_DETECT)
return false;
//power spi & sdcard
SD_EN_ON;
SD_SPI_PWR_ON;
DEBUG("Mounting SD card ... ");
for (uint8_t i = 0; i < 5; i++)
{
//power spi & sdcard
SD_EN_ON;
SD_SPI_PWR_ON;
res = f_mount(&FatFs, "", 1); /* Give a work area to the default drive */
DEBUG("%d ", i + 1);
if (res == RES_OK)
break;
sd_spi_usart.Stop();
//power spi & sdcard
SD_EN_OFF;
SD_SPI_PWR_OFF;
for (uint8_t j = 0; j < i +1; j++)
_delay_ms(10);
}
if (res != RES_OK)
{
DEBUG("Error %02X\n", res);
sd_spi_usart.Stop();
//power spi & sdcard
SD_EN_OFF;
SD_SPI_PWR_OFF;
sd_avalible = false;
return false;
}
DEBUG("OK\n");
uint32_t size;
FATFS * FatFs1;
res = f_getfree("", &size, &FatFs1);
// DEBUG1("f_getfree res = %d, size = %lu MiB", res, size / 256);
uint32_t sector_count;
res = disk_ioctl(0, GET_SECTOR_COUNT, §or_count);
// DEBUG1("GET_SECTOR_COUNT res = %d, size = %lu", res, sector_count);
uint16_t sector_size;
res = disk_ioctl(0, GET_SECTOR_SIZE, §or_size);
// DEBUG1("GET_SECTOR_SIZE res = %d, size = %u", res, sector_size);
storage_space = sector_count * sector_size;
storage_free_space = size * 4 * 1024;
DEBUG("Disk info\n");
DEBUG(" sector size %12u\n", sector_size);
DEBUG(" sector count %12lu\n", sector_count);
DEBUG(" total space %12lu\n", storage_space);
DEBUG(" free space %12lu\n", storage_free_space);
sd_avalible = true;
return true;
}
void uart_init_buffers()
{
uart.InitBuffers(0, BUFFER_SIZE);
}
void gps_change_uart_baudrate()
{
gps_uart.Stop();
gps_uart.Init(GPS_UART, 115200);
}
void gps_init()
{
DEBUG("gps init\n");
gps_uart.InitBuffers(250, 40);
}
void gps_step()
{
while (!gps_uart.isRxBufferEmpty())
gps_parse(&gps_uart);
}
void USART1_IRQHandler(void)
{
usart1.irq();
}
int main(void)
{
NVIC_SetPriorityGrouping(3);
// enable all GPIO clocks
RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN;
RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN;
RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN;
RCC->AHB1ENR |= RCC_AHB1ENR_GPIODEN;
RCC->AHB1ENR |= RCC_AHB1ENR_GPIOEEN;
top_r_led::mode(GPIO_OUTPUT);
top_g_led::mode(GPIO_OUTPUT);
top_b_led::mode(GPIO_OUTPUT);
bot_r_led::mode(GPIO_OUTPUT);
bot_g_led::mode(GPIO_OUTPUT);
bot_b_led::mode(GPIO_OUTPUT);
top_r_led::high();
top_g_led::high();
top_b_led::low();
bot_r_led::low();
bot_g_led::high();
bot_b_led::high();
// setup encoders
RCC->APB1ENR |= RCC_APB1ENR_TIM3EN | RCC_APB1ENR_TIM5EN;
left_enc_a::mode(GPIO_ALTERNATE | GPIO_AF_TIM5);
left_enc_b::mode(GPIO_ALTERNATE | GPIO_AF_TIM5);
right_enc_a::mode(GPIO_ALTERNATE | GPIO_AF_TIM3);
right_enc_b::mode(GPIO_ALTERNATE | GPIO_AF_TIM3);
left_enc.init();
right_enc.init();
// setup motors
RCC->APB2ENR |= RCC_APB2ENR_TIM1EN;
RCC->APB1ENR |= RCC_APB1ENR_SPI2EN;
tim1_ch1::mode(GPIO_ALTERNATE | GPIO_AF_TIM1);
tim1_ch1n::mode(GPIO_ALTERNATE | GPIO_AF_TIM1);
tim1_ch2::mode(GPIO_ALTERNATE | GPIO_AF_TIM1);
tim1_ch2n::mode(GPIO_ALTERNATE | GPIO_AF_TIM1);
sck::mode(GPIO_ALTERNATE | GPIO_AF_SPI2);
miso::mode(GPIO_ALTERNATE | GPIO_AF_SPI2);
mosi::mode(GPIO_ALTERNATE | GPIO_AF_SPI2);
left_motor.init();
right_motor.init();
// setup usart
RCC->APB2ENR |= RCC_APB2ENR_USART1EN;
usart1.init(115200);
NVIC_SetPriority(USART1_IRQn, 1);
NVIC_EnableIRQ(USART1_IRQn);
// setup imu
imu.init(100000);
// setup systick
SysTick_Config(SystemCoreClock/1000);
system_clock = 0;
__enable_irq();
while(1)
{
imu.update(system_clock);
}
}
