Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-02 05:17:03 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

remove some of the data sent to arduimu

Browse Source
This commit is contained in:
Gautier Hattenberger
2011-03-05 23:21:23 +01:00
parent 17e8dfaa0d
commit e7acfc6c81
6 changed files with 51 additions and 151 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/ADC.pde
-4
View File
@@ -35,8 +35,6 @@ float read_adc(int select)
#if PRINT_DEBUG != 0 #if PRINT_DEBUG != 0
Serial.print("!!!ADC:1,VAL:"); Serial.print("!!!ADC:1,VAL:");
Serial.print (temp); Serial.print (temp);
Serial.print (",TOW:");
Serial.print (iTOW);
Serial.println("***"); Serial.println("***");
#endif #endif
#if PERFORMANCE_REPORTING == 1 #if PERFORMANCE_REPORTING == 1
@@ -50,8 +48,6 @@ float read_adc(int select)
#if PRINT_DEBUG != 0 #if PRINT_DEBUG != 0
Serial.print("!!!ADC:2,VAL:"); Serial.print("!!!ADC:2,VAL:");
Serial.print (temp); Serial.print (temp);
Serial.print (",TOW:");
Serial.print (iTOW);
Serial.println("***"); Serial.println("***");
#endif #endif
#if PERFORMANCE_REPORTING == 1 #if PERFORMANCE_REPORTING == 1
sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/DCM.pde
-14
View File
@@ -29,8 +29,6 @@ void Normalize(void)
Serial.print (renorm); Serial.print (renorm);
Serial.print (",ERR:"); Serial.print (",ERR:");
Serial.print (error); Serial.print (error);
Serial.print (",TOW:");
Serial.print (iTOW);
Serial.println("***"); Serial.println("***");
#endif #endif
} else { } else {
@@ -43,8 +41,6 @@ void Normalize(void)
Serial.print (renorm); Serial.print (renorm);
Serial.print (",ERR:"); Serial.print (",ERR:");
Serial.print (error); Serial.print (error);
Serial.print (",TOW:");
Serial.print (iTOW);
Serial.println("***"); Serial.println("***");
#endif #endif
} }
@@ -63,8 +59,6 @@ void Normalize(void)
Serial.print (renorm); Serial.print (renorm);
Serial.print (",ERR:"); Serial.print (",ERR:");
Serial.print (error); Serial.print (error);
Serial.print (",TOW:");
Serial.print (iTOW);
Serial.println("***"); Serial.println("***");
#endif #endif
} else { } else {
@@ -77,8 +71,6 @@ void Normalize(void)
Serial.print (renorm); Serial.print (renorm);
Serial.print (",ERR:"); Serial.print (",ERR:");
Serial.print (error); Serial.print (error);
Serial.print (",TOW:");
Serial.print (iTOW);
Serial.println("***"); Serial.println("***");
#endif #endif
} }
@@ -97,8 +89,6 @@ void Normalize(void)
Serial.print (renorm); Serial.print (renorm);
Serial.print (",ERR:"); Serial.print (",ERR:");
Serial.print (error); Serial.print (error);
Serial.print (",TOW:");
Serial.print (iTOW);
Serial.println("***"); Serial.println("***");
#endif #endif
} else { } else {
@@ -109,8 +99,6 @@ void Normalize(void)
#if PRINT_DEBUG != 0 #if PRINT_DEBUG != 0
Serial.print("???PRB:3,RNM:"); Serial.print("???PRB:3,RNM:");
Serial.print (renorm); Serial.print (renorm);
Serial.print (",TOW:");
Serial.print (iTOW);
Serial.println("***"); Serial.println("***");
#endif #endif
} }
@@ -201,8 +189,6 @@ void Drift_correction(void)
Serial.print("!!!INT:1,MAG:"); Serial.print("!!!INT:1,MAG:");
Serial.print (ToDeg(Integrator_magnitude)); Serial.print (ToDeg(Integrator_magnitude));
Serial.print (",TOW:");
Serial.print (iTOW);
Serial.println("***"); Serial.println("***");
#endif #endif
} }
sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/GPS_UBLOX.pde
+28 -64
View File
@@ -56,76 +56,40 @@ void parse_ubx_gps(){
gps_pos_fix_count++; gps_pos_fix_count++;
#endif #endif
messageNr = Paparazzi_GPS_buffer[0]; speed_3d = (float)join_4_bytes(&Paparazzi_GPS_buffer[0])/100.0; // m/s 0,1,2,3
ground_speed = (float)join_4_bytes(&Paparazzi_GPS_buffer[4])/100.0; // Ground speed 2D 4,5,6,7
ground_course = (float)join_4_bytes(&Paparazzi_GPS_buffer[8])/100000.0; // Heading 2D 8,9,10,11
alt = join_4_bytes(&Paparazzi_GPS_buffer[12]); // 12,13,14,15
alt_MSL = join_4_bytes(&Paparazzi_GPS_buffer[16]); // 16,17,18,19
stGpsFix = Paparazzi_GPS_buffer[20];
stFlags = Paparazzi_GPS_buffer[21];
if(messageNr == 0x00){ if((stGpsFix >= 0x03)&&(stFlags&0x01)){
// Message 0 Bytes: gpsFix=0; //valid position
iTOW2 = join_4_bytes(&Paparazzi_GPS_buffer[1]); //1,2,3,4 digitalWrite(6,HIGH); //Turn LED when gps is fixed.
lon2 = join_4_bytes(&Paparazzi_GPS_buffer[5]); //5,6,7,8 GPS_timer=DIYmillis(); //Restarting timer...
lat2 = join_4_bytes(&Paparazzi_GPS_buffer[9]); //9,10,11,12 }
alt2 = join_4_bytes(&Paparazzi_GPS_buffer[13]); //13,14,15,16 else{
alt_MSL2 = join_4_bytes(&Paparazzi_GPS_buffer[17]); // 17,18,19,20 gpsFix=1; //invalid position
recPakOne=0x01; digitalWrite(6,LOW);
} }
if (ground_speed > SPEEDFILT && gpsFix==0) gc_offset = ground_course - ToDeg(yaw);
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 #if 0
// Serial.print("Time von Arduino ;"); // Serial.print("Time von Arduino ;");
// Serial.print(millis()); // Serial.print(millis());
Serial.print("MesageNr: "); Serial.print("alt ;");
Serial.print((int)(messageNr)); Serial.print(alt);
Serial.print("; itow ;"); Serial.print("; alt_MSL: ;");
Serial.print(iTOW); Serial.print(alt_MSL);
Serial.print("; lon ;"); Serial.print("; speed_3d ;");
Serial.print(lon); Serial.print(speed_3d);
Serial.print("; lat ;"); Serial.print("; ground_speed ;");
Serial.print(lat); Serial.print(ground_speed);
Serial.print("; alt ;"); Serial.print("; ground_course ;");
Serial.print(alt); Serial.print(ground_course);
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 #endif
}
} }
sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/Output.pde
+4 -12
View File
@@ -197,10 +197,6 @@ void printdata(void){
#if PRINT_GPS == 1 #if PRINT_GPS == 1
if(gpsFixnew==1) { if(gpsFixnew==1) {
gpsFixnew=0; gpsFixnew=0;
Serial.print("LAT:");
Serial.print(lat);
Serial.print(",LON:");
Serial.print(lon);
Serial.print(",ALT:"); Serial.print(",ALT:");
Serial.print(alt_MSL/1000); // meters Serial.print(alt_MSL/1000); // meters
Serial.print(",COG:"); Serial.print(",COG:");
@@ -216,10 +212,6 @@ void printdata(void){
} }
#endif #endif
//Serial.print("TOW:");
//Serial.print(iTOW);
//Serial.println("***");
#else #else
// This section outputs binary data messages // This section outputs binary data messages
// Conforms to new binary message standard (12/31/09) // Conforms to new binary message standard (12/31/09)
@@ -270,10 +262,10 @@ void printdata(void){
IMU_buffer[14]=tempint&0xff; IMU_buffer[14]=tempint&0xff;
IMU_buffer[15]=(tempint>>8)&0xff; IMU_buffer[15]=(tempint>>8)&0xff;
IMU_buffer[16]=iTOW&0xff; IMU_buffer[16]=0;
IMU_buffer[17]=(iTOW>>8)&0xff; IMU_buffer[17]=0;
IMU_buffer[18]=(iTOW>>16)&0xff; IMU_buffer[18]=0;
IMU_buffer[19]=(iTOW>>24)&0xff; IMU_buffer[19]=0;
IMU_buffer[20]=(imu_health>>8)&0xff; IMU_buffer[20]=(imu_health>>8)&0xff;
sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/arduimu_modified.pde
-14
View File
@@ -181,10 +181,7 @@ union int_union {
int gpsFix=1; //This variable store the status of the GPS int gpsFix=1; //This variable store the status of the GPS
int gpsFixnew=0; //used to flag when new gps data received - used for binary output message flags int gpsFixnew=0; //used to flag when new gps data received - used for binary output message flags
int gps_fix_count = 5; //used to count 5 good fixes at ground startup int gps_fix_count = 5; //used to count 5 good fixes at ground startup
long lat=0; // store the Latitude from the gps to pass to output
long lon=0; // Store the Longitude from the gps to pass to output
long alt_MSL=0; //This is the altitude in millimeters long alt_MSL=0; //This is the altitude in millimeters
long iTOW=0; //GPS Millisecond Time of Week
long alt=0; //Height above Ellipsoid in millimeters long alt=0; //Height above Ellipsoid in millimeters
float speed_3d=0; //Speed (3-D) 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_speed=0;// This is the velocity your "plane" is traveling in meters for second, 1Meters/Second= 3.6Km/H = 1.944 knots
@@ -192,17 +189,6 @@ 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) float gc_offset = 0; // Force yaw output to ground course when fresh data available (only implemented for ublox&binary message)
unsigned long GPS_timer=0; unsigned long GPS_timer=0;
// übergnagsvariablen für die GPS Werte zwischen zu speichern
long iTOW2=0; //GPS Millisecond Time of Week
long lon2=0; // Store the Longitude from the gps to pass to output
long lat2=0; // store the Latitude from the gps to pass to output
long alt2=0; //Height above Ellipsoid in millimeters
long alt_MSL2=0; //This is the altitude in millimeters
float speed_3d2=0; //Speed (3-D)
float ground_speed2=0;// This is the velocity your "plane" is traveling in meters for second, 1Meters/Second= 3.6Km/H = 1.944 knots
byte recPakOne = 0x00; // Paket eins Noch nicht empfangen
#if GPS_PROTOCOL == 3 #if GPS_PROTOCOL == 3
// GPS UBLOX // GPS UBLOX
byte ck_a=0; // Packet checksum byte ck_a=0; // Packet checksum
sw/airborne/modules/ins/ins_arduimu_modified.c
+18 -42
View File
@@ -60,10 +60,6 @@ void ArduIMU_init( void ) {
ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT; ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
} }
#define GPS_DATA_MSG1 0
#define GPS_DATA_MSG2 1
#define FillBufWith32bit(_buf, _index, _value) { \ #define FillBufWith32bit(_buf, _index, _value) { \
_buf[_index] = (uint8_t) (_value); \ _buf[_index] = (uint8_t) (_value); \
_buf[_index+1] = (uint8_t) ((_value) >> 8); \ _buf[_index+1] = (uint8_t) ((_value) >> 8); \
@@ -71,49 +67,29 @@ void ArduIMU_init( void ) {
_buf[_index+3] = (uint8_t) ((_value) >> 24); \ _buf[_index+3] = (uint8_t) ((_value) >> 24); \
} }
void ArduIMU_periodicGPS( void ) { void ArduIMU_periodicGPS( void ) {
static uint8_t gps_data_status = GPS_DATA_MSG1;
if (ardu_gps_trans.status != I2CTransDone) { return; } if (ardu_gps_trans.status != I2CTransDone) { return; }
if ( gps_data_status == GPS_DATA_MSG1 ) { //velned
//posllh GPS_Data [0] = gps_speed_3d; //speed 3D
GPS_Data [0] = gps_itow; GPS_Data [1] = gps_gspeed; //ground speed
GPS_Data [1] = gps_lon; GPS_Data [2] = gps_course * 100000; //Kurs
GPS_Data [2] = gps_lat; //alt
GPS_Data [3] = gps_alt; // height above elipsoid GPS_Data [3] = gps_alt; // height above elipsoid
GPS_Data [4] = gps_hmsl; // height above sea level GPS_Data [4] = gps_hmsl; // height above sea level
//velned //status
GPS_Data [5] = gps_speed_3d; //speed 3D GPS_Data [5] = gps_mode; //fix
GPS_Data [6] = gps_gspeed; //ground speed GPS_Data [6] = gps_status_flags; //flags
GPS_Data [7] = gps_course * 100000; //Kurs
//status
GPS_Data [8] = gps_mode; //fix
GPS_Data [9] = gps_status_flags; //flags
ardu_gps_trans.buf[0] = 0; //message Nr = 0 FillBufWith32bit(ardu_gps_trans.buf, 0, GPS_Data[0]);
FillBufWith32bit(ardu_gps_trans.buf, 1, GPS_Data[0]); // itow FillBufWith32bit(ardu_gps_trans.buf, 4, GPS_Data[1]);
FillBufWith32bit(ardu_gps_trans.buf, 5, GPS_Data[1]); // lon FillBufWith32bit(ardu_gps_trans.buf, 8, GPS_Data[2]);
FillBufWith32bit(ardu_gps_trans.buf, 9, GPS_Data[2]); // lat FillBufWith32bit(ardu_gps_trans.buf, 12, GPS_Data[3]);
FillBufWith32bit(ardu_gps_trans.buf, 13, GPS_Data[3]); // alt FillBufWith32bit(ardu_gps_trans.buf, 16, GPS_Data[4]);
FillBufWith32bit(ardu_gps_trans.buf, 17, GPS_Data[4]); // hmsl ardu_gps_trans.buf[20] = GPS_Data[5]; // status gps fix
I2CTransmit(ARDUIMU_I2C_DEV, ardu_gps_trans, ArduIMU_SLAVE_ADDR, 21); ardu_gps_trans.buf[21] = GPS_Data[6]; // status flags
I2CTransmit(ARDUIMU_I2C_DEV, ardu_gps_trans, ArduIMU_SLAVE_ADDR, 22);
gps_data_status = GPS_DATA_MSG2;
}
else {
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;
}
} }