mirror of https://github.com/ArduPilot/ardupilot
Add support for the DIYD MTK v1.6 firmware
Nuke AP_GPS_IMU, as nothing is using it anymore. Simplify the handling of no GPS/no fix detection. Fix prototypes for ::init and ::read. Update AP_GPS_Auto and corresponding example, nearly ready for primetime. Use uint8_t rather than byte. Strip some _error() calls to save space. More could still go. git-svn-id: https://arducopter.googlecode.com/svn/trunk@1246 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
parent
53d387bdb4
commit
7fc6515300
|
@ -8,7 +8,7 @@
|
||||||
#include "AP_GPS_406.h"
|
#include "AP_GPS_406.h"
|
||||||
#include "AP_GPS_UBLOX.h"
|
#include "AP_GPS_UBLOX.h"
|
||||||
#include "AP_GPS_MTK.h"
|
#include "AP_GPS_MTK.h"
|
||||||
#include "AP_GPS_IMU.h"
|
#include "AP_GPS_MTK16.h"
|
||||||
#include "AP_GPS_None.h"
|
#include "AP_GPS_None.h"
|
||||||
#include "AP_GPS_Auto.h"
|
#include "AP_GPS_Auto.h"
|
||||||
#include "AP_GPS_HIL.h"
|
#include "AP_GPS_HIL.h"
|
||||||
|
|
|
@ -21,7 +21,7 @@ class AP_GPS_406 : public AP_GPS_SIRF
|
||||||
public:
|
public:
|
||||||
// Methods
|
// Methods
|
||||||
AP_GPS_406(Stream *port);
|
AP_GPS_406(Stream *port);
|
||||||
void init(void);
|
virtual void init(void);
|
||||||
|
|
||||||
private:
|
private:
|
||||||
void _change_to_sirf_protocol(void);
|
void _change_to_sirf_protocol(void);
|
||||||
|
|
|
@ -3,13 +3,20 @@
|
||||||
// Auto-detecting pseudo-GPS driver
|
// Auto-detecting pseudo-GPS driver
|
||||||
//
|
//
|
||||||
|
|
||||||
#include "AP_GPS.h"
|
#include "AP_GPS.h" // includes AP_GPS_Auto.h
|
||||||
#include <stdlib.h>
|
|
||||||
#include <stdio.h>
|
|
||||||
#include <wiring.h>
|
#include <wiring.h>
|
||||||
|
|
||||||
static unsigned int baudrates[] = {38400U, 57600U, 9600U, 4800U};
|
static unsigned int baudrates[] = {38400U, 57600U, 9600U, 4800U};
|
||||||
|
|
||||||
|
AP_GPS_Auto::AP_GPS_Auto(FastSerial *port, GPS **gps) :
|
||||||
|
GPS(port),
|
||||||
|
_FSport(port), // do we need this, or can we cast _port up?
|
||||||
|
_gps(gps)
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// Do nothing at init time - it may be too early to try detecting the GPS
|
||||||
void
|
void
|
||||||
AP_GPS_Auto::init(void)
|
AP_GPS_Auto::init(void)
|
||||||
{
|
{
|
||||||
|
@ -20,7 +27,7 @@ AP_GPS_Auto::init(void)
|
||||||
//
|
//
|
||||||
// We detect the real GPS, then update the pointer we have been called through
|
// We detect the real GPS, then update the pointer we have been called through
|
||||||
// and return.
|
// and return.
|
||||||
void
|
bool
|
||||||
AP_GPS_Auto::read(void)
|
AP_GPS_Auto::read(void)
|
||||||
{
|
{
|
||||||
GPS *gps;
|
GPS *gps;
|
||||||
|
@ -30,8 +37,8 @@ AP_GPS_Auto::read(void)
|
||||||
for (;;) {
|
for (;;) {
|
||||||
// loop through possible baudrates
|
// loop through possible baudrates
|
||||||
for (i = 0; i < (sizeof(baudrates) / sizeof(baudrates[0])); i++) {
|
for (i = 0; i < (sizeof(baudrates) / sizeof(baudrates[0])); i++) {
|
||||||
printf("GPS autodetect at %d:%u\n", i, baudrates[i]);
|
Serial.printf("GPS autodetect at %d:%u\n", i, baudrates[i]);
|
||||||
_port->begin(baudrates[i]);
|
_FSport->begin(baudrates[i]);
|
||||||
if (NULL != (gps = _detect())) {
|
if (NULL != (gps = _detect())) {
|
||||||
// make the detected GPS the default
|
// make the detected GPS the default
|
||||||
*_gps = gps;
|
*_gps = gps;
|
||||||
|
@ -41,9 +48,9 @@ AP_GPS_Auto::read(void)
|
||||||
gps->init();
|
gps->init();
|
||||||
gps->update();
|
gps->update();
|
||||||
|
|
||||||
// drop back to our caller - subsequent calls through
|
// Drop back to our caller - subsequent calls through
|
||||||
// the global will not come here
|
// _gps will not come here.
|
||||||
return;
|
return false;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -72,6 +79,7 @@ AP_GPS_Auto::_detect(void)
|
||||||
// XXX We can detect babble by counting incoming characters, but
|
// XXX We can detect babble by counting incoming characters, but
|
||||||
// what would we do about it?
|
// what would we do about it?
|
||||||
//
|
//
|
||||||
|
Serial.println("draining and waiting");
|
||||||
_port->flush();
|
_port->flush();
|
||||||
then = millis();
|
then = millis();
|
||||||
do {
|
do {
|
||||||
|
@ -84,11 +92,12 @@ AP_GPS_Auto::_detect(void)
|
||||||
//
|
//
|
||||||
// Collect four characters to fingerprint a device
|
// Collect four characters to fingerprint a device
|
||||||
//
|
//
|
||||||
|
Serial.println("collecting fingerprint");
|
||||||
fingerprint[0] = _getc();
|
fingerprint[0] = _getc();
|
||||||
fingerprint[1] = _getc();
|
fingerprint[1] = _getc();
|
||||||
fingerprint[2] = _getc();
|
fingerprint[2] = _getc();
|
||||||
fingerprint[3] = _getc();
|
fingerprint[3] = _getc();
|
||||||
printf("fingerprints 0x%02x 0x%02x 0x%02x 0x%02x\n",
|
Serial.printf("fingerprints 0x%02x 0x%02x 0x%02x 0x%02x\n",
|
||||||
fingerprint[0],
|
fingerprint[0],
|
||||||
fingerprint[1],
|
fingerprint[1],
|
||||||
fingerprint[2],
|
fingerprint[2],
|
||||||
|
@ -104,23 +113,34 @@ AP_GPS_Auto::_detect(void)
|
||||||
|
|
||||||
// message 5 is MTK pretending to talk UBX
|
// message 5 is MTK pretending to talk UBX
|
||||||
if (0x05 == fingerprint[3]) {
|
if (0x05 == fingerprint[3]) {
|
||||||
printf("detected MTK in binary mode\n");
|
Serial.printf("detected MTK in binary mode\n");
|
||||||
gps = new AP_GPS_MTK(_port);
|
gps = new AP_GPS_MTK(_port);
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
// any other message is u-blox
|
// any other message is u-blox
|
||||||
printf("detected u-blox in binary mode\n");
|
Serial.printf("detected u-blox in binary mode\n");
|
||||||
gps = new AP_GPS_UBLOX(_port);
|
gps = new AP_GPS_UBLOX(_port);
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
//
|
||||||
|
// MTK v1.6
|
||||||
|
//
|
||||||
|
if ((0xd0 == fingerprint[0]) &&
|
||||||
|
(0xdd == fingerprint[1]) &&
|
||||||
|
(0x20 == fingerprint[2])) {
|
||||||
|
Serial.printf("detected MTK v1.6\n");
|
||||||
|
gps = new AP_GPS_MTK16(_port);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
//
|
//
|
||||||
// SIRF in binary mode
|
// SIRF in binary mode
|
||||||
//
|
//
|
||||||
if ((0xa0 == fingerprint[0]) &&
|
if ((0xa0 == fingerprint[0]) &&
|
||||||
(0xa2 == fingerprint[1])) {
|
(0xa2 == fingerprint[1])) {
|
||||||
printf("detected SIRF in binary mode\n");
|
Serial.printf("detected SIRF in binary mode\n");
|
||||||
gps = new AP_GPS_SIRF(_port);
|
gps = new AP_GPS_SIRF(_port);
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
@ -130,7 +150,7 @@ AP_GPS_Auto::_detect(void)
|
||||||
// and retry to avoid a false-positive on the NMEA detector.
|
// and retry to avoid a false-positive on the NMEA detector.
|
||||||
//
|
//
|
||||||
if (0 == tries) {
|
if (0 == tries) {
|
||||||
printf("sending setup strings and trying again\n");
|
Serial.printf("sending setup strings and trying again\n");
|
||||||
_port->println(MTK_SET_BINARY);
|
_port->println(MTK_SET_BINARY);
|
||||||
_port->println(UBLOX_SET_BINARY);
|
_port->println(UBLOX_SET_BINARY);
|
||||||
_port->println(SIRF_SET_BINARY);
|
_port->println(SIRF_SET_BINARY);
|
||||||
|
@ -145,7 +165,7 @@ AP_GPS_Auto::_detect(void)
|
||||||
|
|
||||||
// XXX this may be a bit presumptive, might want to give the GPS a couple of
|
// XXX this may be a bit presumptive, might want to give the GPS a couple of
|
||||||
// iterations around the loop to react to init strings?
|
// iterations around the loop to react to init strings?
|
||||||
printf("detected NMEA\n");
|
Serial.printf("detected NMEA\n");
|
||||||
gps = new AP_GPS_NMEA(_port);
|
gps = new AP_GPS_NMEA(_port);
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
|
@ -21,21 +21,19 @@ public:
|
||||||
/// @param ptr Pointer to a GPS * that will be fixed up by ::init
|
/// @param ptr Pointer to a GPS * that will be fixed up by ::init
|
||||||
/// when the GPS type has been detected.
|
/// when the GPS type has been detected.
|
||||||
///
|
///
|
||||||
AP_GPS_Auto(FastSerial *port, GPS **gps) :
|
AP_GPS_Auto(FastSerial *port, GPS **gps);
|
||||||
_port(port),
|
|
||||||
_gps(gps)
|
|
||||||
{};
|
|
||||||
|
|
||||||
void init(void);
|
/// Dummy init routine, does nothing
|
||||||
|
virtual void init(void);
|
||||||
|
|
||||||
/// Detect and initialise the attached GPS unit. Returns a
|
/// Detect and initialise the attached GPS unit. Updates the
|
||||||
/// pointer to the allocated & initialised GPS driver.
|
/// pointer passed into the constructor when a GPS is detected.
|
||||||
///
|
///
|
||||||
void read(void);
|
virtual bool read(void);
|
||||||
|
|
||||||
private:
|
private:
|
||||||
/// Serial port connected to the GPS.
|
/// Serial port connected to the GPS.
|
||||||
FastSerial *_port;
|
FastSerial *_FSport;
|
||||||
|
|
||||||
/// global GPS driver pointer, updated by auto-detection
|
/// global GPS driver pointer, updated by auto-detection
|
||||||
///
|
///
|
||||||
|
|
|
@ -24,18 +24,13 @@ void AP_GPS_HIL::init(void)
|
||||||
{
|
{
|
||||||
}
|
}
|
||||||
|
|
||||||
void AP_GPS_HIL::read(void)
|
bool AP_GPS_HIL::read(void)
|
||||||
{
|
{
|
||||||
}
|
bool result = _updated;
|
||||||
|
|
||||||
int AP_GPS_HIL::status(void)
|
// return true once for each update pushed in
|
||||||
{
|
_updated = false;
|
||||||
if (valid_read)
|
return result;
|
||||||
{
|
|
||||||
if (fix) return 2;
|
|
||||||
else return 1;
|
|
||||||
}
|
|
||||||
else return 0;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void AP_GPS_HIL::setHIL(long _time, float _latitude, float _longitude, float _altitude,
|
void AP_GPS_HIL::setHIL(long _time, float _latitude, float _longitude, float _altitude,
|
||||||
|
@ -49,9 +44,7 @@ void AP_GPS_HIL::setHIL(long _time, float _latitude, float _longitude, float _al
|
||||||
ground_course = _ground_course*1.0e2;
|
ground_course = _ground_course*1.0e2;
|
||||||
speed_3d = _speed_3d*1.0e2;
|
speed_3d = _speed_3d*1.0e2;
|
||||||
num_sats = _num_sats;
|
num_sats = _num_sats;
|
||||||
new_data = true;
|
|
||||||
fix = true;
|
fix = true;
|
||||||
valid_read = true;
|
_updated = true;
|
||||||
_setTime();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -17,9 +17,9 @@
|
||||||
class AP_GPS_HIL : public GPS {
|
class AP_GPS_HIL : public GPS {
|
||||||
public:
|
public:
|
||||||
AP_GPS_HIL(Stream *s);
|
AP_GPS_HIL(Stream *s);
|
||||||
void init(void);
|
virtual void init(void);
|
||||||
void read(void);
|
virtual bool read(void);
|
||||||
int status(void);
|
|
||||||
/**
|
/**
|
||||||
* Hardware in the loop set function
|
* Hardware in the loop set function
|
||||||
* @param latitude - latitude in deggrees
|
* @param latitude - latitude in deggrees
|
||||||
|
@ -32,6 +32,9 @@ public:
|
||||||
*/
|
*/
|
||||||
void setHIL(long time, float latitude, float longitude, float altitude,
|
void setHIL(long time, float latitude, float longitude, float altitude,
|
||||||
float ground_speed, float ground_course, float speed_3d, uint8_t num_sats);
|
float ground_speed, float ground_course, float speed_3d, uint8_t num_sats);
|
||||||
|
|
||||||
|
private:
|
||||||
|
bool _updated;
|
||||||
};
|
};
|
||||||
|
|
||||||
#endif // AP_GPS_HIL_H
|
#endif // AP_GPS_HIL_H
|
||||||
|
|
|
@ -1,224 +0,0 @@
|
||||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
|
||||||
/*
|
|
||||||
GPS_MTK.cpp - Ublox GPS library for Arduino
|
|
||||||
Code by Jordi MuÒoz and Jose Julio. DIYDrones.com
|
|
||||||
This code works with boards based on ATMega168/328 and ATMega1280 (Serial port 1)
|
|
||||||
|
|
||||||
This library is free software; you can redistribute it and/or
|
|
||||||
modify it under the terms of the GNU Lesser General Public
|
|
||||||
License as published by the Free Software Foundation; either
|
|
||||||
version 2.1 of the License, or (at your option) any later version.
|
|
||||||
|
|
||||||
GPS configuration : Costum protocol
|
|
||||||
Baud rate : 38400
|
|
||||||
|
|
||||||
Methods:
|
|
||||||
init() : GPS initialization
|
|
||||||
update() : Call this funcion as often as you want to ensure you read the incomming gps data
|
|
||||||
|
|
||||||
Properties:
|
|
||||||
lattitude : lattitude * 10000000 (long value)
|
|
||||||
longitude : longitude * 10000000 (long value)
|
|
||||||
altitude : altitude * 100 (meters) (long value)
|
|
||||||
ground_speed : Speed (m/s) * 100 (long value)
|
|
||||||
ground_course : Course (degrees) * 100 (long value)
|
|
||||||
new_data : 1 when a new data is received.
|
|
||||||
You need to write a 0 to new_data when you read the data
|
|
||||||
fix : 1: GPS NO fix, 2: 2D fix, 3: 3D fix.
|
|
||||||
|
|
||||||
*/
|
|
||||||
#include "AP_GPS_IMU.h"
|
|
||||||
#include "WProgram.h"
|
|
||||||
|
|
||||||
|
|
||||||
// Constructors ////////////////////////////////////////////////////////////////
|
|
||||||
AP_GPS_IMU::AP_GPS_IMU(Stream *s) : GPS(s)
|
|
||||||
{
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
// Public Methods //////////////////////////////////////////////////////////////
|
|
||||||
void AP_GPS_IMU::init(void)
|
|
||||||
{
|
|
||||||
// we expect the stream to already be open at the corret bitrate
|
|
||||||
}
|
|
||||||
|
|
||||||
// optimization : This code doesn't wait for data. It only proccess the data available.
|
|
||||||
// We can call this function on the main loop (50Hz loop)
|
|
||||||
// If we get a complete packet this function calls parse_IMU_gps() to parse and update the GPS info.
|
|
||||||
void AP_GPS_IMU::read(void)
|
|
||||||
{
|
|
||||||
byte data;
|
|
||||||
int numc = 0;
|
|
||||||
static byte message_num = 0;
|
|
||||||
|
|
||||||
numc = _port->available();
|
|
||||||
|
|
||||||
if (numc > 0){
|
|
||||||
for (int i=0;i<numc;i++){ // Process bytes received
|
|
||||||
|
|
||||||
data = _port->read();
|
|
||||||
|
|
||||||
switch(step){ //Normally we start from zero. This is a state machine
|
|
||||||
case 0:
|
|
||||||
if(data == 0x44) // IMU sync char 1
|
|
||||||
step++; //OH first data packet is correct, so jump to the next step
|
|
||||||
break;
|
|
||||||
|
|
||||||
case 1:
|
|
||||||
if(data == 0x49) // IMU sync char 2
|
|
||||||
step++; //ooh! The second data packet is correct, jump to the step 2
|
|
||||||
else
|
|
||||||
step=0; //Nop, is not correct so restart to step zero and try again.
|
|
||||||
break;
|
|
||||||
|
|
||||||
case 2:
|
|
||||||
if(data == 0x59) // IMU sync char 3
|
|
||||||
step++; //ooh! The second data packet is correct, jump to the step 2
|
|
||||||
else
|
|
||||||
step=0; //Nop, is not correct so restart to step zero and try again.
|
|
||||||
break;
|
|
||||||
|
|
||||||
case 3:
|
|
||||||
if(data == 0x64) // IMU sync char 4
|
|
||||||
step++; //ooh! The second data packet is correct, jump to the step 2
|
|
||||||
else
|
|
||||||
step=0; //Nop, is not correct so restart to step zero and try again.
|
|
||||||
break;
|
|
||||||
|
|
||||||
case 4:
|
|
||||||
payload_length = data;
|
|
||||||
checksum(payload_length);
|
|
||||||
step++;
|
|
||||||
if (payload_length > 28){
|
|
||||||
step = 0; //Bad data, so restart to step zero and try again.
|
|
||||||
payload_counter = 0;
|
|
||||||
ck_a = 0;
|
|
||||||
ck_b = 0;
|
|
||||||
//payload_error_count++;
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
|
|
||||||
case 5:
|
|
||||||
message_num = data;
|
|
||||||
checksum(data);
|
|
||||||
step++;
|
|
||||||
break;
|
|
||||||
|
|
||||||
case 6: // Payload data read...
|
|
||||||
// We stay in this state until we reach the payload_length
|
|
||||||
buffer[payload_counter] = data;
|
|
||||||
checksum(data);
|
|
||||||
payload_counter++;
|
|
||||||
if (payload_counter >= payload_length) {
|
|
||||||
step++;
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
|
|
||||||
case 7:
|
|
||||||
GPS_ck_a = data; // First checksum byte
|
|
||||||
step++;
|
|
||||||
break;
|
|
||||||
|
|
||||||
case 8:
|
|
||||||
GPS_ck_b = data; // Second checksum byte
|
|
||||||
|
|
||||||
// We end the IMU/GPS read...
|
|
||||||
// Verify the received checksum with the generated checksum..
|
|
||||||
if((ck_a == GPS_ck_a) && (ck_b == GPS_ck_b)) {
|
|
||||||
if (message_num == 0x02) {
|
|
||||||
join_data();
|
|
||||||
} else if (message_num == 0x03) {
|
|
||||||
GPS_join_data();
|
|
||||||
} else if (message_num == 0x04) {
|
|
||||||
join_data_xplane();
|
|
||||||
} else if (message_num == 0x0a) {
|
|
||||||
//PERF_join_data();
|
|
||||||
} else {
|
|
||||||
_error("Invalid message number = %d\n", (int)message_num);
|
|
||||||
}
|
|
||||||
} else {
|
|
||||||
_error("XXX Checksum error\n"); //bad checksum
|
|
||||||
//imu_checksum_error_count++;
|
|
||||||
}
|
|
||||||
// Variable initialization
|
|
||||||
step = 0;
|
|
||||||
payload_counter = 0;
|
|
||||||
ck_a = 0;
|
|
||||||
ck_b = 0;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}// End for...
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/****************************************************************
|
|
||||||
*
|
|
||||||
****************************************************************/
|
|
||||||
|
|
||||||
void AP_GPS_IMU::join_data(void)
|
|
||||||
{
|
|
||||||
//Verifing if we are in class 1, you can change this "IF" for a "Switch" in case you want to use other IMU classes..
|
|
||||||
//In this case all the message im using are in class 1, to know more about classes check PAGE 60 of DataSheet.
|
|
||||||
|
|
||||||
//Storing IMU roll
|
|
||||||
roll_sensor = *(int *)&buffer[0];
|
|
||||||
|
|
||||||
//Storing IMU pitch
|
|
||||||
pitch_sensor = *(int *)&buffer[2];
|
|
||||||
|
|
||||||
//Storing IMU heading (yaw)
|
|
||||||
ground_course = *(int *)&buffer[4];
|
|
||||||
imu_ok = true;
|
|
||||||
}
|
|
||||||
|
|
||||||
void AP_GPS_IMU::join_data_xplane()
|
|
||||||
{
|
|
||||||
//Storing IMU roll
|
|
||||||
roll_sensor = *(int *)&buffer[0];
|
|
||||||
|
|
||||||
|
|
||||||
//Storing IMU pitch
|
|
||||||
pitch_sensor = *(int *)&buffer[2];
|
|
||||||
|
|
||||||
//Storing IMU heading (yaw)
|
|
||||||
ground_course = *(unsigned int *)&buffer[4];
|
|
||||||
|
|
||||||
//Storing airspeed
|
|
||||||
airspeed = *(int *)&buffer[6];
|
|
||||||
|
|
||||||
imu_ok = true;
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
void AP_GPS_IMU::GPS_join_data(void)
|
|
||||||
{
|
|
||||||
longitude = *(long *)&buffer[0]; // degrees * 10e7
|
|
||||||
latitude = *(long *)&buffer[4];
|
|
||||||
|
|
||||||
//Storing GPS Height above the sea level
|
|
||||||
altitude = (long)*(int *)&buffer[8] * 10;
|
|
||||||
|
|
||||||
//Storing Speed
|
|
||||||
speed_3d = ground_speed = (float)*(int *)&buffer[10];
|
|
||||||
|
|
||||||
//We skip the gps ground course because we use yaw value from the IMU for ground course
|
|
||||||
time = *(long *)&buffer[14];
|
|
||||||
|
|
||||||
imu_health = buffer[15];
|
|
||||||
|
|
||||||
new_data = true;
|
|
||||||
fix = true;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
/****************************************************************
|
|
||||||
*
|
|
||||||
****************************************************************/
|
|
||||||
// checksum algorithm
|
|
||||||
void AP_GPS_IMU::checksum(byte data)
|
|
||||||
{
|
|
||||||
ck_a += data;
|
|
||||||
ck_b += ck_a;
|
|
||||||
}
|
|
|
@ -1,44 +0,0 @@
|
||||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
|
||||||
#ifndef AP_GPS_IMU_h
|
|
||||||
#define AP_GPS_IMU_h
|
|
||||||
|
|
||||||
#include <GPS.h>
|
|
||||||
#define MAXPAYLOAD 32
|
|
||||||
|
|
||||||
class AP_GPS_IMU : public GPS
|
|
||||||
{
|
|
||||||
public:
|
|
||||||
|
|
||||||
// Methods
|
|
||||||
AP_GPS_IMU(Stream *s);
|
|
||||||
void init();
|
|
||||||
void read();
|
|
||||||
|
|
||||||
// Properties
|
|
||||||
long roll_sensor; // how much we're turning in deg * 100
|
|
||||||
long pitch_sensor; // our angle of attack in deg * 100
|
|
||||||
int airspeed;
|
|
||||||
float imu_health;
|
|
||||||
uint8_t imu_ok;
|
|
||||||
|
|
||||||
private:
|
|
||||||
// Packet checksums
|
|
||||||
uint8_t ck_a;
|
|
||||||
uint8_t ck_b;
|
|
||||||
uint8_t GPS_ck_a;
|
|
||||||
uint8_t GPS_ck_b;
|
|
||||||
|
|
||||||
uint8_t step;
|
|
||||||
uint8_t msg_class;
|
|
||||||
uint8_t message_num;
|
|
||||||
uint8_t payload_length;
|
|
||||||
uint8_t payload_counter;
|
|
||||||
uint8_t buffer[MAXPAYLOAD];
|
|
||||||
|
|
||||||
void join_data();
|
|
||||||
void join_data_xplane();
|
|
||||||
void GPS_join_data();
|
|
||||||
void checksum(unsigned char data);
|
|
||||||
};
|
|
||||||
|
|
||||||
#endif
|
|
|
@ -12,7 +12,7 @@
|
||||||
//
|
//
|
||||||
|
|
||||||
#include "AP_GPS_MTK.h"
|
#include "AP_GPS_MTK.h"
|
||||||
#include "WProgram.h"
|
#include <stdint.h>
|
||||||
|
|
||||||
// Constructors ////////////////////////////////////////////////////////////////
|
// Constructors ////////////////////////////////////////////////////////////////
|
||||||
AP_GPS_MTK::AP_GPS_MTK(Stream *s) : GPS(s)
|
AP_GPS_MTK::AP_GPS_MTK(Stream *s) : GPS(s)
|
||||||
|
@ -20,7 +20,8 @@ AP_GPS_MTK::AP_GPS_MTK(Stream *s) : GPS(s)
|
||||||
}
|
}
|
||||||
|
|
||||||
// Public Methods //////////////////////////////////////////////////////////////
|
// Public Methods //////////////////////////////////////////////////////////////
|
||||||
void AP_GPS_MTK::init(void)
|
void
|
||||||
|
AP_GPS_MTK::init(void)
|
||||||
{
|
{
|
||||||
_port->flush();
|
_port->flush();
|
||||||
// initialize serial port for binary protocol use
|
// initialize serial port for binary protocol use
|
||||||
|
@ -42,10 +43,12 @@ void AP_GPS_MTK::init(void)
|
||||||
// The lack of a standard header length field makes it impossible to skip
|
// The lack of a standard header length field makes it impossible to skip
|
||||||
// unrecognised messages.
|
// unrecognised messages.
|
||||||
//
|
//
|
||||||
void AP_GPS_MTK::read(void)
|
bool
|
||||||
|
AP_GPS_MTK::read(void)
|
||||||
{
|
{
|
||||||
byte data;
|
uint8_t data;
|
||||||
int numc;
|
int numc;
|
||||||
|
bool parsed = false;
|
||||||
|
|
||||||
numc = _port->available();
|
numc = _port->available();
|
||||||
for (int i = 0; i < numc; i++){ // Process bytes received
|
for (int i = 0; i < numc; i++){ // Process bytes received
|
||||||
|
@ -89,7 +92,6 @@ restart:
|
||||||
if (MESSAGE_ID == data) {
|
if (MESSAGE_ID == data) {
|
||||||
_step++;
|
_step++;
|
||||||
_ck_b += (_ck_a += data);
|
_ck_b += (_ck_a += data);
|
||||||
_payload_length = sizeof(diyd_mtk_msg); // prepare to receive our message
|
|
||||||
_payload_counter = 0;
|
_payload_counter = 0;
|
||||||
} else {
|
} else {
|
||||||
_step = 0;
|
_step = 0;
|
||||||
|
@ -102,7 +104,7 @@ restart:
|
||||||
case 4:
|
case 4:
|
||||||
_buffer.bytes[_payload_counter++] = data;
|
_buffer.bytes[_payload_counter++] = data;
|
||||||
_ck_b += (_ck_a += data);
|
_ck_b += (_ck_a += data);
|
||||||
if (_payload_counter == _payload_length)
|
if (_payload_counter == sizeof(_buffer))
|
||||||
_step++;
|
_step++;
|
||||||
break;
|
break;
|
||||||
|
|
||||||
|
@ -121,26 +123,20 @@ restart:
|
||||||
_error("GPS_MTK: checksum error\n");
|
_error("GPS_MTK: checksum error\n");
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
_parse_gps(); // Parse the new GPS packet
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Private Methods
|
fix = (_buffer.msg.fix_type == FIX_3D);
|
||||||
void
|
latitude = _swapl(&_buffer.msg.latitude) * 10;
|
||||||
AP_GPS_MTK::_parse_gps(void)
|
longitude = _swapl(&_buffer.msg.longitude) * 10;
|
||||||
{
|
altitude = _swapl(&_buffer.msg.altitude);
|
||||||
fix = (_buffer.msg.fix_type == FIX_3D);
|
ground_speed = _swapl(&_buffer.msg.ground_speed);
|
||||||
latitude = _swapl(&_buffer.msg.latitude) * 10;
|
ground_course = _swapl(&_buffer.msg.ground_course) / 10000;
|
||||||
longitude = _swapl(&_buffer.msg.longitude) * 10;
|
num_sats = _buffer.msg.satellites;
|
||||||
altitude = _swapl(&_buffer.msg.altitude);
|
|
||||||
ground_speed = _swapl(&_buffer.msg.ground_speed);
|
|
||||||
ground_course = _swapl(&_buffer.msg.ground_course) / 10000;
|
|
||||||
num_sats = _buffer.msg.satellites;
|
|
||||||
|
|
||||||
// XXX docs say this is UTC, but our clients expect msToW
|
// XXX docs say this is UTC, but our clients expect msToW
|
||||||
time = _swapl(&_buffer.msg.utc_time);
|
time = _swapl(&_buffer.msg.utc_time);
|
||||||
_setTime();
|
|
||||||
valid_read = true;
|
parsed = true;
|
||||||
new_data = true;
|
}
|
||||||
|
}
|
||||||
|
return parsed;
|
||||||
}
|
}
|
||||||
|
|
|
@ -10,34 +10,19 @@
|
||||||
//
|
//
|
||||||
// GPS configuration : Custom protocol per "DIYDrones Custom Binary Sentence Specification V1.1"
|
// GPS configuration : Custom protocol per "DIYDrones Custom Binary Sentence Specification V1.1"
|
||||||
//
|
//
|
||||||
|
// Note - see AP_GPS_MTK16.h for firmware 1.6 and later.
|
||||||
|
//
|
||||||
#ifndef AP_GPS_MTK_h
|
#ifndef AP_GPS_MTK_h
|
||||||
#define AP_GPS_MTK_h
|
#define AP_GPS_MTK_h
|
||||||
|
|
||||||
#include <GPS.h>
|
#include <GPS.h>
|
||||||
#define MAXPAYLOAD 32
|
#include "AP_GPS_MTK_Common.h"
|
||||||
|
|
||||||
#define MTK_SET_BINARY "$PGCMD,16,0,0,0,0,0*6A\r\n"
|
|
||||||
#define MTK_SET_NMEA "$PGCMD,16,1,1,1,1,1*6B\r\n"
|
|
||||||
|
|
||||||
#define MTK_OUTPUT_1HZ "$PMTK220,1000*1F\r\n"
|
|
||||||
#define MTK_OUTPUT_2HZ "$PMTK220,500*2B\r\n"
|
|
||||||
#define MTK_OUTPUT_4HZ "$PMTK220,250*29\r\n"
|
|
||||||
#define MTK_OTUPUT_5HZ "$PMTK220,200*2C\r\n"
|
|
||||||
#define MTK_OUTPUT_10HZ "$PMTK220,100*2F\r\n"
|
|
||||||
|
|
||||||
#define MTK_BAUD_RATE_38400 "$PMTK251,38400*27\r\n"
|
|
||||||
|
|
||||||
#define SBAS_ON "$PMTK313,1*2E\r\n"
|
|
||||||
#define SBAS_OFF "$PMTK313,0*2F\r\n"
|
|
||||||
|
|
||||||
#define WAAS_ON "$PSRF151,1*3F\r\n"
|
|
||||||
#define WAAS_OFF "$PSRF151,0*3E\r\n"
|
|
||||||
|
|
||||||
class AP_GPS_MTK : public GPS {
|
class AP_GPS_MTK : public GPS {
|
||||||
public:
|
public:
|
||||||
AP_GPS_MTK(Stream *s);
|
AP_GPS_MTK(Stream *s);
|
||||||
virtual void init(void);
|
virtual void init(void);
|
||||||
virtual void read(void);
|
virtual bool read(void);
|
||||||
|
|
||||||
private:
|
private:
|
||||||
#pragma pack(1)
|
#pragma pack(1)
|
||||||
|
@ -71,7 +56,6 @@ private:
|
||||||
|
|
||||||
// State machine state
|
// State machine state
|
||||||
uint8_t _step;
|
uint8_t _step;
|
||||||
uint8_t _payload_length;
|
|
||||||
uint8_t _payload_counter;
|
uint8_t _payload_counter;
|
||||||
|
|
||||||
// Receive buffer
|
// Receive buffer
|
||||||
|
|
|
@ -0,0 +1,133 @@
|
||||||
|
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
||||||
|
//
|
||||||
|
// DIYDrones Custom Mediatek GPS driver for ArduPilot and ArduPilotMega.
|
||||||
|
// Code by Michael Smith, Jordi Munoz and Jose Julio, DIYDrones.com
|
||||||
|
//
|
||||||
|
// This library is free software; you can redistribute it and / or
|
||||||
|
// modify it under the terms of the GNU Lesser General Public
|
||||||
|
// License as published by the Free Software Foundation; either
|
||||||
|
// version 2.1 of the License, or (at your option) any later version.
|
||||||
|
//
|
||||||
|
// GPS configuration : Custom protocol per "DIYDrones Custom Binary Sentence Specification V1.1"
|
||||||
|
//
|
||||||
|
|
||||||
|
#include "AP_GPS_MTK16.h"
|
||||||
|
#include <stdint.h>
|
||||||
|
|
||||||
|
// Constructors ////////////////////////////////////////////////////////////////
|
||||||
|
AP_GPS_MTK16::AP_GPS_MTK16(Stream *s) : GPS(s)
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
// Public Methods //////////////////////////////////////////////////////////////
|
||||||
|
void
|
||||||
|
AP_GPS_MTK16::init(void)
|
||||||
|
{
|
||||||
|
_port->flush();
|
||||||
|
|
||||||
|
// initialize serial port for binary protocol use
|
||||||
|
// XXX should assume binary, let GPS_AUTO handle dynamic config?
|
||||||
|
_port->print(MTK_SET_BINARY);
|
||||||
|
|
||||||
|
// set 4Hz update rate
|
||||||
|
_port->print(MTK_OUTPUT_4HZ);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Process bytes available from the stream
|
||||||
|
//
|
||||||
|
// The stream is assumed to contain only our custom message. If it
|
||||||
|
// contains other messages, and those messages contain the preamble bytes,
|
||||||
|
// it is possible for this code to become de-synchronised. Without
|
||||||
|
// buffering the entire message and re-processing it from the top,
|
||||||
|
// this is unavoidable.
|
||||||
|
//
|
||||||
|
// The lack of a standard header length field makes it impossible to skip
|
||||||
|
// unrecognised messages.
|
||||||
|
//
|
||||||
|
bool
|
||||||
|
AP_GPS_MTK16::read(void)
|
||||||
|
{
|
||||||
|
uint8_t data;
|
||||||
|
int numc;
|
||||||
|
bool parsed = false;
|
||||||
|
|
||||||
|
numc = _port->available();
|
||||||
|
for (int i = 0; i < numc; i++) { // Process bytes received
|
||||||
|
|
||||||
|
// read the next byte
|
||||||
|
data = _port->read();
|
||||||
|
|
||||||
|
restart:
|
||||||
|
switch(_step){
|
||||||
|
|
||||||
|
// Message preamble, class, ID detection
|
||||||
|
//
|
||||||
|
// If we fail to match any of the expected bytes, we
|
||||||
|
// reset the state machine and re-consider the failed
|
||||||
|
// byte as the first byte of the preamble. This
|
||||||
|
// improves our chances of recovering from a mismatch
|
||||||
|
// and makes it less likely that we will be fooled by
|
||||||
|
// the preamble appearing as data in some other message.
|
||||||
|
//
|
||||||
|
case 0:
|
||||||
|
if(PREAMBLE1 == data)
|
||||||
|
_step++;
|
||||||
|
break;
|
||||||
|
case 1:
|
||||||
|
if (PREAMBLE2 == data) {
|
||||||
|
_step++;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
_step = 0;
|
||||||
|
goto restart;
|
||||||
|
case 2:
|
||||||
|
if (sizeof(_buffer) == data) {
|
||||||
|
_step++;
|
||||||
|
_ck_b = _ck_a = data; // reset the checksum accumulators
|
||||||
|
_payload_counter = 0;
|
||||||
|
} else {
|
||||||
|
_step = 0; // reset and wait for a message of the right class
|
||||||
|
goto restart;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
// Receive message data
|
||||||
|
//
|
||||||
|
case 3:
|
||||||
|
_buffer.bytes[_payload_counter++] = data;
|
||||||
|
_ck_b += (_ck_a += data);
|
||||||
|
if (_payload_counter == sizeof(_buffer))
|
||||||
|
_step++;
|
||||||
|
break;
|
||||||
|
|
||||||
|
// Checksum and message processing
|
||||||
|
//
|
||||||
|
case 4:
|
||||||
|
_step++;
|
||||||
|
if (_ck_a != data) {
|
||||||
|
_step = 0;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
case 5:
|
||||||
|
_step = 0;
|
||||||
|
if (_ck_b != data) {
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
fix = _buffer.msg.fix_type == FIX_3D;
|
||||||
|
latitude = _buffer.msg.latitude * 10; // XXX doc says *10e7 but device says otherwise
|
||||||
|
longitude = _buffer.msg.longitude * 10; // XXX doc says *10e7 but device says otherwise
|
||||||
|
altitude = _buffer.msg.altitude;
|
||||||
|
ground_speed = _buffer.msg.ground_speed;
|
||||||
|
ground_course = _buffer.msg.ground_course;
|
||||||
|
num_sats = _buffer.msg.satellites;
|
||||||
|
hdop = _buffer.msg.hdop;
|
||||||
|
|
||||||
|
// XXX docs say this is UTC, but our clients expect msToW
|
||||||
|
time = _swapl(&_buffer.msg.utc_time);
|
||||||
|
|
||||||
|
parsed = true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return parsed;
|
||||||
|
}
|
|
@ -0,0 +1,66 @@
|
||||||
|
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
||||||
|
//
|
||||||
|
// DIYDrones Custom Mediatek GPS driver for ArduPilot and ArduPilotMega.
|
||||||
|
// Code by Michael Smith, Jordi Munoz and Jose Julio, DIYDrones.com
|
||||||
|
//
|
||||||
|
// This library is free software; you can redistribute it and / or
|
||||||
|
// modify it under the terms of the GNU Lesser General Public
|
||||||
|
// License as published by the Free Software Foundation; either
|
||||||
|
// version 2.1 of the License, or (at your option) any later version.
|
||||||
|
//
|
||||||
|
// GPS configuration : Custom protocol per "Customize Function Specification, 3D Robotics, v1.6"
|
||||||
|
//
|
||||||
|
#ifndef AP_GPS_MTK16_h
|
||||||
|
#define AP_GPS_MTK16_h
|
||||||
|
|
||||||
|
#include <GPS.h>
|
||||||
|
#include "AP_GPS_MTK_Common.h"
|
||||||
|
|
||||||
|
class AP_GPS_MTK16 : public GPS {
|
||||||
|
public:
|
||||||
|
AP_GPS_MTK16(Stream *s);
|
||||||
|
virtual void init(void);
|
||||||
|
virtual bool read(void);
|
||||||
|
|
||||||
|
private:
|
||||||
|
#pragma pack(1)
|
||||||
|
struct diyd_mtk_msg {
|
||||||
|
int32_t latitude;
|
||||||
|
int32_t longitude;
|
||||||
|
int32_t altitude;
|
||||||
|
int32_t ground_speed;
|
||||||
|
int32_t ground_course;
|
||||||
|
uint8_t satellites;
|
||||||
|
uint8_t fix_type;
|
||||||
|
uint32_t utc_date;
|
||||||
|
uint32_t utc_time;
|
||||||
|
uint16_t hdop;
|
||||||
|
};
|
||||||
|
#pragma pack(pop)
|
||||||
|
enum diyd_mtk_fix_type {
|
||||||
|
FIX_NONE = 1,
|
||||||
|
FIX_2D = 2,
|
||||||
|
FIX_3D = 3
|
||||||
|
};
|
||||||
|
|
||||||
|
enum diyd_mtk_protocol_bytes {
|
||||||
|
PREAMBLE1 = 0xd0,
|
||||||
|
PREAMBLE2 = 0xdd,
|
||||||
|
};
|
||||||
|
|
||||||
|
// Packet checksum accumulators
|
||||||
|
uint8_t _ck_a;
|
||||||
|
uint8_t _ck_b;
|
||||||
|
|
||||||
|
// State machine state
|
||||||
|
uint8_t _step;
|
||||||
|
uint8_t _payload_counter;
|
||||||
|
|
||||||
|
// Receive buffer
|
||||||
|
union {
|
||||||
|
diyd_mtk_msg msg;
|
||||||
|
uint8_t bytes[];
|
||||||
|
} _buffer;
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif // AP_GPS_MTK16_H
|
|
@ -0,0 +1,33 @@
|
||||||
|
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
||||||
|
//
|
||||||
|
// DIYDrones Custom Mediatek GPS driver for ArduPilot and ArduPilotMega.
|
||||||
|
// Code by Michael Smith, Jordi Munoz and Jose Julio, DIYDrones.com
|
||||||
|
//
|
||||||
|
// This library is free software; you can redistribute it and / or
|
||||||
|
// modify it under the terms of the GNU Lesser General Public
|
||||||
|
// License as published by the Free Software Foundation; either
|
||||||
|
// version 2.1 of the License, or (at your option) any later version.
|
||||||
|
//
|
||||||
|
// Common definitions for MediaTek GPS modules.
|
||||||
|
|
||||||
|
#ifndef AP_GPS_MTK_Common_h
|
||||||
|
#define AP_GPS_MTK_Common_h
|
||||||
|
|
||||||
|
#define MTK_SET_BINARY "$PGCMD,16,0,0,0,0,0*6A\r\n"
|
||||||
|
#define MTK_SET_NMEA "$PGCMD,16,1,1,1,1,1*6B\r\n"
|
||||||
|
|
||||||
|
#define MTK_OUTPUT_1HZ "$PMTK220,1000*1F\r\n"
|
||||||
|
#define MTK_OUTPUT_2HZ "$PMTK220,500*2B\r\n"
|
||||||
|
#define MTK_OUTPUT_4HZ "$PMTK220,250*29\r\n"
|
||||||
|
#define MTK_OTUPUT_5HZ "$PMTK220,200*2C\r\n"
|
||||||
|
#define MTK_OUTPUT_10HZ "$PMTK220,100*2F\r\n"
|
||||||
|
|
||||||
|
#define MTK_BAUD_RATE_38400 "$PMTK251,38400*27\r\n"
|
||||||
|
|
||||||
|
#define SBAS_ON "$PMTK313,1*2E\r\n"
|
||||||
|
#define SBAS_OFF "$PMTK313,0*2F\r\n"
|
||||||
|
|
||||||
|
#define WAAS_ON "$PSRF151,1*3F\r\n"
|
||||||
|
#define WAAS_OFF "$PSRF151,0*3E\r\n"
|
||||||
|
|
||||||
|
#endif // AP_GPS_MTK_Common_h
|
|
@ -39,7 +39,6 @@
|
||||||
NOTE : This code has been tested on a Locosys 20031 GPS receiver (MTK chipset)
|
NOTE : This code has been tested on a Locosys 20031 GPS receiver (MTK chipset)
|
||||||
*/
|
*/
|
||||||
#include "AP_GPS_NMEA.h"
|
#include "AP_GPS_NMEA.h"
|
||||||
#include "WProgram.h"
|
|
||||||
|
|
||||||
// Constructors ////////////////////////////////////////////////////////////////
|
// Constructors ////////////////////////////////////////////////////////////////
|
||||||
AP_GPS_NMEA::AP_GPS_NMEA(Stream *s) : GPS(s)
|
AP_GPS_NMEA::AP_GPS_NMEA(Stream *s) : GPS(s)
|
||||||
|
@ -62,12 +61,13 @@ AP_GPS_NMEA::init(void)
|
||||||
// This code don´t wait for data, only proccess the data available on serial port
|
// This code don´t wait for data, only proccess the data available on serial port
|
||||||
// We can call this function on the main loop (50Hz loop)
|
// We can call this function on the main loop (50Hz loop)
|
||||||
// If we get a complete packet this function call parse_nmea_gps() to parse and update the GPS info.
|
// If we get a complete packet this function call parse_nmea_gps() to parse and update the GPS info.
|
||||||
void
|
bool
|
||||||
AP_GPS_NMEA::read(void)
|
AP_GPS_NMEA::read(void)
|
||||||
{
|
{
|
||||||
char c;
|
char c;
|
||||||
int numc;
|
int numc;
|
||||||
int i;
|
int i;
|
||||||
|
bool parsed = false;
|
||||||
|
|
||||||
numc = _port->available();
|
numc = _port->available();
|
||||||
|
|
||||||
|
@ -83,7 +83,7 @@ AP_GPS_NMEA::read(void)
|
||||||
}
|
}
|
||||||
if (c == '\r'){ // NMEA End
|
if (c == '\r'){ // NMEA End
|
||||||
buffer[bufferidx++] = 0;
|
buffer[bufferidx++] = 0;
|
||||||
parse_nmea_gps();
|
parsed = parse_nmea_gps();
|
||||||
} else {
|
} else {
|
||||||
if (bufferidx < (GPS_BUFFERSIZE - 1)){
|
if (bufferidx < (GPS_BUFFERSIZE - 1)){
|
||||||
if (c == '*')
|
if (c == '*')
|
||||||
|
@ -104,7 +104,7 @@ AP_GPS_NMEA::read(void)
|
||||||
*
|
*
|
||||||
* * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * **/
|
* * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * **/
|
||||||
// Private Methods //////////////////////////////////////////////////////////////
|
// Private Methods //////////////////////////////////////////////////////////////
|
||||||
void
|
bool
|
||||||
AP_GPS_NMEA::parse_nmea_gps(void)
|
AP_GPS_NMEA::parse_nmea_gps(void)
|
||||||
{
|
{
|
||||||
uint8_t NMEA_check;
|
uint8_t NMEA_check;
|
||||||
|
@ -117,9 +117,6 @@ AP_GPS_NMEA::parse_nmea_gps(void)
|
||||||
NMEA_check = parseHex(buffer[bufferidx - 3]) * 16 + parseHex(buffer[bufferidx - 2]); // Read the checksums characters
|
NMEA_check = parseHex(buffer[bufferidx - 3]) * 16 + parseHex(buffer[bufferidx - 2]); // Read the checksums characters
|
||||||
if (GPS_checksum == NMEA_check){ // Checksum validation
|
if (GPS_checksum == NMEA_check){ // Checksum validation
|
||||||
//Serial.println("buffer");
|
//Serial.println("buffer");
|
||||||
_setTime();
|
|
||||||
valid_read = true;
|
|
||||||
new_data = true; // New GPS Data
|
|
||||||
parseptr = strchr(buffer, ',')+1;
|
parseptr = strchr(buffer, ',')+1;
|
||||||
//parseptr = strchr(parseptr, ',')+1;
|
//parseptr = strchr(parseptr, ',')+1;
|
||||||
time = parsenumber(parseptr, 2); // GPS UTC time hhmmss.ss
|
time = parsenumber(parseptr, 2); // GPS UTC time hhmmss.ss
|
||||||
|
@ -144,21 +141,22 @@ AP_GPS_NMEA::parse_nmea_gps(void)
|
||||||
parseptr = strchr(parseptr, ',')+1;
|
parseptr = strchr(parseptr, ',')+1;
|
||||||
num_sats = parsedecimal(parseptr, 2);
|
num_sats = parsedecimal(parseptr, 2);
|
||||||
parseptr = strchr(parseptr, ',')+1;
|
parseptr = strchr(parseptr, ',')+1;
|
||||||
HDOP = parsenumber(parseptr, 1); // HDOP * 10
|
hdop = parsenumber(parseptr, 1); // HDOP * 10
|
||||||
parseptr = strchr(parseptr, ',')+1;
|
parseptr = strchr(parseptr, ',')+1;
|
||||||
altitude = parsenumber(parseptr, 1) * 100; // altitude in decimeters * 100 = milimeters
|
altitude = parsenumber(parseptr, 1) * 100; // altitude in decimeters * 100 = milimeters
|
||||||
if (fix < 1)
|
if (fix < 1)
|
||||||
quality = 0; // No FIX
|
quality = 0; // No FIX
|
||||||
else if(num_sats < 5)
|
else if(num_sats < 5)
|
||||||
quality = 1; // Bad (Num sats < 5)
|
quality = 1; // Bad (Num sats < 5)
|
||||||
else if(HDOP > 30)
|
else if(hdop > 30)
|
||||||
quality = 2; // Poor (HDOP > 30)
|
quality = 2; // Poor (HDOP > 30)
|
||||||
else if(HDOP > 25)
|
else if(hdop > 25)
|
||||||
quality = 3; // Medium (HDOP > 25)
|
quality = 3; // Medium (HDOP > 25)
|
||||||
else
|
else
|
||||||
quality = 4; // Good (HDOP < 25)
|
quality = 4; // Good (HDOP < 25)
|
||||||
} else {
|
} else {
|
||||||
_error("GPSERR: Checksum error!!\n");
|
_error("GPSERR: Checksum error!!\n");
|
||||||
|
return false;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
} else if (strncmp(buffer,"$GPVTG",6)==0){ // Check if sentence begins with $GPVTG
|
} else if (strncmp(buffer,"$GPVTG",6)==0){ // Check if sentence begins with $GPVTG
|
||||||
|
@ -166,9 +164,6 @@ AP_GPS_NMEA::parse_nmea_gps(void)
|
||||||
if (buffer[bufferidx-4]=='*'){ // Check for the "*" character
|
if (buffer[bufferidx-4]=='*'){ // Check for the "*" character
|
||||||
NMEA_check = parseHex(buffer[bufferidx - 3]) * 16 + parseHex(buffer[bufferidx - 2]); // Read the checksums characters
|
NMEA_check = parseHex(buffer[bufferidx - 3]) * 16 + parseHex(buffer[bufferidx - 2]); // Read the checksums characters
|
||||||
if (GPS_checksum == NMEA_check){ // Checksum validation
|
if (GPS_checksum == NMEA_check){ // Checksum validation
|
||||||
_setTime();
|
|
||||||
valid_read = true;
|
|
||||||
new_data = true; // New GPS Data
|
|
||||||
parseptr = strchr(buffer, ',')+1;
|
parseptr = strchr(buffer, ',')+1;
|
||||||
ground_course = parsenumber(parseptr, 1) * 10; // Ground course in degrees * 100
|
ground_course = parsenumber(parseptr, 1) * 10; // Ground course in degrees * 100
|
||||||
parseptr = strchr(parseptr, ',')+1;
|
parseptr = strchr(parseptr, ',')+1;
|
||||||
|
@ -181,12 +176,15 @@ AP_GPS_NMEA::parse_nmea_gps(void)
|
||||||
//GPS_line = true;
|
//GPS_line = true;
|
||||||
} else {
|
} else {
|
||||||
_error("GPSERR: Checksum error!!\n");
|
_error("GPSERR: Checksum error!!\n");
|
||||||
|
return false;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
} else {
|
} else {
|
||||||
bufferidx = 0;
|
bufferidx = 0;
|
||||||
_error("GPSERR: Bad sentence!!\n");
|
_error("GPSERR: Bad sentence!!\n");
|
||||||
|
return false;
|
||||||
}
|
}
|
||||||
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
|
@ -36,12 +36,11 @@ class AP_GPS_NMEA : public GPS
|
||||||
public:
|
public:
|
||||||
// Methods
|
// Methods
|
||||||
AP_GPS_NMEA(Stream *s);
|
AP_GPS_NMEA(Stream *s);
|
||||||
void init();
|
virtual void init();
|
||||||
void read();
|
virtual bool read();
|
||||||
|
|
||||||
// Properties
|
// Properties
|
||||||
uint8_t quality; // GPS Signal quality
|
uint8_t quality; // GPS Signal quality
|
||||||
int HDOP; // HDOP
|
|
||||||
|
|
||||||
private:
|
private:
|
||||||
// Internal variables
|
// Internal variables
|
||||||
|
@ -50,7 +49,7 @@ class AP_GPS_NMEA : public GPS
|
||||||
char buffer[GPS_BUFFERSIZE];
|
char buffer[GPS_BUFFERSIZE];
|
||||||
int bufferidx;
|
int bufferidx;
|
||||||
|
|
||||||
void parse_nmea_gps(void);
|
bool parse_nmea_gps(void);
|
||||||
uint8_t parseHex(char c);
|
uint8_t parseHex(char c);
|
||||||
long parsedecimal(char *str,uint8_t num_car);
|
long parsedecimal(char *str,uint8_t num_car);
|
||||||
long parsenumber(char *str,uint8_t numdec);
|
long parsenumber(char *str,uint8_t numdec);
|
||||||
|
|
|
@ -8,6 +8,6 @@
|
||||||
class AP_GPS_None : public GPS
|
class AP_GPS_None : public GPS
|
||||||
{
|
{
|
||||||
virtual void init(void) {};
|
virtual void init(void) {};
|
||||||
virtual void update(void) {};
|
virtual bool read(void) { return false; };
|
||||||
};
|
};
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -10,7 +10,7 @@
|
||||||
//
|
//
|
||||||
|
|
||||||
#include "AP_GPS_SIRF.h"
|
#include "AP_GPS_SIRF.h"
|
||||||
#include "WProgram.h"
|
#include <stdint.h>
|
||||||
|
|
||||||
// Initialisation messages
|
// Initialisation messages
|
||||||
//
|
//
|
||||||
|
@ -29,7 +29,8 @@ AP_GPS_SIRF::AP_GPS_SIRF(Stream *s) : GPS(s)
|
||||||
}
|
}
|
||||||
|
|
||||||
// Public Methods //////////////////////////////////////////////////////////////
|
// Public Methods //////////////////////////////////////////////////////////////
|
||||||
void AP_GPS_SIRF::init(void)
|
void
|
||||||
|
AP_GPS_SIRF::init(void)
|
||||||
{
|
{
|
||||||
_port->flush();
|
_port->flush();
|
||||||
|
|
||||||
|
@ -50,10 +51,12 @@ void AP_GPS_SIRF::init(void)
|
||||||
// re-processing it from the top, this is unavoidable. The parser
|
// re-processing it from the top, this is unavoidable. The parser
|
||||||
// attempts to avoid this when possible.
|
// attempts to avoid this when possible.
|
||||||
//
|
//
|
||||||
void AP_GPS_SIRF::read(void)
|
bool
|
||||||
|
AP_GPS_SIRF::read(void)
|
||||||
{
|
{
|
||||||
byte data;
|
uint8_t data;
|
||||||
int numc;
|
int numc;
|
||||||
|
bool parsed = false;
|
||||||
|
|
||||||
numc = _port->available();
|
numc = _port->available();
|
||||||
while(numc--) {
|
while(numc--) {
|
||||||
|
@ -157,13 +160,14 @@ void AP_GPS_SIRF::read(void)
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
if (_gather) {
|
if (_gather) {
|
||||||
_parse_gps(); // Parse the new GPS packet
|
parsed = _parse_gps(); // Parse the new GPS packet
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
return(parsed);
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
bool
|
||||||
AP_GPS_SIRF::_parse_gps(void)
|
AP_GPS_SIRF::_parse_gps(void)
|
||||||
{
|
{
|
||||||
switch(_msg_id) {
|
switch(_msg_id) {
|
||||||
|
@ -179,11 +183,10 @@ AP_GPS_SIRF::_parse_gps(void)
|
||||||
if (ground_speed > 50)
|
if (ground_speed > 50)
|
||||||
ground_course = _swapi(&_buffer.nav.ground_course);
|
ground_course = _swapi(&_buffer.nav.ground_course);
|
||||||
num_sats = _buffer.nav.satellites;
|
num_sats = _buffer.nav.satellites;
|
||||||
_setTime();
|
|
||||||
valid_read = 1;
|
return true;
|
||||||
break;
|
|
||||||
}
|
}
|
||||||
new_data = true;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
|
|
|
@ -20,7 +20,7 @@ public:
|
||||||
AP_GPS_SIRF(Stream *s);
|
AP_GPS_SIRF(Stream *s);
|
||||||
|
|
||||||
virtual void init();
|
virtual void init();
|
||||||
virtual void read();
|
virtual bool read();
|
||||||
|
|
||||||
private:
|
private:
|
||||||
#pragma pack(1)
|
#pragma pack(1)
|
||||||
|
@ -89,7 +89,7 @@ private:
|
||||||
uint8_t bytes[];
|
uint8_t bytes[];
|
||||||
} _buffer;
|
} _buffer;
|
||||||
|
|
||||||
void _parse_gps(void);
|
bool _parse_gps(void);
|
||||||
void _accumulate(uint8_t val);
|
void _accumulate(uint8_t val);
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
|
@ -10,7 +10,7 @@
|
||||||
//
|
//
|
||||||
|
|
||||||
#include "AP_GPS_UBLOX.h"
|
#include "AP_GPS_UBLOX.h"
|
||||||
#include "WProgram.h"
|
#include <stdint.h>
|
||||||
|
|
||||||
// Constructors ////////////////////////////////////////////////////////////////
|
// Constructors ////////////////////////////////////////////////////////////////
|
||||||
|
|
||||||
|
@ -20,7 +20,8 @@ AP_GPS_UBLOX::AP_GPS_UBLOX(Stream *s) : GPS(s)
|
||||||
|
|
||||||
// Public Methods //////////////////////////////////////////////////////////////
|
// Public Methods //////////////////////////////////////////////////////////////
|
||||||
|
|
||||||
void AP_GPS_UBLOX::init(void)
|
void
|
||||||
|
AP_GPS_UBLOX::init(void)
|
||||||
{
|
{
|
||||||
// XXX it might make sense to send some CFG_MSG,CFG_NMEA messages to get the
|
// XXX it might make sense to send some CFG_MSG,CFG_NMEA messages to get the
|
||||||
// right reporting configuration.
|
// right reporting configuration.
|
||||||
|
@ -37,10 +38,12 @@ void AP_GPS_UBLOX::init(void)
|
||||||
// re-processing it from the top, this is unavoidable. The parser
|
// re-processing it from the top, this is unavoidable. The parser
|
||||||
// attempts to avoid this when possible.
|
// attempts to avoid this when possible.
|
||||||
//
|
//
|
||||||
void AP_GPS_UBLOX::read(void)
|
bool
|
||||||
|
AP_GPS_UBLOX::read(void)
|
||||||
{
|
{
|
||||||
byte data;
|
uint8_t data;
|
||||||
int numc;
|
int numc;
|
||||||
|
bool parsed = false;
|
||||||
|
|
||||||
numc = _port->available();
|
numc = _port->available();
|
||||||
for (int i = 0; i < numc; i++){ // Process bytes received
|
for (int i = 0; i < numc; i++){ // Process bytes received
|
||||||
|
@ -84,9 +87,8 @@ void AP_GPS_UBLOX::read(void)
|
||||||
_step++;
|
_step++;
|
||||||
if (CLASS_NAV == data) {
|
if (CLASS_NAV == data) {
|
||||||
_gather = true; // class is interesting, maybe gather
|
_gather = true; // class is interesting, maybe gather
|
||||||
_ck_b = _ck_a = data; // reset the checksum accumulators
|
_ck_b = _ck_a = data; // reset the checksum accumulators
|
||||||
} else {
|
} else {
|
||||||
_error("ignoring class 0x%x\n", (int)data);
|
|
||||||
_gather = false; // class is not interesting, discard
|
_gather = false; // class is not interesting, discard
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
|
@ -109,31 +111,28 @@ void AP_GPS_UBLOX::read(void)
|
||||||
_expect = sizeof(ubx_nav_velned);
|
_expect = sizeof(ubx_nav_velned);
|
||||||
break;
|
break;
|
||||||
default:
|
default:
|
||||||
_error("ignoring message 0x%x\n", (int)data);
|
|
||||||
_gather = false; // message is not interesting
|
_gather = false; // message is not interesting
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
case 4:
|
case 4:
|
||||||
_step++;
|
_step++;
|
||||||
_ck_b += (_ck_a += data); // checksum byte
|
_ck_b += (_ck_a += data); // checksum byte
|
||||||
_payload_length = data; // payload length low byte
|
_payload_length = data; // payload length low byte
|
||||||
break;
|
break;
|
||||||
case 5:
|
case 5:
|
||||||
_step++;
|
_step++;
|
||||||
_ck_b += (_ck_a += data); // checksum byte
|
_ck_b += (_ck_a += data); // checksum byte
|
||||||
_payload_length += (uint16_t)data; // payload length high byte
|
_payload_length += (uint16_t)data; // payload length high byte
|
||||||
_payload_counter = 0; // prepare to receive payload
|
_payload_counter = 0; // prepare to receive payload
|
||||||
if (_payload_length != _expect) {
|
if (_payload_length != _expect)
|
||||||
_error("payload %d expected %d\n", _payload_length, _expect);
|
|
||||||
_gather = false;
|
_gather = false;
|
||||||
}
|
|
||||||
break;
|
break;
|
||||||
|
|
||||||
// Receive message data
|
// Receive message data
|
||||||
//
|
//
|
||||||
case 6:
|
case 6:
|
||||||
_ck_b += (_ck_a += data); // checksum byte
|
_ck_b += (_ck_a += data); // checksum byte
|
||||||
if (_gather) // gather data if requested
|
if (_gather) // gather data if requested
|
||||||
_buffer.bytes[_payload_counter] = data;
|
_buffer.bytes[_payload_counter] = data;
|
||||||
if (++_payload_counter == _payload_length)
|
if (++_payload_counter == _payload_length)
|
||||||
|
@ -144,26 +143,25 @@ void AP_GPS_UBLOX::read(void)
|
||||||
//
|
//
|
||||||
case 7:
|
case 7:
|
||||||
_step++;
|
_step++;
|
||||||
if (_ck_a != data) {
|
if (_ck_a != data)
|
||||||
_error("GPS_UBLOX: checksum error\n");
|
_step = 0; // bad checksum
|
||||||
_step = 0;
|
|
||||||
}
|
|
||||||
break;
|
break;
|
||||||
case 8:
|
case 8:
|
||||||
_step = 0;
|
_step = 0;
|
||||||
if (_ck_b != data) {
|
if (_ck_b != data)
|
||||||
_error("GPS_UBLOX: checksum error\n");
|
break; // bad checksum
|
||||||
break;
|
|
||||||
|
if (_gather) {
|
||||||
|
parsed = _parse_gps(); // Parse the new GPS packet
|
||||||
}
|
}
|
||||||
if (_gather)
|
|
||||||
_parse_gps(); // Parse the new GPS packet
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
return parsed;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Private Methods /////////////////////////////////////////////////////////////
|
// Private Methods /////////////////////////////////////////////////////////////
|
||||||
|
|
||||||
void
|
bool
|
||||||
AP_GPS_UBLOX::_parse_gps(void)
|
AP_GPS_UBLOX::_parse_gps(void)
|
||||||
{
|
{
|
||||||
switch (_msg_id) {
|
switch (_msg_id) {
|
||||||
|
@ -185,8 +183,8 @@ AP_GPS_UBLOX::_parse_gps(void)
|
||||||
ground_speed = _buffer.velned.speed_2d; // cm/s
|
ground_speed = _buffer.velned.speed_2d; // cm/s
|
||||||
ground_course = _buffer.velned.heading_2d / 1000; // Heading 2D deg * 100000 rescaled to deg * 100
|
ground_course = _buffer.velned.heading_2d / 1000; // Heading 2D deg * 100000 rescaled to deg * 100
|
||||||
break;
|
break;
|
||||||
|
default:
|
||||||
|
return false;
|
||||||
}
|
}
|
||||||
_setTime();
|
return true;
|
||||||
valid_read = 1;
|
|
||||||
new_data = 1;
|
|
||||||
}
|
}
|
||||||
|
|
|
@ -19,9 +19,9 @@ class AP_GPS_UBLOX : public GPS
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
// Methods
|
// Methods
|
||||||
AP_GPS_UBLOX(Stream *s = NULL);
|
AP_GPS_UBLOX(Stream *s);
|
||||||
void init(void);
|
virtual void init();
|
||||||
void read();
|
virtual bool read();
|
||||||
|
|
||||||
private:
|
private:
|
||||||
// u-blox UBX protocol essentials
|
// u-blox UBX protocol essentials
|
||||||
|
@ -118,7 +118,7 @@ private:
|
||||||
} _buffer;
|
} _buffer;
|
||||||
|
|
||||||
// Buffer parse & GPS state update
|
// Buffer parse & GPS state update
|
||||||
void _parse_gps();
|
bool _parse_gps();
|
||||||
};
|
};
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -3,35 +3,35 @@
|
||||||
#include "GPS.h"
|
#include "GPS.h"
|
||||||
#include "WProgram.h"
|
#include "WProgram.h"
|
||||||
|
|
||||||
int
|
|
||||||
GPS::status(void)
|
|
||||||
{
|
|
||||||
if (millis() - _lastTime >= 500){
|
|
||||||
return 0;
|
|
||||||
} else if (fix == 0) {
|
|
||||||
return 1;
|
|
||||||
} else {
|
|
||||||
return 2;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void
|
|
||||||
GPS::_setTime(void)
|
|
||||||
{
|
|
||||||
_lastTime = millis();
|
|
||||||
}
|
|
||||||
|
|
||||||
void
|
void
|
||||||
GPS::update(void)
|
GPS::update(void)
|
||||||
{
|
{
|
||||||
if (!status())
|
bool result;
|
||||||
{
|
|
||||||
Serial.println("Reinitializing GPS");
|
// call the GPS driver to process incoming data
|
||||||
init();
|
result = read();
|
||||||
_setTime();
|
|
||||||
}
|
// if we did not get a message, and the idle timer has expired, re-init
|
||||||
else
|
if (!result) {
|
||||||
{
|
if ((millis() - _idleTimer) > _idleTimeout) {
|
||||||
read();
|
_status = NO_GPS;
|
||||||
|
init();
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
// we got a message, update our status correspondingly
|
||||||
|
_status = fix ? GPS_OK : NO_FIX;
|
||||||
|
|
||||||
|
valid_read = true;
|
||||||
|
new_data = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// reset the idle timer
|
||||||
|
_idleTimer = millis();
|
||||||
|
}
|
||||||
|
|
||||||
|
// XXX this is probably the wrong way to do it, too
|
||||||
|
void
|
||||||
|
GPS::_error(const char *msg)
|
||||||
|
{
|
||||||
|
Serial.println(msg);
|
||||||
}
|
}
|
||||||
|
|
|
@ -20,11 +20,37 @@ public:
|
||||||
/// @note The stream is expected to be set up and configured for the
|
/// @note The stream is expected to be set up and configured for the
|
||||||
/// correct bitrate before ::init is called.
|
/// correct bitrate before ::init is called.
|
||||||
///
|
///
|
||||||
/// @param s Stream connected to the GPS module. If NULL, assumed
|
/// @param s Stream connected to the GPS module.
|
||||||
/// to be set up at ::init time. Support for setting
|
|
||||||
/// the port in the ctor for backwards compatibility.
|
|
||||||
///
|
///
|
||||||
GPS(Stream *s = NULL) : _port(s) {};
|
GPS(Stream *s) : _port(s) {};
|
||||||
|
|
||||||
|
/// Update GPS state based on possible bytes received from the module.
|
||||||
|
///
|
||||||
|
/// This routine must be called periodically to process incoming data.
|
||||||
|
///
|
||||||
|
/// GPS drivers should not override this function; they should implement
|
||||||
|
/// ::read instead.
|
||||||
|
///
|
||||||
|
void update(void);
|
||||||
|
|
||||||
|
/// GPS status codes
|
||||||
|
///
|
||||||
|
/// \note Non-intuitive ordering for legacy reasons
|
||||||
|
///
|
||||||
|
enum GPS_Status {
|
||||||
|
NO_GPS = 0, ///< No GPS connected/detected
|
||||||
|
NO_FIX = 1, ///< Receiving valid GPS messages but no lock
|
||||||
|
GPS_OK = 2 ///< Receiving valid messages and locked
|
||||||
|
};
|
||||||
|
|
||||||
|
/// Query GPS status
|
||||||
|
///
|
||||||
|
/// The 'valid message' status indicates that a recognised message was
|
||||||
|
/// received from the GPS within the last 500ms.
|
||||||
|
///
|
||||||
|
/// @returns Current GPS status
|
||||||
|
///
|
||||||
|
GPS_Status status(void) { return _status; }
|
||||||
|
|
||||||
/// Startup initialisation.
|
/// Startup initialisation.
|
||||||
///
|
///
|
||||||
|
@ -33,35 +59,8 @@ public:
|
||||||
///
|
///
|
||||||
/// Must be implemented by the GPS driver.
|
/// Must be implemented by the GPS driver.
|
||||||
///
|
///
|
||||||
/// @param s Stream connected to the GPS module. If NULL, assumed to
|
|
||||||
/// have been set up at constructor time.
|
|
||||||
///
|
|
||||||
virtual void init(void) = 0;
|
virtual void init(void) = 0;
|
||||||
|
|
||||||
/// Update GPS state based on possible bytes received from the module.
|
|
||||||
///
|
|
||||||
/// This routine must be called periodically to process incoming data.
|
|
||||||
///
|
|
||||||
/// Must be implemented by the GPS driver.
|
|
||||||
///
|
|
||||||
void update(void);
|
|
||||||
|
|
||||||
/// Implement specific routines for gps to receive a message.
|
|
||||||
virtual void read(void) = 0;
|
|
||||||
|
|
||||||
/// Query GPS status
|
|
||||||
///
|
|
||||||
/// The 'valid message' status indicates that a recognised message was
|
|
||||||
/// received from the GPS within the last 500ms.
|
|
||||||
///
|
|
||||||
/// @todo should probably return an enumeration here.
|
|
||||||
///
|
|
||||||
/// @return 0 No GPS connected/detected
|
|
||||||
/// @return 1 Receiving valid GPS messages but no lock
|
|
||||||
/// @return 2 Receiving valid messages and locked
|
|
||||||
///
|
|
||||||
int status(void);
|
|
||||||
|
|
||||||
// Properties
|
// Properties
|
||||||
long time; ///< GPS time in milliseconds from the start of the week
|
long time; ///< GPS time in milliseconds from the start of the week
|
||||||
long latitude; ///< latitude in degrees * 10,000,000
|
long latitude; ///< latitude in degrees * 10,000,000
|
||||||
|
@ -70,6 +69,7 @@ public:
|
||||||
long ground_speed; ///< ground speed in cm/sec
|
long ground_speed; ///< ground speed in cm/sec
|
||||||
long ground_course; ///< ground course in 100ths of a degree
|
long ground_course; ///< ground course in 100ths of a degree
|
||||||
long speed_3d; ///< 3D speed in cm/sec (not always available)
|
long speed_3d; ///< 3D speed in cm/sec (not always available)
|
||||||
|
int hdop; ///< horizontal dilution of precision in cm
|
||||||
uint8_t num_sats; ///< Number of visible satelites
|
uint8_t num_sats; ///< Number of visible satelites
|
||||||
|
|
||||||
/// Set to true when new data arrives. A client may set this
|
/// Set to true when new data arrives. A client may set this
|
||||||
|
@ -85,12 +85,15 @@ public:
|
||||||
bool print_errors; ///< deprecated
|
bool print_errors; ///< deprecated
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
Stream *_port; ///< stream port the GPS is attached to
|
Stream *_port; ///< port the GPS is attached to
|
||||||
unsigned long _lastTime; ///< Timer for lost connection
|
|
||||||
|
|
||||||
/// reset the last-message-received timer used by ::status
|
/// read from the GPS stream and update properties
|
||||||
///
|
///
|
||||||
void _setTime(void);
|
/// Must be implemented by the GPS driver.
|
||||||
|
///
|
||||||
|
/// @returns true if a valid message was received from the GPS
|
||||||
|
///
|
||||||
|
virtual bool read(void) = 0;
|
||||||
|
|
||||||
/// perform an endian swap on a long
|
/// perform an endian swap on a long
|
||||||
///
|
///
|
||||||
|
@ -98,14 +101,14 @@ protected:
|
||||||
/// long in the wrong byte order
|
/// long in the wrong byte order
|
||||||
/// @returns endian-swapped value
|
/// @returns endian-swapped value
|
||||||
///
|
///
|
||||||
long _swapl(const void *bytes);
|
long _swapl(const void *bytes);
|
||||||
|
|
||||||
/// perform an endian swap on an int
|
/// perform an endian swap on an int
|
||||||
///
|
///
|
||||||
/// @param bytes pointer to a buffer containing bytes representing an
|
/// @param bytes pointer to a buffer containing bytes representing an
|
||||||
/// int in the wrong byte order
|
/// int in the wrong byte order
|
||||||
/// @returns endian-swapped value
|
/// @returns endian-swapped value
|
||||||
int _swapi(const void *bytes);
|
int _swapi(const void *bytes);
|
||||||
|
|
||||||
/// emit an error message
|
/// emit an error message
|
||||||
///
|
///
|
||||||
|
@ -117,8 +120,21 @@ protected:
|
||||||
/// @note deprecated as-is due to the difficulty of hooking up to a working
|
/// @note deprecated as-is due to the difficulty of hooking up to a working
|
||||||
/// printf vs. the potential benefits
|
/// printf vs. the potential benefits
|
||||||
///
|
///
|
||||||
void _error(const char *msg, ...) {};
|
void _error(const char *msg);
|
||||||
|
|
||||||
|
private:
|
||||||
|
|
||||||
|
/// Time in milliseconds after which we will assume the GPS is no longer
|
||||||
|
/// sending us updates and attempt a re-init.
|
||||||
|
///
|
||||||
|
static const unsigned long _idleTimeout = 500;
|
||||||
|
|
||||||
|
/// Last time that the GPS driver got a good packet from the GPS
|
||||||
|
///
|
||||||
|
unsigned long _idleTimer;
|
||||||
|
|
||||||
|
/// Our current status
|
||||||
|
GPS_Status _status;
|
||||||
};
|
};
|
||||||
|
|
||||||
inline long
|
inline long
|
||||||
|
|
|
@ -1,10 +1,10 @@
|
||||||
|
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
||||||
//
|
//
|
||||||
// Test for AP_GPS_AUTO
|
// Test for AP_GPS_AUTO
|
||||||
//
|
//
|
||||||
|
|
||||||
#include <FastSerial.h>
|
#include <FastSerial.h>
|
||||||
#include <AP_GPS.h>
|
#include <AP_GPS.h>
|
||||||
#include <stdlib.h>
|
|
||||||
|
|
||||||
FastSerialPort0(Serial);
|
FastSerialPort0(Serial);
|
||||||
FastSerialPort1(Serial1);
|
FastSerialPort1(Serial1);
|
||||||
|
@ -28,32 +28,25 @@ void setup()
|
||||||
Serial.println("GPS AUTO library test");
|
Serial.println("GPS AUTO library test");
|
||||||
gps = &GPS;
|
gps = &GPS;
|
||||||
gps->init();
|
gps->init();
|
||||||
delay(1000);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void loop()
|
void loop()
|
||||||
{
|
{
|
||||||
delay(20);
|
|
||||||
gps->update();
|
gps->update();
|
||||||
if (gps->new_data){
|
if (gps->new_data){
|
||||||
Serial.print("gps:");
|
if (gps->fix) {
|
||||||
Serial.print(" Lat:");
|
Serial.printf("\nLat: %.7f Lon: %.7f Alt: %.2fm GSP: %.2fm/s CoG: %d SAT: %d TIM: %lu",
|
||||||
Serial.print((float)gps->latitude / T7, DEC);
|
(float)gps->latitude / T7,
|
||||||
Serial.print(" Lon:");
|
(float)gps->longitude / T7,
|
||||||
Serial.print((float)gps->longitude / T7, DEC);
|
(float)gps->altitude / 100.0,
|
||||||
Serial.print(" Alt:");
|
(float)gps->ground_speed / 100.0,
|
||||||
Serial.print((float)gps->altitude / 100.0, DEC);
|
(int)gps->ground_course / 100,
|
||||||
Serial.print(" GSP:");
|
gps->num_sats,
|
||||||
Serial.print(gps->ground_speed / 100.0);
|
gps->time);
|
||||||
Serial.print(" COG:");
|
} else {
|
||||||
Serial.print(gps->ground_course / 100.0, DEC);
|
Serial.println("No fix");
|
||||||
Serial.print(" SAT:");
|
}
|
||||||
Serial.print(gps->num_sats, DEC);
|
gps->new_data = false;
|
||||||
Serial.print(" FIX:");
|
|
||||||
Serial.print(gps->fix, DEC);
|
|
||||||
Serial.print(" TIM:");
|
|
||||||
Serial.print(gps->time, DEC);
|
|
||||||
Serial.println();
|
|
||||||
gps->new_data = 0;
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue