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

New Libraries 

git-svn-id: https://arducopter.googlecode.com/svn/trunk@286 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2010-08-24 04:13:27 +00:00
parent 55621d583d
commit 2c6e096a3b
16 changed files with 1712 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
libraries/AP_GPS/AP_GPS.h Normal file
View File

@ -0,0 +1,39 @@
#ifndef AP_GPS_h
#define AP_GPS_h
#include <inttypes.h>
#include "WProgram.h"
class AP_GPS
{
public:
// Methods
virtual void init();
virtual void update();
// Properties
long time; //GPS Millisecond Time of Week
long lattitude; // Geographic coordinates
long longitude;
long altitude;
long ground_speed;
long ground_course;
long speed_3d;
uint8_t num_sats; // Number of visible satelites
uint8_t fix; // 1:GPS FIX 0:No FIX (normal logic)
uint8_t new_data; // 1:New GPS Data
uint8_t print_errors; // 1: To Print GPS Errors (for debug)
long GPS_timer;
union long_union {
int32_t dword;
uint8_t byte[4];
} longUnion;
union int_union {
int16_t word;
uint8_t byte[2];
} intUnion;
};
#endif

284
libraries/AP_GPS/AP_GPS_IMU.cpp Executable file
View File

@ -0,0 +1,284 @@
/*
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"
// Constructors ////////////////////////////////////////////////////////////////
AP_GPS_IMU::AP_GPS_IMU()
{
}
// Public Methods //////////////////////////////////////////////////////////////
void AP_GPS_IMU::init(void)
{
ck_a = 0;
ck_b = 0;
step = 0;
new_data = 0;
fix = 0;
print_errors = 0;
// initialize serial port
#if defined(__AVR_ATmega1280__)
Serial1.begin(38400); // Serial port 1 on ATMega1280
#else
Serial.begin(38400);
#endif
}
// 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;
#if defined(__AVR_ATmega1280__) // If AtMega1280 then Serial port 1...
numc = Serial.available();
#else
numc = Serial.available();
#endif
if (numc > 0){
for (int i=0;i<numc;i++){ // Process bytes received
#if defined(__AVR_ATmega1280__)
data = Serial.read();
#else
data = Serial.read();
#endif
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 {
Serial.print("Invalid message number = ");
Serial.println(message_num, DEC);
}
} else {
Serial.println("XXX Checksum error"); //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)
{
int j = 0;
//Verifing if we are in class 1, you can change this "IF" for a "Switch" in case you want to use other IMU classes..
//In this case all the message im using are in class 1, to know more about classes check PAGE 60 of DataSheet.
//Storing IMU roll
intUnion.byte[0] = buffer[j++];
intUnion.byte[1] = buffer[j++];
roll_sensor = intUnion.word;
//Storing IMU pitch
intUnion.byte[0] = buffer[j++];
intUnion.byte[1] = buffer[j++];
pitch_sensor = intUnion.word;
//Storing IMU heading (yaw)
intUnion.byte[0] = buffer[j++];
intUnion.byte[1] = buffer[j++];
ground_course = intUnion.word;
imu_ok = true;
}
void AP_GPS_IMU::join_data_xplane()
{
int j = 0;
//Storing IMU roll
intUnion.byte[0] = buffer[j++];
intUnion.byte[1] = buffer[j++];
roll_sensor = intUnion.word;
//Storing IMU pitch
intUnion.byte[0] = buffer[j++];
intUnion.byte[1] = buffer[j++];
pitch_sensor = intUnion.word;
//Storing IMU heading (yaw)
intUnion.byte[0] = buffer[j++];
intUnion.byte[1] = buffer[j++];
ground_course = (unsigned int)intUnion.word;
//Storing airspeed
intUnion.byte[0] = buffer[j++];
intUnion.byte[1] = buffer[j++];
airspeed = intUnion.word;
imu_ok = true;
}
void AP_GPS_IMU::GPS_join_data(void)
{
//gps_messages_received++;
int j = 0;
longitude = join_4_bytes(&buffer[j]); // Lat and Lon * 10**7
j += 4;
lattitude = join_4_bytes(&buffer[j]);
j += 4;
//Storing GPS Height above the sea level
intUnion.byte[0] = buffer[j++];
intUnion.byte[1] = buffer[j++];
altitude = (long)intUnion.word * 10; // Altitude in meters * 100
//Storing Speed
intUnion.byte[0] = buffer[j++];
intUnion.byte[1] = buffer[j++];
speed_3d = ground_speed = (float)intUnion.word; // Speed in M/S * 100
//We skip the gps ground course because we use yaw value from the IMU for ground course
j += 2;
time = join_4_bytes(&buffer[j]); // Time of Week in milliseconds
j += 4;
imu_health = buffer[j++];
new_data = 1;
fix = 1;
}
/****************************************************************
*
****************************************************************/
// Join 4 bytes into a long
long AP_GPS_IMU::join_4_bytes(unsigned char Buffer[])
{
longUnion.byte[0] = *Buffer;
longUnion.byte[1] = *(Buffer + 1);
longUnion.byte[2] = *(Buffer + 2);
longUnion.byte[3] = *(Buffer + 3);
return(longUnion.dword);
}
/****************************************************************
*
****************************************************************/
// checksum algorithm
void AP_GPS_IMU::checksum(byte data)
{
ck_a += data;
ck_b += ck_a;
}

