kopia lustrzana https://github.com/ogre/pizero_tracker
154 wiersze
3.6 KiB
C++
154 wiersze
3.6 KiB
C++
#include "ublox_cmds.h"
|
|
|
|
#include <unistd.h>
|
|
|
|
#include <cstring>
|
|
#include <memory>
|
|
#include <iostream>
|
|
|
|
|
|
|
|
uint8_t UBX_CMD_EnableOutput_ACK_ACK[] = {
|
|
0xB5, 0x62, // sync
|
|
0x06, 0x01, // class / id
|
|
0x08, 0x00, // payload length
|
|
0x05, 0x01, // ACK
|
|
0x01, 0x01, 0x01, 0x01, 0x01, 0x00, // I2C UART1 UART2 USB SPI ???
|
|
0x1A, 0xE6 // checksum
|
|
};
|
|
|
|
uint8_t UBX_CMD_EnableOutput_ACK_NAK[] = {
|
|
0xB5, 0x62, // sync
|
|
0x06, 0x01, // class / id
|
|
0x08, 0x00, // payload length
|
|
0x05, 0x00, // NAK
|
|
0x01, 0x01, 0x01, 0x01, 0x01, 0x00, // I2C UART1 UART2 USB SPI ???
|
|
0x19, 0xDF // checksum
|
|
};
|
|
|
|
uint8_t UBX_CMD_NAV5_Airbororne1G[] = {
|
|
0xB5, 0x62, // sync
|
|
0x06, 0x24, // class / id
|
|
0x24, 0x00, // payload length
|
|
0xFF, 0xFF, 0x06, 0x03, 0x00, 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00,
|
|
0xFA, 0x00, 0xFA, 0x00, 0x64, 0x00, 0x5E, 0x01, 0x00, 0x3C, 0x00, 0x00, 0x00, 0x00,
|
|
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
|
0x84, 0x08 // checksum
|
|
};
|
|
|
|
uint8_t UBX_CMD_NAV5_Pedestrian[] = {
|
|
0xB5, 0x62, // sync
|
|
0x06, 0x24, // class / id
|
|
0x24, 0x00, // payload length
|
|
0xFF, 0xFF, 0x03, 0x03, 0x00, 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00,
|
|
0xFA, 0x00, 0xFA, 0x00, 0x64, 0x00, 0x2C, 0x01, 0x00, 0x00, 0x00, 0x00, 0x10, 0x27,
|
|
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
|
0x4A, 0x75 // checksum
|
|
};
|
|
|
|
// Disable GSV - GNSS Satellites in View
|
|
uint8_t UBX_CMD_GSV_OFF[] = {
|
|
0xB5, 0x62, // sync
|
|
0x06, 0x01, // class / id
|
|
0x08, 0x00, // payload length
|
|
0xF0, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01,
|
|
0x03, 0x39 // checksum
|
|
};
|
|
|
|
// Disable GLL - Latitude and longitude, with time of position fix and status
|
|
uint8_t UBX_CMD_GLL_OFF[] = {
|
|
0xB5, 0x62, // sync
|
|
0x06, 0x01, // class / id
|
|
0x08, 0x00, // payload length
|
|
0xF0, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01,
|
|
0x01, 0x2B // checksum
|
|
};
|
|
|
|
// Disable GSA - GNSS DOP and Active Satellites
|
|
uint8_t UBX_CMD_GSA_OFF[] = {
|
|
0xB5, 0x62, // sync
|
|
0x06, 0x01, // class / id
|
|
0x08, 0x00, // payload length
|
|
0xF0, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01,
|
|
0x02, 0x32 // checksum
|
|
};
|
|
|
|
// Disable VTG - Course over ground and Ground speed
|
|
uint8_t UBX_CMD_VTG_OFF[] = {
|
|
0xB5, 0x62, // sync
|
|
0x06, 0x01, // class / id
|
|
0x08, 0x00, // payload length
|
|
0xF0, 0x05, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01,
|
|
0x05, 0x47 // checksum
|
|
};
|
|
|
|
|
|
|
|
void UBX_CHECKSUM(uint8_t* buffer, const size_t buffer_sz, uint8_t* ck_a, uint8_t* ck_b)
|
|
{
|
|
*ck_a = 0;
|
|
*ck_b = 0;
|
|
for(size_t i=0; i<buffer_sz; ++i)
|
|
{
|
|
*ck_a += buffer[i];
|
|
*ck_b += *ck_a;
|
|
}
|
|
}
|
|
|
|
|
|
// o_cmd is user-owned array of size 10
|
|
void UBX_MAKE_PACKET_ACK(const uint8_t i_class, const uint8_t i_id, uint8_t* o_cmd)
|
|
{
|
|
// sync
|
|
o_cmd[0] = 0xB5;
|
|
o_cmd[1] = 0x62;
|
|
// class / id
|
|
o_cmd[2] = 0x05;
|
|
o_cmd[3] = 0x01;
|
|
// payload length
|
|
o_cmd[4] = 0x02;
|
|
o_cmd[5] = 0x00;
|
|
// NAK class / id
|
|
o_cmd[6] = i_class;
|
|
o_cmd[7] = i_id;
|
|
|
|
UBX_CHECKSUM( o_cmd + 2, 6, o_cmd + 8, o_cmd + 9 );
|
|
}
|
|
|
|
|
|
// o_cmd is user-owned array of size 10
|
|
void UBX_MAKE_PACKET_NAK(const uint8_t i_class, const uint8_t i_id, uint8_t* o_cmd)
|
|
{
|
|
// sync
|
|
o_cmd[0] = 0xB5;
|
|
o_cmd[1] = 0x62;
|
|
// class / id
|
|
o_cmd[2] = 0x05;
|
|
o_cmd[3] = 0x00;
|
|
// payload length
|
|
o_cmd[4] = 0x02;
|
|
o_cmd[5] = 0x00;
|
|
// NAK class / id
|
|
o_cmd[6] = i_class;
|
|
o_cmd[7] = i_id;
|
|
|
|
UBX_CHECKSUM( o_cmd + 2, 6, o_cmd + 8, o_cmd + 9 );
|
|
}
|
|
|
|
|
|
bool UBX_PACKET_EQ(uint8_t* i_data, const uint8_t* p_ref, const size_t ref_sz)
|
|
{
|
|
size_t i = 0;
|
|
while( i < ref_sz && (i_data[i] == p_ref[i]) )
|
|
++i;
|
|
|
|
return i == ref_sz;
|
|
}
|
|
|
|
bool UBX_PACKET_EQ(const std::vector<char>& i_data, const uint8_t* p_ref, const size_t ref_sz)
|
|
{
|
|
if( i_data.size() != ref_sz )
|
|
return false;
|
|
return UBX_PACKET_EQ( (uint8_t*) i_data.data(), p_ref, ref_sz );
|
|
}
|