Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Push server-native line endings.

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@418 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
DrZiplok@gmail.com 2010-09-06 20:16:50 +00:00
parent add89239f3
commit 7e345e027c
8 changed files with 981 additions and 981 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

384
libraries/AP_GPS/AP_GPS_406.cpp
View File

@ -1,192 +1,192 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/* /*
GPS_406.cpp - 406 GPS library for Arduino GPS_406.cpp - 406 GPS library for Arduino
Code by Jason Short, Jordi Muñoz and Jose Julio. DIYDrones.com Code by Jason Short, Jordi Muñoz and Jose Julio. DIYDrones.com
This code works with boards based on ATMega168/328 ATMega1280 (Serial port 1) This code works with boards based on ATMega168/328 ATMega1280 (Serial port 1)
This library is free software; you can redistribute it and / or This library is free software; you can redistribute it and / or
modify it under the terms of the GNU Lesser General Public modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version. version 2.1 of the License, or (at your option) any later version.
GPS configuration : 406 protocol GPS configuration : 406 protocol
Baud rate : 57600 Baud rate : 57600
Methods: Methods:
init() : GPS initialization init() : GPS initialization
update() : Call this funcion as often as you want to ensure you read the incomming gps data update() : Call this funcion as often as you want to ensure you read the incomming gps data
Properties: Properties:
Latitude : Latitude * 10,000,000 (long value) Latitude : Latitude * 10,000,000 (long value)
Longitude : Longitude * 10,000,000 (long value) Longitude : Longitude * 10,000,000 (long value)
altitude : altitude * 100 (meters) (long value) altitude : altitude * 100 (meters) (long value)
ground_speed : Speed (m/s) * 100 (long value) ground_speed : Speed (m/s) * 100 (long value)
ground_course : Course (degrees) * 100 (long value) ground_course : Course (degrees) * 100 (long value)
new_data : 1 when a new data is received. new_data : 1 when a new data is received.
You need to write a 0 to new_data when you read the data You need to write a 0 to new_data when you read the data
fix : 1: GPS FIX, 0: No fix (normal logic) fix : 1: GPS FIX, 0: No fix (normal logic)
*/ */
#include "../FastSerial/FastSerial.h" // because we need to change baud rates... ugh. #include "../FastSerial/FastSerial.h" // because we need to change baud rates... ugh.
#include "AP_GPS_406.h" #include "AP_GPS_406.h"
#include "WProgram.h" #include "WProgram.h"
#include <Stream.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}; 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 PAYLOAD_LENGTH 92
#define BAUD_RATE 57600 #define BAUD_RATE 57600
// Constructors //////////////////////////////////////////////////////////////// // Constructors ////////////////////////////////////////////////////////////////
AP_GPS_406::AP_GPS_406(Stream *s) : GPS(s) AP_GPS_406::AP_GPS_406(Stream *s) : GPS(s)
{ {
} }
// Public Methods //////////////////////////////////////////////////////////////////// // Public Methods ////////////////////////////////////////////////////////////////////
void AP_GPS_406::init(void) void AP_GPS_406::init(void)
{ {
change_to_sirf_protocol(); // Changes to SIRF protocol and sets baud rate change_to_sirf_protocol(); // Changes to SIRF protocol and sets baud rate
delay(100); // Waits fot the GPS to start_UP delay(100); // Waits fot the GPS to start_UP
configure_gps(); // Function to configure GPS, to output only the desired msg's configure_gps(); // Function to configure GPS, to output only the desired msg's
} }
// optimization : This code doesn´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) // 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. // If we get a complete packet this function calls parse_gps() to parse and update the GPS info.
void AP_GPS_406::update(void) void AP_GPS_406::update(void)
{ {
byte data; byte data;
int numc; int numc;
numc = _port->available(); numc = _port->available();
if (numc > 0){ if (numc > 0){
for (int i = 0; i < numc; i++){ // Process bytes received for (int i = 0; i < numc; i++){ // Process bytes received
data = _port->read(); data = _port->read();
switch(step){ switch(step){
case 0: case 0:
if(data == 0xA0) if(data == 0xA0)
step++; step++;
break; break;
case 1: case 1:
if(data == 0xA2) if(data == 0xA2)
step++; step++;
else else
step = 0; step = 0;
break; break;
/* case 2: /* case 2:
if(data == 0xA2) if(data == 0xA2)
step++; step++;
else else
step = 0; step = 0;
break; break;
*/ */
case 2: case 2:
if(data == 0x00) if(data == 0x00)
step++; step++;
else else
step = 0; step = 0;
break; break;
case 3: case 3:
if(data == 0x5B) if(data == 0x5B)
step++; step++;
else else
step = 0; step = 0;
break; break;
case 4: case 4:
if(data == 0x29){ if(data == 0x29){
payload_counter = 0; payload_counter = 0;
step++; step++;
}else }else
step = 0; step = 0;
break; break;
case 5: // Payload data read... case 5: // Payload data read...
if (payload_counter <= PAYLOAD_LENGTH){ // We stay in this state until we reach the payload_length if (payload_counter <= PAYLOAD_LENGTH){ // We stay in this state until we reach the payload_length
buffer[payload_counter] = data; buffer[payload_counter] = data;
payload_counter++; payload_counter++;
if (payload_counter == PAYLOAD_LENGTH){ if (payload_counter == PAYLOAD_LENGTH){
parse_gps(); parse_gps();
step = 0; step = 0;
} }
} }
break; break;
} }
} // End for... } // End for...
} }
} }
// Private Methods ////////////////////////////////////////////////////////////// // Private Methods //////////////////////////////////////////////////////////////
void void
AP_GPS_406::parse_gps(void) AP_GPS_406::parse_gps(void)
{ {
uint8_t j; uint8_t j;
fix = buffer[1] > 0; fix = buffer[1] > 0;
latitude = _swapl(&buffer[22]); // lat * 10, 000, 000 latitude = _swapl(&buffer[22]); // lat * 10, 000, 000
longitude = _swapl(&buffer[26]); // lon * 10, 000, 000 longitude = _swapl(&buffer[26]); // lon * 10, 000, 000
altitude = _swapl(&buffer[34]); // alt in meters * 100 altitude = _swapl(&buffer[34]); // alt in meters * 100
ground_speed = _swapi(&buffer[39]); // meters / second * 100 ground_speed = _swapi(&buffer[39]); // meters / second * 100
if (ground_speed >= 50) { // Only updates data if we are really moving... if (ground_speed >= 50) { // Only updates data if we are really moving...
ground_course = (unsigned int)_swapi(&buffer[41]); // meters / second * 100 ground_course = (unsigned int)_swapi(&buffer[41]); // meters / second * 100
} }
//climb_rate = _swapi(&buffer[45]); // meters / second * 100 //climb_rate = _swapi(&buffer[45]); // meters / second * 100
if (latitude == 0) { if (latitude == 0) {
new_data = false; new_data = false;
fix = false; fix = false;
} else { } else {
new_data = true; new_data = true;
} }
} }
void void
AP_GPS_406::configure_gps(void) AP_GPS_406::configure_gps(void)
{ {
const uint8_t gps_header[] = {0xA0, 0xA2, 0x00, 0x08, 0xA6, 0x00}; const uint8_t gps_header[] = {0xA0, 0xA2, 0x00, 0x08, 0xA6, 0x00};
const uint8_t gps_payload[] = {0x02, 0x04, 0x07, 0x09, 0x1B}; const uint8_t gps_payload[] = {0x02, 0x04, 0x07, 0x09, 0x1B};
const uint8_t gps_checksum[] = {0xA8, 0xAA, 0xAD, 0xAF, 0xC1}; const uint8_t gps_checksum[] = {0xA8, 0xAA, 0xAD, 0xAF, 0xC1};
const uint8_t gps_ender[] = {0xB0, 0xB3}; const uint8_t gps_ender[] = {0xB0, 0xB3};
for(int z = 0; z < 2; z++){ for(int z = 0; z < 2; z++){
for(int x = 0; x < 5; x++){ for(int x = 0; x < 5; x++){
_port->write(gps_header, sizeof(gps_header)); // Prints the msg header, is the same header for all msg.. _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 _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 for(int y = 0; y < 6; y++) // Prints 6 zeros
_port->write((uint8_t)0); _port->write((uint8_t)0);
_port->write(gps_checksum[x]); // Print the Checksum _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[0]); // Print the Ender of the string, is same on all msg's.
_port->write(gps_ender[1]); // ender _port->write(gps_ender[1]); // ender
} }
} }
} }
void void
AP_GPS_406::change_to_sirf_protocol(void) AP_GPS_406::change_to_sirf_protocol(void)
{ {
FastSerial *fs = (FastSerial *)_port; // this is a bit grody... FastSerial *fs = (FastSerial *)_port; // this is a bit grody...
fs->begin(4800); // First try at 4800bps fs->begin(4800); // First try at 4800bps
delay(300); delay(300);
_port->write(buffer, 28); // Sending special bytes declared at the beginning _port->write(buffer, 28); // Sending special bytes declared at the beginning
delay(300); delay(300);
fs->begin(9600); // Then try at 9600bps fs->begin(9600); // Then try at 9600bps
delay(300); delay(300);
_port->write(buffer, 28); _port->write(buffer, 28);
delay(300); delay(300);
fs->begin(BAUD_RATE); // Finally try at the configured bit rate (57600?) fs->begin(BAUD_RATE); // Finally try at the configured bit rate (57600?)
delay(300); delay(300);
_port->write(buffer, 28); _port->write(buffer, 28);
} }

