bool fiveBaudInit(SerialDriver *sd)
{
uint8_t input_buf[3];
size_t bytes;
// Set pin mode to GPIO
palSetPadMode(PORT_KLINE_TX, PAD_KLINE_RX, PAL_MODE_INPUT_ANALOG);
palSetPadMode(PORT_KLINE_TX, PAD_KLINE_TX, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
const uint8_t prio = chThdGetPriorityX();
chThdSetPriority(HIGHPRIO);
K_HIGH(320); // As per ISO standard
// Send 0x33 at 5 baud (00110011)
// K-line level: |_--__--__-|
K_LOW(200); // Low for 200ms - start bit
K_HIGH(400); // High for 400ms - 00
K_LOW(400); // Low for 400ms - 11
K_HIGH(400); // High for 400ms - 00
K_LOW(400); // Low for 400ms - 11
K_HIGH(200); // High for 200ms - stop bit
// Set pin mode back to UART
palSetPadMode(PORT_KLINE_TX, PAD_KLINE_TX, PAL_MODE_ALTERNATE(7) | \
PAL_STM32_OSPEED_HIGHEST | PAL_STM32_OTYPE_OPENDRAIN | PAL_STM32_PUPDR_PULLUP);
palSetPadMode(PORT_KLINE_TX, PAD_KLINE_RX, PAL_MODE_ALTERNATE(7) | \
PAL_STM32_OTYPE_OPENDRAIN);
chThdSetPriority(prio); // Revert back original priority
chThdSleepMilliseconds(25);
bytes = sdReadTimeout(sd, input_buf, sizeof(input_buf), MS2ST(500)); // 300ms max according to ISO9141
if (bytes != 3 || input_buf[0] != 0x55)
{
return false;
}
chThdSleepMilliseconds(35); // 25-50 ms pause per ISO standard
uint8_t key = input_buf[2] ^ 0xFF; // Invert key byte
sdWriteTimeout(sd, &key, 1, MS2ST(100));
chThdSleepMilliseconds(35); // 25-50 ms pause per ISO standard
bytes = sdReadTimeout(sd, input_buf, 1, MS2ST(100));
if (bytes != 1 || input_buf[0] != 0xCC)
{
return false;
}
return true;
}
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);
}
CCM_FUNC static THD_FUNCTION(ThreadSER1, arg)
{
(void)arg;
uint8_t buffer[SERIAL_BUFFERS_SIZE/2];
size_t read;
chRegSetThreadName("MTS");
while(SD1.state != SD_READY) chThdSleepMilliseconds(10);
while (TRUE) {
read = sdReadTimeout(&SD1, buffer, 6, MS2ST(40));
if (read >= 6)
{
readMtsPackets(buffer);
}
}
return;
}
static int detect_geos(void)
{
int i, j;
uint8_t *gnss_buffer = chHeapAlloc(NULL, 1024);
int detected = 0;
palSetPad(IOPORT1, PIOA_GPS_NRST);
chThdSleepMilliseconds(100);
k2_usart1_geos();
for (i = 0; i < 20; i++) {
int t = sdReadTimeout(&SD1, gnss_buffer, 1024, 250);
if (t > 0) {
for (j = 0; j < t; j++) {
if (packet_header(gnss_buffer[j],
(uint8_t *) "PSGG", 4))
detected = 1;
}
}
}
chHeapFree(gnss_buffer);
return detected;
}
/**
* @brief Sync procedure.
* @details This function implements the synchronization protocol
* between the STM and ESP/RPi
*
* @param[in] driver pointer to the DataLinkLayer driver object
*/
void DLLSyncProcedure(DLLDriver *driver)
{
int FFs = 0;
char c;
DLLSendSyncFrame(driver);
driver->DLLStats.SyncCounter++;
while(FFs != FRAME_SIZE_BYTE)
{
driver->DLLStats.SyncTimeout++;
sdReadTimeout(driver->config->SDriver, &c, 1, US2ST(1000));
if(c == 0xFF)
{
FFs++;
c = 0x00;
}
if(driver->DLLStats.SyncTimeout >= SYNC_TIMEOUT_THRS){
DLLSendSyncFrame(driver);
driver->DLLStats.SyncTimeout = 0;
}
}
driver->DLLStats.SyncTimeout = 0;
return;
}
uint8_t gps_read_msg(size_t *msg_len) {
size_t bytes_readed, msg_readed = 0;
uint8_t correct_byte_count = 0, incorrect_byte_count = 0;
uint8_t is_reading = 1;
uint8_t readed_byte;
// Search for start symbol
while (is_reading) {
bytes_readed = sdReadTimeout(&GPS_SERIAL, &readed_byte, 1,
correct_byte_count == 0 ? GPS_READ_TIMEOUT_TICS : GPS_READ_MSG_TIMEOUT_TICS);
if (bytes_readed <= 0) {
return E_READ_TIMEOUT;
}
switch (correct_byte_count) {
case 0:
if (readed_byte == '$') {
correct_byte_count++;
}
if (incorrect_byte_count++ > 128)
return E_INVALID_DATA;
break;
case 1:
if (readed_byte != 'G') {
return E_INVALID_DATA;
}
correct_byte_count++;
break;
case 2:
if (readed_byte != 'P') {
return E_INVALID_DATA;
}
correct_byte_count = 0;
is_reading = 0;
break;
}
}
// Если мы здесь, значит $GP мы уже получили
is_reading = 1;
while (is_reading) {
bytes_readed = sdReadTimeout(&GPS_SERIAL, &readed_byte, 1, GPS_READ_MSG_TIMEOUT_TICS);
if (bytes_readed <= 0)
return E_READ_TIMEOUT;
gps_data[msg_readed++] = readed_byte;
// If we read the end of the packet
if (readed_byte == '*') {
bytes_readed = sdReadTimeout(&GPS_SERIAL, gps_data + msg_readed, 2, GPS_READ_MSG_TIMEOUT_TICS);
if (bytes_readed != 2) {
return E_READ_TIMEOUT;
}
msg_readed += 2;
break;
}
}
*msg_len = msg_readed;
return E_OK;
}
CCM_FUNC static THD_FUNCTION(ThreadSDU, arg)
{
(void)arg;
uint8_t in_buffer[SERIAL_BUFFERS_SIZE];
uint8_t out_buffer[SERIAL_BUFFERS_SIZE];
//uint8_t buffer_check[SERIAL_BUFFERS_SIZE/2];
size_t in, out;
thread_t* th_shell = NULL;
chRegSetThreadName("SDU");
while(USBD1.state != USB_READY) chThdSleepMilliseconds(10);
while(SDU1.state != SDU_READY) chThdSleepMilliseconds(10);
while(SD3.state != SD_READY) chThdSleepMilliseconds(10);
// Enable K-line transceiver
palSetPad(PORT_KLINE_CS, PAD_KLINE_CS);
shellInit();
while (TRUE) {
if (settings.serialMode == SERIAL_MODE_SHELL) {
if (th_shell == NULL || chThdTerminatedX(th_shell)) {
th_shell = shellCreateStatic(&shell_cfg1, waThreadShell, sizeof(waThreadShell), NORMALPRIO +1);
}
chThdSleepMilliseconds(10);
continue;
}
if (settings.serialMode != SERIAL_MODE_KLINE) {
chThdSleepMilliseconds(10);
continue;
}
/* In case we stop it to change baudrate */
while (SD3.state != SD_READY) chThdSleepMilliseconds(10);
if (doKLineInit && 0)
{
sdStop(&SD3);
klineInit(false);
//fiveBaudInit(&SD3);
//sdReadTimeout(&SD3, buffer_check, 1, MS2ST(5)); // noise
doKLineInit = false;
sdStart(&SD3, &uart1Cfg);
}
in = chnReadTimeout(&SDU1, in_buffer, sizeof(in_buffer), TIME_IMMEDIATE);
out = sdReadTimeout(&SD3, out_buffer, sizeof(out_buffer), TIME_IMMEDIATE);
while (in == 0 && out == 0) {
chThdSleepMilliseconds(1);
in = chnReadTimeout(&SDU1, in_buffer, sizeof(in_buffer), TIME_IMMEDIATE);
out = sdReadTimeout(&SD3, out_buffer, sizeof(out_buffer), TIME_IMMEDIATE);
}
if (in > 0)
{
sdWriteTimeout(&SD3, in_buffer, in, MS2ST(10));
}
if (out > 0)
{
chnWriteTimeout(&SDU1, out_buffer, out, MS2ST(10));
}
}
return;
}
static void cmd_loop(BaseSequentialStream *chp, int argc, char *argv[]) {
uint32_t loop_times=1;
uint32_t loop_delay_ms=100;
uint8_t cmd_match=0;
uint8_t is_infinite=0;
int pass_argc=0;
char *pass_argv[0];
if(argc > 0){
uint8_t cmd_i;
for(cmd_i=0;cmd_i<cmd_name_strings_len;cmd_i++){
if(strcmp(cmd_name_strings[cmd_i],argv[0])==0){
cmd_match++;
break;
}
}
if(cmd_match==1){
if(argc > 1){
if(strcmp("-l", argv[1])==0){
if(argc > 2){
uint8_t is_valid_num = str_is_valid_num(argv[2]);
is_infinite = (strcmp("-i",argv[2])==0);
if(is_valid_num || is_infinite){
if(is_valid_num){loop_times = atol(argv[2]);}
if(argc > 3){
if(strcmp("-s", argv[3])==0){
if(argc > 4){
if(str_is_valid_num(argv[4])){
loop_delay_ms = atol(argv[4]);
} else {
chprintf(chp,invalid_num_str,argv[4]);
}
} else {
chprintf(chp,"please enter millisecond delay\n\r");
}
} else {
chprintf(chp,invalid_arg_str,argv[3]);
}
}
} else {
chprintf(chp,invalid_num_str, argv[2]);
}
} else {
chprintf(chp, "please enter a number of times to loop\n\r");
}
} else if(strcmp("-h",argv[1])==0){
//chprintf(chp, "printing help:\n\r");
chprintf(chp, loop_usage);
} else {
chprintf(chp, invalid_arg_str, argv[1]);
chprintf(chp, loop_usage);
}
}
chprintf(chp, "looping %s command %U times with delay of %U milliseconds:\n\r"
"looping can be halted by pressing CTRL+C or 'q' during operation\n\r",
argv[0],loop_times,loop_delay_ms);
char c;
while(loop_times--){
call_cmd_from_index(chp,pass_argc,pass_argv,cmd_i);
chThdSleepMilliseconds(loop_delay_ms);
if(sdReadTimeout((SerialDriver *)chp,(uint8_t *)&c,1,TIME_IMMEDIATE)!=0){
if((c==3)||(c=='q')){break;} //checks if 'q' or CTRL+C (ascii dec 3) have been input
}
if(is_infinite){loop_times++;}
}
} else if(strcmp("-h",argv[0])==0){
chprintf(chp, loop_usage);
} else {
chprintf(chp, "%s is not a designated command!\n\r", argv[0]);
chprintf(chp, loop_usage);
}
} else {
chprintf(chp, "please enter a cmd to loop\n\r");
chprintf(chp, loop_usage);
}
}
