I have mistake, right lenght 36 bytes for nav VELNED packet.
I put the code for sending binary settings to the GPS module into define and commented it out.
My module freezes when receiving data NOT from the u-center, so I configure it only from a program from a PC, and not in the microcontroller code.
#define SERIAL_BAUD 115200
#define GNSS_BAUD 115200
#define STATUS_LED PC13
HardwareSerial Serial2(USART2); // PA2(TX2), PA3(RX2)
//format hex command strings for sending to UBLOX
const static char velned[] = {0xb5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x12, 0x01, 0x1e, 0x67};
const static char posllh[] = {0xb5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x02, 0x01, 0x0e, 0x47};
const static char sol[] = {0xb5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x01, 0x06, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x17, 0xDa};
const static char airborne[] = {0xb5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xff, 0xff, 0x08, 0x02, 0x00, 0x00, 0x00, 0x00, 0x10, 0x27,
0x00, 0x00, 0x05, 0x00, 0xfa, 0x00, 0xfa, 0x00, 0x64, 0x00, 0x2c, 0x01, 0x00, 0x3c, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x53, 0x0b, 0xb5, 0x62, 0x06, 0x24,
0x00, 0x00, 0x2a, 0x84
};
const static char rate_a[] = {0xb5, 0x62, 0x06, 0x08, 0x06, 0x00, 0x90, 0x01, 0x01, 0x00, 0x01, 0x00, 0xa7, 0x1f}; // 2.5 Hz
const static char rate_b[] = {0xb5, 0x62, 0x06, 0x08, 0x00, 0x00, 0x0e, 0x30}; // 2.5 Hz
// #define SETUP
#define GPS_INFO_BUFFER_SIZE 60 // enough for both the strings (SOL is only 52 long)
byte GPS_info_buffer[GPS_INFO_BUFFER_SIZE];
byte string_length = 58; //VELNED lenght is 42, POSLLH lenght is 34, SOL length is 58. Initialize with longest string.
unsigned int received_char;
int i = 0; // counter
bool message_started = false;
unsigned long iTOW; // time of week
long velN; // speed in centimeter / second
long velE; // speed in centimeter / second
long velD; // speed in centimeter / second
unsigned long velocity_3d; // speed in centimeter / second
float spd_kts; //speed in knots
long longitude ;//longitude
long latitude ;//latitude
long height_above_sea_level;//height above main sea level
unsigned long ground_speed ;
float heading;
//CHECKSUM CALCULATION VARIABLE
unsigned char CK_A = 0;
unsigned char CK_B = 0;
// byte sendlat = 0; // toggle to send either latitude or longitude in same field
byte fixok = 0; // flag to indicate a valid GPS fix was received.
struct NAV {
// posllh
unsigned char cls_posllh;
unsigned char id_posllh;
unsigned short len_posllh;
unsigned long iTOW_posllh;
long lon;
long lat;
long lat_lon;
long height;
long hMSL;
unsigned long hAcc;
unsigned long vAcc;
// velned
unsigned char cls_velned;
unsigned char id_velned;
unsigned short len_velned;
unsigned long iTOW_velned; // U4
long velN; // I4
long velE;
long velD;
unsigned long speed; // Spd (3D)
unsigned long gSpeed;
long heading;
unsigned long sAcc;
} nav;
void setup() {
pinMode(STATUS_LED, OUTPUT);
digitalWrite(STATUS_LED, LOW); // stm32f411 led ON
// hub.registerSensor(gps_lat_lon); //Add sensor to the hub
// hub.registerSensor(gps_alt); //Add sensor to the hub
// hub.registerSensor(gps_speed); //Add sensor to the hub
// hub.registerSensor(gps_cog); //Add sensor to the hub
// hub.registerSensor(gps_date_time); //Add sensor to the hub
// hub.registerSensor(ass_airspeed); //Add sensor to the hub
// hub.begin(); //Start listening
delay(2000); // wait for GPS to boot.
// Configure the telemetry serial port and sensors (remember to use & to specify a pointer to sensor)
Serial.begin(SERIAL_BAUD);
Serial2.begin(GNSS_BAUD);
#ifdef SETUP
delay(100);
Serial.println(F("$PUBX,40,RMC,0,0,0,0*47")); //RMC OFF
delay(100);
Serial.println(F("$PUBX,40,VTG,0,0,0,0*5E")); //VTG OFF
delay(100);
Serial.println(F("$PUBX,40,GGA,0,0,0,0*5A")); //GGA OFF
delay(100);
Serial.println(F("$PUBX,40,GSA,0,0,0,0*4E")); //GSA OFF
delay(100);
Serial.println(F("$PUBX,40,GSV,0,0,0,0*59")); //GSV OFF
delay(100);
Serial.println(F("$PUBX,40,GLL,0,0,0,0*5C")); //GLL OFF
delay(100);
Serial.write(airborne, sizeof(airborne)); //set GPS mode to airborne < 4g
delay(100);
Serial.write(rate_a, sizeof(rate_a)); //set GPS update rate to 4Hz (1st string)
delay(100);
Serial.write(rate_b, sizeof(rate_b)); //set GPS update rate to 4Hz (2nd string)
delay(100);
Serial.write(velned, sizeof(velned)); // VELNED (VELocity, North, East, Down) message ON
delay(100);
Serial.write(posllh, sizeof(posllh)); // POSLLH (POSition Latitude Longitude Height) message ON
delay(100);
Serial.write(sol, sizeof(sol)); // POSLLH (POSition Latitude Longitude Height) message ON
#endif
} // end setup
void loop() {
if (Serial2.available()) { // If GPS data is available
byte GPS_info_char = Serial2.read(); // Read it and store as HEX char
if (GPS_info_char == 0xb5) { // ublox B5 record found
message_started = true;
received_char = 0;
} // end ublox B5 record found
else if (message_started == true) { // the message is already started and I got a new character
if (received_char < string_length) { // buffer not full
GPS_info_buffer[received_char] = GPS_info_char;
if (GPS_info_buffer[2] == 0x12) string_length = 42; //VELNED lenght is 36 + 6
if (GPS_info_buffer[2] == 0x02) string_length = 34; //POSLLH lenght is 28 + 6
if (GPS_info_buffer[2] == 0x06) string_length = 58; //SOL lenght is 52 + 6
received_char++;
} // end buffer not full
else { // final character received
GPS_info_buffer[received_char] = GPS_info_char;
UBX_calculate_checksum(GPS_info_buffer, 1, (string_length - 2)); // calculates checksum
switch (string_length) {
case 42: // VELNED string received
if (CK_A == GPS_info_buffer[41] && CK_B == GPS_info_buffer[42]) { // confirm received and calculated checksums are the same
digitalWrite(STATUS_LED, HIGH); // stm32f411 led OFF
// convert the GPS buffer array bytes 5,6,7,8 into one "unsigned long" (4 byte) field
iTOW = (long)GPS_info_buffer[8] << 24;
iTOW += (long)GPS_info_buffer[7] << 16;
iTOW += (long)GPS_info_buffer[6] << 8;
iTOW += (long)GPS_info_buffer[5];
nav.iTOW_velned = iTOW;
Serial.print(" iTOW: "); Serial.println(nav.iTOW_velned);
// convert the GPS buffer array bytes 9,10,11,12 into one "unsigned long" (4 byte) field
velN = (long)GPS_info_buffer[12] << 24;
velN += (long)GPS_info_buffer[11] << 16;
velN += (long)GPS_info_buffer[10] << 8;
velN += (long)GPS_info_buffer[9];
nav.velN = velN;
Serial.print(" velN: "); Serial.println(nav.velN * 1e-2f);
// convert the GPS buffer array bytes 13,14,15,16 into one "unsigned long" (4 byte) field
velE = (long)GPS_info_buffer[16] << 24;
velE += (long)GPS_info_buffer[15] << 16;
velE += (long)GPS_info_buffer[14] << 8;
velE += (long)GPS_info_buffer[13];
nav.velE = velE;
Serial.print(" velE: "); Serial.println(nav.velE * 1e-2f);
// convert the GPS buffer array bytes 17,18,19,20 into one "unsigned long" (4 byte) field
velD = (long)GPS_info_buffer[20] << 24;
velD += (long)GPS_info_buffer[19] << 16;
velD += (long)GPS_info_buffer[18] << 8;
velD += (long)GPS_info_buffer[17];
nav.velD = velD;
Serial.print(" velD: "); Serial.println(nav.velD * 1e-2f);
// convert the GPS buffer array bytes 24,23,22,21 into one "unsigned long" (4 byte) field
velocity_3d = (long)GPS_info_buffer[24] << 24;
velocity_3d += (long)GPS_info_buffer[23] << 16;
velocity_3d += (long)GPS_info_buffer[22] << 8;
velocity_3d += (long)GPS_info_buffer[21];
spd_kts = (((float)velocity_3d * 1.94381) / 100);
// convert the GPS buffer array bytes 28,27,26,25 into one "unsigned long" (4 byte) field
ground_speed = (long)GPS_info_buffer[28] << 24;
ground_speed += (long)GPS_info_buffer[27] << 16;
ground_speed += (long)GPS_info_buffer[26] << 8;
ground_speed += (long)GPS_info_buffer[25];
ground_speed = ((float)ground_speed / 100);
// convert the GPS buffer array bytes 32,31,30,29 into one "unsigned long" (4 byte) field
heading = (long)GPS_info_buffer[32] << 24;
heading += (long)GPS_info_buffer[31] << 16;
heading += (long)GPS_info_buffer[30] << 8;
heading += (long)GPS_info_buffer[29];
heading = ((float)heading / 100000);
}
else {// when checksum error set speed = 0
spd_kts = 0;
ground_speed = 0;
heading = 0;
}
// gps_speed.value = ground_speed; //in 0.001 kts
nav.gSpeed = ground_speed; //in 0.001 kts
Serial.print(" gSpeed: "); Serial.println(nav.gSpeed);
// gps_cog.value = heading; //in 0.01 degrees
nav.heading = heading; //in 0.01 degrees
Serial.print(" heading: "); Serial.println(nav.heading);
// gps_date_time.value = 0; //t.b.d. difficult to understand formula/mechanism
// ass_airspeed.value = spd_kts; //in 0.1 kts
nav.speed = spd_kts; //in 0.1 kts
Serial.print(" speed: "); Serial.println(nav.speed);
break;
case 34: //POSLLH string received
if (CK_A == GPS_info_buffer[33] && CK_B == GPS_info_buffer[34]) { // confirm received and calculated checksums are the same
// convert the GPS buffer array bytes into one "long" (4 byte) field
longitude = (long)GPS_info_buffer[12] << 24;
longitude += (long)GPS_info_buffer[11] << 16;
longitude += (long)GPS_info_buffer[10] << 8;
longitude += (long)GPS_info_buffer[9];
Serial.print(" lat/lon: ");
Serial.print(longitude/10000000.0f);
latitude = (long)GPS_info_buffer[16] << 24;
latitude += (long)GPS_info_buffer[15] << 16;
latitude += (long)GPS_info_buffer[14] << 8;
latitude += (long)GPS_info_buffer[13];
Serial.print(",");
Serial.println(latitude/10000000.0f);
height_above_sea_level = (long)GPS_info_buffer[24] << 24;
height_above_sea_level += (long)GPS_info_buffer[23] << 16;
height_above_sea_level += (long)GPS_info_buffer[22] << 8;
height_above_sea_level += (long)GPS_info_buffer[21];
height_above_sea_level = (height_above_sea_level / 1000);
nav.height = height_above_sea_level; //in cm but per 10cm displayed
Serial.print(" height: "); Serial.println(nav.height);
}
else {
longitude = 0;
latitude = 0;
height_above_sea_level = 0;
}
break;
// Set GPS data
// if (sendlat == 0) {
// gps_lat_lon.value = (( ((((uint32_t)( longitude < 0 ? -longitude : longitude)) / 10 ) * 6 ) / 10 ) & 0x3FFFFFFF) | 0x80000000;
// // nav.lat_lon = (( ((((uint32_t)( longitude < 0 ? -longitude : longitude)) / 10 ) * 6 ) / 10 ) & 0x3FFFFFFF) | 0x80000000;
// if (longitude < 0) gps_lat_lon.value |= 0x40000000;
// if (longitude < 0) nav.lat_lon |= 0x40000000;
// sendlat = 1;
// Serial.print(" lat_lon: "); Serial.println(nav.lat_lon);
// }
// else {
// gps_lat_lon.value = (( ((((uint32_t)( latitude < 0 ? -latitude : latitude)) / 10 ) * 6 ) / 10 ) & 0x3FFFFFFF ) ;
// // nav.lat_lon = (( ((((uint32_t)( latitude < 0 ? -latitude : latitude)) / 10 ) * 6 ) / 10 ) & 0x3FFFFFFF ) ;
// if (latitude < 0) gps_lat_lon.value |= 0x40000000;
// if (latitude < 0) nav.lat_lon |= 0x40000000;
// sendlat = 0;
// Serial.print(" lat_lon: "); Serial.println(nav.lat_lon);e);
// }
// gps_alt.value = height_above_sea_level; //in cm but per 10cm displayed
case 58: // SOL string received
if (CK_A == GPS_info_buffer[57] && CK_B == GPS_info_buffer[58]) { // confirm received and calculated checksums are the same
fixok = (( GPS_info_buffer[11 + 5] & B00000001) && ( GPS_info_buffer[10 + 5]) == 0x03); // fix status flag = 1 AND a 3Dfix
digitalWrite(STATUS_LED, !fixok);
}
break;
}
// ubxPrint();
message_started = false; // ready for the new message
} // end final character received
} // end the message is already started and I got a new character
} // end If GPS data is available
// hub.handle(); //Handle new data
} // end loop
// CALCULATES THE UBX CHECKSUM OF A CHAR ARRAY
void UBX_calculate_checksum(uint8_t* array, unsigned int array_start, unsigned int array_length) {
CK_A = 0;
CK_B = 0;
unsigned int i;
for (i = 0; i < array_length; i++) {
CK_A = CK_A + array[array_start + i];
CK_B = CK_B + CK_A;
}
}
void ubxPrint(){
// Serial.println("GPS_UBX");
// Serial.print(" cls: "); Serial.print(velned.cls);
// Serial.print(" id: "); Serial.print(velned.id);
// Serial.print(" id: "); Serial.print(velned.id, HEX);
// Serial.print(" len: "); Serial.print(velned.len);
// Serial.print(" iTOW: "); Serial.print(velned.iTOW); // GPS Millisecond Time of Week
// Serial.print(" velN: "); Serial.print(velned.velN * 1e-2f); // m/s
// Serial.print(" velE: "); Serial.print(velned.velE * 1e-2f); // m/s
// Serial.print(" velD: "); Serial.print(velned.velD * 1e-2f); // m/s
// Serial.print(" Spd (3D): "); Serial.print(velned.speed * 1e-2f); // m/s
Serial.print(" height: "); Serial.print(nav.height); // m/s
Serial.print(" lat_lon: "); Serial.print(nav.lat_lon); // m/s
Serial.print(" speed: "); Serial.print(nav.speed); // m/s
Serial.print(" gSpeed: "); Serial.print(nav.gSpeed); // m/s
Serial.print(" heading: "); Serial.print(nav.heading); // m/s
// Serial.print(" GndSpd (2D): "); Serial.print(velned.gSpeed * 1e-2f); // m/s
// Serial.print(" Hdg (2D): "); Serial.print(velned.heading * 1e-5f); // same that divide on 100000.0f
// Serial.print(" Spd Acc: "); Serial.print(velned.sAcc * 1e-2f);
// Serial.print(" Crs/Hdg Acc: "); Serial.print(velned.cAcc/100000.0f);
Serial.println();
}
You have a lot of extra variables, one type is converted to another.
Float to long and back.
This is an extra load on the microcontroller.
To get one field from a message package, you can use only two variables.
One to receive data from the buffer, and the second directly to receive the value, and put everything into a structure.
But in general, your code works quickly, thanks for your help.
The libraries could not get the data from my module.
I'm now interested in getting velN, velE and velD from the NMEA package.
There, you will probably need to mathematically calculate the velocity vectors from the cardinal directions and velocities using the right triangle formula.
