mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
Push server-native line endings.
git-svn-id: https://arducopter.googlecode.com/svn/trunk@418 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
DrZiplok@gmail.com
parent
add89239f3
commit
7e345e027c
@ -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);
|
||||
}
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
@ -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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user