Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 18:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

uncrustify libraries/AP_GPS/AP_GPS_IMU.cpp

Browse Source
This commit is contained in:
uncrustify 2012-08-21 19:19:52 -07:00 committed by Pat Hickey
parent 5ed90851e3
commit 89bd01f86e
1 changed files with 239 additions and 238 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

477
libraries/AP_GPS/AP_GPS_IMU.cpp
View File

@ -1,238 +1,239 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/* /*
GPS_MTK.cpp - Ublox GPS library for Arduino * GPS_MTK.cpp - Ublox GPS library for Arduino
Code by Jordi Mu<EFBFBD>oz and Jose Julio. DIYDrones.com * Code by Jordi Mu<EFBFBD>oz and Jose Julio. DIYDrones.com
This code works with boards based on ATMega168/328 and ATMega1280 (Serial port 1) * 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 * This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public * modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either * License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version. * version 2.1 of the License, or (at your option) any later version.
*
GPS configuration : Costum protocol * GPS configuration : Costum protocol
Baud rate : 38400 * Baud rate : 38400
*
Methods: * Methods:
init() : GPS initialization * init() : GPS initialization
update() : Call this funcion as often as you want to ensure you read the incomming gps data * update() : Call this funcion as often as you want to ensure you read the incomming gps data
*
Properties: * Properties:
lattitude : lattitude * 10000000 (int32_t value) * lattitude : lattitude * 10000000 (int32_t value)
longitude : longitude * 10000000 (int32_t value) * longitude : longitude * 10000000 (int32_t value)
altitude : altitude * 100 (meters) (int32_t value) * altitude : altitude * 100 (meters) (int32_t value)
ground_speed : Speed (m/s) * 100 (int32_t value) * ground_speed : Speed (m/s) * 100 (int32_t value)
ground_course : Course (degrees) * 100 (int32_t value) * ground_course : Course (degrees) * 100 (int32_t value)
new_data : 1 when a new data is received. * new_data : 1 when a new data is received.
You need to write a 0 to new_data when you read the data * 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. * fix : 1: GPS NO fix, 2: 2D fix, 3: 3D fix.
*
*/ */
#include "AP_GPS_IMU.h" #include "AP_GPS_IMU.h"
#if defined(ARDUINO) && ARDUINO >= 100 #if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h" #include "Arduino.h"
#else #else
#include "WProgram.h" #include "WProgram.h"
#endif #endif
// Constructors //////////////////////////////////////////////////////////////// // Constructors ////////////////////////////////////////////////////////////////
AP_GPS_IMU::AP_GPS_IMU(Stream *s) : GPS(s) AP_GPS_IMU::AP_GPS_IMU(Stream *s) : GPS(s)
{ {
} }
// Public Methods ////////////////////////////////////////////////////////////// // Public Methods //////////////////////////////////////////////////////////////
void void
AP_GPS_IMU::init(enum GPS_Engine_Setting nav_setting) AP_GPS_IMU::init(enum GPS_Engine_Setting nav_setting)
{ {
// we expect the stream to already be open at the corret bitrate // we expect the stream to already be open at the corret bitrate
idleTimeout = 1200; idleTimeout = 1200;
} }
// optimization : This code doesn't wait for data. It only proccess the data available. // 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) // 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. // If we get a complete packet this function calls parse_IMU_gps() to parse and update the GPS info.
bool bool
AP_GPS_IMU::read(void) AP_GPS_IMU::read(void)
{ {
byte data; byte data;
int16_t numc = 0; int16_t numc = 0;
numc = _port->available(); numc = _port->available();
if (numc > 0) { if (numc > 0) {
for (int16_t i=0; i<numc; i++) { // Process bytes received for (int16_t i=0; i<numc; i++) { // Process bytes received
data = _port->read(); data = _port->read();
switch(step) { //Normally we start from zero. This is a state machine switch(step) { //Normally we start from zero. This is a state machine
case 0: case 0:
if(data == 0x44) // IMU sync char 1 if(data == 0x44) // IMU sync char 1
step++; //OH first data packet is correct, so jump to the next step step++; //OH first data packet is correct, so jump to the next step
break; break;
case 1: case 1:
if(data == 0x49) // IMU sync char 2 if(data == 0x49) // IMU sync char 2
step++; //ooh! The second data packet is correct, jump to the step 2 step++; //ooh! The second data packet is correct, jump to the step 2
else else
step=0; //Nop, is not correct so restart to step zero and try again. step=0; //Nop, is not correct so restart to step zero and try again.
break; break;
case 2: case 2:
if(data == 0x59) // IMU sync char 3 if(data == 0x59) // IMU sync char 3
step++; //ooh! The second data packet is correct, jump to the step 2 step++; //ooh! The second data packet is correct, jump to the step 2
else else
step=0; //Nop, is not correct so restart to step zero and try again. step=0; //Nop, is not correct so restart to step zero and try again.
break; break;
case 3: case 3:
if(data == 0x64) // IMU sync char 4 if(data == 0x64) // IMU sync char 4
step++; //ooh! The second data packet is correct, jump to the step 2 step++; //ooh! The second data packet is correct, jump to the step 2
else else
step=0; //Nop, is not correct so restart to step zero and try again. step=0; //Nop, is not correct so restart to step zero and try again.
break; break;
case 4: case 4:
payload_length = data; payload_length = data;
checksum(payload_length); checksum(payload_length);
step++; step++;
if (payload_length > 28) { if (payload_length > 28) {
step = 0; //Bad data, so restart to step zero and try again. step = 0; //Bad data, so restart to step zero and try again.
payload_counter = 0; payload_counter = 0;
ck_a = 0; ck_a = 0;
ck_b = 0; ck_b = 0;
//payload_error_count++; //payload_error_count++;
} }
break; break;
case 5: case 5:
message_num = data; message_num = data;
checksum(data); checksum(data);
step++; step++;
break; break;
case 6: // Payload data read... case 6: // Payload data read...
// We stay in this state until we reach the payload_length // We stay in this state until we reach the payload_length
buffer[payload_counter] = data; buffer[payload_counter] = data;
checksum(data); checksum(data);
payload_counter++; payload_counter++;
if (payload_counter >= payload_length) { if (payload_counter >= payload_length) {
step++; step++;
} }
break; break;
case 7: case 7:
GPS_ck_a = data; // First checksum byte GPS_ck_a = data; // First checksum byte
step++; step++;
break; break;
case 8: case 8:
GPS_ck_b = data; // Second checksum byte GPS_ck_b = data; // Second checksum byte
// We end the IMU/GPS read... // We end the IMU/GPS read...
// Verify the received checksum with the generated checksum.. // Verify the received checksum with the generated checksum..
if((ck_a == GPS_ck_a) && (ck_b == GPS_ck_b)) { if((ck_a == GPS_ck_a) && (ck_b == GPS_ck_b)) {
if (message_num == 0x02) { if (message_num == 0x02) {
join_data(); join_data();
} else if (message_num == 0x03) { } else if (message_num == 0x03) {
GPS_join_data(); GPS_join_data();
} else if (message_num == 0x04) { } else if (message_num == 0x04) {
join_data_xplane(); join_data_xplane();
} else if (message_num == 0x0a) { } else if (message_num == 0x0a) {
//PERF_join_data(); //PERF_join_data();
} else { } else {
// _error("Invalid message number = %d\n", (int)message_num); // _error("Invalid message number = %d\n", (int)message_num);
} }
} else { } else {
// _error("XXX Checksum error\n"); //bad checksum // _error("XXX Checksum error\n"); //bad checksum
//imu_checksum_error_count++; //imu_checksum_error_count++;
} }
// Variable initialization // Variable initialization
step = 0; step = 0;
payload_counter = 0; payload_counter = 0;
ck_a = 0; ck_a = 0;
ck_b = 0; ck_b = 0;
break; break;
} }
}// End for... } // End for...
} }
return true; return true;
} }
/**************************************************************** /****************************************************************
* *
****************************************************************/ ****************************************************************/
void AP_GPS_IMU::join_data(void) 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.. //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. //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 //Storing IMU roll
roll_sensor = *(int16_t *)&buffer[0]; roll_sensor = *(int16_t *)&buffer[0];
//Storing IMU pitch //Storing IMU pitch
pitch_sensor = *(int16_t *)&buffer[2]; pitch_sensor = *(int16_t *)&buffer[2];
//Storing IMU heading (yaw) //Storing IMU heading (yaw)
ground_course = *(int16_t *)&buffer[4]; ground_course = *(int16_t *)&buffer[4];
imu_ok = true; imu_ok = true;
} }
void AP_GPS_IMU::join_data_xplane() void AP_GPS_IMU::join_data_xplane()
{ {
//Storing IMU roll //Storing IMU roll
roll_sensor = *(int16_t *)&buffer[0]; roll_sensor = *(int16_t *)&buffer[0];
//Storing IMU pitch //Storing IMU pitch
pitch_sensor = *(int16_t *)&buffer[2]; pitch_sensor = *(int16_t *)&buffer[2];
//Storing IMU heading (yaw) //Storing IMU heading (yaw)
ground_course = *(uint16_t *)&buffer[4]; ground_course = *(uint16_t *)&buffer[4];
//Storing airspeed //Storing airspeed
airspeed = *(int16_t *)&buffer[6]; airspeed = *(int16_t *)&buffer[6];
imu_ok = true; imu_ok = true;
} }
void AP_GPS_IMU::GPS_join_data(void) void AP_GPS_IMU::GPS_join_data(void)
{ {
longitude = *(int32_t *)&buffer[0]; // degrees * 10e7 longitude = *(int32_t *)&buffer[0]; // degrees * 10e7
latitude = *(int32_t *)&buffer[4]; latitude = *(int32_t *)&buffer[4];
//Storing GPS Height above the sea level //Storing GPS Height above the sea level
altitude = (int32_t)*(int16_t *)&buffer[8] * 10; altitude = (int32_t)*(int16_t *)&buffer[8] * 10;
//Storing Speed //Storing Speed
speed_3d = ground_speed = (float)*(int16_t *)&buffer[10]; speed_3d = ground_speed = (float)*(int16_t *)&buffer[10];
//We skip the gps ground course because we use yaw value from the IMU for ground course //We skip the gps ground course because we use yaw value from the IMU for ground course
time = *(int32_t *)&buffer[14]; time = *(int32_t *)&buffer[14];
imu_health = buffer[15]; imu_health = buffer[15];
new_data = true; new_data = true;
fix = true; fix = true;
} }
/**************************************************************** /****************************************************************
* *
****************************************************************/ ****************************************************************/
// checksum algorithm // checksum algorithm
void AP_GPS_IMU::checksum(unsigned char data) void AP_GPS_IMU::checksum(unsigned char data)
{ {
ck_a += data; ck_a += data;
ck_b += ck_a; ck_b += ck_a;
} }
/**************************************************************** /****************************************************************
* Unused * Unused
****************************************************************/ ****************************************************************/
void AP_GPS_IMU::setHIL(uint32_t _time, float _latitude, float _longitude, float _altitude, void AP_GPS_IMU::setHIL(uint32_t _time, float _latitude, float _longitude, float _altitude,
float _ground_speed, float _ground_course, float _speed_3d, uint8_t _num_sats) {}; float _ground_speed, float _ground_course, float _speed_3d, uint8_t _num_sats) {
};