pizero_tracker/tracker/code/ublox/ublox_cmds.cpp

#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 );
}