58
libraries/AP_GPS/AP_GPS_406.h
View File

@ -1,29 +1,29 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
#ifndef AP_GPS_406_h #ifndef AP_GPS_406_h
#define AP_GPS_406_h #define AP_GPS_406_h
#include <GPS.h> #include <GPS.h>
#define MAXPAYLOAD 100 #define MAXPAYLOAD 100
class AP_GPS_406 : public GPS class AP_GPS_406 : public GPS
{ {
public: public:
// Methods // Methods
AP_GPS_406(Stream *port); AP_GPS_406(Stream *port);
void init(); void init();
void update(); void update();
private: private:
// Internal variables // Internal variables
uint8_t step; uint8_t step;
uint8_t payload_counter; uint8_t payload_counter;
static uint8_t buffer[MAXPAYLOAD]; static uint8_t buffer[MAXPAYLOAD];
void parse_gps(); void parse_gps();
void change_to_sirf_protocol(void); void change_to_sirf_protocol(void);
void configure_gps(void); void configure_gps(void);
}; };
#endif #endif

448
libraries/AP_GPS/AP_GPS_IMU.cpp
View File

@ -1,224 +1,224 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/* /*
GPS_MTK.cpp - Ublox GPS library for Arduino GPS_MTK.cpp - Ublox GPS library for Arduino
Code by Jordi Muñoz and Jose Julio. DIYDrones.com 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 code works with boards based on ATMega168/328 and ATMega1280 (Serial port 1)
This library is free software; you can redistribute it and/or This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version. version 2.1 of the License, or (at your option) any later version.
GPS configuration : Costum protocol GPS configuration : Costum protocol
Baud rate : 38400 Baud rate : 38400
Methods: Methods:
init() : GPS initialization init() : GPS initialization
update() : Call this funcion as often as you want to ensure you read the incomming gps data update() : Call this funcion as often as you want to ensure you read the incomming gps data
Properties: Properties:
lattitude : lattitude * 10000000 (long value) lattitude : lattitude * 10000000 (long value)
longitude : longitude * 10000000 (long value) longitude : longitude * 10000000 (long value)
altitude : altitude * 100 (meters) (long value) altitude : altitude * 100 (meters) (long value)
ground_speed : Speed (m/s) * 100 (long value) ground_speed : Speed (m/s) * 100 (long value)
ground_course : Course (degrees) * 100 (long value) ground_course : Course (degrees) * 100 (long value)
new_data : 1 when a new data is received. new_data : 1 when a new data is received.
You need to write a 0 to new_data when you read the data 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. fix : 1: GPS NO fix, 2: 2D fix, 3: 3D fix.
*/ */
#include "AP_GPS_IMU.h" #include "AP_GPS_IMU.h"
#include "WProgram.h" #include "WProgram.h"
// Constructors //////////////////////////////////////////////////////////////// // Constructors ////////////////////////////////////////////////////////////////
AP_GPS_IMU::AP_GPS_IMU(Stream *s) : GPS(s) AP_GPS_IMU::AP_GPS_IMU(Stream *s) : GPS(s)
{ {
} }
// Public Methods ////////////////////////////////////////////////////////////// // Public Methods //////////////////////////////////////////////////////////////
void AP_GPS_IMU::init(void) void AP_GPS_IMU::init(void)
{ {
// we expect the stream to already be open at the corret bitrate // 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. // 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) // 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. // If we get a complete packet this function calls parse_IMU_gps() to parse and update the GPS info.
void AP_GPS_IMU::update(void) void AP_GPS_IMU::update(void)
{ {
byte data; byte data;
int numc = 0; int numc = 0;
static byte message_num = 0; static byte message_num = 0;
numc = _port->available(); numc = _port->available();
if (numc > 0){ if (numc > 0){
for (int i=0;i<numc;i++){ // Process bytes received for (int i=0;i<numc;i++){ // Process bytes received
data = _port->read(); data = _port->read();
switch(step){ //Normally we start from zero. This is a state machine switch(step){ //Normally we start from zero. This is a state machine
case 0: case 0:
if(data == 0x44) // IMU sync char 1 if(data == 0x44) // IMU sync char 1
step++; //OH first data packet is correct, so jump to the next step step++; //OH first data packet is correct, so jump to the next step
break; break;
case 1: case 1:
if(data == 0x49) // IMU sync char 2 if(data == 0x49) // IMU sync char 2
step++; //ooh! The second data packet is correct, jump to the step 2 step++; //ooh! The second data packet is correct, jump to the step 2
else else
step=0; //Nop, is not correct so restart to step zero and try again. step=0; //Nop, is not correct so restart to step zero and try again.
break; break;
case 2: case 2:
if(data == 0x59) // IMU sync char 3 if(data == 0x59) // IMU sync char 3
step++; //ooh! The second data packet is correct, jump to the step 2 step++; //ooh! The second data packet is correct, jump to the step 2
else else
step=0; //Nop, is not correct so restart to step zero and try again. step=0; //Nop, is not correct so restart to step zero and try again.
break; break;
case 3: case 3:
if(data == 0x64) // IMU sync char 4 if(data == 0x64) // IMU sync char 4
step++; //ooh! The second data packet is correct, jump to the step 2 step++; //ooh! The second data packet is correct, jump to the step 2
else else
step=0; //Nop, is not correct so restart to step zero and try again. step=0; //Nop, is not correct so restart to step zero and try again.
break; break;
case 4: case 4:
payload_length = data; payload_length = data;
checksum(payload_length); checksum(payload_length);
step++; step++;
if (payload_length > 28){ if (payload_length > 28){
step = 0; //Bad data, so restart to step zero and try again. step = 0; //Bad data, so restart to step zero and try again.
payload_counter = 0; payload_counter = 0;
ck_a = 0; ck_a = 0;
ck_b = 0; ck_b = 0;
//payload_error_count++; //payload_error_count++;
} }
break; break;
case 5: case 5:
message_num = data; message_num = data;
checksum(data); checksum(data);
step++; step++;
break; break;
case 6: // Payload data read... case 6: // Payload data read...
// We stay in this state until we reach the payload_length // We stay in this state until we reach the payload_length
buffer[payload_counter] = data; buffer[payload_counter] = data;
checksum(data); checksum(data);
payload_counter++; payload_counter++;
if (payload_counter >= payload_length) { if (payload_counter >= payload_length) {
step++; step++;
} }
break; break;
case 7: case 7:
GPS_ck_a = data; // First checksum byte GPS_ck_a = data; // First checksum byte
step++; step++;
break; break;
case 8: case 8:
GPS_ck_b = data; // Second checksum byte GPS_ck_b = data; // Second checksum byte
// We end the IMU/GPS read... // We end the IMU/GPS read...
// Verify the received checksum with the generated checksum.. // Verify the received checksum with the generated checksum..
if((ck_a == GPS_ck_a) && (ck_b == GPS_ck_b)) { if((ck_a == GPS_ck_a) && (ck_b == GPS_ck_b)) {
if (message_num == 0x02) { if (message_num == 0x02) {
join_data(); join_data();
} else if (message_num == 0x03) { } else if (message_num == 0x03) {
GPS_join_data(); GPS_join_data();
} else if (message_num == 0x04) { } else if (message_num == 0x04) {
join_data_xplane(); join_data_xplane();
} else if (message_num == 0x0a) { } else if (message_num == 0x0a) {
//PERF_join_data(); //PERF_join_data();
} else { } else {
_error("Invalid message number = %d\n", (int)message_num); _error("Invalid message number = %d\n", (int)message_num);
} }
} else { } else {
_error("XXX Checksum error\n"); //bad checksum _error("XXX Checksum error\n"); //bad checksum
//imu_checksum_error_count++; //imu_checksum_error_count++;
} }
// Variable initialization // Variable initialization
step = 0; step = 0;
payload_counter = 0; payload_counter = 0;
ck_a = 0; ck_a = 0;
ck_b = 0; ck_b = 0;
break; break;
} }
}// End for... }// End for...
} }
} }
/**************************************************************** /****************************************************************
* *
****************************************************************/ ****************************************************************/
void AP_GPS_IMU::join_data(void) 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.. //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. //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 //Storing IMU roll
roll_sensor = *(int *)&buffer[0]; roll_sensor = *(int *)&buffer[0];
//Storing IMU pitch //Storing IMU pitch
pitch_sensor = *(int *)&buffer[2]; pitch_sensor = *(int *)&buffer[2];
//Storing IMU heading (yaw) //Storing IMU heading (yaw)
ground_course = *(int *)&buffer[4]; ground_course = *(int *)&buffer[4];
imu_ok = true; imu_ok = true;
} }
void AP_GPS_IMU::join_data_xplane() void AP_GPS_IMU::join_data_xplane()
{ {
//Storing IMU roll //Storing IMU roll
roll_sensor = *(int *)&buffer[0]; roll_sensor = *(int *)&buffer[0];
//Storing IMU pitch //Storing IMU pitch
pitch_sensor = *(int *)&buffer[2]; pitch_sensor = *(int *)&buffer[2];
//Storing IMU heading (yaw) //Storing IMU heading (yaw)
ground_course = *(int *)&buffer[4]; ground_course = *(int *)&buffer[4];
//Storing airspeed //Storing airspeed
airspeed = *(int *)&buffer[6]; airspeed = *(int *)&buffer[6];
imu_ok = true; imu_ok = true;
} }
void AP_GPS_IMU::GPS_join_data(void) void AP_GPS_IMU::GPS_join_data(void)
{ {
longitude = *(long *)&buffer[0]; // degrees * 10e7 longitude = *(long *)&buffer[0]; // degrees * 10e7
latitude = *(long *)&buffer[4]; latitude = *(long *)&buffer[4];
//Storing GPS Height above the sea level //Storing GPS Height above the sea level
altitude = (long)*(int *)&buffer[8] * 10; altitude = (long)*(int *)&buffer[8] * 10;
//Storing Speed //Storing Speed
speed_3d = ground_speed = (float)*(int *)&buffer[10]; 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 //We skip the gps ground course because we use yaw value from the IMU for ground course
time = *(long *)&buffer[14]; time = *(long *)&buffer[14];
imu_health = buffer[15]; imu_health = buffer[15];
new_data = true; new_data = true;
fix = true; fix = true;
} }
/**************************************************************** /****************************************************************
* *
****************************************************************/ ****************************************************************/
// checksum algorithm // checksum algorithm
void AP_GPS_IMU::checksum(byte data) void AP_GPS_IMU::checksum(byte data)
{ {
ck_a += data; ck_a += data;
ck_b += ck_a; ck_b += ck_a;
} }

