Just as an FYI the python version has print "Height above Ellipsoid: " + str(aaa[3]/1000)
The C++ library suggests in the comments // printf(“Height: %lf\n”, position_data[3]/100); but the example does use /1000
the pdf says its in mm, which suggests the /1000 is correct.
Also is anyone able to comment on if this code looks correct, I’ll test it over the weekend. (worst case scenario is the thread dies keeping the spi locked, this will lock up the whole program and result in the quad continuing with its current pwm state not a good one, but will test it unarmed)
void *agpsthread(void *args)
{
std::string gps_spi_device_name = "/dev/spidev0.0";
unsigned char to_gps_data = 0x00, from_gps_data = 0x00;
unsigned char *todata = new unsigned char[1024],*fromdata = new unsigned char[1024];
memset(todata, 0x00, 1024);
memset(fromdata, 0x00, 1024);
int idlecount = 0;
while(!quiting)
{
pthread_mutex_lock(&spimutex);
SPIdev::transfer(gps_spi_device_name.c_str(), &to_gps_data, &from_gps_data, 1, 5000000);
if (from_gps_data==0xff)
idlecount++;
else
idlecount = 0;
if (from_gps_data==0xb5) // All UBX messages start with 2 sync chars: 0xb5 and 0x62
{
SPIdev::transfer(gps_spi_device_name.c_str(), &to_gps_data, &from_gps_data, 1, 5000000);
if (from_gps_data==0x62)
{
// message, read the message type first
SPIdev::transfer(gps_spi_device_name.c_str(), todata, fromdata, 2, 5000000);
if ((fromdata[0]==0x01) && (fromdata[1]==0x02)) // nav_posllh is 0x01 0x02
{
SPIdev::transfer(gps_spi_device_name.c_str(), todata, fromdata, 2, 5000000);// read length
int len = fromdata[0]+(fromdata[1] << 8);
if (len<1022)
{
SPIdev::transfer(gps_spi_device_name.c_str(), todata, fromdata, len+2, 5000000);// read length
//0 U4 - iTOW ms GPS Millisecond Time of Week
//4 I4 1e-7 lon deg Longitude
//8 I4 1e-7 lat deg Latitude
//12 I4 - height mm Height above Ellipsoid
//16 I4 - hMSL mm Height above mean sea level
//20 U4 - hAcc mm Horizontal Accuracy Estimate
//24 U4 - vAcc mm Vertical Accuracy Estimate
//iTOW
unsigned int tiTOW = ((fromdata[3] << 24) | (fromdata[2] << 16) | (fromdata[1] << 8) | (fromdata[0]))/1000;
//Longitude
double tlongitude = ((fromdata[7] << 24) | (fromdata[6] << 16) | (fromdata[5] << 8) | (fromdata[4]))/10000000;
//Latitude
double tlatitude = ((fromdata[11] << 24) | (fromdata[10] << 16) | (fromdata[9] << 8) | (fromdata[8]))/10000000;
//Height
double theight = ((fromdata[15] << 24) | (fromdata[14] << 16) | (fromdata[13] << 8) | (fromdata[12]))/(10*100);
// Horizontal Accuracy Estimate
double thacc = ((fromdata[23] << 24) | (fromdata[22] << 16) | (fromdata[21] << 8) | (fromdata[20]))/(10*100);
// Vertical Accuracy Estimate
double tvacc = ((fromdata[27] << 24) | (fromdata[26] << 16) | (fromdata[25] << 8) | (fromdata[24]))/(10*100);
pthread_mutex_lock(&gpsmutex);
latitude = tlatitude;
longitude = tlongitude;
iTOW = tiTOW;
gpsheight = theight;
//hAcc = thacc;
//vAcc = tvacc;
pthread_mutex_unlock(&gpsmutex);
}
}
else
{// unhandled message for now
SPIdev::transfer(gps_spi_device_name.c_str(), todata, fromdata, 2, 5000000);// read length
int len = fromdata[0]+(fromdata[1] << 8);
if (len<1022)
{
SPIdev::transfer(gps_spi_device_name.c_str(), todata, fromdata, len+2, 5000000);// +2 so we read the checksum
}
}
}
}
pthread_mutex_unlock(&spimutex);
if (idlecount>50)
usleep(10000);
}
}
Edit: Looks like that works fine, couple of small code changes, making sure the spi buffer is idle before the usleep and setting the spi speed to 5000000. had an issue with the antenna connection not sitting correctly on the connector, added a bit of tape to hold the cable down.