43
libraries/AP_GPS/AP_GPS_IMU.h Executable file
View File

@ -0,0 +1,43 @@
#ifndef AP_GPS_IMU_h
#define AP_GPS_IMU_h
#include "AP_GPS.h"
#define MAXPAYLOAD 32
class AP_GPS_IMU : public AP_GPS
{
public:
// Methods
AP_GPS_IMU();
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);
long join_4_bytes(unsigned char Buffer[]);
};
#endif

186
libraries/AP_GPS/AP_GPS_MTK.cpp Normal file
View File

@ -0,0 +1,186 @@
/*
AP_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_MTK.h"
// Constructors ////////////////////////////////////////////////////////////////
AP_GPS_MTK::AP_GPS_MTK()
{
}
// Public Methods //////////////////////////////////////////////////////////////
void AP_GPS_MTK::init(void)
{
new_data = 0;
ck_a = 0;
ck_b = 0;
step = 0;
fix = 0;
print_errors = 0;
// initialize serial port
#if defined(__AVR_ATmega1280__)
Serial1.begin(38400); // Serial port 1 on ATMega1280
#else
Serial.begin(38400);
#endif
}
// optimization : This code don¥t wait for data, only proccess the data available
// We can call this function on the main loop (50Hz loop)
// If we get a complete packet this function calls parse_gps() to parse and update the GPS info.
void AP_GPS_MTK::update(void)
{
byte data;
int numc;
#if defined(__AVR_ATmega1280__) // If AtMega1280 then Serial port 1...
numc = Serial1.available();
#else
numc = Serial.available();
#endif
if (numc > 0)
for (int i=0;i<numc;i++){ // Process bytes received
#if defined(__AVR_ATmega1280__)
data = Serial1.read();
#else
data = Serial.read();
#endif
switch(step){
case 0:
if(data==0xB5) // UBX sync char 1
step++; //OH first data packet is correct, so jump to the next step
break;
case 1:
if(data==0x62) // UBX 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:
msg_class = data;
checksum(msg_class);
step++;
break;
case 3:
id = data;
step = 4;
payload_length_hi = 26;
payload_length_lo = 0;
payload_counter = 0;
checksum(id);
break;
case 4:
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 5:
GPS_ck_a = data; // First checksum byte
step++;
break;
case 6:
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 {
if (print_errors)
Serial.println("ERR:GPS_CHK!!");
}
// Variable initialization
step = 0;
ck_a = 0;
ck_b = 0;
break;
}// End switch
}// End for
}
// Private Methods
void
AP_GPS_MTK::parse_gps(void)
{
int j;
//Verifing if we are in class 1, you can change this "IF" for a "Switch" in case you want to use other UBX classes..
//In this case all the message im using are in class 1, to know more about classes check PAGE 60 of DataSheet.
if(msg_class == 0x01) {
switch(id){
//Checking the UBX ID
case 0x05: //ID Custom
j = 0;
lattitude= join_4_bytes(&buffer[j]); // lon*10000000
j += 4;
longitude = join_4_bytes(&buffer[j]); // lat*10000000
j += 4;
altitude = join_4_bytes(&buffer[j]); // MSL
j += 4;
speed_3d = ground_speed = join_4_bytes(&buffer[j]);
j += 4;
ground_course = join_4_bytes(&buffer[j]);
j += 4;
num_sats = buffer[j];
j++;
fix = buffer[j];
j++;
time = join_4_bytes(&buffer[j]);
new_data = 1;
break;
}
}
}
// Join 4 bytes into a long
long
AP_GPS_MTK::join_4_bytes(unsigned char buffer[])
{
longUnion.byte[3] = *buffer;
longUnion.byte[2] = *(buffer + 1);
longUnion.byte[1] = *(buffer + 2);
longUnion.byte[0] = *(buffer + 3);
return(longUnion.dword);
}
// checksum algorithm
void
AP_GPS_MTK::checksum(byte data)
{
ck_a += data;
ck_b += ck_a;
}

34
libraries/AP_GPS/AP_GPS_MTK.h Normal file
View File

@ -0,0 +1,34 @@
#ifndef AP_GPS_MTK_h
#define AP_GPS_MTK_h
#include <AP_GPS.h>
#define MAXPAYLOAD 32
class AP_GPS_MTK : public AP_GPS
{
public:
// Methods
AP_GPS_MTK();
void init();
void update();
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 id;
uint8_t payload_length_hi;
uint8_t payload_length_lo;
uint8_t payload_counter;
uint8_t buffer[MAXPAYLOAD];
void parse_gps();
void checksum(unsigned char data);
long join_4_bytes(unsigned char Buffer[]);
};
#endif

257
libraries/AP_GPS/AP_GPS_NMEA.cpp Normal file
View File

@ -0,0 +1,257 @@
/*
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:
lattitude : lattitude * 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"
// Constructors ////////////////////////////////////////////////////////////////
AP_GPS_NMEA::AP_GPS_NMEA()
{
}
// Public Methods //////////////////////////////////////////////////////////////
void
AP_GPS_NMEA::init(void)
{
//Type = 2;
GPS_checksum_calc = false;
bufferidx = 0;
new_data = 0;
fix = 0;
quality = 0;
print_errors = 0;
// Initialize serial port
#if defined(__AVR_ATmega1280__)
Serial1.begin(38400); // Serial port 1 on ATMega1280
#else
Serial.begin(38400);
#endif
}
// This code don´t wait for data, only proccess the data available on serial port
// 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;
#if defined(__AVR_ATmega1280__) // If AtMega1280 then Serial port 1...
numc = Serial1.available();
#else
numc = Serial.available();
#endif
if (numc > 0){
for (i = 0; i < numc; i++){
#if defined(__AVR_ATmega1280__) // If AtMega1280 then Serial port 1...
c = Serial1.read();
#else
c = Serial.read();
#endif
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
lattitude = aux_deg * 10000000 + (aux_min * 50) / 3; // degrees + minutes / 0.6 ( * 10000000) (0.6 = 3 / 5)
parseptr = strchr(parseptr, ', ')+1;
if ( * parseptr == 'S')
lattitude = -1 * lattitude; // South lattitudes are negative
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 {
if (print_errors)
Serial.println("GPSERR: Checksum error!!");
}
}
} 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 {
if (print_errors)
Serial.println("GPSERR: Checksum error!!");
}
}
} else {
bufferidx = 0;
if (print_errors)
Serial.println("GPSERR: Bad sentence!!");
}
}
// 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;
}

35
libraries/AP_GPS/AP_GPS_NMEA.h Normal file
View File

@ -0,0 +1,35 @@
#ifndef AP_GPS_NMEA_h
#define AP_GPS_NMEA_h
#include "AP_GPS.h"
#define GPS_BUFFERSIZE 120
class AP_GPS_NMEA : public AP_GPS
{
public:
// Methods
AP_GPS_NMEA();
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);
};
extern AP_GPS_NMEA GPS;
#endif

245
libraries/AP_GPS/AP_GPS_UBLOX.cpp Normal file
View File

@ -0,0 +1,245 @@
/*
AP_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"
// Constructors // /// /// /// /// /// /// /// /// /// /// /// /// /// /// /// /// /// /// /// /// //
AP_GPS_UBLOX::AP_GPS_UBLOX()
{
print_errors = 1;
}
// Public Methods // /// /// /// /// /// /// /// /// /// /// /// /// /// /// /// /// /// /// /// ///
void AP_GPS_UBLOX::init(void)
{
ck_a = 0;
ck_b = 0;
step = 0;
new_data = 0;
fix = 0;
print_errors = 0;
// initialize serial port
#if defined(__AVR_ATmega1280__)
Serial1.begin(38400); // Serial port 1 on ATMega1280
#else
Serial.begin(38400);
#endif
}
// optimization : This code don´t wait for data, only proccess the data available
// 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;
#if defined(__AVR_ATmega1280__) // If AtMega1280 then Serial port 1...
numc = Serial1.available();
#else
numc = Serial.available();
#endif
Serial.print("numc ");
Serial.println(numc,DEC);
if (numc > 0){
Serial.println(" ");
for (int i = 0;i < numc;i++){ // Process bytes received
#if defined(__AVR_ATmega1280__)
data = Serial1.read();
#else
data = Serial.read();
#endif
Serial.print(data,HEX);
Serial.println(",");
switch(step){ // Normally we start from zero. This is a state machine
case 0:
if(data == 0xB5) // UBX sync char 1
step++; // OH first data packet is correct, so jump to the next step
break;
case 1:
if(data == 0x62) // UBX 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:
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){
if (print_errors)
Serial.println("ERR:GPS_BAD_PAYLOAD_LENGTH!!");
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{
if (print_errors) Serial.println("ERR:GPS_CHK!!");
}
// Variable initialization
step = 0;
ck_a = 0;
ck_b = 0;
break;
}
} // End for...
}
}
// Private Methods //////////////////////////////////////////////////////////////
void AP_GPS_UBLOX::parse_gps(void)
{
int j;
//Verifing if we are in msg_class 1, you can change this "IF" for a "Switch" in case you want to use other UBX classes..
// In this case all the message im using are in msg_class 1, to know more about classes check PAGE 60 of DataSheet.
if(msg_class == 0x01)
{
switch(id) //Checking the UBX ID
{
case 0x02: // ID NAV - POSLLH
j = 0;
time = join_4_bytes(&buffer[j]); // ms Time of week
j += 4;
longitude = join_4_bytes(&buffer[j]); // lon * 10000000
j += 4;
lattitude = join_4_bytes(&buffer[j]); // lat * 10000000
j += 4;
//altitude = join_4_bytes(&buffer[j]); // elipsoid heigth mm
j += 4;
altitude = (float)join_4_bytes(&buffer[j]); // MSL heigth mm
//j+=4;
/*
hacc = (float)join_4_bytes(&buffer[j]) / (float)1000;
j += 4;
vacc = (float)join_4_bytes(&buffer[j]) / (float)1000;
j += 4;
*/
new_data = 1;
break;
case 0x03: //ID NAV - STATUS
//if(buffer[4] >= 0x03)
if((buffer[4] >= 0x03) && (buffer[5] & 0x01))
fix = 1; // valid position
else
fix = 0; // invalid position
break;
case 0x06: //ID NAV - SOL
if((buffer[10] >= 0x03) && (buffer[11] & 0x01))
fix = 1; // valid position
else
fix = 0; // invalid position
//ecefVZ = join_4_bytes(&buffer[36]); // Vertical Speed in cm / s
num_sats = buffer[47]; // Number of sats...
break;
case 0x12: // ID NAV - VELNED
j = 16;
speed_3d = join_4_bytes(&buffer[j]); // cm / s
j += 4;
ground_speed = join_4_bytes(&buffer[j]); // Ground speed 2D cm / s
j += 4;
ground_course = join_4_bytes(&buffer[j]); // Heading 2D deg * 100000
ground_course /= 1000; // Rescale heading to deg * 100
j += 4;
break;
}
}
}
// Join 4 bytes into a long
long AP_GPS_UBLOX::join_4_bytes(unsigned char Buffer[])
{
longUnion.byte[0] = *Buffer;
longUnion.byte[1] = *(Buffer + 1);
longUnion.byte[2] = *(Buffer + 2);
longUnion.byte[3] = *(Buffer + 3);
return(longUnion.dword);
}
// Ublox checksum algorithm
void AP_GPS_UBLOX::checksum(byte data)
{
ck_a += data;
ck_b += ck_a;
}

