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_UBLOX.h"
|
||||
#include "AP_GPS_MTK.h"
|
||||
#include "AP_GPS_IMU.h"
|
||||
#include "AP_GPS_MTK16.h"
|
||||
#include "AP_GPS_None.h"
|
||||
#include "AP_GPS_Auto.h"
|
||||
#include "AP_GPS_HIL.h"
|
||||
|
|
|
@ -21,7 +21,7 @@ class AP_GPS_406 : public AP_GPS_SIRF
|
|||
public:
|
||||
// Methods
|
||||
AP_GPS_406(Stream *port);
|
||||
void init(void);
|
||||
virtual void init(void);
|
||||
|
||||
private:
|
||||
void _change_to_sirf_protocol(void);
|
||||
|
|
|
@ -3,13 +3,20 @@
|
|||
// Auto-detecting pseudo-GPS driver
|
||||
//
|
||||
|
||||
#include "AP_GPS.h"
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
#include "AP_GPS.h" // includes AP_GPS_Auto.h
|
||||
#include <wiring.h>
|
||||
|
||||
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
|
||||
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
|
||||
// and return.
|
||||
void
|
||||
bool
|
||||
AP_GPS_Auto::read(void)
|
||||
{
|
||||
GPS *gps;
|
||||
|
@ -30,8 +37,8 @@ AP_GPS_Auto::read(void)
|
|||
for (;;) {
|
||||
// loop through possible baudrates
|
||||
for (i = 0; i < (sizeof(baudrates) / sizeof(baudrates[0])); i++) {
|
||||
printf("GPS autodetect at %d:%u\n", i, baudrates[i]);
|
||||
_port->begin(baudrates[i]);
|
||||
Serial.printf("GPS autodetect at %d:%u\n", i, baudrates[i]);
|
||||
_FSport->begin(baudrates[i]);
|
||||
if (NULL != (gps = _detect())) {
|
||||
// make the detected GPS the default
|
||||
*_gps = gps;
|
||||
|
@ -41,9 +48,9 @@ AP_GPS_Auto::read(void)
|
|||
gps->init();
|
||||
gps->update();
|
||||
|
||||
// drop back to our caller - subsequent calls through
|
||||
// the global will not come here
|
||||
return;
|
||||
// Drop back to our caller - subsequent calls through
|
||||
// _gps will not come here.
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
@ -72,6 +79,7 @@ AP_GPS_Auto::_detect(void)
|
|||
// XXX We can detect babble by counting incoming characters, but
|
||||
// what would we do about it?
|
||||
//
|
||||
Serial.println("draining and waiting");
|
||||
_port->flush();
|
||||
then = millis();
|
||||
do {
|
||||
|
@ -84,11 +92,12 @@ AP_GPS_Auto::_detect(void)
|
|||
//
|
||||
// Collect four characters to fingerprint a device
|
||||
//
|
||||
Serial.println("collecting fingerprint");
|
||||
fingerprint[0] = _getc();
|
||||
fingerprint[1] = _getc();
|
||||
fingerprint[2] = _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[1],
|
||||
fingerprint[2],
|
||||
|
@ -104,23 +113,34 @@ AP_GPS_Auto::_detect(void)
|
|||
|
||||
// message 5 is MTK pretending to talk UBX
|
||||
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);
|
||||
break;
|
||||
}
|
||||
|
||||
// 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);
|
||||
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
|
||||
//
|
||||
if ((0xa0 == fingerprint[0]) &&
|
||||
(0xa2 == fingerprint[1])) {
|
||||
printf("detected SIRF in binary mode\n");
|
||||
Serial.printf("detected SIRF in binary mode\n");
|
||||
gps = new AP_GPS_SIRF(_port);
|
||||
break;
|
||||
}
|
||||
|
@ -130,7 +150,7 @@ AP_GPS_Auto::_detect(void)
|
|||
// and retry to avoid a false-positive on the NMEA detector.
|
||||
//
|
||||
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(UBLOX_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
|
||||
// iterations around the loop to react to init strings?
|
||||
printf("detected NMEA\n");
|
||||
Serial.printf("detected NMEA\n");
|
||||
gps = new AP_GPS_NMEA(_port);
|
||||
break;
|
||||
}
|
||||
|
|
|
@ -21,21 +21,19 @@ public:
|
|||
/// @param ptr Pointer to a GPS * that will be fixed up by ::init
|
||||
/// when the GPS type has been detected.
|
||||
///
|
||||
AP_GPS_Auto(FastSerial *port, GPS **gps) :
|
||||
_port(port),
|
||||
_gps(gps)
|
||||
{};
|
||||
AP_GPS_Auto(FastSerial *port, GPS **gps);
|
||||
|
||||
void init(void);
|
||||
/// Dummy init routine, does nothing
|
||||
virtual void init(void);
|
||||
|
||||
/// Detect and initialise the attached GPS unit. Returns a
|
||||
/// pointer to the allocated & initialised GPS driver.
|
||||
/// Detect and initialise the attached GPS unit. Updates the
|
||||
/// pointer passed into the constructor when a GPS is detected.
|
||||
///
|
||||
void read(void);
|
||||
virtual bool read(void);
|
||||
|
||||
private:
|
||||
/// Serial port connected to the GPS.
|
||||
FastSerial *_port;
|
||||
FastSerial *_FSport;
|
||||
|
||||
/// 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)
|
||||
{
|
||||
if (valid_read)
|
||||
{
|
||||
if (fix) return 2;
|
||||
else return 1;
|
||||
}
|
||||
else return 0;
|
||||
// return true once for each update pushed in
|
||||
_updated = false;
|
||||
return result;
|
||||
}
|
||||
|
||||
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;
|
||||
speed_3d = _speed_3d*1.0e2;
|
||||
num_sats = _num_sats;
|
||||
new_data = true;
|
||||
fix = true;
|
||||
valid_read = true;
|
||||
_setTime();
|
||||
_updated = true;
|
||||
}
|
||||
|
||||
|
|
|
@ -17,9 +17,9 @@
|
|||
class AP_GPS_HIL : public GPS {
|
||||
public:
|
||||
AP_GPS_HIL(Stream *s);
|
||||
void init(void);
|
||||
void read(void);
|
||||
int status(void);
|
||||
virtual void init(void);
|
||||
virtual bool read(void);
|
||||
|
||||
/**
|
||||
* Hardware in the loop set function
|
||||
* @param latitude - latitude in deggrees
|
||||
|
@ -32,6 +32,9 @@ public:
|
|||
*/
|
||||
void setHIL(long time, float latitude, float longitude, float altitude,
|
||||
float ground_speed, float ground_course, float speed_3d, uint8_t num_sats);
|
||||
|
||||
private:
|
||||
bool _updated;
|
||||
};
|
||||
|
||||
#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 "WProgram.h"
|
||||
#include <stdint.h>
|
||||
|
||||
// Constructors ////////////////////////////////////////////////////////////////
|
||||
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 //////////////////////////////////////////////////////////////
|
||||
void AP_GPS_MTK::init(void)
|
||||
void
|
||||
AP_GPS_MTK::init(void)
|
||||
{
|
||||
_port->flush();
|
||||
// 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
|
||||
// unrecognised messages.
|
||||
//
|
||||
void AP_GPS_MTK::read(void)
|
||||
bool
|
||||
AP_GPS_MTK::read(void)
|
||||
{
|
||||
byte data;
|
||||
uint8_t data;
|
||||
int numc;
|
||||
bool parsed = false;
|
||||
|
||||
numc = _port->available();
|
||||
for (int i = 0; i < numc; i++){ // Process bytes received
|
||||
|
@ -89,7 +92,6 @@ restart:
|
|||
if (MESSAGE_ID == data) {
|
||||
_step++;
|
||||
_ck_b += (_ck_a += data);
|
||||
_payload_length = sizeof(diyd_mtk_msg); // prepare to receive our message
|
||||
_payload_counter = 0;
|
||||
} else {
|
||||
_step = 0;
|
||||
|
@ -102,7 +104,7 @@ restart:
|
|||
case 4:
|
||||
_buffer.bytes[_payload_counter++] = data;
|
||||
_ck_b += (_ck_a += data);
|
||||
if (_payload_counter == _payload_length)
|
||||
if (_payload_counter == sizeof(_buffer))
|
||||
_step++;
|
||||
break;
|
||||
|
||||
|
@ -121,15 +123,7 @@ restart:
|
|||
_error("GPS_MTK: checksum error\n");
|
||||
break;
|
||||
}
|
||||
_parse_gps(); // Parse the new GPS packet
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Private Methods
|
||||
void
|
||||
AP_GPS_MTK::_parse_gps(void)
|
||||
{
|
||||
fix = (_buffer.msg.fix_type == FIX_3D);
|
||||
latitude = _swapl(&_buffer.msg.latitude) * 10;
|
||||
longitude = _swapl(&_buffer.msg.longitude) * 10;
|
||||
|
@ -140,7 +134,9 @@ AP_GPS_MTK::_parse_gps(void)
|
|||
|
||||
// XXX docs say this is UTC, but our clients expect msToW
|
||||
time = _swapl(&_buffer.msg.utc_time);
|
||||
_setTime();
|
||||
valid_read = true;
|
||||
new_data = true;
|
||||
|
||||
parsed = true;
|
||||
}
|
||||
}
|
||||
return parsed;
|
||||
}
|
||||
|
|
|
@ -10,34 +10,19 @@
|
|||
//
|
||||
// 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
|
||||
#define AP_GPS_MTK_h
|
||||
|
||||
#include <GPS.h>
|
||||
#define MAXPAYLOAD 32
|
||||
|
||||
#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"
|
||||
#include "AP_GPS_MTK_Common.h"
|
||||
|
||||
class AP_GPS_MTK : public GPS {
|
||||
public:
|
||||
AP_GPS_MTK(Stream *s);
|
||||
virtual void init(void);
|
||||
virtual void read(void);
|
||||
virtual bool read(void);
|
||||
|
||||
private:
|
||||
#pragma pack(1)
|
||||
|
@ -71,7 +56,6 @@ private:
|
|||
|
||||
// State machine state
|
||||
uint8_t _step;
|
||||
uint8_t _payload_length;
|
||||
uint8_t _payload_counter;
|
||||
|
||||
// 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)
|
||||
*/
|
||||
#include "AP_GPS_NMEA.h"
|
||||
#include "WProgram.h"
|
||||
|
||||
// Constructors ////////////////////////////////////////////////////////////////
|
||||
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
|
||||
// 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.
|
||||
void
|
||||
bool
|
||||
AP_GPS_NMEA::read(void)
|
||||
{
|
||||
char c;
|
||||
int numc;
|
||||
int i;
|
||||
bool parsed = false;
|
||||
|
||||
numc = _port->available();
|
||||
|
||||
|
@ -83,7 +83,7 @@ AP_GPS_NMEA::read(void)
|
|||
}
|
||||
if (c == '\r'){ // NMEA End
|
||||
buffer[bufferidx++] = 0;
|
||||
parse_nmea_gps();
|
||||
parsed = parse_nmea_gps();
|
||||
} else {
|
||||
if (bufferidx < (GPS_BUFFERSIZE - 1)){
|
||||
if (c == '*')
|
||||
|
@ -104,7 +104,7 @@ AP_GPS_NMEA::read(void)
|
|||
*
|
||||
* * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * **/
|
||||
// Private Methods //////////////////////////////////////////////////////////////
|
||||
void
|
||||
bool
|
||||
AP_GPS_NMEA::parse_nmea_gps(void)
|
||||
{
|
||||
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
|
||||
if (GPS_checksum == NMEA_check){ // Checksum validation
|
||||
//Serial.println("buffer");
|
||||
_setTime();
|
||||
valid_read = true;
|
||||
new_data = true; // New GPS Data
|
||||
parseptr = strchr(buffer, ',')+1;
|
||||
//parseptr = strchr(parseptr, ',')+1;
|
||||
time = parsenumber(parseptr, 2); // GPS UTC time hhmmss.ss
|
||||
|
@ -144,21 +141,22 @@ AP_GPS_NMEA::parse_nmea_gps(void)
|
|||
parseptr = strchr(parseptr, ',')+1;
|
||||
num_sats = parsedecimal(parseptr, 2);
|
||||
parseptr = strchr(parseptr, ',')+1;
|
||||
HDOP = parsenumber(parseptr, 1); // HDOP * 10
|
||||
hdop = parsenumber(parseptr, 1); // HDOP * 10
|
||||
parseptr = strchr(parseptr, ',')+1;
|
||||
altitude = parsenumber(parseptr, 1) * 100; // altitude in decimeters * 100 = milimeters
|
||||
if (fix < 1)
|
||||
quality = 0; // No FIX
|
||||
else if(num_sats < 5)
|
||||
quality = 1; // Bad (Num sats < 5)
|
||||
else if(HDOP > 30)
|
||||
else if(hdop > 30)
|
||||
quality = 2; // Poor (HDOP > 30)
|
||||
else if(HDOP > 25)
|
||||
else if(hdop > 25)
|
||||
quality = 3; // Medium (HDOP > 25)
|
||||
else
|
||||
quality = 4; // Good (HDOP < 25)
|
||||
} else {
|
||||
_error("GPSERR: Checksum error!!\n");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
} 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
|
||||
NMEA_check = parseHex(buffer[bufferidx - 3]) * 16 + parseHex(buffer[bufferidx - 2]); // Read the checksums characters
|
||||
if (GPS_checksum == NMEA_check){ // Checksum validation
|
||||
_setTime();
|
||||
valid_read = true;
|
||||
new_data = true; // New GPS Data
|
||||
parseptr = strchr(buffer, ',')+1;
|
||||
ground_course = parsenumber(parseptr, 1) * 10; // Ground course in degrees * 100
|
||||
parseptr = strchr(parseptr, ',')+1;
|
||||
|
@ -181,12 +176,15 @@ AP_GPS_NMEA::parse_nmea_gps(void)
|
|||
//GPS_line = true;
|
||||
} else {
|
||||
_error("GPSERR: Checksum error!!\n");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
} else {
|
||||
bufferidx = 0;
|
||||
_error("GPSERR: Bad sentence!!\n");
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
|
|
|
@ -36,12 +36,11 @@ class AP_GPS_NMEA : public GPS
|
|||
public:
|
||||
// Methods
|
||||
AP_GPS_NMEA(Stream *s);
|
||||
void init();
|
||||
void read();
|
||||
virtual void init();
|
||||
virtual bool read();
|
||||
|
||||
// Properties
|
||||
uint8_t quality; // GPS Signal quality
|
||||
int HDOP; // HDOP
|
||||
|
||||
private:
|
||||
// Internal variables
|
||||
|
@ -50,7 +49,7 @@ class AP_GPS_NMEA : public GPS
|
|||
char buffer[GPS_BUFFERSIZE];
|
||||
int bufferidx;
|
||||
|
||||
void parse_nmea_gps(void);
|
||||
bool parse_nmea_gps(void);
|
||||
uint8_t parseHex(char c);
|
||||
long parsedecimal(char *str,uint8_t num_car);
|
||||
long parsenumber(char *str,uint8_t numdec);
|
||||
|
|
|
@ -8,6 +8,6 @@
|
|||
class AP_GPS_None : public GPS
|
||||
{
|
||||
virtual void init(void) {};
|
||||
virtual void update(void) {};
|
||||
virtual bool read(void) { return false; };
|
||||
};
|
||||
#endif
|
||||
|
|
|
@ -10,7 +10,7 @@
|
|||
//
|
||||
|
||||
#include "AP_GPS_SIRF.h"
|
||||
#include "WProgram.h"
|
||||
#include <stdint.h>
|
||||
|
||||
// Initialisation messages
|
||||
//
|
||||
|
@ -29,7 +29,8 @@ AP_GPS_SIRF::AP_GPS_SIRF(Stream *s) : GPS(s)
|
|||
}
|
||||
|
||||
// Public Methods //////////////////////////////////////////////////////////////
|
||||
void AP_GPS_SIRF::init(void)
|
||||
void
|
||||
AP_GPS_SIRF::init(void)
|
||||
{
|
||||
_port->flush();
|
||||
|
||||
|
@ -50,10 +51,12 @@ void AP_GPS_SIRF::init(void)
|
|||
// re-processing it from the top, this is unavoidable. The parser
|
||||
// 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;
|
||||
bool parsed = false;
|
||||
|
||||
numc = _port->available();
|
||||
while(numc--) {
|
||||
|
@ -157,13 +160,14 @@ void AP_GPS_SIRF::read(void)
|
|||
break;
|
||||
}
|
||||
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)
|
||||
{
|
||||
switch(_msg_id) {
|
||||
|
@ -179,11 +183,10 @@ AP_GPS_SIRF::_parse_gps(void)
|
|||
if (ground_speed > 50)
|
||||
ground_course = _swapi(&_buffer.nav.ground_course);
|
||||
num_sats = _buffer.nav.satellites;
|
||||
_setTime();
|
||||
valid_read = 1;
|
||||
break;
|
||||
|
||||
return true;
|
||||
}
|
||||
new_data = true;
|
||||
return false;
|
||||
}
|
||||
|
||||
void
|
||||
|
|
|
@ -20,7 +20,7 @@ public:
|
|||
AP_GPS_SIRF(Stream *s);
|
||||
|
||||
virtual void init();
|
||||
virtual void read();
|
||||
virtual bool read();
|
||||
|
||||
private:
|
||||
#pragma pack(1)
|
||||
|
@ -89,7 +89,7 @@ private:
|
|||
uint8_t bytes[];
|
||||
} _buffer;
|
||||
|
||||
void _parse_gps(void);
|
||||
bool _parse_gps(void);
|
||||
void _accumulate(uint8_t val);
|
||||
};
|
||||
|
||||
|
|
|
@ -10,7 +10,7 @@
|
|||
//
|
||||
|
||||
#include "AP_GPS_UBLOX.h"
|
||||
#include "WProgram.h"
|
||||
#include <stdint.h>
|
||||
|
||||
// Constructors ////////////////////////////////////////////////////////////////
|
||||
|
||||
|
@ -20,7 +20,8 @@ AP_GPS_UBLOX::AP_GPS_UBLOX(Stream *s) : GPS(s)
|
|||
|
||||
// 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
|
||||
// right reporting configuration.
|
||||
|
@ -37,10 +38,12 @@ void AP_GPS_UBLOX::init(void)
|
|||
// re-processing it from the top, this is unavoidable. The parser
|
||||
// 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;
|
||||
bool parsed = false;
|
||||
|
||||
numc = _port->available();
|
||||
for (int i = 0; i < numc; i++){ // Process bytes received
|
||||
|
@ -86,7 +89,6 @@ void AP_GPS_UBLOX::read(void)
|
|||
_gather = true; // class is interesting, maybe gather
|
||||
_ck_b = _ck_a = data; // reset the checksum accumulators
|
||||
} else {
|
||||
_error("ignoring class 0x%x\n", (int)data);
|
||||
_gather = false; // class is not interesting, discard
|
||||
}
|
||||
break;
|
||||
|
@ -109,7 +111,6 @@ void AP_GPS_UBLOX::read(void)
|
|||
_expect = sizeof(ubx_nav_velned);
|
||||
break;
|
||||
default:
|
||||
_error("ignoring message 0x%x\n", (int)data);
|
||||
_gather = false; // message is not interesting
|
||||
}
|
||||
}
|
||||
|
@ -124,10 +125,8 @@ void AP_GPS_UBLOX::read(void)
|
|||
_ck_b += (_ck_a += data); // checksum byte
|
||||
_payload_length += (uint16_t)data; // payload length high byte
|
||||
_payload_counter = 0; // prepare to receive payload
|
||||
if (_payload_length != _expect) {
|
||||
_error("payload %d expected %d\n", _payload_length, _expect);
|
||||
if (_payload_length != _expect)
|
||||
_gather = false;
|
||||
}
|
||||
break;
|
||||
|
||||
// Receive message data
|
||||
|
@ -144,26 +143,25 @@ void AP_GPS_UBLOX::read(void)
|
|||
//
|
||||
case 7:
|
||||
_step++;
|
||||
if (_ck_a != data) {
|
||||
_error("GPS_UBLOX: checksum error\n");
|
||||
_step = 0;
|
||||
}
|
||||
if (_ck_a != data)
|
||||
_step = 0; // bad checksum
|
||||
break;
|
||||
case 8:
|
||||
_step = 0;
|
||||
if (_ck_b != data) {
|
||||
_error("GPS_UBLOX: checksum error\n");
|
||||
break;
|
||||
}
|
||||
if (_gather)
|
||||
_parse_gps(); // Parse the new GPS packet
|
||||
if (_ck_b != data)
|
||||
break; // bad checksum
|
||||
|
||||
if (_gather) {
|
||||
parsed = _parse_gps(); // Parse the new GPS packet
|
||||
}
|
||||
}
|
||||
}
|
||||
return parsed;
|
||||
}
|
||||
|
||||
// Private Methods /////////////////////////////////////////////////////////////
|
||||
|
||||
void
|
||||
bool
|
||||
AP_GPS_UBLOX::_parse_gps(void)
|
||||
{
|
||||
switch (_msg_id) {
|
||||
|
@ -185,8 +183,8 @@ AP_GPS_UBLOX::_parse_gps(void)
|
|||
ground_speed = _buffer.velned.speed_2d; // cm/s
|
||||
ground_course = _buffer.velned.heading_2d / 1000; // Heading 2D deg * 100000 rescaled to deg * 100
|
||||
break;
|
||||
default:
|
||||
return false;
|
||||
}
|
||||
_setTime();
|
||||
valid_read = 1;
|
||||
new_data = 1;
|
||||
return true;
|
||||
}
|
||||
|
|
|
@ -19,9 +19,9 @@ class AP_GPS_UBLOX : public GPS
|
|||
{
|
||||
public:
|
||||
// Methods
|
||||
AP_GPS_UBLOX(Stream *s = NULL);
|
||||
void init(void);
|
||||
void read();
|
||||
AP_GPS_UBLOX(Stream *s);
|
||||
virtual void init();
|
||||
virtual bool read();
|
||||
|
||||
private:
|
||||
// u-blox UBX protocol essentials
|
||||
|
@ -118,7 +118,7 @@ private:
|
|||
} _buffer;
|
||||
|
||||
// Buffer parse & GPS state update
|
||||
void _parse_gps();
|
||||
bool _parse_gps();
|
||||
};
|
||||
|
||||
#endif
|
||||
|
|
|
@ -3,35 +3,35 @@
|
|||
#include "GPS.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
|
||||
GPS::update(void)
|
||||
{
|
||||
if (!status())
|
||||
{
|
||||
Serial.println("Reinitializing GPS");
|
||||
bool result;
|
||||
|
||||
// call the GPS driver to process incoming data
|
||||
result = read();
|
||||
|
||||
// if we did not get a message, and the idle timer has expired, re-init
|
||||
if (!result) {
|
||||
if ((millis() - _idleTimer) > _idleTimeout) {
|
||||
_status = NO_GPS;
|
||||
init();
|
||||
_setTime();
|
||||
}
|
||||
else
|
||||
{
|
||||
read();
|
||||
} 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
|
||||
/// correct bitrate before ::init is called.
|
||||
///
|
||||
/// @param s Stream connected to the GPS module. If NULL, assumed
|
||||
/// to be set up at ::init time. Support for setting
|
||||
/// the port in the ctor for backwards compatibility.
|
||||
/// @param s Stream connected to the GPS module.
|
||||
///
|
||||
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.
|
||||
///
|
||||
|
@ -33,35 +59,8 @@ public:
|
|||
///
|
||||
/// 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;
|
||||
|
||||
/// 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
|
||||
long time; ///< GPS time in milliseconds from the start of the week
|
||||
long latitude; ///< latitude in degrees * 10,000,000
|
||||
|
@ -70,6 +69,7 @@ public:
|
|||
long ground_speed; ///< ground speed in cm/sec
|
||||
long ground_course; ///< ground course in 100ths of a degree
|
||||
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
|
||||
|
||||
/// Set to true when new data arrives. A client may set this
|
||||
|
@ -85,12 +85,15 @@ public:
|
|||
bool print_errors; ///< deprecated
|
||||
|
||||
protected:
|
||||
Stream *_port; ///< stream port the GPS is attached to
|
||||
unsigned long _lastTime; ///< Timer for lost connection
|
||||
Stream *_port; ///< port the GPS is attached to
|
||||
|
||||
/// 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
|
||||
///
|
||||
|
@ -117,8 +120,21 @@ protected:
|
|||
/// @note deprecated as-is due to the difficulty of hooking up to a working
|
||||
/// 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
|
||||
|
|
|
@ -1,10 +1,10 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
||||
//
|
||||
// Test for AP_GPS_AUTO
|
||||
//
|
||||
|
||||
#include <FastSerial.h>
|
||||
#include <AP_GPS.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
FastSerialPort0(Serial);
|
||||
FastSerialPort1(Serial1);
|
||||
|
@ -28,32 +28,25 @@ void setup()
|
|||
Serial.println("GPS AUTO library test");
|
||||
gps = &GPS;
|
||||
gps->init();
|
||||
delay(1000);
|
||||
}
|
||||
|
||||
void loop()
|
||||
{
|
||||
delay(20);
|
||||
gps->update();
|
||||
if (gps->new_data){
|
||||
Serial.print("gps:");
|
||||
Serial.print(" Lat:");
|
||||
Serial.print((float)gps->latitude / T7, DEC);
|
||||
Serial.print(" Lon:");
|
||||
Serial.print((float)gps->longitude / T7, DEC);
|
||||
Serial.print(" Alt:");
|
||||
Serial.print((float)gps->altitude / 100.0, DEC);
|
||||
Serial.print(" GSP:");
|
||||
Serial.print(gps->ground_speed / 100.0);
|
||||
Serial.print(" COG:");
|
||||
Serial.print(gps->ground_course / 100.0, DEC);
|
||||
Serial.print(" SAT:");
|
||||
Serial.print(gps->num_sats, DEC);
|
||||
Serial.print(" FIX:");
|
||||
Serial.print(gps->fix, DEC);
|
||||
Serial.print(" TIM:");
|
||||
Serial.print(gps->time, DEC);
|
||||
Serial.println();
|
||||
gps->new_data = 0;
|
||||
if (gps->fix) {
|
||||
Serial.printf("\nLat: %.7f Lon: %.7f Alt: %.2fm GSP: %.2fm/s CoG: %d SAT: %d TIM: %lu",
|
||||
(float)gps->latitude / T7,
|
||||
(float)gps->longitude / T7,
|
||||
(float)gps->altitude / 100.0,
|
||||
(float)gps->ground_speed / 100.0,
|
||||
(int)gps->ground_course / 100,
|
||||
gps->num_sats,
|
||||
gps->time);
|
||||
} else {
|
||||
Serial.println("No fix");
|
||||
}
|
||||
gps->new_data = false;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
Loading…
Reference in New Issue