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

Added GPS_IMY 

git-svn-id: https://arducopter.googlecode.com/svn/trunk@340 f9c3cf11-9bcb-44bc-f272-b75c42450872
master
analoguedevices 15 years ago
parent
68ba755660
commit
7d5fbed772
4 changed files with 408 additions and 0 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 279
      libraries/GPS_IMU/GPS_IMU.cpp
  2. 69
      libraries/GPS_IMU/GPS_IMU.h
  3. 44
      libraries/GPS_IMU/examples/GPS_IMU_test/GPS_IMU_test.pde
  4. 16
      libraries/GPS_IMU/keywords.txt

279
libraries/GPS_IMU/GPS_IMU.cpp
Unescape View File

@ -0,0 +1,279 @@
/*
GPS_IMU.cpp - IMU/X-Plane GPS library for Arduino
*/
#include "GPS_IMU.h"
#include <avr/interrupt.h>
#include "WProgram.h"
// Constructors ////////////////////////////////////////////////////////////////
GPS_IMU_Class::GPS_IMU_Class()
{
}
// Public Methods //////////////////////////////////////////////////////////////
void GPS_IMU_Class::Init(void)
{
ck_a = 0;
ck_b = 0;
IMU_step = 0;
NewData = 0;
Fix = 0;
PrintErrors = 0;
IMU_timer = millis(); //Restarting timer...
// 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_IMU_gps() to parse and update the GPS info.
void GPS_IMU_Class::Read(void)
{
static unsigned long GPS_timer = 0;
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(IMU_step){ //Normally we start from zero. This is a state machine
case 0:
if(data == 0x44) // IMU sync char 1
IMU_step++; //OH first data packet is correct, so jump to the next IMU_step
break;
case 1:
if(data == 0x49) // IMU sync char 2
IMU_step++; //ooh! The second data packet is correct, jump to the IMU_step 2
else
IMU_step=0; //Nop, is not correct so restart to IMU_step zero and try again.
break;
case 2:
if(data == 0x59) // IMU sync char 3
IMU_step++; //ooh! The second data packet is correct, jump to the IMU_step 2
else
IMU_step=0; //Nop, is not correct so restart to IMU_step zero and try again.
break;
case 3:
if(data == 0x64) // IMU sync char 4
IMU_step++; //ooh! The second data packet is correct, jump to the IMU_step 2
else
IMU_step=0; //Nop, is not correct so restart to IMU_step zero and try again.
break;
case 4:
payload_length = data;
checksum(payload_length);
IMU_step++;
if (payload_length > 28){
IMU_step = 0; //Bad data, so restart to IMU_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);
IMU_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) {
IMU_step++;
}
break;
case 7:
IMU_ck_a = data; // First checksum byte
IMU_step++;
break;
case 8:
IMU_ck_b = data; // Second checksum byte
// We end the IMU/GPS read...
// Verify the received checksum with the generated checksum..
if((ck_a == IMU_ck_a) && (ck_b == IMU_ck_b)) {
if (message_num == 0x02) {
IMU_join_data();
IMU_timer = millis();
} else if (message_num == 0x03) {
GPS_join_data();
GPS_timer = millis();
} else if (message_num == 0x04) {
IMU2_join_data();
IMU_timer = millis();
} 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
IMU_step = 0;
payload_counter = 0;
ck_a = 0;
ck_b = 0;
IMU_timer = millis(); //Restarting timer...
break;
}
}// End for...
}
// If we don't receive GPS packets in 2 seconds => Bad FIX state
if ((millis() - GPS_timer)>2000){
Fix = 0;
}
if (PrintErrors){
Serial.println("ERR:GPS_TIMEOUT!!");
}
}
/****************************************************************
*
****************************************************************/
void GPS_IMU_Class::IMU_join_data(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 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 GPS_IMU_Class::IMU2_join_data()
{
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 GPS_IMU_Class::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 (3-D)
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++];
NewData = 1;
Fix = 1;
}
/****************************************************************
*
****************************************************************/
// Join 4 bytes into a long
long GPS_IMU_Class::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 GPS_IMU_Class::checksum(byte IMU_data)
{
ck_a+=IMU_data;
ck_b+=ck_a;
}
GPS_IMU_Class GPS;

