Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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:
DrZiplok@gmail.com 2010-12-24 06:35:09 +00:00
parent 53d387bdb4
commit 7fc6515300
23 changed files with 472 additions and 507 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_GPS/AP_GPS.h
View File

@ -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"

2
libraries/AP_GPS/AP_GPS_406.h
View File

@ -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);

50
libraries/AP_GPS/AP_GPS_Auto.cpp
View File

@ -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;
}

16
libraries/AP_GPS/AP_GPS_Auto.h
View File

@ -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
///

19
libraries/AP_GPS/AP_GPS_HIL.cpp
View File

@ -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;
}

9
libraries/AP_GPS/AP_GPS_HIL.h
View File

@ -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

224
libraries/AP_GPS/AP_GPS_IMU.cpp
View File

@ -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;
}

44
libraries/AP_GPS/AP_GPS_IMU.h
View File

@ -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

50
libraries/AP_GPS/AP_GPS_MTK.cpp
View File

@ -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;
int numc;
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,26 +123,20 @@ 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;
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;
fix = (_buffer.msg.fix_type == FIX_3D);
latitude = _swapl(&_buffer.msg.latitude) * 10;
longitude = _swapl(&_buffer.msg.longitude) * 10;
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
time = _swapl(&_buffer.msg.utc_time);
_setTime();
valid_read = true;
new_data = true;
// XXX docs say this is UTC, but our clients expect msToW
time = _swapl(&_buffer.msg.utc_time);
parsed = true;
}
}
return parsed;
}

24
libraries/AP_GPS/AP_GPS_MTK.h
View File

@ -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

133
libraries/AP_GPS/AP_GPS_MTK16.cpp Normal file
View File

@ -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;
}

66
libraries/AP_GPS/AP_GPS_MTK16.h Normal file
View File

@ -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

33
libraries/AP_GPS/AP_GPS_MTK_Common.h Normal file
View File

@ -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

24
libraries/AP_GPS/AP_GPS_NMEA.cpp
View File

@ -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;
}

7
libraries/AP_GPS/AP_GPS_NMEA.h
View File

@ -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);

2
libraries/AP_GPS/AP_GPS_None.h
View File

@ -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

25
libraries/AP_GPS/AP_GPS_SIRF.cpp
View File

@ -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;
int numc;
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

4
libraries/AP_GPS/AP_GPS_SIRF.h
View File

@ -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);
};

52
libraries/AP_GPS/AP_GPS_UBLOX.cpp
View File

@ -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;
int numc;
uint8_t data;
int numc;
bool parsed = false;
numc = _port->available();
for (int i = 0; i < numc; i++){ // Process bytes received
@ -84,9 +87,8 @@ void AP_GPS_UBLOX::read(void)
_step++;
if (CLASS_NAV == data) {
_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 {
_error("ignoring class 0x%x\n", (int)data);
_gather = false; // class is not interesting, discard
}
break;
@ -109,31 +111,28 @@ 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
}
}
break;
case 4:
_step++;
_ck_b += (_ck_a += data); // checksum byte
_ck_b += (_ck_a += data); // checksum byte
_payload_length = data; // payload length low byte
break;
case 5:
_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_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
//
case 6:
_ck_b += (_ck_a += data); // checksum byte
_ck_b += (_ck_a += data); // checksum byte
if (_gather) // gather data if requested
_buffer.bytes[_payload_counter] = data;
if (++_payload_counter == _payload_length)
@ -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 (_ck_b != data)
break; // bad checksum
if (_gather) {
parsed = _parse_gps(); // Parse the new GPS packet
}
if (_gather)
_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;
}

8
libraries/AP_GPS/AP_GPS_UBLOX.h
View File

@ -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

54
libraries/AP_GPS/GPS.cpp
View File

@ -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");
init();
_setTime();
}
else
{
read();
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();
}
} 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);
}

94
libraries/AP_GPS/GPS.h
View File

@ -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
///
@ -98,14 +101,14 @@ protected:
/// long in the wrong byte order
/// @returns endian-swapped value
///
long _swapl(const void *bytes);
long _swapl(const void *bytes);
/// perform an endian swap on an int
///
/// @param bytes pointer to a buffer containing bytes representing an
/// int in the wrong byte order
/// @returns endian-swapped value
int _swapi(const void *bytes);
int _swapi(const void *bytes);
/// emit an error message
///
@ -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

37
libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.pde
View File

@ -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;
}
}