88
libraries/AP_GPS/AP_GPS_IMU.h
View File

@ -1,44 +1,44 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
#ifndef AP_GPS_IMU_h #ifndef AP_GPS_IMU_h
#define AP_GPS_IMU_h #define AP_GPS_IMU_h
#include <GPS.h> #include <GPS.h>
#define MAXPAYLOAD 32 #define MAXPAYLOAD 32
class AP_GPS_IMU : public GPS class AP_GPS_IMU : public GPS
{ {
public: public:
// Methods // Methods
AP_GPS_IMU(Stream *s); AP_GPS_IMU(Stream *s);
void init(); void init();
void update(); void update();
// Properties // Properties
long roll_sensor; // how much we're turning in deg * 100 long roll_sensor; // how much we're turning in deg * 100
long pitch_sensor; // our angle of attack in deg * 100 long pitch_sensor; // our angle of attack in deg * 100
int airspeed; int airspeed;
float imu_health; float imu_health;
uint8_t imu_ok; uint8_t imu_ok;
private: private:
// Packet checksums // Packet checksums
uint8_t ck_a; uint8_t ck_a;
uint8_t ck_b; uint8_t ck_b;
uint8_t GPS_ck_a; uint8_t GPS_ck_a;
uint8_t GPS_ck_b; uint8_t GPS_ck_b;
uint8_t step; uint8_t step;
uint8_t msg_class; uint8_t msg_class;
uint8_t message_num; uint8_t message_num;
uint8_t payload_length; uint8_t payload_length;
uint8_t payload_counter; uint8_t payload_counter;
uint8_t buffer[MAXPAYLOAD]; uint8_t buffer[MAXPAYLOAD];
void join_data(); void join_data();
void join_data_xplane(); void join_data_xplane();
void GPS_join_data(); void GPS_join_data();
void checksum(unsigned char data); void checksum(unsigned char data);
}; };
#endif #endif

