kopia lustrzana https://github.com/sq2ips/m20-custom-firmware
rewrite of nmea.c for fixing errors and saving memory
sq2ips
rodzic
573dadfdf2
commit
05c49e3000
|
|
@ -8,13 +8,35 @@
|
|||
"servertype": "openocd",
|
||||
"cwd": "${workspaceFolder}",
|
||||
"runToEntryPoint": "main",
|
||||
"executable": "./build/m20.elf",
|
||||
"executable": "./m20/build/m20.elf",
|
||||
"device": "STM32L051R6T6",
|
||||
"configFiles": [
|
||||
"debug/stlink.cfg",
|
||||
"debug/stm32l0.cfg"
|
||||
"./m20/openocd/openocd_m20.cfg"
|
||||
],
|
||||
"serverArgs": [
|
||||
"-s",
|
||||
"./m20/openocd/"
|
||||
],
|
||||
"showDevDebugOutput": "raw"
|
||||
},
|
||||
},
|
||||
{
|
||||
"name": "C/C++ Runner: Debug Session",
|
||||
"type": "cppdbg",
|
||||
"request": "launch",
|
||||
"args": [],
|
||||
"stopAtEntry": false,
|
||||
"externalConsole": false,
|
||||
"cwd": "/home/pawel/coding/m20-custom-firmware",
|
||||
"program": "/home/pawel/coding/m20-custom-firmware/build/Debug/outDebug",
|
||||
"MIMode": "gdb",
|
||||
"miDebuggerPath": "gdb",
|
||||
"setupCommands": [
|
||||
{
|
||||
"description": "Enable pretty-printing for gdb",
|
||||
"text": "-enable-pretty-printing",
|
||||
"ignoreFailures": true
|
||||
}
|
||||
]
|
||||
}
|
||||
]
|
||||
}
|
||||
|
|
@ -8,16 +8,16 @@
|
|||
/*-----------------------------------------------------------------*/
|
||||
// Sonde configuration, parameters that should be changed
|
||||
|
||||
#define PAYLOAD_ID 256 // Sonde payload ID 256 - for 4FSKTEST-V2
|
||||
#define PAYLOAD_ID 256 // Sonde payload ID 256 - for 4FSKTEST-V2
|
||||
|
||||
#define TIME_PERIOD 6 // Time betwen starts of transmissions (in seconds) (must be more than 3)
|
||||
#define TIME_PERIOD 6 // Time betwen starts of transmissions (in seconds) (must be more than 3)
|
||||
|
||||
#define GPS_TYPE 1 // Type of GPS module: 1 - u-blox | 2 - XM1110
|
||||
#define GPS_TYPE 1 // Type of GPS module: 1 - u-blox | 2 - XM1110
|
||||
|
||||
#define QRG_FSK4 435100000 // Frequency fo horus modulation (in Hz)
|
||||
#define QRG_FSK4 435100000 // Frequency fo horus modulation (in Hz)
|
||||
|
||||
#define PA_FSK4 10 // RF power setting for horus transmission values 0-63
|
||||
#define RF_BOOST_ACTIVE 1 // RF booster enabled for transmissions about 15dB gain, but more power consumed - normally should be ON(1).
|
||||
#define PA_FSK4 10 // RF power setting for horus transmission values 0-63
|
||||
#define RF_BOOST_ACTIVE 0 // RF booster enabled for transmissions about 15dB gain, but more power consumed - normally should be ON(1).
|
||||
|
||||
#define ADF_FREQ_CORRECTION 19 // correction of frequency from crystal inaccuracy in 270Hz steps. To be individually set for each sonde.
|
||||
|
||||
|
|
@ -40,7 +40,10 @@
|
|||
#define AscentRateTime 10 // Time of ascent rate mesure
|
||||
|
||||
// type 1
|
||||
#define DATA_SIZE 35 // Max number of NMEA sentences in one parsing
|
||||
#define DATA_SIZE 35 // Max number of NMEA sentences in one parsing
|
||||
#define SENTENCE_SIZE 82+1 // Max lenght of a NMEA sentence is 82 characters
|
||||
#define MAX_SENTENCE_ELEMENTS 10 // Max number of NMEA sentence elements (no element with number bigger than 9 is used)
|
||||
#define SENTENCE_ELEMENT_LEN 12 // Max lenght of a sentence element
|
||||
|
||||
// type 2
|
||||
#define FrameLen 62 // Length of XM1110 frame
|
||||
|
|
|
|||
|
|
@ -13,13 +13,13 @@
|
|||
typedef struct NMEA_DATA {
|
||||
float Lat; //latitude in degrees with decimal places + for N - for S
|
||||
float Lon; //longitude in degrees with decimal places
|
||||
float Alt; //altitude in meters
|
||||
float Speed;
|
||||
uint16_t Alt; //altitude in meters
|
||||
uint16_t Speed;
|
||||
float AscentRate;
|
||||
uint8_t Hours;
|
||||
uint8_t Minutes;
|
||||
uint8_t Seconds;
|
||||
float HDOP; //horizontal dilution of precision
|
||||
//float HDOP; //horizontal dilution of precision
|
||||
uint8_t Sats; //number of satellites used in measurement
|
||||
uint8_t Fix; // 0 = no data, 1 = no fix, 2 = 2D fix, 3 = 3D fix
|
||||
uint8_t Corr; // number of correct frames
|
||||
|
|
|
|||
|
|
@ -109,7 +109,7 @@ void main_loop(void){
|
|||
// LED
|
||||
LL_GPIO_TogglePin(LED_GPIO_Port, LED_Pin);
|
||||
#ifdef DEBUG
|
||||
//printf("%s", GpsDataBuffer);
|
||||
printf("Frame: %d\r\n", HorusPacket.PacketCount);
|
||||
#endif
|
||||
while(GpsBufferReady){}
|
||||
|
||||
|
|
@ -131,13 +131,13 @@ void main_loop(void){
|
|||
HorusPacket.Seconds = NmeaData.Seconds;
|
||||
HorusPacket.Lat = NmeaData.Lat;
|
||||
HorusPacket.Lon = NmeaData.Lon;
|
||||
HorusPacket.Speed = (uint8_t)round(NmeaData.Speed);
|
||||
HorusPacket.Speed = (uint8_t)NmeaData.Speed;
|
||||
HorusPacket.AscentRate = (int16_t)round(NmeaData.AscentRate*100);
|
||||
HorusPacket.Alt = (uint16_t)round(NmeaData.Alt);
|
||||
HorusPacket.Alt = NmeaData.Alt;
|
||||
HorusPacket.Sats = NmeaData.Sats;
|
||||
#ifdef DEBUG
|
||||
printf("\r\nFix: %d, Lat: %d, Lon: %d, Alt: %d m, Speed: %d km/h, Ascent rate: %d m/s Satellites: %d HDOP: %d, Time: %d:%d:%d, correct frames: %d\r\n",
|
||||
NmeaData.Fix, (uint32_t)(NmeaData.Lat*10e6), (uint32_t)(NmeaData.Lon*10e6), (uint16_t)(NmeaData.Alt*10), (uint16_t)(NmeaData.Speed*10e3), (int16_t)round(NmeaData.AscentRate), NmeaData.Sats, (uint16_t)round(NmeaData.HDOP*1e2), NmeaData.Hours, NmeaData.Minutes, NmeaData.Seconds, NmeaData.Corr);
|
||||
printf("\r\nFix: %d, Lat: %d, Lon: %d, Alt: %d m, Speed: %d km/h, Ascent rate: %d m/s Satellites: %d, Time: %d:%d:%d, correct frames: %d\r\n",
|
||||
NmeaData.Fix, (uint32_t)(NmeaData.Lat*10e6), (uint32_t)(NmeaData.Lon*10e6), NmeaData.Alt, NmeaData.Speed, (int16_t)round(NmeaData.AscentRate*100), NmeaData.Sats, NmeaData.Hours, NmeaData.Minutes, NmeaData.Seconds, NmeaData.Corr);
|
||||
#endif
|
||||
|
||||
// GPS type 2 data
|
||||
|
|
@ -255,7 +255,7 @@ int main(void)
|
|||
}
|
||||
GpsBufferReady = false;
|
||||
}
|
||||
LL_mDelay(20);
|
||||
LL_mDelay(10);
|
||||
}
|
||||
/* USER CODE END 3 */
|
||||
}
|
||||
|
|
@ -409,7 +409,7 @@ static void MX_IWDG_Init(void)
|
|||
/* USER CODE END IWDG_Init 1 */
|
||||
LL_IWDG_Enable(IWDG);
|
||||
LL_IWDG_EnableWriteAccess(IWDG);
|
||||
LL_IWDG_SetPrescaler(IWDG, LL_IWDG_PRESCALER_4);
|
||||
LL_IWDG_SetPrescaler(IWDG, LL_IWDG_PRESCALER_8);
|
||||
LL_IWDG_SetReloadCounter(IWDG, 4095);
|
||||
while (LL_IWDG_IsReady(IWDG) != 1)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -7,14 +7,34 @@
|
|||
#include "main.h"
|
||||
#include "config.h"
|
||||
|
||||
#include <stdlib.h>
|
||||
#ifdef DEBUG
|
||||
#include <stdio.h>
|
||||
#endif
|
||||
#include <string.h>
|
||||
|
||||
char *data[DATA_SIZE];
|
||||
char data[DATA_SIZE][SENTENCE_SIZE];
|
||||
|
||||
uint8_t correct = 0;
|
||||
uint16_t olddTime = 0;
|
||||
float olddAlt = 0;
|
||||
uint16_t olddAlt = 0;
|
||||
|
||||
uint16_t a_strtof(char *buffer){
|
||||
uint8_t d_pos = 0;
|
||||
uint16_t value = 0;
|
||||
|
||||
while(buffer[d_pos]!='.'){
|
||||
if(buffer[d_pos] == '\0') return 0;
|
||||
d_pos++;
|
||||
}
|
||||
uint16_t e = 1;
|
||||
for(int8_t pos = d_pos-1; pos >= 0; pos--){
|
||||
value += (buffer[pos]-'0') * e;
|
||||
e *= 10;
|
||||
}
|
||||
if((buffer[d_pos+1]-'0') >= 5) value++; // rounding first decimal place
|
||||
|
||||
return value;
|
||||
}
|
||||
|
||||
uint8_t checksum(char *nmea_frame)
|
||||
{
|
||||
|
|
@ -30,7 +50,19 @@ uint8_t checksum(char *nmea_frame)
|
|||
for (i = 0; i < strlen(nmea_frame) - 5; i ++) {
|
||||
crc ^= nmea_frame[i];
|
||||
}
|
||||
int receivedHash = strtol(recv_crc, NULL, 16);
|
||||
int receivedHash = 0;
|
||||
for (int i = 0; i < 2; i++) {
|
||||
receivedHash <<= 4; // Shift left by 4 bits (equivalent to multiplying by 16)
|
||||
|
||||
if (recv_crc[i] >= '0' && recv_crc[i] <= '9') {
|
||||
receivedHash += recv_crc[i] - '0'; // Convert '0'-'9' to 0-9
|
||||
} else if (recv_crc[i] >= 'A' && recv_crc[i] <= 'F') {
|
||||
receivedHash += recv_crc[i] - 'A' + 10; // Convert 'A'-'F' to 10-15
|
||||
} else {
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
if (crc == receivedHash) {
|
||||
return 1;
|
||||
}else{
|
||||
|
|
@ -38,21 +70,41 @@ uint8_t checksum(char *nmea_frame)
|
|||
}
|
||||
}
|
||||
|
||||
int getValues(char*inputString, char *values[]){
|
||||
char *marker = strtok(inputString, ",");
|
||||
int counter = 0;
|
||||
while (marker != NULL) {
|
||||
values[counter++] = malloc(strlen(marker) + 1); //free later!!!!!!
|
||||
strcpy(values[counter - 1], marker);
|
||||
marker = strtok(NULL, ",");
|
||||
uint8_t getValues(char *inputString, char values[MAX_SENTENCE_ELEMENTS][SENTENCE_ELEMENT_LEN]) {
|
||||
uint8_t pos = 0;
|
||||
uint8_t d_pos = 0;
|
||||
uint8_t cnt = 0;
|
||||
char buffer[SENTENCE_ELEMENT_LEN];
|
||||
memset(buffer, 0, SENTENCE_ELEMENT_LEN);
|
||||
|
||||
while (pos < strlen(inputString) && inputString[pos] != '\n' && pos < SENTENCE_SIZE && cnt < MAX_SENTENCE_ELEMENTS) {
|
||||
if (inputString[pos] == ',') {
|
||||
// If the length of the value is within buffer limits
|
||||
if (pos - d_pos < SENTENCE_ELEMENT_LEN) {
|
||||
strncpy(values[cnt], buffer, pos - d_pos); // Copy the buffer into values[cnt]
|
||||
memset(values[cnt] + (pos - d_pos), 0, SENTENCE_ELEMENT_LEN - (pos - d_pos)); // Ensure null termination
|
||||
//cnt++;
|
||||
}
|
||||
cnt++;
|
||||
// Reset buffer and update d_pos to start from next character
|
||||
memset(buffer, 0, SENTENCE_ELEMENT_LEN);
|
||||
d_pos = pos + 1;
|
||||
} else {
|
||||
// Check if writing to buffer exceeds its size
|
||||
if (pos - d_pos < SENTENCE_ELEMENT_LEN - 1) { // Ensure space for null terminator
|
||||
buffer[pos - d_pos] = inputString[pos];
|
||||
}
|
||||
}
|
||||
pos++;
|
||||
}
|
||||
return counter;
|
||||
return cnt;
|
||||
}
|
||||
|
||||
int nmea_GGA(NMEA *nmea_data, char*inputString){
|
||||
char *values[25];
|
||||
int nmea_GGA(NMEA *nmea_data, char *inputString){
|
||||
char values[MAX_SENTENCE_ELEMENTS][SENTENCE_ELEMENT_LEN];
|
||||
memset(values, 0, sizeof(values));
|
||||
int counter = getValues(inputString, values);
|
||||
uint8_t len = getValues(inputString, values);
|
||||
if(len<9) return 0;
|
||||
|
||||
uint8_t h = (values[1][0]-'0')*10 + (values[1][1]-'0');
|
||||
uint8_t m = (values[1][2]-'0')*10 + (values[1][3]-'0');
|
||||
|
|
@ -92,9 +144,10 @@ int nmea_GGA(NMEA *nmea_data, char*inputString){
|
|||
if(latSide == 'S') nmea_data->Lat *= -1;
|
||||
if(lonSide == 'W') nmea_data->Lon *= -1;
|
||||
|
||||
float altitude = strtof(values[9], NULL);
|
||||
|
||||
if(altitude !=0){
|
||||
uint16_t altitude = a_strtof(values[9]);
|
||||
|
||||
if(altitude != 0){
|
||||
nmea_data->Alt = altitude;
|
||||
uint16_t currentTime = h*3600+m*60+s;
|
||||
if(olddTime==0 || currentTime == 0){
|
||||
|
|
@ -108,47 +161,53 @@ int nmea_GGA(NMEA *nmea_data, char*inputString){
|
|||
}
|
||||
}
|
||||
|
||||
nmea_data->Sats = strtol(values[7], NULL, 10);
|
||||
nmea_data->Sats = (values[7][0]-'0')*10 + (values[7][1]-'0');
|
||||
|
||||
//nmea_data->Fix = strtol(values[6], NULL, 10);
|
||||
//nmea_data->Fix = values[6][0]-'0';
|
||||
|
||||
float hdop = strtof(values[8], NULL);
|
||||
if(nmea_data->Fix > 1) nmea_data->HDOP = hdop!=0 ? hdop : nmea_data->HDOP;
|
||||
//float hdop = strtof(values[8], NULL);
|
||||
//if(nmea_data->Fix > 1) nmea_data->HDOP = hdop!=0 ? hdop : nmea_data->HDOP;
|
||||
|
||||
for(int i=0; i<counter; i++) free(values[i]);
|
||||
return 1;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
for(int i=0; i<counter; i++) free(values[i]);
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
int nmea_GSA(NMEA *nmea_data, char*inputString){
|
||||
char *values[25];
|
||||
char values[MAX_SENTENCE_ELEMENTS][SENTENCE_ELEMENT_LEN];
|
||||
memset(values, 0, sizeof(values));
|
||||
int counter = getValues(inputString, values);
|
||||
uint8_t len = getValues(inputString, values);
|
||||
if(len<2) return 0;
|
||||
|
||||
nmea_data->Fix = strtol(values[2], NULL, 10);
|
||||
int satelliteCount = 0;
|
||||
uint8_t fix = (values[2][0]-'0');
|
||||
if(fix<=3){
|
||||
nmea_data->Fix = fix;
|
||||
}else{
|
||||
nmea_data->Fix = 0;
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*int satelliteCount = 0;
|
||||
for(int i=3; i<15; i++){
|
||||
if(values[i][0] != 0){
|
||||
satelliteCount++;
|
||||
}
|
||||
}
|
||||
//nmea_data->Sats = satelliteCount;
|
||||
for(int i=0; i<counter; i++) free(values[i]);
|
||||
nmea_data->Sats = satelliteCount;*/
|
||||
return 1;
|
||||
}
|
||||
|
||||
|
||||
|
||||
int nmea_GLL(NMEA *nmea_data, char*inputString) {
|
||||
char *values[25];
|
||||
char values[MAX_SENTENCE_ELEMENTS][SENTENCE_ELEMENT_LEN];
|
||||
memset(values, 0, sizeof(values));
|
||||
int counter = getValues(inputString, values);
|
||||
uint8_t len = getValues(inputString, values);
|
||||
if(len<3) return 0;
|
||||
|
||||
uint8_t lonSide = values[4][0];
|
||||
uint8_t latSide = values[2][0];
|
||||
|
|
@ -177,54 +236,50 @@ int nmea_GLL(NMEA *nmea_data, char*inputString) {
|
|||
nmea_data->Lon = lon;
|
||||
if(latSide == 'S') nmea_data->Lat *= -1;
|
||||
if(lonSide == 'W') nmea_data->Lon *= -1;
|
||||
for(int i = 0; i<counter; i++) free(values[i]);
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
|
||||
for(int i = 0; i<counter; i++) free(values[i]);
|
||||
return 0;
|
||||
|
||||
}
|
||||
|
||||
int nmea_VTG(NMEA *nmea_data, char*inputString) {
|
||||
char *values[25];
|
||||
char values[MAX_SENTENCE_ELEMENTS][SENTENCE_ELEMENT_LEN];
|
||||
memset(values, 0, sizeof(values));
|
||||
int counter = getValues(inputString, values);
|
||||
uint8_t len = getValues(inputString, values);
|
||||
if(len<5) return 0;
|
||||
|
||||
float speed = strtof(values[5], NULL); // 5 for kph, 3 for knots
|
||||
if(speed != 0.0) nmea_data->Speed = speed;
|
||||
|
||||
for(int i=0; i<counter; i++) free(values[i]);
|
||||
nmea_data->Speed = a_strtof(values[5]); // 5 for kph, 3 for knots
|
||||
return 1;
|
||||
}
|
||||
|
||||
void ParseNMEA(NMEA *nmea_data, uint8_t *buffer){
|
||||
memset(data, 0, sizeof(data));
|
||||
char * token = strtok(buffer, "$");
|
||||
char * token = strtok((char *)buffer, "$");
|
||||
uint8_t cnt = 0;
|
||||
while( token != NULL && cnt < DATA_SIZE) {
|
||||
data[cnt] = malloc(strlen(token)+1);
|
||||
strcpy(data[cnt], token);
|
||||
cnt++;
|
||||
|
||||
if(strlen(token)<SENTENCE_SIZE){
|
||||
strncpy(data[cnt], token, SENTENCE_SIZE - 1);
|
||||
data[cnt][SENTENCE_SIZE - 1] = '\0';
|
||||
cnt++;
|
||||
}
|
||||
token = strtok(NULL, "$");
|
||||
}
|
||||
correct = 0;
|
||||
for(uint8_t i = 0; i<cnt; i++){
|
||||
if(strstr(data[i], "GN")!=NULL && strstr(data[i], "\r\n")!=NULL && checksum(data[i])){
|
||||
if(strstr(data[i], "GNGLL")!=NULL){
|
||||
nmea_GLL(nmea_data, data[i]);
|
||||
correct++;
|
||||
#ifdef DEBUG
|
||||
//printf(">%s", data[i]);
|
||||
#endif
|
||||
if(strstr(data[i], "GNGLL")!=NULL){
|
||||
if(nmea_GLL(nmea_data, data[i])) correct++;
|
||||
}else if(strstr(data[i], "GNGSA")!=NULL){
|
||||
nmea_GSA(nmea_data, data[i]);
|
||||
correct++;
|
||||
if(nmea_GSA(nmea_data, data[i])) correct++;
|
||||
}else if(strstr(data[i], "GNGGA")!=NULL){
|
||||
nmea_GGA(nmea_data, data[i]);
|
||||
correct++;
|
||||
if (nmea_GGA(nmea_data, data[i])) correct++;
|
||||
}else if(strstr(data[i], "GNVTG")!=NULL){
|
||||
nmea_VTG(nmea_data, data[i]);
|
||||
correct++;
|
||||
if (nmea_VTG(nmea_data, data[i])) correct++;
|
||||
}
|
||||
}
|
||||
nmea_data->Corr = correct;
|
||||
|
|
@ -232,6 +287,4 @@ void ParseNMEA(NMEA *nmea_data, uint8_t *buffer){
|
|||
nmea_data->Fix = 0;
|
||||
}
|
||||
}
|
||||
//printf("\r\n%d\r\n", SysTick_counter);
|
||||
for(uint8_t i = 0; i<cnt; i++) free(data[i]);
|
||||
}
|
||||
|
|
@ -1,5 +1,5 @@
|
|||
##########################################################################################################################
|
||||
# File automatically-generated by tool: [projectgenerator] version: [4.5.0-RC5] date: [Fri Feb 21 18:31:14 CET 2025]
|
||||
# File automatically-generated by tool: [projectgenerator] version: [4.5.0-RC5] date: [Tue Feb 25 13:01:11 CET 2025]
|
||||
##########################################################################################################################
|
||||
|
||||
# ------------------------------------------------
|
||||
|
|
@ -137,7 +137,7 @@ C_INCLUDES = \
|
|||
# compile gcc flags
|
||||
ASFLAGS = $(MCU) $(AS_DEFS) $(AS_INCLUDES) $(OPT) -Wall -fdata-sections -ffunction-sections
|
||||
|
||||
CFLAGS += $(MCU) $(C_DEFS) $(C_INCLUDES) $(OPT) -Wall -fdata-sections -ffunction-sections -DNDEBUG -flto
|
||||
CFLAGS += $(MCU) $(C_DEFS) $(C_INCLUDES) $(OPT) -Wall -fdata-sections -ffunction-sections# -DNDEBUG -flto
|
||||
|
||||
ifeq ($(DEBUG), 1)
|
||||
CFLAGS += -g -gdwarf-2
|
||||
|
|
|
|||
|
|
@ -4,7 +4,8 @@ CAD.pinconfig=Dual
|
|||
CAD.provider=
|
||||
File.Version=6
|
||||
GPIO.groupedBy=Group By Peripherals
|
||||
IWDG.IPParameters=Window,Reload
|
||||
IWDG.IPParameters=Window,Reload,Prescaler
|
||||
IWDG.Prescaler=IWDG_PRESCALER_8
|
||||
IWDG.Reload=4095
|
||||
IWDG.Window=4095
|
||||
KeepUserPlacement=false
|
||||
|
|
|
|||
Ładowanie…
Reference in New Issue