/* 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() { } // 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 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; }