472
libraries/AP_GPS/AP_GPS_NMEA.cpp
View File

@ -1,236 +1,236 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/* /*
GPS_NMEA.cpp - Generic NMEA GPS library for Arduino GPS_NMEA.cpp - Generic NMEA GPS library for Arduino
Code by Jordi Muñoz and Jose Julio. DIYDrones.com 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 code works with boards based on ATMega168 / 328 and ATMega1280 (Serial port 1)
This library is free software; you can redistribute it and / or This library is free software; you can redistribute it and / or
modify it under the terms of the GNU Lesser General Public modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version. version 2.1 of the License, or (at your option) any later version.
GPS configuration : NMEA protocol GPS configuration : NMEA protocol
Baud rate : 38400 Baud rate : 38400
NMEA Sentences : NMEA Sentences :
$GPGGA : Global Positioning System fix Data $GPGGA : Global Positioning System fix Data
$GPVTG : Ttack and Ground Speed $GPVTG : Ttack and Ground Speed
Methods: Methods:
init() : GPS Initialization init() : GPS Initialization
update() : Call this funcion as often as you want to ensure you read the incomming gps data update() : Call this funcion as often as you want to ensure you read the incomming gps data
Properties: Properties:
latitude : latitude * 10000000 (long value) latitude : latitude * 10000000 (long value)
longitude : longitude * 10000000 (long value) longitude : longitude * 10000000 (long value)
altitude : altitude * 1000 (milimeters) (long value) altitude : altitude * 1000 (milimeters) (long value)
ground_speed : Speed (m / s) * 100 (long value) ground_speed : Speed (m / s) * 100 (long value)
ground_course : Course (degrees) * 100 (long value) ground_course : Course (degrees) * 100 (long value)
Type : 2 (This indicate that we are using the Generic NMEA library) Type : 2 (This indicate that we are using the Generic NMEA library)
new_data : 1 when a new data is received. new_data : 1 when a new data is received.
You need to write a 0 to new_data when you read the data You need to write a 0 to new_data when you read the data
fix : > = 1: GPS FIX, 0: No fix (normal logic) fix : > = 1: GPS FIX, 0: No fix (normal logic)
quality : 0 = No fix quality : 0 = No fix
1 = Bad (Num sats < 5) 1 = Bad (Num sats < 5)
2 = Poor 2 = Poor
3 = Medium 3 = Medium
4 = Good 4 = Good
NOTE : This code has been tested on a Locosys 20031 GPS receiver (MTK chipset) NOTE : This code has been tested on a Locosys 20031 GPS receiver (MTK chipset)
*/ */
#include "AP_GPS_NMEA.h" #include "AP_GPS_NMEA.h"
#include "WProgram.h" #include "WProgram.h"
// Constructors //////////////////////////////////////////////////////////////// // Constructors ////////////////////////////////////////////////////////////////
AP_GPS_NMEA::AP_GPS_NMEA(Stream *s) : GPS(s) AP_GPS_NMEA::AP_GPS_NMEA(Stream *s) : GPS(s)
{ {
} }
// Public Methods ////////////////////////////////////////////////////////////// // Public Methods //////////////////////////////////////////////////////////////
void void
AP_GPS_NMEA::init(void) AP_GPS_NMEA::init(void)
{ {
//Type = 2; //Type = 2;
} }
// This code don´t wait for data, only proccess the data available on serial port // This code don´t wait for data, only proccess the data available on serial port
// We can call this function on the main loop (50Hz loop) // We can call this function on the main loop (50Hz loop)
// If we get a complete packet this function call parse_nmea_gps() to parse and update the GPS info. // If we get a complete packet this function call parse_nmea_gps() to parse and update the GPS info.
void void
AP_GPS_NMEA::update(void) AP_GPS_NMEA::update(void)
{ {
char c; char c;
int numc; int numc;
int i; int i;
numc = _port->available(); numc = _port->available();
if (numc > 0){ if (numc > 0){
for (i = 0; i < numc; i++){ for (i = 0; i < numc; i++){
c = _port->read(); c = _port->read();
if (c == '$'){ // NMEA Start if (c == '$'){ // NMEA Start
bufferidx = 0; bufferidx = 0;
buffer[bufferidx++] = c; buffer[bufferidx++] = c;
GPS_checksum = 0; GPS_checksum = 0;
GPS_checksum_calc = true; GPS_checksum_calc = true;
continue; continue;
} }
if (c == '\r'){ // NMEA End if (c == '\r'){ // NMEA End
buffer[bufferidx++] = 0; buffer[bufferidx++] = 0;
parse_nmea_gps(); parse_nmea_gps();
} else { } else {
if (bufferidx < (GPS_BUFFERSIZE - 1)){ if (bufferidx < (GPS_BUFFERSIZE - 1)){
if (c == ' * ') if (c == ' * ')
GPS_checksum_calc = false; // Checksum calculation end GPS_checksum_calc = false; // Checksum calculation end
buffer[bufferidx++] = c; buffer[bufferidx++] = c;
if (GPS_checksum_calc){ if (GPS_checksum_calc){
GPS_checksum ^= c; // XOR GPS_checksum ^= c; // XOR
} }
} else { } else {
bufferidx = 0; // Buffer overflow : restart bufferidx = 0; // Buffer overflow : restart
} }
} }
} }
} }
} }
/**************************************************************** /****************************************************************
* *
* * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * **/ * * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * **/
// Private Methods ////////////////////////////////////////////////////////////// // Private Methods //////////////////////////////////////////////////////////////
void void
AP_GPS_NMEA::parse_nmea_gps(void) AP_GPS_NMEA::parse_nmea_gps(void)
{ {
uint8_t NMEA_check; uint8_t NMEA_check;
long aux_deg; long aux_deg;
long aux_min; long aux_min;
char *parseptr; char *parseptr;
if (strncmp(buffer,"$GPGGA",6)==0){ // Check if sentence begins with $GPGGA if (strncmp(buffer,"$GPGGA",6)==0){ // Check if sentence begins with $GPGGA
if (buffer[bufferidx-4]=='*'){ // Check for the "*" character if (buffer[bufferidx-4]=='*'){ // Check for the "*" character
NMEA_check = parseHex(buffer[bufferidx - 3]) * 16 + parseHex(buffer[bufferidx - 2]); // Read the checksums characters NMEA_check = parseHex(buffer[bufferidx - 3]) * 16 + parseHex(buffer[bufferidx - 2]); // Read the checksums characters
if (GPS_checksum == NMEA_check){ // Checksum validation if (GPS_checksum == NMEA_check){ // Checksum validation
//Serial.println("buffer"); //Serial.println("buffer");
new_data = 1; // New GPS Data new_data = 1; // New GPS Data
parseptr = strchr(buffer, ', ')+1; parseptr = strchr(buffer, ', ')+1;
//parseptr = strchr(parseptr, ',')+1; //parseptr = strchr(parseptr, ',')+1;
time = parsenumber(parseptr, 2); // GPS UTC time hhmmss.ss time = parsenumber(parseptr, 2); // GPS UTC time hhmmss.ss
parseptr = strchr(parseptr, ', ')+1; parseptr = strchr(parseptr, ', ')+1;
// //
aux_deg = parsedecimal(parseptr, 2); // degrees aux_deg = parsedecimal(parseptr, 2); // degrees
aux_min = parsenumber(parseptr + 2, 4); // minutes (sexagesimal) => Convert to decimal aux_min = parsenumber(parseptr + 2, 4); // minutes (sexagesimal) => Convert to decimal
latitude = 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; parseptr = strchr(parseptr, ', ')+1;
if ( * parseptr == 'S') if ( * parseptr == 'S')
latitude = -1 * latitude; // South latitudes are negative latitude = -1 * latitude; // South latitudes are negative
parseptr = strchr(parseptr, ', ')+1; parseptr = strchr(parseptr, ', ')+1;
// W longitudes are Negative // W longitudes are Negative
aux_deg = parsedecimal(parseptr, 3); // degrees aux_deg = parsedecimal(parseptr, 3); // degrees
aux_min = parsenumber(parseptr + 3, 4); // minutes (sexagesimal) aux_min = parsenumber(parseptr + 3, 4); // minutes (sexagesimal)
longitude = aux_deg * 10000000 + (aux_min * 50) / 3; // degrees + minutes / 0.6 ( * 10000000) longitude = aux_deg * 10000000 + (aux_min * 50) / 3; // degrees + minutes / 0.6 ( * 10000000)
//longitude = -1*longitude; // This Assumes that we are in W longitudes... //longitude = -1*longitude; // This Assumes that we are in W longitudes...
parseptr = strchr(parseptr, ', ')+1; parseptr = strchr(parseptr, ', ')+1;
if ( * parseptr == 'W') if ( * parseptr == 'W')
longitude = -1 * longitude; // West longitudes are negative longitude = -1 * longitude; // West longitudes are negative
parseptr = strchr(parseptr, ', ')+1; parseptr = strchr(parseptr, ', ')+1;
fix = parsedecimal(parseptr, 1); fix = parsedecimal(parseptr, 1);
parseptr = strchr(parseptr, ', ')+1; parseptr = strchr(parseptr, ', ')+1;
num_sats = parsedecimal(parseptr, 2); num_sats = parsedecimal(parseptr, 2);
parseptr = strchr(parseptr, ', ')+1; parseptr = strchr(parseptr, ', ')+1;
HDOP = parsenumber(parseptr, 1); // HDOP * 10 HDOP = parsenumber(parseptr, 1); // HDOP * 10
parseptr = strchr(parseptr, ', ')+1; parseptr = strchr(parseptr, ', ')+1;
altitude = parsenumber(parseptr, 1) * 100; // altitude in decimeters * 100 = milimeters altitude = parsenumber(parseptr, 1) * 100; // altitude in decimeters * 100 = milimeters
if (fix < 1) if (fix < 1)
quality = 0; // No FIX quality = 0; // No FIX
else if(num_sats < 5) else if(num_sats < 5)
quality = 1; // Bad (Num sats < 5) quality = 1; // Bad (Num sats < 5)
else if(HDOP > 30) else if(HDOP > 30)
quality = 2; // Poor (HDOP > 30) quality = 2; // Poor (HDOP > 30)
else if(HDOP > 25) else if(HDOP > 25)
quality = 3; // Medium (HDOP > 25) quality = 3; // Medium (HDOP > 25)
else else
quality = 4; // Good (HDOP < 25) quality = 4; // Good (HDOP < 25)
} else { } else {
_error("GPSERR: Checksum error!!\n"); _error("GPSERR: Checksum error!!\n");
} }
} }
} else if (strncmp(buffer,"$GPVTG",6)==0){ // Check if sentence begins with $GPVTG } else if (strncmp(buffer,"$GPVTG",6)==0){ // Check if sentence begins with $GPVTG
//Serial.println(buffer); //Serial.println(buffer);
if (buffer[bufferidx-4]=='*'){ // Check for the "*" character if (buffer[bufferidx-4]=='*'){ // Check for the "*" character
NMEA_check = parseHex(buffer[bufferidx - 3]) * 16 + parseHex(buffer[bufferidx - 2]); // Read the checksums characters NMEA_check = parseHex(buffer[bufferidx - 3]) * 16 + parseHex(buffer[bufferidx - 2]); // Read the checksums characters
if (GPS_checksum == NMEA_check){ // Checksum validation if (GPS_checksum == NMEA_check){ // Checksum validation
parseptr = strchr(buffer, ', ')+1; parseptr = strchr(buffer, ', ')+1;
ground_course = parsenumber(parseptr, 2); // Ground course in degrees * 100 ground_course = parsenumber(parseptr, 2); // Ground course in degrees * 100
parseptr = strchr(parseptr, ', ')+1; parseptr = strchr(parseptr, ', ')+1;
parseptr = strchr(parseptr, ', ')+1; parseptr = strchr(parseptr, ', ')+1;
parseptr = strchr(parseptr, ', ')+1; parseptr = strchr(parseptr, ', ')+1;
parseptr = strchr(parseptr, ', ')+1; parseptr = strchr(parseptr, ', ')+1;
parseptr = strchr(parseptr, ', ')+1; parseptr = strchr(parseptr, ', ')+1;
parseptr = strchr(parseptr, ', ')+1; parseptr = strchr(parseptr, ', ')+1;
ground_speed = parsenumber(parseptr, 2) * 10 / 36; // Convert Km / h to m / s ( * 100) ground_speed = parsenumber(parseptr, 2) * 10 / 36; // Convert Km / h to m / s ( * 100)
//GPS_line = true; //GPS_line = true;
} else { } else {
_error("GPSERR: Checksum error!!\n"); _error("GPSERR: Checksum error!!\n");
} }
} }
} else { } else {
bufferidx = 0; bufferidx = 0;
_error("GPSERR: Bad sentence!!\n"); _error("GPSERR: Bad sentence!!\n");
} }
} }
// Parse hexadecimal numbers // Parse hexadecimal numbers
uint8_t uint8_t
AP_GPS_NMEA::parseHex(char c) { AP_GPS_NMEA::parseHex(char c) {
if (c < '0') if (c < '0')
return (0); return (0);
if (c <= '9') if (c <= '9')
return (c - '0'); return (c - '0');
if (c < 'A') if (c < 'A')
return (0); return (0);
if (c <= 'F') if (c <= 'F')
return ((c - 'A')+10); return ((c - 'A')+10);
} }
// Decimal number parser // Decimal number parser
long long
AP_GPS_NMEA::parsedecimal(char *str, uint8_t num_car) { AP_GPS_NMEA::parsedecimal(char *str, uint8_t num_car) {
long d = 0; long d = 0;
uint8_t i; uint8_t i;
i = num_car; i = num_car;
while ((str[0] != 0) && (i > 0)) { while ((str[0] != 0) && (i > 0)) {
if ((str[0] > '9') || (str[0] < '0')) if ((str[0] > '9') || (str[0] < '0'))
return d; return d;
d *= 10; d *= 10;
d += str[0] - '0'; d += str[0] - '0';
str++; str++;
i--; i--;
} }
return d; return d;
} }
// Function to parse fixed point numbers (numdec=number of decimals) // Function to parse fixed point numbers (numdec=number of decimals)
long long
AP_GPS_NMEA::parsenumber(char *str, uint8_t numdec) { AP_GPS_NMEA::parsenumber(char *str, uint8_t numdec) {
long d = 0; long d = 0;
uint8_t ndec = 0; uint8_t ndec = 0;
while (str[0] != 0) { while (str[0] != 0) {
if (str[0] == '.'){ if (str[0] == '.'){
ndec = 1; ndec = 1;
} else { } else {
if ((str[0] > '9') || (str[0] < '0')) if ((str[0] > '9') || (str[0] < '0'))
return d; return d;
d *= 10; d *= 10;
d += str[0] - '0'; d += str[0] - '0';
if (ndec > 0) if (ndec > 0)
ndec++; ndec++;
if (ndec > numdec) // we reach the number of decimals... if (ndec > numdec) // we reach the number of decimals...
return d; return d;
} }
str++; str++;
} }
return d; return d;
} }

