/**
* Verify that the uBlox 6 GPS receiver is set to the <1g airborne
* navigaion mode.
*/
uint8_t gps_check_nav(void)
{
GPSerror = 0;
static uint8_t request[8] = {0xB5, 0x62, 0x06, 0x24, 0x00, 0x00,
0x2A, 0x84};
sendUBX(request, 8);
// Get the message back from the GPS
gps_get_data();
// Verify sync and header bytes
if( gps_buf[0] != 0xB5 || gps_buf[1] != 0x62 ){
GPSerror = 41;
}
if( gps_buf[2] != 0x06 || gps_buf[3] != 0x24 ){
GPSerror = 42;
}
// Check 40 bytes of message checksum
if( !_gps_verify_checksum(&gps_buf[2], 40) ) {
GPSerror = 43;
}
// Return the navigation mode and let the caller analyse it
if (GPSerror == 0){
return gps_buf[8];
}
else {
return -1;
}
}
static int tomtom_gps_remove(struct platform_device *pdev)
{
struct gps_device *gpsd = NULL;
tomtom_gps_data_t *gps_drv_data = NULL;
BUG_ON(!pdev);
gpsd = platform_get_drvdata(pdev);
BUG_ON(!gpsd);
gps_drv_data = gps_get_data(gpsd);
BUG_ON(!gps_drv_data);
tomtom_gps_release_resource(pdev);
gpsd->props.power = GPS_OFF;
gps_set_power(gpsd);
gps_device_unregister(gpsd);
kfree(gps_drv_data);
printk(KERN_INFO PFX "Removed.\n");
return 0;
}
static void tomtom_gps_reset(struct gps_device *gpsd)
{
tomtom_gps_data_t *gps_drv_data = NULL;
BUG_ON(!gpsd);
gps_drv_data = gps_get_data(gpsd);
BUG_ON(!gps_drv_data);
gps_drv_data->machinfo->gps_reset();
}
static void tomtom_gps_release_resource(struct platform_device *pdev)
{
struct gps_device *gpsd = NULL;
tomtom_gps_data_t *gps_drv_data = NULL;
BUG_ON(!pdev);
gpsd = platform_get_drvdata(pdev);
BUG_ON(!gpsd);
gps_drv_data = gps_get_data(gpsd);
BUG_ON(!gps_drv_data);
if (TOMTOM_GPS_NO_IRQ != gps_drv_data->gps_irq)
free_irq(gps_drv_data->gps_irq, pdev);
}
static void tomtom_gps_set_power(struct gps_device *gpsd)
{
tomtom_gps_data_t *gps_drv_data = NULL;
BUG_ON(!gpsd);
gps_drv_data = gps_get_data(gpsd);
BUG_ON(!gps_drv_data);
if (gps_drv_data->flags & TOMTOM_GPS_SUSPENDED) {
gps_drv_data->suspend_power = gpsd->props.power;
gpsd->props.power = GPS_OFF;
}
if (gps_drv_data->flags & TOMTOM_GPS_RESUMED)
gpsd->props.power = gps_drv_data->suspend_power;
gps_drv_data->machinfo->gps_set_power(gpsd->props.power);
}
static int tomtom_gps_suspend(struct platform_device *pdev, pm_message_t state)
{
struct gps_device *gpsd = NULL;
tomtom_gps_data_t *gps_drv_data = NULL;
BUG_ON(!pdev);
gpsd = platform_get_drvdata(pdev);
BUG_ON(!gpsd);
gps_drv_data = gps_get_data(gpsd);
BUG_ON(!gps_drv_data);
gps_drv_data->flags |= TOMTOM_GPS_SUSPENDED;
gps_set_power(gpsd);
printk(KERN_INFO PFX "Suspended.\n");
return 0;
}
/**
* Poll the GPS for a position message then extract the useful
* information from it - POSLLH.
*/
void gps_get_position()
{
GPSerror = 0;
// Request a NAV-POSLLH message from the GPS
static uint8_t request[8] = {0xB5, 0x62, 0x01, 0x02, 0x00, 0x00, 0x03,
0x0A};
sendUBX(request, 8);
// Get the message back from the GPS
gps_get_data();
// Verify the sync and header bits
if( gps_buf[0] != 0xB5 || gps_buf[1] != 0x62 )
GPSerror = 21;
if( gps_buf[2] != 0x01 || gps_buf[3] != 0x02 )
GPSerror = 22;
if( !_gps_verify_checksum(&gps_buf[2], 32) ) {
GPSerror = 23;
}
if(GPSerror == 0) {
// 4 bytes of longitude (1e-7)
lon = (int32_t)gps_buf[10] | (int32_t)gps_buf[11] << 8 |
(int32_t)gps_buf[12] << 16 | (int32_t)gps_buf[13] << 24;
lon /= 1000;
// 4 bytes of latitude (1e-7)
lat = (int32_t)gps_buf[14] | (int32_t)gps_buf[15] << 8 |
(int32_t)gps_buf[16] << 16 | (int32_t)gps_buf[17] << 24;
lat /= 1000;
// 4 bytes of altitude above MSL (mm)
alt = (int32_t)gps_buf[22] | (int32_t)gps_buf[23] << 8 |
(int32_t)gps_buf[24] << 16 | (int32_t)gps_buf[25] << 24;
alt /= 1000;
}
}
static int tomtom_gps_resume(struct platform_device *pdev)
{
struct gps_device *gpsd = NULL;
tomtom_gps_data_t *gps_drv_data = NULL;
BUG_ON(!pdev);
gpsd = platform_get_drvdata(pdev);
BUG_ON(!gpsd);
gps_drv_data = gps_get_data(gpsd);
BUG_ON(!gps_drv_data);
gps_drv_data->flags &= ~TOMTOM_GPS_SUSPENDED;
gps_drv_data->flags |= TOMTOM_GPS_RESUMED;
gps_set_power(gpsd);
gps_drv_data->flags &= ~TOMTOM_GPS_RESUMED;
printk(KERN_INFO PFX "Resumed.\n");
return 0;
}
static int tomtom_gps_request_resource(struct platform_device *pdev)
{
struct gps_device *gpsd = NULL;
tomtom_gps_data_t *gps_drv_data = NULL;
struct resource *res;
int ret = 0;
BUG_ON(!pdev);
gpsd = platform_get_drvdata(pdev);
BUG_ON(!gpsd);
gps_drv_data = gps_get_data(gpsd);
BUG_ON(!gps_drv_data);
res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
if (NULL != res) {
/* The platform device provides a IRQ resource. */
printk(KERN_INFO PFX " %s device provides an IRQ resource (%s)\n",
gpsd->dev.bus_id, res->name);
gps_drv_data->gps_irq = res->start;
/* Get irqs */
if (request_irq(gps_drv_data->gps_irq, &tomtom_gps_irq_handler,
res->flags & IRQF_TRIGGER_MASK,
res->name, pdev)) {
gps_drv_data->gps_irq = TOMTOM_GPS_NO_IRQ;
printk(KERN_ERR PFX " Could not allocate IRQ (%s)!\n",
res->name);
ret = -EIO;
}
}
return ret;
}
/**
* Check the navigation status to determine the quality of the
* fix currently held by the receiver with a NAV-STATUS message.
*/
void gps_check_lock()
{
GPSerror = 0;
// Construct the request to the GPS
static uint8_t request[8] = {0xB5, 0x62, 0x01, 0x06, 0x00, 0x00,
0x07, 0x16};
sendUBX(request, 8);
// Get the message back from the GPS
gps_get_data();
// Verify the sync and header bits
if( gps_buf[0] != 0xB5 || gps_buf[1] != 0x62 ) {
GPSerror = 11;
}
if( gps_buf[2] != 0x01 || gps_buf[3] != 0x06 ) {
GPSerror = 12;
}
// Check 60 bytes minus SYNC and CHECKSUM (4 bytes)
if( !_gps_verify_checksum(&gps_buf[2], 56) ) {
GPSerror = 13;
}
if(GPSerror == 0){
// Return the value if GPSfixOK is set in 'flags'
if( gps_buf[17] & 0x01 )
lock = gps_buf[16];
else
lock = 0;
sats = gps_buf[53];
}
else {
lock = 0;
}
}
int test_function() {
while(1) {
gps_get_data();
}
return 1;
}
