zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Push server-native line endings. 

git-svn-id: https://arducopter.googlecode.com/svn/trunk@418 f9c3cf11-9bcb-44bc-f272-b75c42450872
master
DrZiplok@gmail.com 15 years ago
parent
add89239f3
commit
7e345e027c
8 changed files with 981 additions and 981 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 384
      libraries/AP_GPS/AP_GPS_406.cpp
  2. 58
      libraries/AP_GPS/AP_GPS_406.h
  3. 448
      libraries/AP_GPS/AP_GPS_IMU.cpp
  4. 88
      libraries/AP_GPS/AP_GPS_IMU.h
  5. 472
      libraries/AP_GPS/AP_GPS_NMEA.cpp
  6. 68
      libraries/AP_GPS/AP_GPS_NMEA.h
  7. 372
      libraries/AP_GPS/AP_GPS_UBLOX.cpp
  8. 72
      libraries/AP_GPS/AP_GPS_UBLOX.h

384
libraries/AP_GPS/AP_GPS_406.cpp
Unescape View File

@ -1,192 +1,192 @@ @@ -1,192 +1,192 @@
// -*- 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
This code works with boards based on ATMega168/328 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 : 406 protocol
Baud rate : 57600
Methods:
init() : GPS initialization
update() : Call this funcion as often as you want to ensure you read the incomming gps data
Properties:
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)
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 FIX, 0: No fix (normal logic)
*/
#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(Stream *s) : GPS(s)
{
}
// Public Methods ////////////////////////////////////////////////////////////////////
void AP_GPS_406::init(void)
{
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 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)
{
byte data;
int numc;
numc = _port->available();
if (numc > 0){
for (int i = 0; i < numc; i++){ // Process bytes received
data = _port->read();
switch(step){
case 0:
if(data == 0xA0)
step++;
break;
case 1:
if(data == 0xA2)
step++;
else
step = 0;
break;
/* case 2:
if(data == 0xA2)
step++;
else
step = 0;
break;
*/
case 2:
if(data == 0x00)
step++;
else
step = 0;
break;
case 3:
if(data == 0x5B)
step++;
else
step = 0;
break;
case 4:
if(data == 0x29){
payload_counter = 0;
step++;
}else
step = 0;
break;
case 5: // Payload data read...
if (payload_counter <= PAYLOAD_LENGTH){ // We stay in this state until we reach the payload_length
buffer[payload_counter] = data;
payload_counter++;
if (payload_counter == PAYLOAD_LENGTH){
parse_gps();
step = 0;
}
}
break;
}
} // End for...
}
}
// Private Methods //////////////////////////////////////////////////////////////
void
AP_GPS_406::parse_gps(void)
{
uint8_t j;
fix = buffer[1] > 0;
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
if (ground_speed >= 50) { // Only updates data if we are really moving...
ground_course = (unsigned int)_swapi(&buffer[41]); // meters / second * 100
}
//climb_rate = _swapi(&buffer[45]); // meters / second * 100
if (latitude == 0) {
new_data = false;
fix = false;
} else {
new_data = true;
}
}
void
AP_GPS_406::configure_gps(void)
{
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_checksum[] = {0xA8, 0xAA, 0xAD, 0xAF, 0xC1};
const uint8_t gps_ender[] = {0xB0, 0xB3};
for(int z = 0; z < 2; z++){
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_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
}
}
}
void
AP_GPS_406::change_to_sirf_protocol(void)
{
FastSerial *fs = (FastSerial *)_port; // this is a bit grody...
fs->begin(4800); // First try at 4800bps
delay(300);
_port->write(buffer, 28); // Sending special bytes declared at the beginning
delay(300);
fs->begin(9600); // Then try at 9600bps
delay(300);
_port->write(buffer, 28);
delay(300);
fs->begin(BAUD_RATE); // Finally try at the configured bit rate (57600?)
delay(300);
_port->write(buffer, 28);
}
// -*- 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
This code works with boards based on ATMega168/328 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 : 406 protocol
Baud rate : 57600
Methods:
init() : GPS initialization
update() : Call this funcion as often as you want to ensure you read the incomming gps data
Properties:
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)
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 FIX, 0: No fix (normal logic)
*/
#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(Stream *s) : GPS(s)
{
}
// Public Methods ////////////////////////////////////////////////////////////////////
void AP_GPS_406::init(void)
{
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 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)
{
byte data;
int numc;
numc = _port->available();
if (numc > 0){
for (int i = 0; i < numc; i++){ // Process bytes received
data = _port->read();
switch(step){
case 0:
if(data == 0xA0)
step++;
break;
case 1:
if(data == 0xA2)
step++;
else
step = 0;
break;
/* case 2:
if(data == 0xA2)
step++;
else
step = 0;
break;
*/
case 2:
if(data == 0x00)
step++;
else
step = 0;
break;
case 3:
if(data == 0x5B)
step++;
else
step = 0;
break;
case 4:
if(data == 0x29){
payload_counter = 0;
step++;
}else
step = 0;
break;
case 5: // Payload data read...
if (payload_counter <= PAYLOAD_LENGTH){ // We stay in this state until we reach the payload_length
buffer[payload_counter] = data;
payload_counter++;
if (payload_counter == PAYLOAD_LENGTH){
parse_gps();
step = 0;
}
}
break;
}
} // End for...
}
}
// Private Methods //////////////////////////////////////////////////////////////
void
AP_GPS_406::parse_gps(void)
{
uint8_t j;
fix = buffer[1] > 0;
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
if (ground_speed >= 50) { // Only updates data if we are really moving...
ground_course = (unsigned int)_swapi(&buffer[41]); // meters / second * 100
}
//climb_rate = _swapi(&buffer[45]); // meters / second * 100
if (latitude == 0) {
new_data = false;
fix = false;
} else {
new_data = true;
}
}
void
AP_GPS_406::configure_gps(void)
{
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_checksum[] = {0xA8, 0xAA, 0xAD, 0xAF, 0xC1};
const uint8_t gps_ender[] = {0xB0, 0xB3};
for(int z = 0; z < 2; z++){
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_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
}
}
}
void
AP_GPS_406::change_to_sirf_protocol(void)
{
FastSerial *fs = (FastSerial *)_port; // this is a bit grody...
fs->begin(4800); // First try at 4800bps
delay(300);
_port->write(buffer, 28); // Sending special bytes declared at the beginning
delay(300);
fs->begin(9600); // Then try at 9600bps
delay(300);
_port->write(buffer, 28);
delay(300);
fs->begin(BAUD_RATE); // Finally try at the configured bit rate (57600?)
delay(300);
_port->write(buffer, 28);
}

