mirror of https://github.com/ArduPilot/ardupilot
Add back AP_GPS_IMU to provide Xplane functionality
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1504 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
parent
709b56f477
commit
c9788d7b0b
|
@ -13,3 +13,4 @@
|
||||||
#include "AP_GPS_Auto.h"
|
#include "AP_GPS_Auto.h"
|
||||||
#include "AP_GPS_HIL.h"
|
#include "AP_GPS_HIL.h"
|
||||||
#include "AP_GPS_Shim.h" // obsoletes AP_GPS_HIL, use in preference
|
#include "AP_GPS_Shim.h" // obsoletes AP_GPS_HIL, use in preference
|
||||||
|
#include "AP_GPS_IMU.h" // temporarily reinstated for Xplane support
|
||||||
|
|
|
@ -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;i<numc;i++){ // Process bytes received
|
||||||
|
|
||||||
|
data = _port->read();
|
||||||
|
|
||||||
|
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;
|
||||||
|
}
|
|
@ -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 <GPS.h>
|
||||||
|
#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
|
Loading…
Reference in New Issue