68
libraries/AP_GPS/AP_GPS_NMEA.h
View File

@ -1,34 +1,34 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
#ifndef AP_GPS_NMEA_h #ifndef AP_GPS_NMEA_h
#define AP_GPS_NMEA_h #define AP_GPS_NMEA_h
#include <GPS.h> #include <GPS.h>
#define GPS_BUFFERSIZE 120 #define GPS_BUFFERSIZE 120
class AP_GPS_NMEA : public GPS class AP_GPS_NMEA : public GPS
{ {
public: public:
// Methods // Methods
AP_GPS_NMEA(Stream *s); AP_GPS_NMEA(Stream *s);
void init(); void init();
void update(); void update();
// Properties // Properties
uint8_t quality; // GPS Signal quality uint8_t quality; // GPS Signal quality
int HDOP; // HDOP int HDOP; // HDOP
private: private:
// Internal variables // Internal variables
uint8_t GPS_checksum; uint8_t GPS_checksum;
uint8_t GPS_checksum_calc; uint8_t GPS_checksum_calc;
char buffer[GPS_BUFFERSIZE]; char buffer[GPS_BUFFERSIZE];
int bufferidx; int bufferidx;
void parse_nmea_gps(void); void parse_nmea_gps(void);
uint8_t parseHex(char c); uint8_t parseHex(char c);
long parsedecimal(char *str,uint8_t num_car); long parsedecimal(char *str,uint8_t num_car);
long parsenumber(char *str,uint8_t numdec); long parsenumber(char *str,uint8_t numdec);
}; };
#endif #endif

372
libraries/AP_GPS/AP_GPS_UBLOX.cpp
View File

@ -1,186 +1,186 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/* /*
GPS_UBLOX.cpp - Ublox GPS library for Arduino GPS_UBLOX.cpp - Ublox GPS library for Arduino
Code by Jordi Muñoz and Jose Julio. DIYDrones.com 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 code works with boards based on ATMega168/328 and ATMega1280 (Serial port 1)
This library is free software; you can redistribute it and / or This library is free software; you can redistribute it and / or
modify it under the terms of the GNU Lesser General Public modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version. version 2.1 of the License, or (at your option) any later version.
GPS configuration : Ublox protocol GPS configuration : Ublox protocol
Baud rate : 38400 Baud rate : 38400
Active messages : Active messages :
NAV - POSLLH Geodetic Position Solution, PAGE 66 of datasheet NAV - POSLLH Geodetic Position Solution, PAGE 66 of datasheet
NAV - VELNED Velocity Solution in NED, PAGE 71 of datasheet NAV - VELNED Velocity Solution in NED, PAGE 71 of datasheet
NAV - STATUS Receiver Navigation Status NAV - STATUS Receiver Navigation Status
or or
NAV - SOL Navigation Solution Information NAV - SOL Navigation Solution Information
Methods: Methods:
init() : GPS initialization init() : GPS initialization
update() : Call this funcion as often as you want to ensure you read the incomming gps data update() : Call this funcion as often as you want to ensure you read the incomming gps data
Properties: Properties:
Lattitude : Lattitude * 10000000 (long value) Lattitude : Lattitude * 10000000 (long value)
Longitude : Longitude * 10000000 (long value) Longitude : Longitude * 10000000 (long value)
altitude : altitude * 100 (meters) (long value) altitude : altitude * 100 (meters) (long value)
ground_speed : Speed (m/s) * 100 (long value) ground_speed : Speed (m/s) * 100 (long value)
ground_course : Course (degrees) * 100 (long value) ground_course : Course (degrees) * 100 (long value)
new_data : 1 when a new data is received. new_data : 1 when a new data is received.
You need to write a 0 to new_data when you read the data You need to write a 0 to new_data when you read the data
fix : 1: GPS FIX, 0: No fix (normal logic) fix : 1: GPS FIX, 0: No fix (normal logic)
*/ */
#include "AP_GPS_UBLOX.h" #include "AP_GPS_UBLOX.h"
#include "WProgram.h" #include "WProgram.h"
// Constructors //////////////////////////////////////////////////////////////// // Constructors ////////////////////////////////////////////////////////////////
AP_GPS_UBLOX::AP_GPS_UBLOX(Stream *s) : GPS(s) AP_GPS_UBLOX::AP_GPS_UBLOX(Stream *s) : GPS(s)
{ {
} }
// Public Methods //////////////////////////////////////////////////////////////////// // Public Methods ////////////////////////////////////////////////////////////////////
void AP_GPS_UBLOX::init(void) void AP_GPS_UBLOX::init(void)
{ {
} }
// optimization : This code doesn'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) // 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. // If we get a complete packet this function calls parse_gps() to parse and update the GPS info.
void AP_GPS_UBLOX::update(void) void AP_GPS_UBLOX::update(void)
{ {
byte data; byte data;
int numc; int numc;
numc = _port->available(); numc = _port->available();
if (numc > 0){ if (numc > 0){
for (int i = 0;i < numc;i++){ // Process bytes received for (int i = 0;i < numc;i++){ // Process bytes received
data = _port->read(); data = _port->read();
switch(step){ switch(step){
case 0: case 0:
if(data == 0xB5) if(data == 0xB5)
step++; step++;
break; break;
case 1: case 1:
if(data == 0x62) if(data == 0x62)
step++; step++;
else else
step = 0; step = 0;
break; break;
case 2: case 2:
msg_class = data; msg_class = data;
checksum(msg_class); checksum(msg_class);
step++; step++;
break; break;
case 3: case 3:
id = data; id = data;
checksum(id); checksum(id);
step++; step++;
break; break;
case 4: case 4:
payload_length_hi = data; payload_length_hi = data;
checksum(payload_length_hi); checksum(payload_length_hi);
step++; step++;
// We check if the payload lenght is valid... // We check if the payload lenght is valid...
if (payload_length_hi >= MAXPAYLOAD){ if (payload_length_hi >= MAXPAYLOAD){
_error("ERR:GPS_BAD_PAYLOAD_LENGTH!!\n"); _error("ERR:GPS_BAD_PAYLOAD_LENGTH!!\n");
step = 0; // Bad data, so restart to step zero and try again. step = 0; // Bad data, so restart to step zero and try again.
ck_a = 0; ck_a = 0;
ck_b = 0; ck_b = 0;
} }
break; break;
case 5: case 5:
payload_length_lo = data; payload_length_lo = data;
checksum(payload_length_lo); checksum(payload_length_lo);
step++; step++;
payload_counter = 0; payload_counter = 0;
break; break;
case 6: // Payload data read... case 6: // Payload data read...
if (payload_counter < payload_length_hi){ // We stay in this state until we reach the payload_length if (payload_counter < payload_length_hi){ // We stay in this state until we reach the payload_length
buffer[payload_counter] = data; buffer[payload_counter] = data;
checksum(data); checksum(data);
payload_counter++; payload_counter++;
if (payload_counter == payload_length_hi) if (payload_counter == payload_length_hi)
step++; step++;
} }
break; break;
case 7: case 7:
GPS_ck_a = data; // First checksum byte GPS_ck_a = data; // First checksum byte
step++; step++;
break; break;
case 8: case 8:
GPS_ck_b = data; // Second checksum byte GPS_ck_b = data; // Second checksum byte
// We end the GPS read... // We end the GPS read...
if((ck_a == GPS_ck_a) && (ck_b == GPS_ck_b)){ // Verify the received checksum with the generated checksum.. 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 parse_gps(); // Parse the new GPS packet
}else{ }else{
_error("ERR:GPS_CHK!!\n"); _error("ERR:GPS_CHK!!\n");
} }
// Variable initialization // Variable initialization
step = 0; step = 0;
ck_a = 0; ck_a = 0;
ck_b = 0; ck_b = 0;
break; break;
} }
} // End for... } // End for...
} }
} }
// Private Methods ////////////////////////////////////////////////////////////// // Private Methods //////////////////////////////////////////////////////////////
void void
AP_GPS_UBLOX::parse_gps(void) AP_GPS_UBLOX::parse_gps(void)
{ {
//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.. //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. // 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){ if(msg_class == 0x01){
switch(id) {//Checking the UBX ID switch(id) {//Checking the UBX ID
case 0x02: // ID NAV - POSLLH case 0x02: // ID NAV - POSLLH
time = *(long *)&buffer[0]; // ms Time of week time = *(long *)&buffer[0]; // ms Time of week
longitude = *(long *)&buffer[4]; // lon * 10,000,000 longitude = *(long *)&buffer[4]; // lon * 10,000,000
latitude = *(long *)&buffer[8]; // lat * 10,000,000 latitude = *(long *)&buffer[8]; // lat * 10,000,000
//altitude = *(long *)&buffer[12]; // elipsoid heigth mm //altitude = *(long *)&buffer[12]; // elipsoid heigth mm
altitude = *(long *)&buffer[16] / 10; // MSL heigth mm altitude = *(long *)&buffer[16] / 10; // MSL heigth mm
/* /*
hacc = (float)*(long *)&buffer[20]; hacc = (float)*(long *)&buffer[20];
vacc = (float)*(long *)&buffer[24]; vacc = (float)*(long *)&buffer[24];
*/ */
new_data = true; new_data = true;
break; break;
case 0x03: //ID NAV - STATUS case 0x03: //ID NAV - STATUS
fix = ((buffer[4] >= 0x03) && (buffer[5] & 0x01)); fix = ((buffer[4] >= 0x03) && (buffer[5] & 0x01));
break; break;
case 0x06: //ID NAV - SOL case 0x06: //ID NAV - SOL
fix = ((buffer[10] >= 0x03) && (buffer[11] & 0x01)); fix = ((buffer[10] >= 0x03) && (buffer[11] & 0x01));
num_sats = buffer[47]; // Number of sats... num_sats = buffer[47]; // Number of sats...
break; break;
case 0x12: // ID NAV - VELNED case 0x12: // ID NAV - VELNED
speed_3d = *(long *)&buffer[16]; // cm / s speed_3d = *(long *)&buffer[16]; // cm / s
ground_speed = *(long *)&buffer[20]; // Ground speed 2D 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 ground_course = *(long *)&buffer[24] / 1000; // Heading 2D deg * 100000 rescaled to deg * 100
break; break;
} }
} }
} }
// Ublox checksum algorithm // Ublox checksum algorithm
void AP_GPS_UBLOX::checksum(byte data) void AP_GPS_UBLOX::checksum(byte data)
{ {
ck_a += data; ck_a += data;
ck_b += ck_a; ck_b += ck_a;
} }

72
libraries/AP_GPS/AP_GPS_UBLOX.h
View File

@ -1,36 +1,36 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
#ifndef AP_GPS_UBLOX_h #ifndef AP_GPS_UBLOX_h
#define AP_GPS_UBLOX_h #define AP_GPS_UBLOX_h
#include <GPS.h> #include <GPS.h>
#define MAXPAYLOAD 60 #define MAXPAYLOAD 60
class AP_GPS_UBLOX : public GPS class AP_GPS_UBLOX : public GPS
{ {
public: public:
// Methods // Methods
AP_GPS_UBLOX(Stream *s); AP_GPS_UBLOX(Stream *s);
void init(); void init();
void update(); void update();
private: private:
// Internal variables // Internal variables
uint8_t ck_a; // Packet checksum uint8_t ck_a; // Packet checksum
uint8_t ck_b; uint8_t ck_b;
uint8_t GPS_ck_a; uint8_t GPS_ck_a;
uint8_t GPS_ck_b; uint8_t GPS_ck_b;
uint8_t step; uint8_t step;
uint8_t msg_class; uint8_t msg_class;
uint8_t id; uint8_t id;
uint8_t payload_length_hi; uint8_t payload_length_hi;
uint8_t payload_length_lo; uint8_t payload_length_lo;
uint8_t payload_counter; uint8_t payload_counter;
uint8_t buffer[MAXPAYLOAD]; uint8_t buffer[MAXPAYLOAD];
long ecefVZ; long ecefVZ;
void parse_gps(); void parse_gps();
void checksum(unsigned char data); void checksum(unsigned char data);
long join_4_bytes(unsigned char Buffer[]); long join_4_bytes(unsigned char Buffer[]);
}; };
#endif #endif