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

libs: removed unused library GPS_IMU

This commit is contained in:
Andrew Tridgell 2012-02-24 14:31:40 +11:00
parent 1a21c78062
commit f221bd13ab
4 changed files with 0 additions and 412 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

283
libraries/GPS_IMU/GPS_IMU.cpp
View File

@ -1,283 +0,0 @@
/*
GPS_IMU.cpp - IMU/X-Plane GPS library for Arduino
*/
#include "GPS_IMU.h"
#include <avr/interrupt.h>
#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#else
#include "WProgram.h"
#endif
// 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__) || defined(__AVR_ATmega2560__)
Serial1.begin(38400); // Serial port 1 on ATMega1280/2560
#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__) || defined(__AVR_ATmega2560__) // If AtMega1280/2560 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__) || defined(__AVR_ATmega2560__)
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) > 3000){
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
View File

@ -1,69 +0,0 @@
#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
View File

@ -1,44 +0,0 @@
/*
Example of GPS IMU library.
Code by Jordi Munoz, 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> // 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 read the data
}
delay(20);
}

16
libraries/GPS_IMU/keywords.txt
View File

@ -1,16 +0,0 @@
GPS KEYWORD1
GPS_IMU 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