I didn’t have any luck over in navigation sensors, so I thought I’d try here. Has anyone successfully enabled the extended functions on the dGPS using C and the BrickPi?
Hey Deven, we’re working onextending these functions to C and Python right now. Just a heads up.
Any idea on a timeline?
No idea on a timeline. However, if you want to contribute, you can see the registers are down at the bottom and it’s just a matter of extending the current code a bit: http://www.dexterindustries.com/manual/dgps-2/
I haven’t field tested it yet, but this code seems to output good values. There are some anomalous spikes in the data, so I think I will experiment with a process to dampen rouge values.
/*
* dGPS with extended firmware
*/
#include <stdio.h>
#include <math.h>
#include <time.h>
#include <math.h>
#include "tick.h"
#include <wiringPi.h>
#include "BrickPi.h"
#include <linux/i2c-dev.h>
#include <fcntl.h>
int result;
long UTC, lat, lon, alttd, hdop, vwsat;
unsigned short head, velo, status;
#define I2C_PORT PORT_1 // I2C port for the dGPS
#define I2C_SPEED 0 // delay for as little time as possible. Usually about 100k baud
#define I2C_DEVICE_DGPS 0 // dGPS is device 0 on this I2C bus
#define DGPS_I2C_ADDR 0x06 // Sensor device address
#define DGPS_CMD_UTC 0x00 // Fetch UTC
#define DGPS_CMD_STATUS 0x01 // Fetch status of satellite link: 0 no link, 1 link
#define DGPS_CMD_LAT 0x02 // Fetch latitude
#define DGPS_CMD_LONG 0x04 // Fetch longitude
#define DGPS_CMD_VELO 0x06 // Fetch velocity in cm/s
#define DGPS_CMD_HEAD 0x07 // Fetch heading in degrees
#define DGPS_CMD_DIST 0x08 // Fetch distance to destination
#define DGPS_CMD_ANGD 0x09 // Fetch angle to destination
#define DGPS_CMD_ANGR 0x0A // Fetch angle travelled since last request
#define DGPS_CMD_SLAT 0x0B // Set latitude of destination
#define DGPS_CMD_SLONG 0x0C // Set longitude of destination
#define DGPS_CMD_XFIRM 0x0D // Extended firmware
#define DGPS_CMD_ALTTD 0x0E // Altitude
#define DGPS_CMD_HDOP 0x0F // HDOP
#define DGPS_CMD_VWSAT 0x10 // Satellites in View
int main() {
ClearTick();
result = BrickPiSetup();
printf("BrickPiSetup: %dn", result);
if(result)
return 0;
BrickPi.Address[0] = 1;
BrickPi.Address[1] = 2;
BrickPi.SensorType [I2C_PORT] = TYPE_SENSOR_I2C;
BrickPi.SensorI2CSpeed [I2C_PORT] = I2C_SPEED;
BrickPi.SensorI2CDevices [I2C_PORT] = 1;
BrickPi.SensorSettings [I2C_PORT][I2C_DEVICE_DGPS] = 0;
BrickPi.SensorI2CAddr [I2C_PORT][I2C_DEVICE_DGPS] = DGPS_I2C_ADDR; // Address for writing
if(BrickPiSetupSensors())
return 0;
BrickPi.SensorSettings [I2C_PORT][I2C_DEVICE_DGPS] = 1;
BrickPi.SensorI2COut [I2C_PORT][I2C_DEVICE_DGPS][0] = DGPS_CMD_XFIRM;
BrickPi.SensorI2CWrite [I2C_PORT][I2C_DEVICE_DGPS] = 1; // Number of bytes to write
BrickPi.SensorI2CRead [I2C_PORT][I2C_DEVICE_DGPS] = 3; // Number of bytes to read
BrickPiSetupSensors();
result = BrickPiUpdateValues();
printf("Extended firmware: %dn", result);
BrickPi.SensorSettings [I2C_PORT][I2C_DEVICE_DGPS] = BIT_I2C_MID; // The dGPS device requires a clock change between reading and writing
BrickPi.SensorI2CWrite [I2C_PORT][I2C_DEVICE_DGPS] = 1; // Number of bytes to write
BrickPi.SensorI2CRead [I2C_PORT][I2C_DEVICE_DGPS] = 4; // Number of bytes to read
result = BrickPiSetupSensors();
printf("BrickPiSetupSensors: %dn", result);
if(!result){
while(1){
//read UTC
BrickPi.SensorI2CRead [I2C_PORT][I2C_DEVICE_DGPS] = 4;
BrickPi.SensorI2COut [I2C_PORT][I2C_DEVICE_DGPS][0] = DGPS_CMD_UTC; //byte to write
BrickPiSetupSensors();
result = BrickPiUpdateValues(); //write and read
if(!result){
if(BrickPi.Sensor[I2C_PORT] & (0x01 << I2C_DEVICE_DGPS)){
UTC = ((long)BrickPi.SensorI2CIn[I2C_PORT][I2C_DEVICE_DGPS][0]<<24) + ((long)BrickPi.SensorI2CIn[I2C_PORT][I2C_DEVICE_DGPS][1]<<16) + ((long)BrickPi.SensorI2CIn[I2C_PORT][I2C_DEVICE_DGPS][2]<<8) + (long)BrickPi.SensorI2CIn[I2C_PORT][I2C_DEVICE_DGPS][3];
}
}
//read Latitude
BrickPi.SensorI2CRead [I2C_PORT][I2C_DEVICE_DGPS] = 4;
BrickPi.SensorI2COut [I2C_PORT][I2C_DEVICE_DGPS][0] = DGPS_CMD_LAT; //byte to write
BrickPiSetupSensors();
result = BrickPiUpdateValues(); //write and read
if(!result){
if(BrickPi.Sensor[I2C_PORT] & (0x01 << I2C_DEVICE_DGPS)){
lat = ((long)BrickPi.SensorI2CIn[I2C_PORT][I2C_DEVICE_DGPS][0]<<24) + ((long)BrickPi.SensorI2CIn[I2C_PORT][I2C_DEVICE_DGPS][1]<<16) + ((long)BrickPi.SensorI2CIn[I2C_PORT][I2C_DEVICE_DGPS][2]<<8) + (long)BrickPi.SensorI2CIn[I2C_PORT][I2C_DEVICE_DGPS][3];
}
}
//read Longitude
BrickPi.SensorI2CRead [I2C_PORT][I2C_DEVICE_DGPS] = 4;
BrickPi.SensorI2COut [I2C_PORT][I2C_DEVICE_DGPS][0] = DGPS_CMD_LONG; //byte to write
BrickPiSetupSensors();
result = BrickPiUpdateValues(); //write and read
if(!result){
if(BrickPi.Sensor[I2C_PORT] & (0x01 << I2C_DEVICE_DGPS)){
lon = ((long)BrickPi.SensorI2CIn[I2C_PORT][I2C_DEVICE_DGPS][0]<<24) + ((long)BrickPi.SensorI2CIn[I2C_PORT][I2C_DEVICE_DGPS][1]<<16) + ((long)BrickPi.SensorI2CIn[I2C_PORT][I2C_DEVICE_DGPS][2]<<8) + (long)BrickPi.SensorI2CIn[I2C_PORT][I2C_DEVICE_DGPS][3];
}
}
//read heading
BrickPi.SensorI2CRead [I2C_PORT][I2C_DEVICE_DGPS] = 2;
BrickPi.SensorI2COut [I2C_PORT][I2C_DEVICE_DGPS][0] = DGPS_CMD_HEAD; //byte to write
BrickPiSetupSensors();
result = BrickPiUpdateValues(); //write and read
if(!result){
if(BrickPi.Sensor[I2C_PORT] & (0x01 << I2C_DEVICE_DGPS)){
head = ((long)BrickPi.SensorI2CIn[I2C_PORT][I2C_DEVICE_DGPS][0]<<8) + ((long)BrickPi.SensorI2CIn[I2C_PORT][I2C_DEVICE_DGPS][1]);
}
}
//read status
BrickPi.SensorI2CRead [I2C_PORT][I2C_DEVICE_DGPS] = 1;
BrickPi.SensorI2COut [I2C_PORT][I2C_DEVICE_DGPS][0] = DGPS_CMD_STATUS; //byte to write
BrickPiSetupSensors();
result = BrickPiUpdateValues(); //write and read
if(!result){
if(BrickPi.Sensor[I2C_PORT] & (0x01 << I2C_DEVICE_DGPS)){
status = ((long)BrickPi.SensorI2CIn[I2C_PORT][I2C_DEVICE_DGPS][0]);
}
}
//read velocity
BrickPi.SensorI2CRead [I2C_PORT][I2C_DEVICE_DGPS] = 3;
BrickPi.SensorI2COut [I2C_PORT][I2C_DEVICE_DGPS][0] = DGPS_CMD_VELO; //byte to write
BrickPiSetupSensors();
result = BrickPiUpdateValues(); //write and read
if(!result){
if(BrickPi.Sensor[I2C_PORT] & (0x01 << I2C_DEVICE_DGPS)){
velo = ((long)BrickPi.SensorI2CIn[I2C_PORT][I2C_DEVICE_DGPS][0]<<16) + ((long)BrickPi.SensorI2CIn[I2C_PORT][I2C_DEVICE_DGPS][1]<<8) + ((long)BrickPi.SensorI2CIn[I2C_PORT][I2C_DEVICE_DGPS][2]);
}
}
//read altitude
BrickPi.SensorI2CRead [I2C_PORT][I2C_DEVICE_DGPS] = 4;
BrickPi.SensorI2COut [I2C_PORT][I2C_DEVICE_DGPS][0] = DGPS_CMD_ALTTD; //byte to write
BrickPiSetupSensors();
result = BrickPiUpdateValues(); //write and read
if(!result){
if(BrickPi.Sensor[I2C_PORT] & (0x01 << I2C_DEVICE_DGPS)){
alttd = ((long)BrickPi.SensorI2CIn[I2C_PORT][I2C_DEVICE_DGPS][0]<<24) + ((long)BrickPi.SensorI2CIn[I2C_PORT][I2C_DEVICE_DGPS][1]<<16) + ((long)BrickPi.SensorI2CIn[I2C_PORT][I2C_DEVICE_DGPS][2]<<8) + (long)BrickPi.SensorI2CIn[I2C_PORT][I2C_DEVICE_DGPS][3];
}
}
//read hdop
BrickPi.SensorI2CRead [I2C_PORT][I2C_DEVICE_DGPS] = 4;
BrickPi.SensorI2COut [I2C_PORT][I2C_DEVICE_DGPS][0] = DGPS_CMD_HDOP; //byte to write
BrickPiSetupSensors();
result = BrickPiUpdateValues(); //write and read
if(!result){
if(BrickPi.Sensor[I2C_PORT] & (0x01 << I2C_DEVICE_DGPS)){
hdop = ((long)BrickPi.SensorI2CIn[I2C_PORT][I2C_DEVICE_DGPS][0]<<24) + ((long)BrickPi.SensorI2CIn[I2C_PORT][I2C_DEVICE_DGPS][1]<<16) + ((long)BrickPi.SensorI2CIn[I2C_PORT][I2C_DEVICE_DGPS][2]<<8) + (long)BrickPi.SensorI2CIn[I2C_PORT][I2C_DEVICE_DGPS][3];
}
}
//read vwsat
BrickPi.SensorI2CRead [I2C_PORT][I2C_DEVICE_DGPS] = 4;
BrickPi.SensorI2COut [I2C_PORT][I2C_DEVICE_DGPS][0] = DGPS_CMD_VWSAT; //byte to write
BrickPiSetupSensors();
result = BrickPiUpdateValues(); //write and read
if(!result){
if(BrickPi.Sensor[I2C_PORT] & (0x01 << I2C_DEVICE_DGPS)){
vwsat = ((long)BrickPi.SensorI2CIn[I2C_PORT][I2C_DEVICE_DGPS][0]<<24) + ((long)BrickPi.SensorI2CIn[I2C_PORT][I2C_DEVICE_DGPS][1]<<16) + ((long)BrickPi.SensorI2CIn[I2C_PORT][I2C_DEVICE_DGPS][2]<<8) + (long)BrickPi.SensorI2CIn[I2C_PORT][I2C_DEVICE_DGPS][3];
}
}
printf("Stat:%d UTC:%06ld Lat:%ld Lon:%ld Hdng:%d Vel:%d Alt:%ld HDOP:%ld SATS:%ld n",status,UTC,lat,lon,head,velo,alttd,hdop,vwsat);
usleep(50000);
}
}
return 0;
}
Hey Devenknight, thanks so much! We’ve updated the dGPS example a bit here:
https://github.com/DexterInd/BrickPi_Python/blob/master/Sensor_Examples/DI-dGPS_test.py
What are you working on and how’s it going?