58
libraries/AP_GPS/AP_GPS_406.h
Unescape View File

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

448
libraries/AP_GPS/AP_GPS_IMU.cpp
Unescape View File

@ -1,224 +1,224 @@ @@ -1,224 +1,224 @@
// -*- 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::update(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 = *(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;
}
// -*- 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::update(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 = *(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;
}

88
libraries/AP_GPS/AP_GPS_IMU.h
Unescape View File

@ -1,44 +1,44 @@ @@ -1,44 +1,44 @@
// -*- 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 update();
// 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
// -*- 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 update();
// 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

472
libraries/AP_GPS/AP_GPS_NMEA.cpp
Unescape View File

@ -1,236 +1,236 @@ @@ -1,236 +1,236 @@
// -*- 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
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 : NMEA protocol
Baud rate : 38400
NMEA Sentences :
$GPGGA : Global Positioning System fix Data
$GPVTG : Ttack and Ground Speed
Methods:
init() : GPS Initialization
update() : Call this funcion as often as you want to ensure you read the incomming gps data
Properties:
latitude : latitude * 10000000 (long value)
longitude : longitude * 10000000 (long value)
altitude : altitude * 1000 (milimeters) (long value)
ground_speed : Speed (m / s) * 100 (long value)
ground_course : Course (degrees) * 100 (long value)
Type : 2 (This indicate that we are using the Generic NMEA library)
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 FIX, 0: No fix (normal logic)
quality : 0 = No fix
1 = Bad (Num sats < 5)
2 = Poor
3 = Medium
4 = Good
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)
{
}
// Public Methods //////////////////////////////////////////////////////////////
void
AP_GPS_NMEA::init(void)
{
//Type = 2;
}
// 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
AP_GPS_NMEA::update(void)
{
char c;
int numc;
int i;
numc = _port->available();
if (numc > 0){
for (i = 0; i < numc; i++){
c = _port->read();
if (c == '$'){ // NMEA Start
bufferidx = 0;
buffer[bufferidx++] = c;
GPS_checksum = 0;
GPS_checksum_calc = true;
continue;
}
if (c == '\r'){ // NMEA End
buffer[bufferidx++] = 0;
parse_nmea_gps();
} else {
if (bufferidx < (GPS_BUFFERSIZE - 1)){
if (c == ' * ')
GPS_checksum_calc = false; // Checksum calculation end
buffer[bufferidx++] = c;
if (GPS_checksum_calc){
GPS_checksum ^= c; // XOR
}
} else {
bufferidx = 0; // Buffer overflow : restart
}
}
}
}
}
/****************************************************************
*
* * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * **/
// Private Methods //////////////////////////////////////////////////////////////
void
AP_GPS_NMEA::parse_nmea_gps(void)
{
uint8_t NMEA_check;
long aux_deg;
long aux_min;
char *parseptr;
if (strncmp(buffer,"$GPGGA",6)==0){ // Check if sentence begins with $GPGGA
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
//Serial.println("buffer");
new_data = 1; // New GPS Data
parseptr = strchr(buffer, ', ')+1;
//parseptr = strchr(parseptr, ',')+1;
time = parsenumber(parseptr, 2); // GPS UTC time hhmmss.ss
parseptr = strchr(parseptr, ', ')+1;
//
aux_deg = parsedecimal(parseptr, 2); // degrees
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)
parseptr = strchr(parseptr, ', ')+1;
if ( * parseptr == 'S')
latitude = -1 * latitude; // South latitudes are negative
parseptr = strchr(parseptr, ', ')+1;
// W longitudes are Negative
aux_deg = parsedecimal(parseptr, 3); // degrees
aux_min = parsenumber(parseptr + 3, 4); // minutes (sexagesimal)
longitude = aux_deg * 10000000 + (aux_min * 50) / 3; // degrees + minutes / 0.6 ( * 10000000)
//longitude = -1*longitude; // This Assumes that we are in W longitudes...
parseptr = strchr(parseptr, ', ')+1;
if ( * parseptr == 'W')
longitude = -1 * longitude; // West longitudes are negative
parseptr = strchr(parseptr, ', ')+1;
fix = parsedecimal(parseptr, 1);
parseptr = strchr(parseptr, ', ')+1;
num_sats = parsedecimal(parseptr, 2);
parseptr = strchr(parseptr, ', ')+1;
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)
quality = 2; // Poor (HDOP > 30)
else if(HDOP > 25)
quality = 3; // Medium (HDOP > 25)
else
quality = 4; // Good (HDOP < 25)
} else {
_error("GPSERR: Checksum error!!\n");
}
}
} else if (strncmp(buffer,"$GPVTG",6)==0){ // Check if sentence begins with $GPVTG
//Serial.println(buffer);
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
parseptr = strchr(buffer, ', ')+1;
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;
ground_speed = parsenumber(parseptr, 2) * 10 / 36; // Convert Km / h to m / s ( * 100)
//GPS_line = true;
} else {
_error("GPSERR: Checksum error!!\n");
}
}
} else {
bufferidx = 0;
_error("GPSERR: Bad sentence!!\n");
}
}
// Parse hexadecimal numbers
uint8_t
AP_GPS_NMEA::parseHex(char c) {
if (c < '0')
return (0);
if (c <= '9')
return (c - '0');
if (c < 'A')
return (0);
if (c <= 'F')
return ((c - 'A')+10);
}
// Decimal number parser
long
AP_GPS_NMEA::parsedecimal(char *str, uint8_t num_car) {
long d = 0;
uint8_t i;
i = num_car;
while ((str[0] != 0) && (i > 0)) {
if ((str[0] > '9') || (str[0] < '0'))
return d;
d *= 10;
d += str[0] - '0';
str++;
i--;
}
return d;
}
// Function to parse fixed point numbers (numdec=number of decimals)
long
AP_GPS_NMEA::parsenumber(char *str, uint8_t numdec) {
long d = 0;
uint8_t ndec = 0;
while (str[0] != 0) {
if (str[0] == '.'){
ndec = 1;
} else {
if ((str[0] > '9') || (str[0] < '0'))
return d;
d *= 10;
d += str[0] - '0';
if (ndec > 0)
ndec++;
if (ndec > numdec) // we reach the number of decimals...
return d;
}
str++;
}
return d;
}
// -*- 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
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 : NMEA protocol
Baud rate : 38400
NMEA Sentences :
$GPGGA : Global Positioning System fix Data
$GPVTG : Ttack and Ground Speed
Methods:
init() : GPS Initialization
update() : Call this funcion as often as you want to ensure you read the incomming gps data
Properties:
latitude : latitude * 10000000 (long value)
longitude : longitude * 10000000 (long value)
altitude : altitude * 1000 (milimeters) (long value)
ground_speed : Speed (m / s) * 100 (long value)
ground_course : Course (degrees) * 100 (long value)
Type : 2 (This indicate that we are using the Generic NMEA library)
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 FIX, 0: No fix (normal logic)
quality : 0 = No fix
1 = Bad (Num sats < 5)
2 = Poor
3 = Medium
4 = Good
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)
{
}
// Public Methods //////////////////////////////////////////////////////////////
void
AP_GPS_NMEA::init(void)
{
//Type = 2;
}
// 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
AP_GPS_NMEA::update(void)
{
char c;
int numc;
int i;
numc = _port->available();
if (numc > 0){
for (i = 0; i < numc; i++){
c = _port->read();
if (c == '$'){ // NMEA Start
bufferidx = 0;
buffer[bufferidx++] = c;
GPS_checksum = 0;
GPS_checksum_calc = true;
continue;
}
if (c == '\r'){ // NMEA End
buffer[bufferidx++] = 0;
parse_nmea_gps();
} else {
if (bufferidx < (GPS_BUFFERSIZE - 1)){
if (c == ' * ')
GPS_checksum_calc = false; // Checksum calculation end
buffer[bufferidx++] = c;
if (GPS_checksum_calc){
GPS_checksum ^= c; // XOR
}
} else {
bufferidx = 0; // Buffer overflow : restart
}
}
}
}
}
/****************************************************************
*
* * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * ** * **/
// Private Methods //////////////////////////////////////////////////////////////
void
AP_GPS_NMEA::parse_nmea_gps(void)
{
uint8_t NMEA_check;
long aux_deg;
long aux_min;
char *parseptr;
if (strncmp(buffer,"$GPGGA",6)==0){ // Check if sentence begins with $GPGGA
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
//Serial.println("buffer");
new_data = 1; // New GPS Data
parseptr = strchr(buffer, ', ')+1;
//parseptr = strchr(parseptr, ',')+1;
time = parsenumber(parseptr, 2); // GPS UTC time hhmmss.ss
parseptr = strchr(parseptr, ', ')+1;
//
aux_deg = parsedecimal(parseptr, 2); // degrees
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)
parseptr = strchr(parseptr, ', ')+1;
if ( * parseptr == 'S')
latitude = -1 * latitude; // South latitudes are negative
parseptr = strchr(parseptr, ', ')+1;
// W longitudes are Negative
aux_deg = parsedecimal(parseptr, 3); // degrees
aux_min = parsenumber(parseptr + 3, 4); // minutes (sexagesimal)
longitude = aux_deg * 10000000 + (aux_min * 50) / 3; // degrees + minutes / 0.6 ( * 10000000)
//longitude = -1*longitude; // This Assumes that we are in W longitudes...
parseptr = strchr(parseptr, ', ')+1;
if ( * parseptr == 'W')
longitude = -1 * longitude; // West longitudes are negative
parseptr = strchr(parseptr, ', ')+1;
fix = parsedecimal(parseptr, 1);
parseptr = strchr(parseptr, ', ')+1;
num_sats = parsedecimal(parseptr, 2);
parseptr = strchr(parseptr, ', ')+1;
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)
quality = 2; // Poor (HDOP > 30)
else if(HDOP > 25)
quality = 3; // Medium (HDOP > 25)
else
quality = 4; // Good (HDOP < 25)
} else {
_error("GPSERR: Checksum error!!\n");
}
}
} else if (strncmp(buffer,"$GPVTG",6)==0){ // Check if sentence begins with $GPVTG
//Serial.println(buffer);
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
parseptr = strchr(buffer, ', ')+1;
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;
ground_speed = parsenumber(parseptr, 2) * 10 / 36; // Convert Km / h to m / s ( * 100)
//GPS_line = true;
} else {
_error("GPSERR: Checksum error!!\n");
}
}
} else {
bufferidx = 0;
_error("GPSERR: Bad sentence!!\n");
}
}
// Parse hexadecimal numbers
uint8_t
AP_GPS_NMEA::parseHex(char c) {
if (c < '0')
return (0);
if (c <= '9')
return (c - '0');
if (c < 'A')
return (0);
if (c <= 'F')
return ((c - 'A')+10);
}
// Decimal number parser
long
AP_GPS_NMEA::parsedecimal(char *str, uint8_t num_car) {
long d = 0;
uint8_t i;
i = num_car;
while ((str[0] != 0) && (i > 0)) {
if ((str[0] > '9') || (str[0] < '0'))
return d;
d *= 10;
d += str[0] - '0';
str++;
i--;
}
return d;
}
// Function to parse fixed point numbers (numdec=number of decimals)
long
AP_GPS_NMEA::parsenumber(char *str, uint8_t numdec) {
long d = 0;
uint8_t ndec = 0;
while (str[0] != 0) {
if (str[0] == '.'){
ndec = 1;
} else {
if ((str[0] > '9') || (str[0] < '0'))
return d;
d *= 10;
d += str[0] - '0';
if (ndec > 0)
ndec++;
if (ndec > numdec) // we reach the number of decimals...
return d;
}
str++;
}
return d;
}