69
libraries/GPS_IMU/GPS_IMU.h
Unescape View File

@ -0,0 +1,69 @@
#ifndef GPS_IMU_h
#define GPS_IMU_h
#include <inttypes.h>
#define IMU_MAXPAYLOAD 32
class GPS_IMU_Class
{
private:
// Internal variables
union int_union {
int16_t word;
uint8_t byte[2];
} intUnion;
union long_union {
int32_t dword;
uint8_t byte[4];
} longUnion;
uint8_t ck_a; // Packet checksum
uint8_t ck_b;
uint8_t IMU_ck_a;
uint8_t IMU_ck_b;
uint8_t IMU_step;
uint8_t IMU_class;
uint8_t message_num;
uint8_t payload_length;
uint8_t payload_counter;
uint8_t buffer[IMU_MAXPAYLOAD];
long IMU_timer;
long IMU_ecefVZ;
void IMU_join_data();
void IMU2_join_data();
void GPS_join_data();
void checksum(unsigned char IMU_data);
long join_4_bytes(unsigned char Buffer[]);
public:
// Methods
GPS_IMU_Class();
void Init();
void Read();
// 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;
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 NumSats; // Number of visible satelites
uint8_t Fix; // 1:GPS FIX 0:No FIX (normal logic)
uint8_t NewData; // 1:New GPS Data
uint8_t PrintErrors; // 1: To Print GPS Errors (for debug)
};
extern GPS_IMU_Class GPS;
#endif

44
libraries/GPS_IMU/examples/GPS_IMU_test/GPS_IMU_test.pde
Unescape View File

@ -0,0 +1,44 @@
/*
Example of GPS IMU library.
Code by Jordi MuÔøΩoz, Jose Julio and, Jason Short . DIYDrones.com
Works with Ardupilot Mega Hardware (GPS on Serial Port1)
and with standard ATMega168 and ATMega328 on Serial Port 0
*/
#include <GPS_IMU.h> // UBLOX GPS Library
void setup()
{
Serial.begin(38400);
Serial.println("GPS IMU library test");
GPS.Init(); // GPS Initialization
delay(1000);
}
void loop()
{
GPS.Read();
if (1){ // New GPS data?
Serial.print("GPS:");
Serial.print(" Lat:");
Serial.print(GPS.Lattitude);
Serial.print(" Lon:");
Serial.print(GPS.Longitude);
Serial.print(" Alt:");
Serial.print((float)GPS.Altitude/100.0);
Serial.print(" GSP:");
Serial.print((float)GPS.Ground_Speed/100.0);
Serial.print(" COG:");
Serial.print(GPS.Ground_Course/1000000);
Serial.print(" SAT:");
Serial.print((int)GPS.NumSats);
Serial.print(" FIX:");
Serial.print((int)GPS.Fix);
Serial.print(" TIM:");
Serial.print(GPS.Time);
Serial.println();
GPS.NewData = 0; // We have readed the data
}
delay(20);
}

16
libraries/GPS_IMU/keywords.txt
Unescape View File

@ -0,0 +1,16 @@
GPS KEYWORD1
GPS_UBLOX KEYWORD1
Init KEYWORD2
Read KEYWORD2
Time KEYWORD2
Lattitude KEYWORD2
Longitude KEYWORD2
Altitude KEYWORD2
Ground_Speed KETWORD2
Ground_Course KEYWORD2
Speed_3d KEYWORD2
NumSats KEYWORD2
Fix KEYWORD2
NewData KEYWORD2
Write Preview
Loading…
Cancel
Save