Eigen
Dependencies: Vector3
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: GPSUBX_UART.cpp
- Revision:
- 2:6218fe8e54f4
- Parent:
- 1:71f5168e48c8
- Child:
- 10:a90d07e4c34d
diff -r 71f5168e48c8 -r 6218fe8e54f4 GPSUBX_UART.cpp --- a/GPSUBX_UART.cpp Fri Sep 17 15:06:47 2021 +0000 +++ b/GPSUBX_UART.cpp Fri Sep 17 15:45:25 2021 +0000 @@ -23,76 +23,11 @@ void GPSUBX_UART::Receive() { -// volatile static int receive_start = 0; -// volatile static int receive_index = 0; -// volatile static int sentence_start = 0; -// volatile static int sentence_length = 0; -// volatile static int sentence_counter = 0; -// -// volatile static char m_class = 0x00; -// volatile static char m_id = 0x00; - while (serial.readable()) { char c; c = serial.getc(); receive_buffer[receive_index] = c; -//// pc.printf("%d\r\n", c); -// if (sentence_counter >= 2) -// { -// sentence_counter++; -// if (sentence_counter == 3) -// { -// m_class = c; -//// pc.printf("@%x\r\n", m_class); -// } -// else if (sentence_counter == 4) -// { -// m_id = c; -//// pc.printf("@@%x\r\n", m_id); -// } -// else if (sentence_counter == 5) -// { -// } -// else if (sentence_counter == 6) -// { -// int sss = (receive_index+RECEIVE_SIZE-1)%RECEIVE_SIZE; -// sentence_length = (int)(c << 8 | receive_buffer[sss]); -//// printf("%d\r\n", sentence_length); -// } -// else if (sentence_counter >= sentence_length+8) -// { -// for (int i = 0; i < sentence_length+8; i++) -// { -// sentence_buffer[i] = receive_buffer[(sentence_start+i)%RECEIVE_SIZE]; -// } -//// for (int i = 0; i < sentence_length+8; i++) -//// { -//// pc.printf("%x ", sentence_buffer[i]); -//// } -//// pc.printf("\r\n"); -// char ca, cb; -// Checksum(sentence_buffer, sentence_length, &ca, &cb); -//// pc.printf("^%x %x\r\n", m_class, m_id); -// if (ca == sentence_buffer[sentence_length+6] && cb == sentence_buffer[sentence_length+7]) -// { -//// pc.printf("=%x %x\r\n", m_class, m_id); -// Decode(sentence_buffer, m_class, m_id); -// } -// sentence_start = 0; -// sentence_length = 0; -// sentence_counter = 0; -// m_class = 0x00; -// m_id = 0x00; -// } -// } -// -// int ss = (receive_index+RECEIVE_SIZE-1)%RECEIVE_SIZE; -// if (c == 0x62 && receive_buffer[ss] == 0xb5) -// { -// sentence_start = ss; -// sentence_counter = 2; -// } receive_index = (receive_index + 1) % RECEIVE_SIZE; } } @@ -100,8 +35,6 @@ void GPSUBX_UART::Update() { -// volatile static int receive_start = 0; -// volatile static int receive_index = 0; volatile static int sentence_start = 0; volatile static int sentence_length = 0; volatile static int sentence_counter = 0; @@ -120,12 +53,10 @@ if (sentence_counter == 3) { m_class = c; -// pc.printf("@%x\r\n", m_class); } else if (sentence_counter == 4) { m_id = c; -// pc.printf("@@%x\r\n", m_id); } else if (sentence_counter == 5) { @@ -134,7 +65,6 @@ { int sss = (read_index+RECEIVE_SIZE-1)%RECEIVE_SIZE; sentence_length = (int)(c << 8 | receive_buffer[sss]); -// printf("%d\r\n", sentence_length); } else if (sentence_counter >= sentence_length+8) { @@ -142,17 +72,10 @@ { sentence_buffer[i] = receive_buffer[(sentence_start+i)%RECEIVE_SIZE]; } -// for (int i = 0; i < sentence_length+8; i++) -// { -// pc.printf("%x ", sentence_buffer[i]); -// } -// pc.printf("\r\n"); char ca, cb; Checksum(sentence_buffer, sentence_length, &ca, &cb); -// pc.printf("^%x %x\r\n", m_class, m_id); if (ca == sentence_buffer[sentence_length+6] && cb == sentence_buffer[sentence_length+7]) { -// pc.printf("=%x %x\r\n", m_class, m_id); Decode(sentence_buffer, m_class, m_id); } sentence_start = 0; @@ -192,7 +115,7 @@ Longitude = (float)posllh.data.lon * 1e-7f; Latitude = (float)posllh.data.lat * 1e-7f; Height = (float)posllh.data.height / 1000.0f; - pc.printf("!%d, %f, %f, %f\r\n", iTOW_POSLLH, Longitude, Latitude, Height); +// pc.printf("!%d, %f, %f, %f\r\n", iTOW_POSLLH, Longitude, Latitude, Height); } // TIMEUTC else if (mc == 0x01 && mi == 0x21) @@ -218,7 +141,7 @@ } Minutes = timeutc.data.min; Seconds = timeutc.data.sec; - pc.printf("&%4d/%2d/%2d %2d:%2d %2d\r\n", Year, Month, Day, Hours, Minutes, Seconds); +// pc.printf("&%4d/%2d/%2d %2d:%2d %2d\r\n", Year, Month, Day, Hours, Minutes, Seconds); } // VELNED if (mc == 0x01 && mi == 0x12) @@ -235,7 +158,7 @@ Speed = (float)velned.data.speed / 100.0f; GroundSpeed = (float)velned.data.gSpeed / 100.0f; Heading = (float)velned.data.heading * 1e-5f; - pc.printf("#%d, %f, %f, %f\r\n", iTOW_VELNED, VelocityNED.x, VelocityNED.y, VelocityNED.z); +// pc.printf("#%d, %f, %f, %f\r\n", iTOW_VELNED, VelocityNED.x, VelocityNED.y, VelocityNED.z); } }