Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-31 20:38:27 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

clean arduino driver and firmware on modified version

Browse Source
This commit is contained in:
Gautier Hattenberger
2011-03-02 17:18:53 +01:00
parent 39eca56ab5
commit 17e8dfaa0d
5 changed files with 143 additions and 200 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/GPS_UBLOX.pde
+75 -96
View File
@@ -39,13 +39,11 @@ void init_gps(void)
****************************************************************/
void decode_gps(void){
if(DIYmillis() - GPS_timer > 2000){
digitalWrite(6, LOW); //If we don't receive any byte in two seconds turn off gps fix LED...
debug_print("Yeah, your GPS is disconnected");
gpsFix=1;
digitalWrite(6, LOW); //If we don't receive any byte in two seconds turn off gps fix LED...
debug_print("Yeah, your GPS is disconnected");
gpsFix=1;
}
}
@@ -53,101 +51,82 @@ void decode_gps(void){
*
****************************************************************/
void parse_ubx_gps(){
#if PERFORMANCE_REPORTING == 1
gps_pos_fix_count++;
#endif
messageNr = Paparazzi_GPS_buffer[0];
if(messageNr == 0x00){
//Nachricht 0 Bytes:
iTOW2 = join_4_bytes(&Paparazzi_GPS_buffer[1]); //1,2,3,4
lon2 = join_4_bytes(&Paparazzi_GPS_buffer[5]); //5,6,7,8
lat2 = join_4_bytes(&Paparazzi_GPS_buffer[9]); //9,10,11,12
alt2 = join_4_bytes(&Paparazzi_GPS_buffer[13]); //13,14,15,16
alt_MSL2 = join_4_bytes(&Paparazzi_GPS_buffer[17]); // 17,18,19,20
speed_3d2 = (float)join_4_bytes(&Paparazzi_GPS_buffer[21])/100.0; // m/s 21,22,23,24
ground_speed2 = (float)join_4_bytes(&Paparazzi_GPS_buffer[25])/100.0; // Ground speed 2D 25,26,27,28 29,30 31
recPakOne=0x01;
}
#if PERFORMANCE_REPORTING == 1
gps_pos_fix_count++;
#endif
messageNr = Paparazzi_GPS_buffer[0];
if(messageNr == 0x00){
// Message 0 Bytes:
iTOW2 = join_4_bytes(&Paparazzi_GPS_buffer[1]); //1,2,3,4
lon2 = join_4_bytes(&Paparazzi_GPS_buffer[5]); //5,6,7,8
lat2 = join_4_bytes(&Paparazzi_GPS_buffer[9]); //9,10,11,12
alt2 = join_4_bytes(&Paparazzi_GPS_buffer[13]); //13,14,15,16
alt_MSL2 = join_4_bytes(&Paparazzi_GPS_buffer[17]); // 17,18,19,20
recPakOne=0x01;
}
if(messageNr == 0x01 && recPakOne==0x01){
// Nachricht 1
ground_course = (float)join_4_bytes(&Paparazzi_GPS_buffer[1])/100000.0; // Heading 2D 1,2,3,4
ecefVZ=(float)join_4_bytes(&Paparazzi_GPS_buffer[5])/100; //Vertical Speed 5,6,7,8
numSV=Paparazzi_GPS_buffer[9]; //Number of sats... 9
stGpsFix=Paparazzi_GPS_buffer[10];
stFlags=Paparazzi_GPS_buffer[11];
solGpsFix=Paparazzi_GPS_buffer[12];
solFlags=Paparazzi_GPS_buffer[13];
iTOW = iTOW2;
lon = lon2;
lat = lat2;
alt = alt2;
alt_MSL = alt_MSL2;
speed_3d = speed_3d2;
ground_speed = ground_speed2;
messageNr=messageNr+1; //2
}
if(messageNr == 0x02){
if((stGpsFix >= 0x03)&&(stFlags&0x01)){
gpsFix=0; //valid position
digitalWrite(6,HIGH); //Turn LED when gps is fixed.
GPS_timer=DIYmillis(); //Restarting timer...
}
else{
gpsFix=1; //invalid position
digitalWrite(6,LOW);
}
if((solGpsFix >= 0x03)&&(solFlags&0x01)){
gpsFix=0; //valid position
digitalWrite(6,HIGH); //Turn LED when gps is fixed.
GPS_timer=DIYmillis(); //Restarting timer...
}
else{
gpsFix=1; //invalid position
digitalWrite(6,LOW);
}
if (ground_speed > SPEEDFILT && gpsFix==0) gc_offset = ground_course - ToDeg(yaw);
recPakOne=0x00;
//messageNr= 0x05; // kommt so nicht mehr in die Abfage !!!
if(messageNr == 0x01 && recPakOne==0x01){
// Message 1
speed_3d2 = (float)join_4_bytes(&Paparazzi_GPS_buffer[1])/100.0; // m/s 1,2,3,4
ground_speed2 = (float)join_4_bytes(&Paparazzi_GPS_buffer[25])/100.0; // Ground speed 2D 5,6,7,8
ground_course = (float)join_4_bytes(&Paparazzi_GPS_buffer[1])/100000.0; // Heading 2D 9,10,11,12
stGpsFix=Paparazzi_GPS_buffer[13];
stFlags=Paparazzi_GPS_buffer[14];
iTOW = iTOW2;
lon = lon2;
lat = lat2;
alt = alt2;
alt_MSL = alt_MSL2;
speed_3d = speed_3d2;
ground_speed = ground_speed2;
messageNr=messageNr+1; //2
}
if(messageNr == 0x02){
if((stGpsFix >= 0x03)&&(stFlags&0x01)){
gpsFix=0; //valid position
digitalWrite(6,HIGH); //Turn LED when gps is fixed.
GPS_timer=DIYmillis(); //Restarting timer...
}
else{
gpsFix=1; //invalid position
digitalWrite(6,LOW);
}
if (ground_speed > SPEEDFILT && gpsFix==0) gc_offset = ground_course - ToDeg(yaw);
recPakOne=0x00;
#if 0
// Serial.print("Time von Arduino ;");
// Serial.print(millis());
Serial.print("MesageNr: ");
Serial.print((int)(messageNr));
Serial.print("; itow ;");
Serial.print(iTOW);
Serial.print("; lon ;");
Serial.print(lon);
Serial.print("; lat ;");
Serial.print(lat);
Serial.print("; alt ;");
Serial.print(alt);
Serial.print("; alt_MSL: ;");
Serial.print(alt_MSL);
Serial.print("; speed_3d ;");
Serial.print(speed_3d);
Serial.print("; ground_speed ;");
Serial.print(ground_speed);
Serial.print("; ground_course ;");
Serial.print(ground_course);
Serial.print("; ecefVZ ;");
Serial.print(ecefVZ);
Serial.print("; numSV ;");
Serial.println((int)(numSV));
// Serial.print("Time von Arduino ;");
// Serial.print(millis());
Serial.print("MesageNr: ");
Serial.print((int)(messageNr));
Serial.print("; itow ;");
Serial.print(iTOW);
Serial.print("; lon ;");
Serial.print(lon);
Serial.print("; lat ;");
Serial.print(lat);
Serial.print("; alt ;");
Serial.print(alt);
Serial.print("; alt_MSL: ;");
Serial.print(alt_MSL);
Serial.print("; speed_3d ;");
Serial.print(speed_3d);
Serial.print("; ground_speed ;");
Serial.print(ground_speed);
Serial.print("; ground_course ;");
Serial.print(ground_course);
#endif
}
}
}
sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/Output.pde
+10 -12
View File
@@ -14,17 +14,18 @@ void requestEvent(){
I2C_Message_ar[3] = int(Gyro_Vector[0]*(1<<12));
I2C_Message_ar[4] = int(Gyro_Vector[1]*(1<<12));
I2C_Message_ar[5] = int(Gyro_Vector[2]*(1<<12));
I2C_Message_ar[6] = int(Accel_Vector[0]*(1<<10));
I2C_Message_ar[7] = int(Accel_Vector[1]*(1<<10));
I2C_Message_ar[8] = int(Accel_Vector[2]*(1<<10));
I2C_Message_ar[6] = int((9.81*Accel_Vector[0]/GRAVITY)*(1<<10));
I2C_Message_ar[7] = int((9.81*Accel_Vector[1]/GRAVITY)*(1<<10));
I2C_Message_ar[8] = int((9.81*Accel_Vector[2]/GRAVITY)*(1<<10));
byte* pointer;
pointer = (byte*) &I2C_Message_ar;
Wire.send(pointer, 12);
Wire.send(pointer, 18);
/* Uncomment for debug on serial link */
/*
//Serial.println();
/*
Serial.print("Time ;");
Serial.print(millis());
Serial.print("; Roll ;");
@@ -46,6 +47,7 @@ void requestEvent(){
Serial.print("; ACCZ ;");
Serial.println(I2C_Message_ar[8]);
*/
}
//********GPS Data from PAPArazzi UBLOX**********************************************************************
@@ -80,9 +82,9 @@ void printdata(void){
I2C_Message_ar[3] = int(Gyro_Vector[0]*(1<<12));
I2C_Message_ar[4] = int(Gyro_Vector[1]*(1<<12));
I2C_Message_ar[5] = int(Gyro_Vector[2]*(1<<12));
I2C_Message_ar[6] = int(Accel_Vector[0]*(1<<10));
I2C_Message_ar[7] = int(Accel_Vector[1]*(1<<10));
I2C_Message_ar[8] = int(Accel_Vector[2]*(1<<10));
I2C_Message_ar[6] = int((9.81*Accel_Vector[0]/GRAVITY)*(1<<10));
I2C_Message_ar[7] = int((9.81*Accel_Vector[1]/GRAVITY)*(1<<10));
I2C_Message_ar[8] = int((9.81*Accel_Vector[2]/GRAVITY)*(1<<10));
Serial.println();
Serial.print("Time ;");
@@ -207,10 +209,6 @@ void printdata(void){
Serial.print(ground_speed);
Serial.print(",FIX:");
Serial.print((int)gpsFix);
Serial.print(",EVZ:"); //Vertical Speed
Serial.print(ecefVZ);
Serial.print(",SAT:");
Serial.print((int)numSV);
Serial.print (",");
#if PERFORMANCE_REPORTING == 1
gps_messages_sent++;
sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/arduimu_modified.pde
-5
View File
@@ -190,8 +190,6 @@ float speed_3d=0; //Speed (3-D)
float ground_speed=0;// This is the velocity your "plane" is traveling in meters for second, 1Meters/Second= 3.6Km/H = 1.944 knots
float ground_course=90;//This is the runaway direction of you "plane" in degrees
float gc_offset = 0; // Force yaw output to ground course when fresh data available (only implemented for ublox&binary message)
byte numSV=0; //Number of Sats used.
float ecefVZ=0; //Vertical Speed in m/s
unsigned long GPS_timer=0;
// übergnagsvariablen für die GPS Werte zwischen zu speichern
@@ -227,10 +225,7 @@ byte Paparazzi_GPS_buffer[UBX_MAXPAYLOAD];
int gpsDataReady = 0; // sind neuen GPS daten vorhanden ??
byte stGpsFix;
byte stFlags;
byte solGpsFix;
byte solFlags;
byte messageNr;
#endif
//************************************************************************************************************
sw/airborne/modules/ins/ins_arduimu_modified.c
+54 -80
View File
@@ -15,7 +15,9 @@ Autoren@ZHAW: schmiemi
// für das Senden von GPS-Daten an den ArduIMU
#include "gps.h"
int32_t GPS_Data[14];
int32_t GPS_Data[10];
#define NB_DATA 9
#ifndef ARDUIMU_I2C_DEV
#define ARDUIMU_I2C_DEV i2c0
@@ -38,12 +40,19 @@ struct i2c_transaction ardu_gps_trans;
struct i2c_transaction ardu_ins_trans;
static int16_t recievedData[NB_DATA];
float ArduIMU_data[NB_DATA];
struct FloatEulers arduimu_eulers;
struct FloatRates arduimu_rates;
struct FloatVect3 arduimu_accel;
float ins_roll_neutral;
float ins_pitch_neutral;
void ArduIMU_init( void ) {
FLOAT_EULERS_ZERO(arduimu_eulers);
FLOAT_RATES_ZERO(arduimu_rates);
FLOAT_VECT3_ZERO(arduimu_accel);
ardu_ins_trans.status = I2CTransDone;
ardu_gps_trans.status = I2CTransDone;
@@ -55,6 +64,14 @@ void ArduIMU_init( void ) {
#define GPS_DATA_MSG1 0
#define GPS_DATA_MSG2 1
#define FillBufWith32bit(_buf, _index, _value) { \
_buf[_index] = (uint8_t) (_value); \
_buf[_index+1] = (uint8_t) ((_value) >> 8); \
_buf[_index+2] = (uint8_t) ((_value) >> 16); \
_buf[_index+3] = (uint8_t) ((_value) >> 24); \
}
void ArduIMU_periodicGPS( void ) {
static uint8_t gps_data_status = GPS_DATA_MSG1;
@@ -65,74 +82,35 @@ void ArduIMU_periodicGPS( void ) {
GPS_Data [0] = gps_itow;
GPS_Data [1] = gps_lon;
GPS_Data [2] = gps_lat;
GPS_Data [3] = gps_alt; //höhe über elipsoid
GPS_Data [4] = gps_hmsl; //höhe über sea level
GPS_Data [3] = gps_alt; // height above elipsoid
GPS_Data [4] = gps_hmsl; // height above sea level
//velned
GPS_Data [5] = gps_speed_3d; //speed 3D
GPS_Data [6] = gps_gspeed; //ground speed
GPS_Data [5] = gps_speed_3d; //speed 3D
GPS_Data [6] = gps_gspeed; //ground speed
GPS_Data [7] = gps_course * 100000; //Kurs
//status
GPS_Data [8] = gps_mode; //fix
GPS_Data [9] = gps_status_flags; //flags
//sol
GPS_Data [10] = gps_mode; //fix
GPS_Data [11] = gps_sol_flags; //flags
GPS_Data [12] = gps_ecefVZ; //ecefVZ
GPS_Data [13] = gps_numSV;
GPS_Data [8] = gps_mode; //fix
GPS_Data [9] = gps_status_flags; //flags
//test für 32bit in byte packete abzupacken:
//GPS_Data [0] = -1550138773;
ardu_gps_trans.buf[0] = 0; //message Nr = 0 --> itow bis ground speed
ardu_gps_trans.buf[1] = (uint8_t) GPS_Data[0]; //itow
ardu_gps_trans.buf[2] = (uint8_t) (GPS_Data[0] >>8);
ardu_gps_trans.buf[3] = (uint8_t) (GPS_Data[0] >>16);
ardu_gps_trans.buf[4] = (uint8_t) (GPS_Data[0] >>24);
ardu_gps_trans.buf[5] = (uint8_t) GPS_Data[1]; //lon
ardu_gps_trans.buf[6] = (uint8_t) (GPS_Data[1] >>8);
ardu_gps_trans.buf[7] = (uint8_t) (GPS_Data[1] >>16);
ardu_gps_trans.buf[8] = (uint8_t) (GPS_Data[1] >>24);
ardu_gps_trans.buf[9] = (uint8_t) GPS_Data[2]; //lat
ardu_gps_trans.buf[10] = (uint8_t) (GPS_Data[2] >>8);
ardu_gps_trans.buf[11] = (uint8_t) (GPS_Data[2] >>16);
ardu_gps_trans.buf[12] = (uint8_t) (GPS_Data[2] >>24);
ardu_gps_trans.buf[13] = (uint8_t) GPS_Data[3]; //height
ardu_gps_trans.buf[14] = (uint8_t) (GPS_Data[3] >>8);
ardu_gps_trans.buf[15] = (uint8_t) (GPS_Data[3] >>16);
ardu_gps_trans.buf[16] = (uint8_t) (GPS_Data[3] >>24);
ardu_gps_trans.buf[17] = (uint8_t) GPS_Data[4]; //hmsl
ardu_gps_trans.buf[18] = (uint8_t) (GPS_Data[4] >>8);
ardu_gps_trans.buf[19] = (uint8_t) (GPS_Data[4] >>16);
ardu_gps_trans.buf[20] = (uint8_t) (GPS_Data[4] >>24);
ardu_gps_trans.buf[21] = (uint8_t) GPS_Data[5]; //speed
ardu_gps_trans.buf[22] = (uint8_t) (GPS_Data[5] >>8);
ardu_gps_trans.buf[23] = (uint8_t) (GPS_Data[5] >>16);
ardu_gps_trans.buf[24] = (uint8_t) (GPS_Data[5] >>24);
ardu_gps_trans.buf[25] = (uint8_t) GPS_Data[6]; //gspeed
ardu_gps_trans.buf[26] = (uint8_t) (GPS_Data[6] >>8);
ardu_gps_trans.buf[27] = (uint8_t) (GPS_Data[6] >>16);
ardu_gps_trans.buf[28] = (uint8_t) (GPS_Data[6] >>24);
I2CTransmit(ARDUIMU_I2C_DEV, ardu_gps_trans, ArduIMU_SLAVE_ADDR, 28);
ardu_gps_trans.buf[0] = 0; //message Nr = 0
FillBufWith32bit(ardu_gps_trans.buf, 1, GPS_Data[0]); // itow
FillBufWith32bit(ardu_gps_trans.buf, 5, GPS_Data[1]); // lon
FillBufWith32bit(ardu_gps_trans.buf, 9, GPS_Data[2]); // lat
FillBufWith32bit(ardu_gps_trans.buf, 13, GPS_Data[3]); // alt
FillBufWith32bit(ardu_gps_trans.buf, 17, GPS_Data[4]); // hmsl
I2CTransmit(ARDUIMU_I2C_DEV, ardu_gps_trans, ArduIMU_SLAVE_ADDR, 21);
gps_data_status = GPS_DATA_MSG2;
}
else {
ardu_gps_trans.buf[0] = 1; //message Nr = 1 --> ground course, ecefVZ, numSV, Fix, flags, fix, flags
ardu_gps_trans.buf[1] = GPS_Data[7]; //ground course
ardu_gps_trans.buf[2] = (GPS_Data[7] >>8);
ardu_gps_trans.buf[3] = (GPS_Data[7] >>16);
ardu_gps_trans.buf[4] = (GPS_Data[7] >>24);
ardu_gps_trans.buf[5] = GPS_Data[12]; //ecefVZ
ardu_gps_trans.buf[6] = (GPS_Data[12] >>8);
ardu_gps_trans.buf[7] = (GPS_Data[12] >>16);
ardu_gps_trans.buf[8] = (GPS_Data[12] >>24);
ardu_gps_trans.buf[9] = GPS_Data[13]; //numSV
ardu_gps_trans.buf[10] = GPS_Data[8]; //status gps fix
ardu_gps_trans.buf[11] = GPS_Data[9]; //status flags
ardu_gps_trans.buf[12] = GPS_Data[10]; //sol gps fix
ardu_gps_trans.buf[13] = GPS_Data[11]; //sol flags
I2CTransmit(ARDUIMU_I2C_DEV, ardu_gps_trans, ArduIMU_SLAVE_ADDR, 13);
ardu_gps_trans.buf[0] = 1; //message Nr = 1
FillBufWith32bit(ardu_gps_trans.buf, 1, GPS_Data[5]); // speed_3d
FillBufWith32bit(ardu_gps_trans.buf, 5, GPS_Data[6]); // gspeed
FillBufWith32bit(ardu_gps_trans.buf, 9, GPS_Data[7]); // course
ardu_gps_trans.buf[13] = GPS_Data[8]; // status gps fix
ardu_gps_trans.buf[14] = GPS_Data[9]; // status flags
I2CTransmit(ARDUIMU_I2C_DEV, ardu_gps_trans, ArduIMU_SLAVE_ADDR, 15);
gps_data_status = GPS_DATA_MSG1;
}
@@ -177,29 +155,25 @@ void ArduIMU_event( void ) {
recievedData[8] = (ardu_ins_trans.buf[17]<<8) | ardu_ins_trans.buf[16];
// Update ArduIMU data
ArduIMU_data[0] = ANGLE_FLOAT_OF_BFP(recievedData[0]);
ArduIMU_data[1] = ANGLE_FLOAT_OF_BFP(recievedData[1]);
ArduIMU_data[2] = ANGLE_FLOAT_OF_BFP(recievedData[2]);
ArduIMU_data[3] = RATE_FLOAT_OF_BFP(recievedData[3]);
ArduIMU_data[4] = RATE_FLOAT_OF_BFP(recievedData[4]);
ArduIMU_data[5] = RATE_FLOAT_OF_BFP(recievedData[5]);
ArduIMU_data[6] = ACCEL_FLOAT_OF_BFP(recievedData[6]);
ArduIMU_data[7] = ACCEL_FLOAT_OF_BFP(recievedData[7]);
ArduIMU_data[8] = ACCEL_FLOAT_OF_BFP(recievedData[8]);
arduimu_eulers.phi = ANGLE_FLOAT_OF_BFP(recievedData[0]);
arduimu_eulers.theta = ANGLE_FLOAT_OF_BFP(recievedData[1]);
arduimu_eulers.psi = ANGLE_FLOAT_OF_BFP(recievedData[2]);
arduimu_rates.p = RATE_FLOAT_OF_BFP(recievedData[3]);
arduimu_rates.q = RATE_FLOAT_OF_BFP(recievedData[4]);
arduimu_rates.r = RATE_FLOAT_OF_BFP(recievedData[5]);
arduimu_accel.x = ACCEL_FLOAT_OF_BFP(recievedData[6]);
arduimu_accel.y = ACCEL_FLOAT_OF_BFP(recievedData[7]);
arduimu_accel.z = ACCEL_FLOAT_OF_BFP(recievedData[8]);
// Update estimator
estimator_phi = ArduIMU_data[0] - ins_roll_neutral;
estimator_theta = ArduIMU_data[1] - ins_pitch_neutral;
estimator_p = ArduIMU_data[3];
//imu_daten_angefordert = FALSE;
estimator_phi = arduimu_eulers.phi - ins_roll_neutral;
estimator_theta = arduimu_eulers.theta - ins_pitch_neutral;
estimator_p = arduimu_rates.p;
ardu_ins_trans.status = I2CTransDone;
{
float psi=0;
float ax=ArduIMU_data[6];
RunOnceEvery(15, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, &estimator_phi, &estimator_theta, &psi));
RunOnceEvery(15, DOWNLINK_SEND_IMU_ACCEL(DefaultChannel, &ax, &(ArduIMU_data[7]), &(ArduIMU_data[8])));
}
RunOnceEvery(15, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, &arduimu_eulers.phi, &arduimu_eulers.theta, &arduimu_eulers.psi));
RunOnceEvery(15, DOWNLINK_SEND_IMU_GYRO(DefaultChannel, &arduimu_rates.p, &arduimu_rates.q, &arduimu_rates.r));
RunOnceEvery(15, DOWNLINK_SEND_IMU_ACCEL(DefaultChannel, &arduimu_accel.x, &arduimu_accel.y, &arduimu_accel.z));
}
else if (ardu_ins_trans.status == I2CTransFailed) {
ardu_ins_trans.status = I2CTransDone;
sw/airborne/modules/ins/ins_arduimu_modified.h
+4 -7
View File
@@ -2,18 +2,15 @@
#define ArduIMU_H
#include <inttypes.h>
#include "math/pprz_algebra_float.h"
#define NB_DATA 9
extern float ArduIMU_data[NB_DATA];
extern struct FloatEulers arduimu_eulers;
extern struct FloatRates arduimu_rates;
extern struct FloatVect3 arduimu_accel;
extern float ins_roll_neutral;
extern float ins_pitch_neutral;
//mixer
extern float pitch_of_throttle_gain;
extern float throttle_slew;
void ArduIMU_init( void );
void ArduIMU_periodic( void );
void ArduIMU_periodicGPS( void );