u-blox UBX library for I2C
Dependencies: Vector3
It will not work
Diff: GPSUBX.cpp
- Revision:
- 0:6256752abece
diff -r 000000000000 -r 6256752abece GPSUBX.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/GPSUBX.cpp Sun Sep 12 16:38:51 2021 +0000 @@ -0,0 +1,104 @@ +#include "GPSUBX.hpp" +#include "mbed.h" + +GPSUBX::GPSUBX(PinName sda, PinName scl, char address = GPS_ADDRESS, int timedifference = 9, int hz = 100000) + :i2c(sda, scl), Address(address), TimeDifference(timedifference) +{ + i2c.frequency(hz); +} + +void GPSUBX::Checksum(char payload[], int n, char* ck_a, char* ck_b) +{ + int ca = 0; + int cb = 0; + for (int i = 0; i < n+4; i++) + { + ca += (unsigned char)payload[i+2]; + cb += ca; + } + *ck_a = (char)(ca & 0xff); + *ck_b = (char)(cb & 0xff); +} + +int GPSUBX::GetGPSData(GPSData* pdata) +{ + POSLLH posllh; + char command[8] = {0xb5, 0x62, 0x01, 0x02, 0x00, 0x00, 0x00, 0x00}; + Checksum(command, 0, &command[6], &command[7]); + + i2c.write(Address << 1, command, 8); + i2c.read(Address << 1 | 1, posllh.byte_data, POSLLH_LEN+8); + + for (int i = 0; i < POSLLH_LEN+8; i++) + { + pc.printf("%x ", posllh.byte_data[i]); + } + pc.printf("\r\n"); + if (posllh.data.sync == UBX_SYNC && posllh.data.m_class == 0x01 && posllh.data.m_id == 0x02 && posllh.data.pay_len == POSLLH_LEN) + { + char ca, cb; + Checksum(posllh.byte_data, POSLLH_LEN, &ca, &cb); + if (posllh.data.check_a == ca && posllh.data.check_b == cb) + { + pdata->Longitude = (float)posllh.data.lon * 1e-7f; + pdata->Latitude = (float)posllh.data.lat * 1e-7f; + pdata->Height = (float)posllh.data.height / 1000.0f; + pc.printf("%f, %f, %f\r\n", pdata->Longitude, pdata->Latitude, pdata->Height); + } + else + { + pc.printf("#1\r\n"); + return 0; + } + pc.printf("#2\r\n"); + return 1; + } + else + { + pc.printf("#3\r\n"); + return 0; + } +} + +int GPSUBX::GetTimeData(GPSData* pdata) +{ + TIMEUTC timeutc; + char command[8] = {0xb5, 0x62, 0x01, 0x21, 0x00, 0x00, 0x00, 0x00}; + Checksum(command, 0, &command[6], &command[7]); + + i2c.write(Address << 1, command, 8); + i2c.read(Address << 1 | 1, timeutc.byte_data, TIMEUTC_LEN+8); + + for (int i = 0; i < TIMEUTC_LEN+8; i++) + { + pc.printf("%x ", timeutc.byte_data[i]); + } + pc.printf("\r\n"); + if (timeutc.data.sync == UBX_SYNC && timeutc.data.m_class == 0x01 && timeutc.data.m_id == 0x21 && timeutc.data.pay_len == TIMEUTC_LEN) + { + char ca, cb; + Checksum(timeutc.byte_data, TIMEUTC_LEN, &ca, &cb); + if (timeutc.data.check_a == ca && timeutc.data.check_b == cb) + { + pdata->Year = timeutc.data.year; + pdata->Month = timeutc.data.month; + pdata->Day = timeutc.data.day; + pdata->Hours = timeutc.data.hour; + pdata->Minutes = timeutc.data.min; + pdata->Seconds = timeutc.data.sec; + pc.printf("%4d/%2d/%2d %2d:%2d %2d\r\n", pdata->Year, pdata->Month, pdata->Day, pdata->Hours, pdata->Minutes, pdata->Seconds); + } + else + { + pc.printf("#4\r\n"); + return 0; + } + pc.printf("#5\r\n"); + return 1; + } + else + { + pc.printf("#6\r\n"); + return 0; + } +} \ No newline at end of file