68
libraries/AP_GPS/AP_GPS_NMEA.h
Unescape View File

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

372
libraries/AP_GPS/AP_GPS_UBLOX.cpp
Unescape View File

@ -1,186 +1,186 @@ @@ -1,186 +1,186 @@
// -*- 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
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 : Ublox protocol
Baud rate : 38400
Active messages :
NAV - POSLLH Geodetic Position Solution, PAGE 66 of datasheet
NAV - VELNED Velocity Solution in NED, PAGE 71 of datasheet
NAV - STATUS Receiver Navigation Status
or
NAV - SOL Navigation Solution Information
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 FIX, 0: No fix (normal logic)
*/
#include "AP_GPS_UBLOX.h"
#include "WProgram.h"
// Constructors ////////////////////////////////////////////////////////////////
AP_GPS_UBLOX::AP_GPS_UBLOX(Stream *s) : GPS(s)
{
}
// Public Methods ////////////////////////////////////////////////////////////////////
void AP_GPS_UBLOX::init(void)
{
}
// 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)
{
byte data;
int numc;
numc = _port->available();
if (numc > 0){
for (int i = 0;i < numc;i++){ // Process bytes received
data = _port->read();
switch(step){
case 0:
if(data == 0xB5)
step++;
break;
case 1:
if(data == 0x62)
step++;
else
step = 0;
break;
case 2:
msg_class = data;
checksum(msg_class);
step++;
break;
case 3:
id = data;
checksum(id);
step++;
break;
case 4:
payload_length_hi = data;
checksum(payload_length_hi);
step++;
// We check if the payload lenght is valid...
if (payload_length_hi >= MAXPAYLOAD){
_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;
}
break;
case 5:
payload_length_lo = data;
checksum(payload_length_lo);
step++;
payload_counter = 0;
break;
case 6: // Payload data read...
if (payload_counter < payload_length_hi){ // We stay in this state until we reach the payload_length
buffer[payload_counter] = data;
checksum(data);
payload_counter++;
if (payload_counter == payload_length_hi)
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 GPS read...
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{
_error("ERR:GPS_CHK!!\n");
}
// Variable initialization
step = 0;
ck_a = 0;
ck_b = 0;
break;
}
} // End for...
}
}
// Private Methods //////////////////////////////////////////////////////////////
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..
// 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
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)*(long *)&buffer[20];
vacc = (float)*(long *)&buffer[24];
*/
new_data = true;
break;
case 0x03: //ID NAV - STATUS
fix = ((buffer[4] >= 0x03) && (buffer[5] & 0x01));
break;
case 0x06: //ID NAV - SOL
fix = ((buffer[10] >= 0x03) && (buffer[11] & 0x01));
num_sats = buffer[47]; // Number of sats...
break;
case 0x12: // ID NAV - VELNED
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;
}
}
}
// Ublox checksum algorithm
void AP_GPS_UBLOX::checksum(byte data)
{
ck_a += data;
ck_b += ck_a;
}
// -*- 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
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 : Ublox protocol
Baud rate : 38400
Active messages :
NAV - POSLLH Geodetic Position Solution, PAGE 66 of datasheet
NAV - VELNED Velocity Solution in NED, PAGE 71 of datasheet
NAV - STATUS Receiver Navigation Status
or
NAV - SOL Navigation Solution Information
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 FIX, 0: No fix (normal logic)
*/
#include "AP_GPS_UBLOX.h"
#include "WProgram.h"
// Constructors ////////////////////////////////////////////////////////////////
AP_GPS_UBLOX::AP_GPS_UBLOX(Stream *s) : GPS(s)
{
}
// Public Methods ////////////////////////////////////////////////////////////////////
void AP_GPS_UBLOX::init(void)
{
}
// 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)
{
byte data;
int numc;
numc = _port->available();
if (numc > 0){
for (int i = 0;i < numc;i++){ // Process bytes received
data = _port->read();
switch(step){
case 0:
if(data == 0xB5)
step++;
break;
case 1:
if(data == 0x62)
step++;
else
step = 0;
break;
case 2:
msg_class = data;
checksum(msg_class);
step++;
break;
case 3:
id = data;
checksum(id);
step++;
break;
case 4:
payload_length_hi = data;
checksum(payload_length_hi);
step++;
// We check if the payload lenght is valid...
if (payload_length_hi >= MAXPAYLOAD){
_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;
}
break;
case 5:
payload_length_lo = data;
checksum(payload_length_lo);
step++;
payload_counter = 0;
break;
case 6: // Payload data read...
if (payload_counter < payload_length_hi){ // We stay in this state until we reach the payload_length
buffer[payload_counter] = data;
checksum(data);
payload_counter++;
if (payload_counter == payload_length_hi)
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 GPS read...
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{
_error("ERR:GPS_CHK!!\n");
}
// Variable initialization
step = 0;
ck_a = 0;
ck_b = 0;
break;
}
} // End for...
}
}
// Private Methods //////////////////////////////////////////////////////////////
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..
// 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
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)*(long *)&buffer[20];
vacc = (float)*(long *)&buffer[24];
*/
new_data = true;
break;
case 0x03: //ID NAV - STATUS
fix = ((buffer[4] >= 0x03) && (buffer[5] & 0x01));
break;
case 0x06: //ID NAV - SOL
fix = ((buffer[10] >= 0x03) && (buffer[11] & 0x01));
num_sats = buffer[47]; // Number of sats...
break;
case 0x12: // ID NAV - VELNED
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;
}
}
}
// Ublox checksum algorithm
void AP_GPS_UBLOX::checksum(byte data)
{
ck_a += data;
ck_b += ck_a;
}

72
libraries/AP_GPS/AP_GPS_UBLOX.h
Unescape View File

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

Write Preview
Loading…
Cancel
Save