From c9788d7b0b35acea3832d563ecf608fe829244e5 Mon Sep 17 00:00:00 2001 From: "deweibel@gmail.com" Date: Mon, 17 Jan 2011 01:50:34 +0000 Subject: [PATCH] Add back AP_GPS_IMU to provide Xplane functionality git-svn-id: https://arducopter.googlecode.com/svn/trunk@1504 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- libraries/AP_GPS/AP_GPS.h | 1 + libraries/AP_GPS/AP_GPS_IMU.cpp | 226 ++++++++++++++++++++++++++++++++ libraries/AP_GPS/AP_GPS_IMU.h | 44 +++++++ 3 files changed, 271 insertions(+) create mode 100644 libraries/AP_GPS/AP_GPS_IMU.cpp create mode 100644 libraries/AP_GPS/AP_GPS_IMU.h diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index 5f3a149ead..20b7d3fcfd 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -13,3 +13,4 @@ #include "AP_GPS_Auto.h" #include "AP_GPS_HIL.h" #include "AP_GPS_Shim.h" // obsoletes AP_GPS_HIL, use in preference +#include "AP_GPS_IMU.h" // temporarily reinstated for Xplane support diff --git a/libraries/AP_GPS/AP_GPS_IMU.cpp b/libraries/AP_GPS/AP_GPS_IMU.cpp new file mode 100644 index 0000000000..cc9d0667cf --- /dev/null +++ b/libraries/AP_GPS/AP_GPS_IMU.cpp @@ -0,0 +1,226 @@ +// -*- 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. +bool +AP_GPS_IMU::read(void) +{ + byte data; + int numc = 0; + static byte message_num = 0; + + numc = _port->available(); + + if (numc > 0){ + for (int i=0;iread(); + + 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 = *(unsigned 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; +} \ No newline at end of file diff --git a/libraries/AP_GPS/AP_GPS_IMU.h b/libraries/AP_GPS/AP_GPS_IMU.h new file mode 100644 index 0000000000..5d4718da95 --- /dev/null +++ b/libraries/AP_GPS/AP_GPS_IMU.h @@ -0,0 +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 +#define MAXPAYLOAD 32 + +class AP_GPS_IMU : public GPS { + public: + + // Methods + AP_GPS_IMU(Stream *s); + virtual void init(void); + virtual bool read(void); + + // 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 \ No newline at end of file