static uint32_t read_from_serial(SerialDriver* driver, uint8_t link) {
const uint32_t buffer_size = 16;
uint8_t buffer[buffer_size];
uint32_t bytes_read = sdAsynchronousRead(driver, buffer, buffer_size);
uint8_t* current = buffer;
uint8_t* end = current + bytes_read;
while(current < end) {
byte_stuffer_recv_byte(link, *current);
current++;
}
return bytes_read;
}
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);
}
