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

Cleanup. 

Teach AP_GPS about FastSerial (in the few places it needs to know) and about Stream everywhere else.

Do some minor code cleanup.

Tested with Mega and uBlox.  Some issues (e.g. reporting 0 satelites) remain.



git-svn-id: https://arducopter.googlecode.com/svn/trunk@404 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
DrZiplok@gmail.com 2010-09-06 09:20:44 +00:00
parent 6b6637bd06
commit 96a80f1c66
15 changed files with 302 additions and 464 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

164
libraries/AP_GPS/AP_GPS_406.cpp
View File

@ -1,3 +1,4 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/*
GPS_406.cpp - 406 GPS library for Arduino
Code by Jason Short, Jordi Muñoz and Jose Julio. DIYDrones.com
@ -16,7 +17,7 @@
update() : Call this funcion as often as you want to ensure you read the incomming gps data
Properties:
Lattitude : Lattitude * 10,000,000 (long value)
Latitude : Latitude * 10,000,000 (long value)
Longitude : Longitude * 10,000,000 (long value)
altitude : altitude * 100 (meters) (long value)
ground_speed : Speed (m/s) * 100 (long value)
@ -27,36 +28,30 @@
*/
#include "../FastSerial/FastSerial.h" // because we need to change baud rates... ugh.
#include "AP_GPS_406.h"
#include "WProgram.h"
#include <Stream.h>
uint8_t AP_GPS_406::buffer[MAXPAYLOAD] = {0x24,0x50,0x53,0x52,0x46,0x31,0x30,0x30,0x2C,0x30,0x2C,0x35,0x37,0x36,0x30,0x30,0x2C,0x38,0x2C,0x31,0x2C,0x30,0x2A,0x33,0x37,0x0D,0x0A};
#define PAYLOAD_LENGTH 92
#define BAUD_RATE 57600
// Constructors ////////////////////////////////////////////////////////////////
AP_GPS_406::AP_GPS_406()
AP_GPS_406::AP_GPS_406(Stream *s) : GPS(s)
{
}
// Public Methods ////////////////////////////////////////////////////////////////////
void AP_GPS_406::init(void)
{
change_to_sirf_protocol(); //Changes to SIFR protocol and sets baud rate
delay(100); //Waits fot the GPS to start_UP
configure_gps(); //Function to configure GPS, to output only the desired msg's
step = 0;
new_data = 0;
fix = 0;
print_errors = 0;
change_to_sirf_protocol(); // Changes to SIRF protocol and sets baud rate
delay(100); // Waits fot the GPS to start_UP
configure_gps(); // Function to configure GPS, to output only the desired msg's
}
// optimization : This code don´t wait for data, only proccess the data available
// optimization : This code doesn´t wait for data, 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_gps() to parse and update the GPS info.
void AP_GPS_406::update(void)
@ -64,21 +59,11 @@ void AP_GPS_406::update(void)
byte data;
int numc;
#if defined(__AVR_ATmega1280__) // If AtMega1280 then Serial port 1...
numc = Serial1.available();
#else
numc = Serial.available();
#endif
numc = _port->available();
if (numc > 0){
for (int i = 0; i < numc; i++){ // Process bytes received
#if defined(__AVR_ATmega1280__)
data = Serial1.read();
//Serial.print(data,HEX);
//Serial.println(" ");
#else
data = Serial.read();
#endif
data = _port->read();
switch(step){
case 0:
@ -143,57 +128,27 @@ AP_GPS_406::parse_gps(void)
{
uint8_t j;
fix = (buffer[1] > 0) ? 0:1;
fix = buffer[1] > 0;
j = 22;
lattitude = join_4_bytes(&buffer[j]); // lat * 10, 000, 000
latitude = _swapl(&buffer[22]); // lat * 10, 000, 000
longitude = _swapl(&buffer[26]); // lon * 10, 000, 000
altitude = _swapl(&buffer[34]); // alt in meters * 100
ground_speed = _swapi(&buffer[39]); // meters / second * 100
j = 26;
longitude = join_4_bytes(&buffer[j]); // lon * 10, 000, 000
j = 34;
altitude = join_4_bytes(&buffer[j]); // alt in meters * 100
j = 39;
ground_speed = join_2_bytes(&buffer[j]); // meters / second * 100
if(ground_speed >= 50){
//Only updates data if we are really moving...
j = 41;
ground_course = (unsigned int)join_2_bytes(&buffer[j]); // meters / second * 100
if (ground_speed >= 50) { // Only updates data if we are really moving...
ground_course = (unsigned int)_swapi(&buffer[41]); // meters / second * 100
}
j = 45;
//climb_rate = join_2_bytes(&buffer[j]); // meters / second * 100
//climb_rate = _swapi(&buffer[45]); // meters / second * 100
if(lattitude == 0){
if (latitude == 0) {
new_data = false;
fix = 0;
}else{
fix = false;
} else {
new_data = true;
}
}
// Join 4 bytes into a long
int32_t
AP_GPS_406::join_4_bytes(unsigned char Buffer[])
{
longUnion.byte[3] = *Buffer;
longUnion.byte[2] = *(Buffer + 1);
longUnion.byte[1] = *(Buffer + 2);
longUnion.byte[0] = *(Buffer + 3);
return(longUnion.dword);
}
// Join 2 bytes into an int
int16_t
AP_GPS_406::join_2_bytes(unsigned char Buffer[])
{
intUnion.byte[1] = *Buffer;
intUnion.byte[0] = *(Buffer + 1);
return(intUnion.word);
}
void
AP_GPS_406::configure_gps(void)
{
@ -201,82 +156,37 @@ AP_GPS_406::configure_gps(void)
const uint8_t gps_payload[] = {0x02, 0x04, 0x07, 0x09, 0x1B};
const uint8_t gps_checksum[] = {0xA8, 0xAA, 0xAD, 0xAF, 0xC1};
const uint8_t gps_ender[] = {0xB0, 0xB3};
const uint8_t cero = 0x00;
#if defined(__AVR_ATmega1280__)
for(int z = 0; z < 2; z++){
for(int x = 0; x < 5; x++){
for(int y = 0; y < 6; y++){
Serial1.print(gps_header[y]); // Prints the msg header, is the same header for all msg..
}
Serial1.print(gps_payload[x]); // Prints the payload, is not the same for every msg
for(int y = 0; y < 6; y++){
Serial1.print(cero); // Prints 6 zeros
}
Serial1.print(gps_checksum[x]); // Print the Checksum
Serial1.print(gps_ender[0]); // Print the Ender of the string, is same on all msg's.
Serial1.print(gps_ender[1]); // ender
_port->write(gps_header, sizeof(gps_header)); // Prints the msg header, is the same header for all msg..
_port->write(gps_payload[x]); // Prints the payload, is not the same for every msg
for(int y = 0; y < 6; y++) // Prints 6 zeros
_port->write((uint8_t)0);
_port->write(gps_checksum[x]); // Print the Checksum
_port->write(gps_ender[0]); // Print the Ender of the string, is same on all msg's.
_port->write(gps_ender[1]); // ender
}
}
#else
for(int z = 0; z < 2; z++){
for(int x = 0; x < 5; x++){
for(int y = 0; y < 6; y++){
Serial.print(gps_header[y]); // Prints the msg header, is the same header for all msg..
}
Serial.print(gps_payload[x]); // Prints the payload, is not the same for every msg
for(int y = 0; y < 6; y++){
Serial.print(cero); // Prints 6 zeros
}
Serial.print(gps_checksum[x]); // Print the Checksum
Serial.print(gps_ender[0]); // Print the Ender of the string, is same on all msg's.
Serial.print(gps_ender[1]); // ender
}
}
#endif
}
void
AP_GPS_406::change_to_sirf_protocol(void)
{
#if defined(__AVR_ATmega1280__)
Serial1.begin(4800); // Serial port 1 on ATMega1280 First try in 4800
FastSerial *fs = (FastSerial *)_port; // this is a bit grody...
fs->begin(4800); // First try at 4800bps
delay(300);
for (byte x = 0; x <= 28; x++){
Serial1.print(buffer[x]); // Sending special bytes declared at the beginning
}
_port->write(buffer, 28); // Sending special bytes declared at the beginning
delay(300);
Serial1.begin(9600); // Then try in 9600
fs->begin(9600); // Then try at 9600bps
delay(300);
for (byte x = 0; x <= 28; x++){
Serial1.print(buffer[x]);
}
_port->write(buffer, 28);
delay(300);
Serial1.begin(BAUD_RATE); //
fs->begin(BAUD_RATE); // Finally try at the configured bit rate (57600?)
delay(300);
for (byte x = 0; x <= 28; x++){
Serial1.print(buffer[x]);
}
delay(300);
#else
Serial.begin(4800); // Serial port (0) on AT168/328 First try in 4800
delay(300);
for (byte x = 0; x <= 28; x++){
Serial.print(buffer[x]); // Sending special bytes declared at the beginning
}
delay(300);
Serial.begin(9600); // Then try in 9600
delay(300);
for (byte x = 0; x <= 28; x++){
Serial.print(buffer[x]);
}
Serial.begin(BAUD_RATE); // Universal Sincronus Asyncronus Receiveing Transmiting
#endif
_port->write(buffer, 28);
}

10
libraries/AP_GPS/AP_GPS_406.h
View File

@ -1,3 +1,5 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
#ifndef AP_GPS_406_h
#define AP_GPS_406_h
@ -6,21 +8,19 @@
class AP_GPS_406 : public GPS
{
public:
public:
// Methods
AP_GPS_406();
AP_GPS_406(Stream *port);
void init();
void update();
private:
private:
// Internal variables
uint8_t step;
uint8_t payload_counter;
static uint8_t buffer[MAXPAYLOAD];
void parse_gps();
int32_t join_4_bytes(uint8_t Buffer[]);
int16_t join_2_bytes(uint8_t Buffer[]);
void change_to_sirf_protocol(void);
void configure_gps(void);
};

107
libraries/AP_GPS/AP_GPS_IMU.cpp
View File

@ -1,3 +1,4 @@
// -*- 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
@ -31,7 +32,7 @@
// Constructors ////////////////////////////////////////////////////////////////
AP_GPS_IMU::AP_GPS_IMU()
AP_GPS_IMU::AP_GPS_IMU(Stream *s) : GPS(s)
{
}
@ -39,19 +40,7 @@ AP_GPS_IMU::AP_GPS_IMU()
// Public Methods //////////////////////////////////////////////////////////////
void AP_GPS_IMU::init(void)
{
ck_a = 0;
ck_b = 0;
step = 0;
new_data = 0;
fix = 0;
print_errors = 0;
// initialize serial port
#if defined(__AVR_ATmega1280__)
Serial1.begin(38400); // Serial port 1 on ATMega1280
#else
Serial.begin(38400);
#endif
// 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.
@ -63,20 +52,12 @@ void AP_GPS_IMU::update(void)
int numc = 0;
static byte message_num = 0;
#if defined(__AVR_ATmega1280__) // If AtMega1280 then Serial port 1...
numc = Serial.available();
#else
numc = Serial.available();
#endif
numc = _port->available();
if (numc > 0){
for (int i=0;i<numc;i++){ // Process bytes received
#if defined(__AVR_ATmega1280__)
data = Serial.read();
#else
data = Serial.read();
#endif
data = _port->read();
switch(step){ //Normally we start from zero. This is a state machine
case 0:
@ -154,11 +135,10 @@ void AP_GPS_IMU::update(void)
} else if (message_num == 0x0a) {
//PERF_join_data();
} else {
Serial.print("Invalid message number = ");
Serial.println(message_num, DEC);
_error("Invalid message number = %d\n", (int)message_num);
}
} else {
Serial.println("XXX Checksum error"); //bad checksum
_error("XXX Checksum error\n"); //bad checksum
//imu_checksum_error_count++;
}
// Variable initialization
@ -178,50 +158,34 @@ void AP_GPS_IMU::update(void)
void AP_GPS_IMU::join_data(void)
{
int j = 0;
//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
intUnion.byte[0] = buffer[j++];
intUnion.byte[1] = buffer[j++];
roll_sensor = intUnion.word;
roll_sensor = *(int *)&buffer[0];
//Storing IMU pitch
intUnion.byte[0] = buffer[j++];
intUnion.byte[1] = buffer[j++];
pitch_sensor = intUnion.word;
pitch_sensor = *(int *)&buffer[2];
//Storing IMU heading (yaw)
intUnion.byte[0] = buffer[j++];
intUnion.byte[1] = buffer[j++];
ground_course = intUnion.word;
ground_course = *(int *)&buffer[4];
imu_ok = true;
}
void AP_GPS_IMU::join_data_xplane()
{
int j = 0;
//Storing IMU roll
intUnion.byte[0] = buffer[j++];
intUnion.byte[1] = buffer[j++];
roll_sensor = intUnion.word;
roll_sensor = *(int *)&buffer[0];
//Storing IMU pitch
intUnion.byte[0] = buffer[j++];
intUnion.byte[1] = buffer[j++];
pitch_sensor = intUnion.word;
pitch_sensor = *(int *)&buffer[2];
//Storing IMU heading (yaw)
intUnion.byte[0] = buffer[j++];
intUnion.byte[1] = buffer[j++];
ground_course = (unsigned int)intUnion.word;
ground_course = *(int *)&buffer[4];
//Storing airspeed
intUnion.byte[0] = buffer[j++];
intUnion.byte[1] = buffer[j++];
airspeed = intUnion.word;
airspeed = *(int *)&buffer[6];
imu_ok = true;
@ -229,50 +193,25 @@ void AP_GPS_IMU::join_data_xplane()
void AP_GPS_IMU::GPS_join_data(void)
{
//gps_messages_received++;
int j = 0;
longitude = join_4_bytes(&buffer[j]); // Lat and Lon * 10**7
j += 4;
lattitude = join_4_bytes(&buffer[j]);
j += 4;
longitude = *(long *)&buffer[0]; // degrees * 10e7
latitude = *(long *)&buffer[4];
//Storing GPS Height above the sea level
intUnion.byte[0] = buffer[j++];
intUnion.byte[1] = buffer[j++];
altitude = (long)intUnion.word * 10; // Altitude in meters * 100
altitude = (long)*(int *)&buffer[8] * 10;
//Storing Speed
intUnion.byte[0] = buffer[j++];
intUnion.byte[1] = buffer[j++];
speed_3d = ground_speed = (float)intUnion.word; // Speed in M/S * 100
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
j += 2;
time = join_4_bytes(&buffer[j]); // Time of Week in milliseconds
time = *(long *)&buffer[14];
j += 4;
imu_health = buffer[j++];
imu_health = buffer[15];
new_data = 1;
fix = 1;
new_data = true;
fix = true;
}
/****************************************************************
*
****************************************************************/
// Join 4 bytes into a long
long AP_GPS_IMU::join_4_bytes(unsigned char Buffer[])
{
longUnion.byte[0] = *Buffer;
longUnion.byte[1] = *(Buffer + 1);
longUnion.byte[2] = *(Buffer + 2);
longUnion.byte[3] = *(Buffer + 3);
return(longUnion.dword);
}
/****************************************************************
*

5
libraries/AP_GPS/AP_GPS_IMU.h
View File

@ -1,3 +1,4 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
#ifndef AP_GPS_IMU_h
#define AP_GPS_IMU_h
@ -7,8 +8,9 @@
class AP_GPS_IMU : public GPS
{
public:
// Methods
AP_GPS_IMU();
AP_GPS_IMU(Stream *s);
void init();
void update();
@ -37,7 +39,6 @@ class AP_GPS_IMU : public GPS
void join_data_xplane();
void GPS_join_data();
void checksum(unsigned char data);
long join_4_bytes(unsigned char Buffer[]);
};
#endif

80
libraries/AP_GPS/AP_GPS_MTK.cpp
View File

@ -1,3 +1,4 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/*
GPS_MTK.cpp - Ublox GPS library for Arduino
Code by Jordi Munoz and Jose Julio. DIYDrones.com
@ -31,7 +32,7 @@
#include "WProgram.h"
// Constructors ////////////////////////////////////////////////////////////////
AP_GPS_MTK::AP_GPS_MTK()
AP_GPS_MTK::AP_GPS_MTK(Stream *s) : GPS(s)
{
}
@ -39,26 +40,12 @@ AP_GPS_MTK::AP_GPS_MTK()
// Public Methods //////////////////////////////////////////////////////////////
void AP_GPS_MTK::init(void)
{
ck_a = 0;
ck_b = 0;
step = 0;
new_data = 0;
fix = 0;
print_errors = 0;
// initialize serial port for binary protocol use
#if defined(__AVR_ATmega1280__)
Serial1.begin(38400); // Serial port 1 on ATMega1280
Serial1.print(MTK_SET_BINARY);
Serial1.print(MTK_OUTPUT_4HZ);
#else
Serial.begin(38400);
Serial.print(MTK_SET_BINARY);
Serial.print(MTK_OUTPUT_4HZ);
#endif
_port->print(MTK_SET_BINARY);
_port->print(MTK_OUTPUT_4HZ);
}
// optimization : This code don¥t wait for data, only proccess the data available
// optimization : This code doesn't wait for data, 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_gps() to parse and update the GPS info.
void AP_GPS_MTK::update(void)
@ -66,18 +53,10 @@ void AP_GPS_MTK::update(void)
byte data;
int numc;
#if defined(__AVR_ATmega1280__) // If AtMega1280 then Serial port 1...
numc = Serial1.available();
#else
numc = Serial.available();
#endif
numc = _port->available();
if (numc > 0)
for (int i = 0; i < numc; i++){ // Process bytes received
#if defined(__AVR_ATmega1280__)
data = Serial1.read();
#else
data = Serial.read();
#endif
data = _port->read();
switch(step){
case 0:
@ -126,8 +105,7 @@ void AP_GPS_MTK::update(void)
if((ck_a == GPS_ck_a) && (ck_b == GPS_ck_b)){ // Verify the received checksum with the generated checksum..
parse_gps(); // Parse the new GPS packet
}else {
if (print_errors)
Serial.println("ERR:GPS_CHK!!");
_error("ERR:GPS_CHK!!\n");
}
// Variable initialization
step = 0;
@ -143,50 +121,26 @@ void AP_GPS_MTK::update(void)
void
AP_GPS_MTK::parse_gps(void)
{
int j;
//Verifing if we are in class 1, you can change this "IF" for a "Switch" in case you want to use other UBX classes..
//In this case all the message im using are in class 1, to know more about classes check PAGE 60 of DataSheet.
if(msg_class == 0x01) {
switch(id){
//Checking the UBX ID
case 0x05: // ID Custom
j = 0;
lattitude= join_4_bytes(&buffer[j]) * 10; // lon * 10000000
j += 4;
longitude = join_4_bytes(&buffer[j]) * 10; // lat * 10000000
j += 4;
altitude = join_4_bytes(&buffer[j]); // MSL
j += 4;
speed_3d = ground_speed = join_4_bytes(&buffer[j]);
j += 4;
ground_course = join_4_bytes(&buffer[j]) / 10000;
j += 4;
num_sats = buffer[j];
j++;
fix = buffer[j];
if (fix==3)
fix = 1;
else
fix = 0;
j++;
time = join_4_bytes(&buffer[j]);
new_data = 1;
latitude = _swapl(&buffer[0]);
longitude = _swapl(&buffer[4]);
altitude = _swapl(&buffer[8]);
speed_3d = ground_speed = _swapl(&buffer[12]);
ground_course = _swapl(&buffer[16]) / 10000;
num_sats = buffer[20];
fix = buffer[21] == 3;
time = _swapl(&buffer[22]);
new_data = true;
break;
}
}
}
// Join 4 bytes into a long
long
AP_GPS_MTK::join_4_bytes(unsigned char buffer[])
{
longUnion.byte[3] = *buffer;
longUnion.byte[2] = *(buffer + 1);
longUnion.byte[1] = *(buffer + 2);
longUnion.byte[0] = *(buffer + 3);
return(longUnion.dword);
}
// checksum algorithm
void
AP_GPS_MTK::checksum(byte data)

26
libraries/AP_GPS/AP_GPS_MTK.h
View File

@ -1,31 +1,32 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
#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_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_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 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"
#define WAAS_ON "$PSRF151,1*3F\r\n"
#define WAAS_OFF "$PSRF151,0*3E\r\n"
class AP_GPS_MTK : public GPS
{
public:
// Methods
AP_GPS_MTK();
AP_GPS_MTK(Stream *s);
void init();
void update();
@ -46,6 +47,5 @@ class AP_GPS_MTK : public GPS
void parse_gps();
void checksum(unsigned char data);
long join_4_bytes(unsigned char Buffer[]);
};
#endif

42
libraries/AP_GPS/AP_GPS_NMEA.cpp
View File

@ -1,3 +1,4 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/*
GPS_NMEA.cpp - Generic NMEA GPS library for Arduino
Code by Jordi Muñoz and Jose Julio. DIYDrones.com
@ -19,7 +20,7 @@
update() : Call this funcion as often as you want to ensure you read the incomming gps data
Properties:
lattitude : lattitude * 10000000 (long value)
latitude : latitude * 10000000 (long value)
longitude : longitude * 10000000 (long value)
altitude : altitude * 1000 (milimeters) (long value)
ground_speed : Speed (m / s) * 100 (long value)
@ -40,7 +41,7 @@
#include "WProgram.h"
// Constructors ////////////////////////////////////////////////////////////////
AP_GPS_NMEA::AP_GPS_NMEA()
AP_GPS_NMEA::AP_GPS_NMEA(Stream *s) : GPS(s)
{
}
@ -49,18 +50,6 @@ void
AP_GPS_NMEA::init(void)
{
//Type = 2;
GPS_checksum_calc = false;
bufferidx = 0;
new_data = 0;
fix = 0;
quality = 0;
print_errors = 0;
// Initialize serial port
#if defined(__AVR_ATmega1280__)
Serial1.begin(38400); // Serial port 1 on ATMega1280
#else
Serial.begin(38400);
#endif
}
// This code don´t wait for data, only proccess the data available on serial port
@ -73,19 +62,11 @@ AP_GPS_NMEA::update(void)
int numc;
int i;
#if defined(__AVR_ATmega1280__) // If AtMega1280 then Serial port 1...
numc = Serial1.available();
#else
numc = Serial.available();
#endif
numc = _port->available();
if (numc > 0){
for (i = 0; i < numc; i++){
#if defined(__AVR_ATmega1280__) // If AtMega1280 then Serial port 1...
c = Serial1.read();
#else
c = Serial.read();
#endif
c = _port->read();
if (c == '$'){ // NMEA Start
bufferidx = 0;
buffer[bufferidx++] = c;
@ -138,10 +119,10 @@ AP_GPS_NMEA::parse_nmea_gps(void)
//
aux_deg = parsedecimal(parseptr, 2); // degrees
aux_min = parsenumber(parseptr + 2, 4); // minutes (sexagesimal) => Convert to decimal
lattitude = aux_deg * 10000000 + (aux_min * 50) / 3; // degrees + minutes / 0.6 ( * 10000000) (0.6 = 3 / 5)
latitude = aux_deg * 10000000 + (aux_min * 50) / 3; // degrees + minutes / 0.6 ( * 10000000) (0.6 = 3 / 5)
parseptr = strchr(parseptr, ', ')+1;
if ( * parseptr == 'S')
lattitude = -1 * lattitude; // South lattitudes are negative
latitude = -1 * latitude; // South latitudes are negative
parseptr = strchr(parseptr, ', ')+1;
// W longitudes are Negative
aux_deg = parsedecimal(parseptr, 3); // degrees
@ -170,8 +151,7 @@ AP_GPS_NMEA::parse_nmea_gps(void)
else
quality = 4; // Good (HDOP < 25)
} else {
if (print_errors)
Serial.println("GPSERR: Checksum error!!");
_error("GPSERR: Checksum error!!\n");
}
}
} else if (strncmp(buffer,"$GPVTG",6)==0){ // Check if sentence begins with $GPVTG
@ -190,14 +170,12 @@ AP_GPS_NMEA::parse_nmea_gps(void)
ground_speed = parsenumber(parseptr, 2) * 10 / 36; // Convert Km / h to m / s ( * 100)
//GPS_line = true;
} else {
if (print_errors)
Serial.println("GPSERR: Checksum error!!");
_error("GPSERR: Checksum error!!\n");
}
}
} else {
bufferidx = 0;
if (print_errors)
Serial.println("GPSERR: Bad sentence!!");
_error("GPSERR: Bad sentence!!\n");
}
}

3
libraries/AP_GPS/AP_GPS_NMEA.h
View File

@ -1,3 +1,4 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
#ifndef AP_GPS_NMEA_h
#define AP_GPS_NMEA_h
@ -8,7 +9,7 @@ class AP_GPS_NMEA : public GPS
{
public:
// Methods
AP_GPS_NMEA();
AP_GPS_NMEA(Stream *s);
void init();
void update();

93
libraries/AP_GPS/AP_GPS_UBLOX.cpp
View File

@ -1,3 +1,4 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/*
GPS_UBLOX.cpp - Ublox GPS library for Arduino
Code by Jordi Muñoz and Jose Julio. DIYDrones.com
@ -37,7 +38,7 @@
#include "WProgram.h"
// Constructors ////////////////////////////////////////////////////////////////
AP_GPS_UBLOX::AP_GPS_UBLOX()
AP_GPS_UBLOX::AP_GPS_UBLOX(Stream *s) : GPS(s)
{
}
@ -45,22 +46,9 @@ AP_GPS_UBLOX::AP_GPS_UBLOX()
// Public Methods ////////////////////////////////////////////////////////////////////
void AP_GPS_UBLOX::init(void)
{
ck_a = 0;
ck_b = 0;
step = 0;
new_data = 0;
fix = 0;
print_errors = 0;
// initialize serial port
#if defined(__AVR_ATmega1280__)
Serial1.begin(38400); // Serial port 1 on ATMega1280
#else
Serial.begin(38400);
#endif
}
// optimization : This code don´t wait for data, only proccess the data available
// optimization : This code doesn't wait for data, 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_gps() to parse and update the GPS info.
void AP_GPS_UBLOX::update(void)
@ -68,19 +56,11 @@ void AP_GPS_UBLOX::update(void)
byte data;
int numc;
#if defined(__AVR_ATmega1280__) // If AtMega1280 then Serial port 1...
numc = Serial1.available();
#else
numc = Serial.available();
#endif
numc = _port->available();
if (numc > 0){
for (int i = 0;i < numc;i++){ // Process bytes received
#if defined(__AVR_ATmega1280__)
data = Serial1.read();
#else
data = Serial.read();
#endif
data = _port->read();
switch(step){
case 0:
@ -113,8 +93,7 @@ void AP_GPS_UBLOX::update(void)
step++;
// We check if the payload lenght is valid...
if (payload_length_hi >= MAXPAYLOAD){
if (print_errors)
Serial.println("ERR:GPS_BAD_PAYLOAD_LENGTH!!");
_error("ERR:GPS_BAD_PAYLOAD_LENGTH!!\n");
step = 0; // Bad data, so restart to step zero and try again.
ck_a = 0;
ck_b = 0;
@ -148,7 +127,7 @@ void AP_GPS_UBLOX::update(void)
if((ck_a == GPS_ck_a) && (ck_b == GPS_ck_b)){ // Verify the received checksum with the generated checksum..
parse_gps(); // Parse the new GPS packet
}else{
if (print_errors) Serial.println("ERR:GPS_CHK!!");
_error("ERR:GPS_CHK!!\n");
}
// Variable initialization
step = 0;
@ -164,73 +143,41 @@ void AP_GPS_UBLOX::update(void)
void
AP_GPS_UBLOX::parse_gps(void)
{
int j;
//Verifing if we are in msg_class 1, you can change this "IF" for a "Switch" in case you want to use other UBX classes..
// In this case all the message im using are in msg_class 1, to know more about classes check PAGE 60 of DataSheet.
if(msg_class == 0x01){
switch(id) {//Checking the UBX ID
case 0x02: // ID NAV - POSLLH
j = 0;
time = join_4_bytes(&buffer[j]); // ms Time of week
j += 4;
longitude = join_4_bytes(&buffer[j]); // lon * 10,000,000
j += 4;
lattitude = join_4_bytes(&buffer[j]); // lat * 10,000,000
j += 4;
//altitude = join_4_bytes(&buffer[j]); // elipsoid heigth mm
j += 4;
altitude = (float)join_4_bytes(&buffer[j]) / 10; // MSL heigth mm
//j+=4;
time = *(long *)&buffer[0]; // ms Time of week
longitude = *(long *)&buffer[4]; // lon * 10,000,000
latitude = *(long *)&buffer[8]; // lat * 10,000,000
//altitude = *(long *)&buffer[12]; // elipsoid heigth mm
altitude = *(long *)&buffer[16] / 10; // MSL heigth mm
/*
hacc = (float)join_4_bytes(&buffer[j]) / (float)1000;
j += 4;
vacc = (float)join_4_bytes(&buffer[j]) / (float)1000;
j += 4;
hacc = (float)*(long *)&buffer[20];
vacc = (float)*(long *)&buffer[24];
*/
new_data = 1;
new_data = true;
break;
case 0x03: //ID NAV - STATUS
//if(buffer[4] >= 0x03)
if((buffer[4] >= 0x03) && (buffer[5] & 0x01))
fix = 1; // valid position
else
fix = 0; // invalid position
fix = ((buffer[4] >= 0x03) && (buffer[5] & 0x01));
break;
case 0x06: //ID NAV - SOL
if((buffer[10] >= 0x03) && (buffer[11] & 0x01))
fix = 1; // valid position
else
fix = 0; // invalid position
//ecefVZ = join_4_bytes(&buffer[36]); // Vertical Speed in cm / s
fix = ((buffer[10] >= 0x03) && (buffer[11] & 0x01));
num_sats = buffer[47]; // Number of sats...
break;
case 0x12: // ID NAV - VELNED
j = 16;
speed_3d = join_4_bytes(&buffer[j]); // cm / s
j += 4;
ground_speed = join_4_bytes(&buffer[j]); // Ground speed 2D cm / s
j += 4;
ground_course = join_4_bytes(&buffer[j]); // Heading 2D deg * 100000
ground_course /= 1000; // Rescale heading to deg * 100
j += 4;
speed_3d = *(long *)&buffer[16]; // cm / s
ground_speed = *(long *)&buffer[20]; // Ground speed 2D cm / s
ground_course = *(long *)&buffer[24] / 1000; // Heading 2D deg * 100000 rescaled to deg * 100
break;
}
}
}
// Join 4 bytes into a long
long AP_GPS_UBLOX::join_4_bytes(unsigned char Buffer[])
{
longUnion.byte[0] = *Buffer;
longUnion.byte[1] = *(Buffer + 1);
longUnion.byte[2] = *(Buffer + 2);
longUnion.byte[3] = *(Buffer + 3);
return(longUnion.dword);
}
// Ublox checksum algorithm
void AP_GPS_UBLOX::checksum(byte data)
{

3
libraries/AP_GPS/AP_GPS_UBLOX.h
View File

@ -1,3 +1,4 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
#ifndef AP_GPS_UBLOX_h
#define AP_GPS_UBLOX_h
@ -8,7 +9,7 @@ class AP_GPS_UBLOX : public GPS
{
public:
// Methods
AP_GPS_UBLOX();
AP_GPS_UBLOX(Stream *s);
void init();
void update();

122
libraries/AP_GPS/GPS.h
View File

@ -1,38 +1,116 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/// @file GPS.h
/// @brief Interface definition for the various GPS drivers.
#ifndef GPS_h
#define GPS_h
#include <inttypes.h>
#include <Stream.h>
/// @class GPS
/// @brief Abstract base class for GPS receiver drivers.
class GPS
{
public:
// Methods
public:
/// Constructor
///
/// @note The stream is expected to be set up and configured for the
/// correct bitrate before ::init is called.
///
/// @param port Stream connected to the GPS module.
///
GPS(Stream *port) : _port(port) {};
/// Startup initialisation.
///
/// This routine performs any one-off initialisation required to set the
/// GPS up for use.
///
virtual void init();
/// Update GPS state based on possible bytes received from the module.
///
/// This routine must be called periodically to process incoming data.
///
virtual void update();
// Properties
long time; //GPS Millisecond Time of Week
long lattitude; // Geographic coordinates
long longitude;
long altitude;
long ground_speed;
long ground_course;
long speed_3d;
uint8_t num_sats; // Number of visible satelites
uint8_t fix; // 1:GPS FIX 0:No FIX (normal logic)
uint8_t new_data; // 1:New GPS Data
uint8_t print_errors; // 1: To Print GPS Errors (for debug)
long GPS_timer;
long time; ///< GPS time in milliseconds from the start of the week
long latitude; ///< latitude in degrees * 10,000,000
long longitude; ///< longitude in degrees * 10,000,000
long altitude; ///< altitude in cm
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)
uint8_t num_sats; ///< Number of visible satelites
bool fix; ///< true if we have a position fix
union long_union {
int32_t dword;
uint8_t byte[4];
} longUnion;
/// Set to true when new data arrives. A client may set this
/// to false in order to avoid processing data they have
/// already seen.
bool new_data;
union int_union {
int16_t word;
uint8_t byte[2];
} intUnion;
bool print_errors; ///< if true, errors will be printed to stderr
protected:
Stream *_port; ///< stream port the GPS is attached to
/// perform an endian swap on a long
///
/// @param bytes pointer to a buffer containing bytes representing a
/// long in the wrong byte order
/// @returns endian-swapped value
///
long _swapl(const uint8_t *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 uint8_t *bytes);
/// emit an error message
///
/// based on the value of print_errors, emits the printf-formatted message
/// in msg,... to stderr
///
/// @param fmt printf-like format string
///
void _error(const char *msg, ...);
};
inline long
GPS::_swapl(const uint8_t *bytes)
{
union {
long v;
uint8_t b[4];
} u;
u.b[0] = bytes[3];
u.b[1] = bytes[2];
u.b[2] = bytes[1];
u.b[3] = bytes[0];
return(u.v);
}
inline int16_t
GPS::_swapi(const uint8_t *bytes)
{
union {
int16_t v;
uint8_t b[2];
} u;
u.b[0] = bytes[1];
u.b[1] = bytes[0];
return(u.v);
}
#endif

21
libraries/AP_GPS/examples/GPS_406_test/GPS_406_test.pde
View File

@ -1,23 +1,28 @@
/*
Example of GPS MTK library.
Example of GPS 406 library.
Code by Jordi Munoz and Jose Julio. DIYDrones.com
Works with Ardupilot Mega Hardware (GPS on Serial Port1)
and with standard ATMega168 and ATMega328 on Serial Port 0
*/
#include <AP_GPS_406.h> // UBLOX GPS Library
#include <FastSerial.h>
#include <AP_GPS_406.h>
#include <stdio.h>
AP_GPS_406 gps;
FastSerialPort0(Serial);
FastSerialPort1(Serial1);
AP_GPS_406 gps(&Serial1);
#define T6 1000000
#define T7 10000000
void setup()
{
Serial.begin(38400);
Serial.println("Switching to 57600 Baud");
delay(500);
Serial.begin(57600);
Serial1.begin(57600);
stderr = stdout;
gps.print_errors = true;
Serial.println("GPS 406 library test");
gps.init(); // GPS Initialization
delay(1000);
@ -29,7 +34,7 @@ void loop()
if (gps.new_data){
Serial.print("gps:");
Serial.print(" Lat:");
Serial.print((float)gps.lattitude / T7, DEC);
Serial.print((float)gps.latitude / T7, DEC);
Serial.print(" Lon:");
Serial.print((float)gps.longitude / T7, DEC);
Serial.print(" Alt:");

16
libraries/AP_GPS/examples/GPS_MTK_test/GPS_MTK_test.pde
View File

@ -3,18 +3,26 @@
Code by Jordi Munoz and Jose Julio. DIYDrones.com
Works with Ardupilot Mega Hardware (GPS on Serial Port1)
and with standard ATMega168 and ATMega328 on Serial Port 0
*/
#include <AP_GPS_MTK.h> // UBLOX GPS Library
#include <FastSerial.h>
#include <AP_GPS_MTK.h>
#include <stdio.h>
AP_GPS_MTK gps;
FastSerialPort0(Serial);
FastSerialPort1(Serial1);
AP_GPS_MTK gps(&Serial);
#define T6 1000000
#define T7 10000000
void setup()
{
Serial.begin(38400);
Serial1.begin(57600);
stderr = stdout;
gps.print_errors = true;
Serial.println("GPS MTK library test");
gps.init(); // GPS Initialization
delay(1000);
@ -26,7 +34,7 @@ void loop()
if (gps.new_data){
Serial.print("gps:");
Serial.print(" Lat:");
Serial.print((float)gps.lattitude / T7, DEC);
Serial.print((float)gps.latitude / T7, DEC);
Serial.print(" Lon:");
Serial.print((float)gps.longitude / T7, DEC);
Serial.print(" Alt:");

20
libraries/AP_GPS/examples/GPS_NMEA_test/GPS_NMEA_test.pde
View File

@ -1,20 +1,28 @@
/*
Example of GPS MTK library.
Example of GPS NMEA library.
Code by Jordi Munoz and Jose Julio. DIYDrones.com
Works with Ardupilot Mega Hardware (GPS on Serial Port1)
and with standard ATMega168 and ATMega328 on Serial Port 0
*/
#include <AP_GPS_NMEA.h> // UBLOX GPS Library
#include <FastSerial.h>
#include <AP_GPS_NMEA.h>
#include <stdio.h>
AP_GPS_NMEA gps;
FastSerialPort0(Serial);
FastSerialPort1(Serial1);
AP_GPS_NMEA gps(&Serial1);
#define T6 1000000
#define T7 10000000
void setup()
{
Serial.begin(57600);
Serial.begin(38400);
Serial1.begin(38400);
stderr = stdout;
gps.print_errors = true;
Serial.println("GPS NMEA library test");
gps.init(); // GPS Initialization
delay(1000);
@ -27,7 +35,7 @@ void loop()
if (gps.new_data){
Serial.print("gps:");
Serial.print(" Lat:");
Serial.print((float)gps.lattitude / T7, DEC);
Serial.print((float)gps.latitude / T7, DEC);
Serial.print(" Lon:");
Serial.print((float)gps.longitude / T7, DEC);
Serial.print(" Alt:");

18
libraries/AP_GPS/examples/GPS_UBLOX_test/GPS_UBLOX_test.pde
View File

@ -1,20 +1,28 @@
/*
Example of GPS MTK library.
Example of GPS UBlox library.
Code by Jordi Munoz and Jose Julio. DIYDrones.com
Works with Ardupilot Mega Hardware (GPS on Serial Port1)
and with standard ATMega168 and ATMega328 on Serial Port 0
*/
#include <AP_GPS_UBLOX.h> // UBLOX GPS Library
#include <FastSerial.h>
#include <AP_GPS_UBLOX.h>
#include <stdio.h>
AP_GPS_UBLOX gps;
FastSerialPort0(Serial);
FastSerialPort1(Serial1);
AP_GPS_UBLOX gps(&Serial1);
#define T6 1000000
#define T7 10000000
void setup()
{
Serial.begin(38400);
Serial1.begin(38400);
stderr = stdout;
gps.print_errors = true;
Serial.println("GPS UBLOX library test");
gps.init(); // GPS Initialization
delay(1000);
@ -26,7 +34,7 @@ void loop()
if (gps.new_data){
Serial.print("gps:");
Serial.print(" Lat:");
Serial.print((float)gps.lattitude / T7, DEC);
Serial.print((float)gps.latitude / T7, DEC);
Serial.print(" Lon:");
Serial.print((float)gps.longitude / T7, DEC);
Serial.print(" Alt:");