Michael Ernst Peter
/
Test_GPS
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Diff: NEOM9N_thread.cpp
- Revision:
- 72:b749014457b7
- Parent:
- 71:4bc51407bff0
diff -r 4bc51407bff0 -r b749014457b7 NEOM9N_thread.cpp
--- a/NEOM9N_thread.cpp Sat Jun 18 18:36:19 2022 +0200
+++ b/NEOM9N_thread.cpp Sat Jun 18 21:39:32 2022 +0200
@@ -117,7 +117,7 @@
{
static char buffer[256]; // buffer to readout from the SerialBuffer
static char check_buffer[4]; // buffer to check if a seuenz is starting
- const static int msg_buffer_len = 96; // message length 94 + 2 (UBX_PAYLOAD_INDEX), do not know why +2, do not care
+ const static int msg_buffer_len = 100; // message length 94 + 2 (UBX_PAYLOAD_INDEX), do not know why +2, do not care
static char msg_buffer[msg_buffer_len]; // buffer to decode message
// ? | ? | PAYLOAD 92 | CK_A | CK_B
static uint8_t msg_buffer_cntr = 0;
@@ -133,64 +133,68 @@
for (uint32_t i = 0; i < read_len; i++) {
if (is_msg_sequenz) {
msg_buffer[msg_buffer_cntr++] = buffer[i];
- if (msg_buffer_cntr == msg_buffer_len) {
- ubxNavPVT_t ubxNavPVT = decodeUbxNavPVTmsg(msg_buffer);
- // update fixType and numSV always
- m_ubxNavPVT.fixType = ubxNavPVT.fixType;
- m_ubxNavPVT.numSV = ubxNavPVT.numSV;
- if(ubxNavPVT.fixType == 3 && ubxNavPVT.numSV >= M_MIN_SATS) {
- m_ubxNavPVT = ubxNavPVT;
- static bool is_reset_local_internal = false;
- if (!is_reset_local_internal) {
- reset_local();
- is_reset_local_internal = true;
- }
- m_pos_ecef = transformWGS84ToECEF(m_ubxNavPVT);
- m_pos_enu = m_R_ecefToLocal_0 * ( m_pos_ecef - m_pos_ecef_0 );
+ if (msg_buffer_cntr == msg_buffer_len) {
+ // checksum, we need to set those values because B5 62 01 07 are at the end of the message and we also need 01 and 07
+ uint8_t CK_A = 0x08; // 0x01 + 0x07
+ uint8_t CK_B = 0x09; // 0x01 + 0x01 + 0x07;
+ for (int i = 0; i < msg_buffer_len - 2 - 4; i++) {
+ CK_A += msg_buffer[i];
+ CK_B += CK_A;
}
- is_msg_sequenz = false;
- // HACK: update float sens_GPS[13]; // pos_enu(3), vel_enu(3), headMot, numSV, hAcc, vAcc, sAcc, headAcc, timestamp
- /*
- data.sens_GPS[0] = m_pos_enu(0);
- data.sens_GPS[1] = m_pos_enu(1);
- data.sens_GPS[2] = m_pos_enu(2);
- Eigen::Vector3f vel_enu = GetVelENU();
- data.sens_GPS[3] = vel_enu(0);
- data.sens_GPS[4] = vel_enu(1);
- data.sens_GPS[5] = vel_enu(2);
- data.sens_GPS[6] = GetHeadMot();
- data.sens_GPS[7] = (float)GetNumSV();
- data.sens_GPS[8] = GethAcc();
- data.sens_GPS[9] = GetvAcc();
- data.sens_GPS[10] = GetsAcc();
- data.sens_GPS[11] = GetHeadAcc();
- data.sens_GPS[12] = global_timer.read();
- */
+ // proceed if checksum is valid
+ if (CK_A == msg_buffer[msg_buffer_len - 2 - 4] && CK_B == msg_buffer[msg_buffer_len - 1 - 4]) {
+ ubxNavPVT_t ubxNavPVT = decodeUbxNavPVTmsg(msg_buffer);
+ // update fixType and numSV always
+ m_ubxNavPVT.fixType = ubxNavPVT.fixType;
+ m_ubxNavPVT.numSV = ubxNavPVT.numSV;
+ // update if fixtype 3 and numSV sufficent
+ if(ubxNavPVT.fixType == 3 && ubxNavPVT.numSV >= M_MIN_SATS) {
+ m_ubxNavPVT = ubxNavPVT;
+ static bool is_reset_local_internal = false;
+ if (!is_reset_local_internal) {
+ reset_local();
+ is_reset_local_internal = true;
+ }
+ m_pos_ecef = transformWGS84ToECEF(m_ubxNavPVT);
+ m_pos_enu = m_R_ecefToLocal_0 * ( m_pos_ecef - m_pos_ecef_0 );
+ }
+ is_msg_sequenz = false;
+ // HACK: update float sens_GPS[13]; // pos_enu(3), vel_enu(3), headMot, numSV, hAcc, vAcc, sAcc, headAcc, timestamp
+ /*
+ data.sens_GPS[0] = m_pos_enu(0);
+ data.sens_GPS[1] = m_pos_enu(1);
+ data.sens_GPS[2] = m_pos_enu(2);
+ Eigen::Vector3f vel_enu = GetVelENU();
+ data.sens_GPS[3] = vel_enu(0);
+ data.sens_GPS[4] = vel_enu(1);
+ data.sens_GPS[5] = vel_enu(2);
+ data.sens_GPS[6] = GetHeadMot();
+ data.sens_GPS[7] = (float)GetNumSV();
+ data.sens_GPS[8] = GethAcc();
+ data.sens_GPS[9] = GetvAcc();
+ data.sens_GPS[10] = GetsAcc();
+ data.sens_GPS[11] = GetHeadAcc();
+ data.sens_GPS[12] = global_timer.read();
+ */
#if PRINT_FOR_DEBUG
- printf("%d, %d, %d, %d\r\n", ubxNavPVT.numSV, ubxNavPVT.lon, ubxNavPVT.lat, ubxNavPVT.height);
-#endif
+ printf("%d, %d, %d, %d\r\n", ubxNavPVT.numSV, ubxNavPVT.lon, ubxNavPVT.lat, ubxNavPVT.height);
+#endif
+ }
}
}
// shift past readings and check if a message starts
- check_buffer[3] = check_buffer[2]; // UBX_PVT_HEADER_0
- check_buffer[2] = check_buffer[1]; // UBX_PVT_HEADER_1
- check_buffer[1] = check_buffer[0]; // UBX_PVT_CLASS
- check_buffer[0] = buffer[i]; // UBX_PVT_ID
+ check_buffer[3] = check_buffer[2]; // UBX_PVT_HEADER_0: 0xB5
+ check_buffer[2] = check_buffer[1]; // UBX_PVT_HEADER_1: 0x62
+ check_buffer[1] = check_buffer[0]; // UBX_PVT_CLASS : 0x01
+ check_buffer[0] = buffer[i]; // UBX_PVT_ID : 0x07
if(check_buffer[3] == UBX_PVT_HEADER_0 && check_buffer[2] == UBX_PVT_HEADER_1 && check_buffer[1] == UBX_PVT_CLASS && check_buffer[0] == UBX_PVT_ID) {
#if PRINT_FOR_DEBUG
int run_time = std::chrono::duration_cast<std::chrono::microseconds>(m_run_timer.elapsed_time()).count();
m_run_timer.reset();
printf("Time since last received package: %d, MSG len: %d\r\n", run_time, msg_buffer_cntr);
- /*
- printf("%02x, %02x, %02x, %02x\r\n", msg_buffer[msg_buffer_len-4], // this makes only sense if you use msg_buffer_len = 100
- msg_buffer[msg_buffer_len-3],
- msg_buffer[msg_buffer_len-2],
- msg_buffer[msg_buffer_len-1]);
- */
#endif
is_msg_sequenz = true;
msg_buffer_cntr = 0;
- //memset(msg_buffer, 0, sizeof(msg_buffer));
}
}
}