35
libraries/AP_GPS/AP_GPS_UBLOX.h Normal file
View File

@ -0,0 +1,35 @@
#ifndef GPS_UBLOX_h
#define GPS_UBLOX_h
#include <AP_GPS.h>
#define MAXPAYLOAD 60
class AP_GPS_UBLOX : public AP_GPS
{
public:
// Methods
AP_GPS_UBLOX();
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

47
libraries/AP_GPS/examples/GPS_MTK_test/GPS_MTK_test.pde Normal file
View File

@ -0,0 +1,47 @@
/*
Example of GPS MTK library.
Code by Jordi Munoz and Jose Julio. DIYDrones.com
Works with Ardupilot Mega Hardware (GPS on Serial Port1)
and with standard ATMega168 and ATMega328 on Serial Port 0
*/
#include <AP_GPS_MTK.h> // UBLOX GPS Library
AP_GPS_MTK gps;
#define T6 1000000
void setup()
{
Serial.begin(38400);
Serial.println("GPS MTK library test");
gps.init(); // GPS Initialization
delay(1000);
}
void loop()
{
delay(20);
gps.update();
if (gps.new_data){
Serial.print("gps:");
Serial.print(" Lat:");
Serial.print((float)gps.lattitude / T6, DEC);
Serial.print(" Lon:");
Serial.print((float)gps.longitude / T6, DEC);
Serial.print(" Alt:");
Serial.print((float)gps.altitude / 100.0, DEC);
Serial.print(" GSP:");
Serial.print(gps.ground_speed / 100.0);
Serial.print(" COG:");
Serial.print(gps.ground_course / 1000000, DEC);
Serial.print(" SAT:");
Serial.print(gps.num_sats, DEC);
Serial.print(" FIX:");
Serial.print(gps.fix, DEC);
Serial.print(" TIM:");
Serial.print(gps.time, DEC);
Serial.println();
gps.new_data = 0; // We have readed the data
}
}

49
libraries/AP_GPS/examples/GPS_NMEA_test/GPS_NMEA_test.pde Normal file
View File

@ -0,0 +1,49 @@
/*
Example of GPS MTK library.
Code by Jordi Munoz and Jose Julio. DIYDrones.com
Works with Ardupilot Mega Hardware (GPS on Serial Port1)
and with standard ATMega168 and ATMega328 on Serial Port 0
*/
#include <AP_GPS_NMEA.h> // UBLOX GPS Library
AP_GPS_NMEA gps;
#define T6 1000000
#define T7 1000000
void setup()
{
Serial.begin(57600);
Serial.println("GPS NMEA library test");
gps.init(); // GPS Initialization
delay(1000);
}
void loop()
{
delay(20);
gps.update();
if (gps.new_data){
Serial.print("gps:");
Serial.print(" Lat:");
Serial.print((float)gps.lattitude / T7, DEC);
Serial.print(" Lon:");
Serial.print((float)gps.longitude / T7, DEC);
Serial.print(" Alt:");
Serial.print((float)gps.altitude / 100.0, DEC);
Serial.print(" GSP:");
Serial.print(gps.ground_speed / 100.0);
Serial.print(" COG:");
Serial.print(gps.ground_course / 100, DEC);
Serial.print(" SAT:");
Serial.print(gps.num_sats, DEC);
Serial.print(" FIX:");
Serial.print(gps.fix, DEC);
Serial.print(" TIM:");
Serial.print(gps.time, DEC);
Serial.println();
gps.new_data = 0; // We have readed the data
}
}

48
libraries/AP_GPS/examples/GPS_UBLOX_test/GPS_UBLOX_test.pde Normal file
View File

@ -0,0 +1,48 @@
/*
Example of GPS MTK library.
Code by Jordi Munoz and Jose Julio. DIYDrones.com
Works with Ardupilot Mega Hardware (GPS on Serial Port1)
and with standard ATMega168 and ATMega328 on Serial Port 0
*/
#include <AP_GPS_UBLOX.h> // UBLOX GPS Library
AP_GPS_UBLOX gps;
#define T6 1000000
#define T7 1000000
void setup()
{
Serial.begin(38400);
Serial.println("GPS UBLOX library test");
gps.init(); // GPS Initialization
delay(1000);
}
void loop()
{
delay(20);
gps.update();
if (gps.new_data){
Serial.print("gps:");
Serial.print(" Lat:");
Serial.print((float)gps.lattitude / T7, DEC);
Serial.print(" Lon:");
Serial.print((float)gps.longitude / T7, DEC);
Serial.print(" Alt:");
Serial.print((float)gps.altitude / 100.0, DEC);
Serial.print(" GSP:");
Serial.print(gps.ground_speed / 100.0);
Serial.print(" COG:");
Serial.print(gps.ground_course / 100, DEC);
Serial.print(" SAT:");
Serial.print(gps.num_sats, DEC);
Serial.print(" FIX:");
Serial.print(gps.fix, DEC);
Serial.print(" TIM:");
Serial.print(gps.time, DEC);
Serial.println();
gps.new_data = 0; // We have readed the data
}
}

180
libraries/AP_Navigation/Navigation.cpp Normal file
View File

@ -0,0 +1,180 @@
#include "Navigation.h"
Navigation::Navigation(AP_GPS *withGPS) :
_gps(withGPS),
_hold_course(-1)
{
}
void
Navigation::update_gps()
{
location.alt = _gps->altitude;
location.lng = _gps->longitude;
location.lat = _gps->lattitude;
// target_bearing is where we should be heading
bearing = get_bearing(&location, &next_wp);
// waypoint distance from plane
distance = get_distance(&location, &next_wp);
calc_bearing_error();
calc_altitude_error();
altitude_above_home = location.alt - home.alt;
// check if we have missed the WP
_loiter_delta = (bearing - _old_bearing) / 100;
// reset the old value
_old_bearing = bearing;
// wrap values
if (_loiter_delta > 170) _loiter_delta -= 360;
if (_loiter_delta < -170) _loiter_delta += 360;
loiter_sum += abs(_loiter_delta);
if (distance <= 0){
distance = -1;
Serial.print("MSG wp error ");
Serial.println(distance, DEC);
}
}
void
Navigation::set_home(Waypoints::WP loc)
{
home = loc;
location = home;
}
void
Navigation::set_next_wp(Waypoints::WP loc)
{
prev_wp = next_wp;
next_wp = loc;
if(_scaleLongDown == 0)
calc_long_scaling(loc.lat);
total_distance = get_distance(&location, &next_wp);
distance = total_distance;
bearing = get_bearing(&location, &next_wp);
_old_bearing = bearing;
// clear loitering code
_loiter_delta = 0;
loiter_sum = 0;
// set a new crosstrack bearing
// ----------------------------
reset_crosstrack();
}
void
Navigation::calc_long_scaling(int32_t lat)
{
// this is used to offset the shrinking longitude as we go towards the poles
float rads = (abs(lat) / T7) * 0.0174532925;
_scaleLongDown = cos(rads);
_scaleLongUp = 1.0f / cos(rads);
}
void
Navigation::hold_course(int8_t b)
{
if(b)
_hold_course = bearing;
else
_hold_course = -1;
}
void
Navigation::calc_distance_error()
{
//distance_estimate += (float)_gps->ground_speed * .0002 * cos(radians(bearing_error * .01));
//distance_estimate -= DST_EST_GAIN * (float)(distance_estimate - GPS_distance);
//distance = max(distance_estimate,10);
}
void
Navigation::calc_bearing_error(void)
{
if(_hold_course == -1){
bearing_error = wrap_360(bearing - _gps->ground_course);
}else{
bearing_error = _hold_course;
}
}
int32_t
Navigation::wrap_360(int32_t error)
{
if (error > 18000) error -= 36000;
if (error < -18000) error += 36000;
return error;
}
/*****************************************
* Altitude error with Airspeed correction
*****************************************/
void
Navigation::calc_altitude_error(void)
{
// limit climb rates
_target_altitude = next_wp.alt - ((float)((distance -20) * _offset_altitude) / (float)(total_distance - 20));
if(prev_wp.alt > next_wp.alt){
_target_altitude = constrain(_target_altitude, next_wp.alt, prev_wp.alt);
}else{
_target_altitude = constrain(_target_altitude, prev_wp.alt, next_wp.alt);
}
altitude_error = _target_altitude - location.alt;
}
void
Navigation::update_crosstrack(void)
{
// Crosstrack Error
// ----------------
if (abs(bearing - _crosstrack_bearing) < 4500) { // If we are too far off or too close we don't do track following
_crosstrack_error = sin(radians((bearing - _crosstrack_bearing) / 100)) * distance; // Meters we are off track line
bearing += constrain(_crosstrack_error * XTRACK_GAIN, -XTRACK_ENTRY_ANGLE, XTRACK_ENTRY_ANGLE);
}
}
void
Navigation::reset_crosstrack(void)
{
_crosstrack_bearing = get_bearing(&location, &next_wp); // Used for track following
}
long
Navigation::get_distance(Waypoints::WP *loc1, Waypoints::WP *loc2)
{
if(loc1->lat == 0 || loc1->lng == 0)
return -1;
if(loc2->lat == 0 || loc2->lng == 0)
return -1;
if(_scaleLongDown == 0)
calc_long_scaling(loc2->lat);
float dlat = (float)(loc2->lat - loc1->lat);
float dlong = ((float)(loc2->lng - loc1->lng)) * _scaleLongDown;
return sqrt(sq(dlat) + sq(dlong)) * .01113195;
}
long
Navigation::get_bearing(Waypoints::WP *loc1, Waypoints::WP *loc2)
{
if(loc1->lat == 0 || loc1->lng == 0)
return -1;
if(loc2->lat == 0 || loc2->lng == 0)
return -1;
if(_scaleLongDown == 0)
calc_long_scaling(loc2->lat);
long off_x = loc2->lng - loc1->lng;
long off_y = (loc2->lat - loc1->lat) * _scaleLongUp;
long bearing = 9000 + atan2(-off_y, off_x) * 5729.57795;
if (bearing < 0)
bearing += 36000;
return bearing;
}

50
libraries/AP_Navigation/Navigation.h Normal file
View File

@ -0,0 +1,50 @@
#define XTRACK_GAIN 10 // Amount to compensate for crosstrack (degrees/100 per meter)
#define XTRACK_ENTRY_ANGLE 3000 // Max angle used to correct for track following degrees*100
#include <AP_GPS.h> // ArduPilot GPS Library
#include "Waypoints.h" // ArduPilot Waypoints Library
#include "WProgram.h"
#define T7 10000000
class Navigation {
public:
Navigation(AP_GPS *withGPS);
void update_gps(void); // called 50 Hz
void set_home(Waypoints::WP loc);
void set_next_wp(Waypoints::WP loc);
void hold_course(int8_t b); // 1 = hold a current course, 0 disables course hold
long get_distance(Waypoints::WP *loc1, Waypoints::WP *loc2);
long get_bearing(Waypoints::WP *loc1, Waypoints::WP *loc2);
long bearing; // deg * 100 : 0 to 360 current desired bearing to navigate
long distance; // meters : distance plane to next waypoint
long altitude_above_home; // meters * 100 relative to home position
long total_distance; // meters : distance between waypoints
long bearing_error; // deg * 100 : 18000 to -18000
long altitude_error; // deg * 100 : 18000 to -18000
int16_t loiter_sum;
Waypoints::WP home, location, prev_wp, next_wp;
private:
void calc_long_scaling(int32_t lat);
void calc_bearing_error(void);
void calc_altitude_error(void);
void calc_distance_error(void);
void update_crosstrack(void);
void reset_crosstrack(void);
int32_t wrap_360(int32_t error); // utility
int16_t _old_bearing; // used to track delta on the bearing
AP_GPS *_gps;
long _crosstrack_bearing; // deg * 100 : 0 to 360 desired angle of plane to target
float _crosstrack_error; // deg * 100 : 18000 to -18000 meters we are off trackline
long _hold_course; // deg * 100 dir of plane
long _target_altitude; // used for
long _offset_altitude; // used for
float _altitude_estimate;
float _scaleLongUp; // used to reverse longtitude scaling
float _scaleLongDown; // used to reverse longtitude scaling
int16_t _loiter_delta;
};

152
libraries/AP_Navigation/examples/Navigation/Navigation.pde Normal file
View File

@ -0,0 +1,152 @@
/*
This test assumes you are at the LOWl demo Airport
*/
#include "Waypoints.h"
#include "Navigation.h"
#include "AP_GPS_IMU.h"
#include "AP_RC.h"
AP_GPS_IMU gps;
Navigation nav((AP_GPS *) & gps);
AP_RC rc;
#define CH_ROLL 0
#define CH_PITCH 1
#define CH_THROTTLE 2
#define CH_RUDDER 3
#define CH3_MIN 1100
#define CH3_MAX 1900
#define REVERSE_RUDDER 1
#define REVERSE_ROLL 1
#define REVERSE_PITCH 1
int8_t did_init_home;
int16_t servo_out[4];
int16_t radio_trim[4] = {1500,1500,1100,1500};
int16_t radio_in[4];
void setup()
{
Serial.begin(38400);
Serial.println("Navigation test");
Waypoints::WP location_B = {0, 0, 74260, 472586364, 113366012};
nav.set_next_wp(location_B);
rc.init(radio_trim);
}
void loop()
{
delay(20);
gps.update();
rc.read_pwm();
for(int y=0; y<4; y++) {
//rc.set_ch_pwm(y, rc.input[y]); // send to Servos
radio_in[y] = rc.input[y];
}
servo_out[CH_ROLL] = ((radio_in[CH_ROLL] - radio_trim[CH_ROLL]) * 45 * REVERSE_ROLL) / 500;
servo_out[CH_PITCH] = ((radio_in[CH_PITCH] - radio_trim[CH_PITCH]) * 45 * REVERSE_PITCH) / 500;
servo_out[CH_RUDDER] = ((radio_in[CH_RUDDER] - radio_trim[CH_RUDDER]) * 45 * REVERSE_RUDDER) / 500;
servo_out[CH_THROTTLE] = (float)(radio_in[CH_THROTTLE] - CH3_MIN) / (float)(CH3_MAX - CH3_MIN) *100;
servo_out[CH_THROTTLE] = constrain(servo_out[CH_THROTTLE], 0, 100);
output_Xplane();
if(gps.new_data /* && gps.fix */ ){
if(did_init_home == 0){
did_init_home = 1;
Waypoints::WP wp = {0, 0, gps.altitude, gps.lattitude, gps.longitude};
nav.set_home(wp);
Serial.println("MSG init home");
}else{
nav.update_gps();
}
//print_loc();
}
}
void print_loc()
{
Serial.print("MSG GPS: ");
Serial.print(nav.location.lat, DEC);
Serial.print(", ");
Serial.print(nav.location.lng, DEC);
Serial.print(", ");
Serial.print(nav.location.alt, DEC);
Serial.print("\tDistance = ");
Serial.print(nav.distance, DEC);
Serial.print(" Bearing = ");
Serial.print(nav.bearing/100, DEC);
Serial.print(" Bearing err = ");
Serial.print(nav.bearing_error/100, DEC);
Serial.print(" alt err = ");
Serial.print(nav.altitude_error/100, DEC);
Serial.print(" Alt above home = ");
Serial.println(nav.altitude_above_home/100, DEC);
}
void print_pwm()
{
Serial.print("ch1 ");
Serial.print(rc.input[CH_ROLL],DEC);
Serial.print("\tch2: ");
Serial.print(rc.input[CH_PITCH],DEC);
Serial.print("\tch3 :");
Serial.print(rc.input[CH_THROTTLE],DEC);
Serial.print("\tch4 :");
Serial.println(rc.input[CH_RUDDER],DEC);
}
byte buf_len = 0;
byte out_buffer[32];
void output_Xplane(void)
{
// output real-time sensor data
Serial.print("AAA"); // Message preamble
output_int((int)(servo_out[CH_ROLL]*100)); // 0 bytes 0,1
output_int((int)(servo_out[CH_PITCH]*100)); // 1 bytes 2,3
output_int((int)(servo_out[CH_THROTTLE])); // 2 bytes 4,5
output_int((int)(servo_out[CH_RUDDER]*100)); // 3 bytes 6,7
output_int((int)nav.distance); // 4 bytes 8,9
output_int((int)nav.bearing_error); // 5 bytes 10,11
output_int((int)nav.altitude_error); // 6 bytes 12,13
output_int(0); // 7 bytes 14,15
output_byte(1); // 8 bytes 16
output_byte(1); // 9 bytes 17
// print out the buffer and checksum
// ---------------------------------
print_buffer();
}
void output_int(int value)
{
//buf_len += 2;
out_buffer[buf_len++] = value & 0xff;
out_buffer[buf_len++] = (value >> 8) & 0xff;
}
void output_byte(byte value)
{
out_buffer[buf_len++] = value;
}
void print_buffer()
{
byte ck_a = 0;
byte ck_b = 0;
for (byte i = 0;i < buf_len; i++){
Serial.print (out_buffer[i]);
}
Serial.print('\r');
Serial.print('\n');
buf_len = 0;
}

28
libraries/AP_Navigation/examples/Navigation_simple/Navigation_simple.pde Normal file
View File

@ -0,0 +1,28 @@
#include <Waypoints.h>
#include <Navigation.h>
#include <AP_GPS_IMU.h>
AP_GPS_IMU gps;
Navigation nav((AP_GPS *) & gps);
void setup()
{
Serial.begin(38400);
Serial.println("Navigation test");
Waypoints::WP location_A = {0, 0, 38.579633 * T7, -122.566265 * T7, 10000};
Waypoints::WP location_B = {0, 0, 38.578743 * T7, -122.572782 * T7, 5000};
long distance = nav.get_distance(&location_A, &location_B);
long bearing = nav.get_bearing(&location_A, &location_B);
Serial.print("\tDistance = ");
Serial.print(distance, DEC);
Serial.print(" Bearing = ");
Serial.println(bearing, DEC);
}
void loop()
{
}