diff --git a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/ADC.pde b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/ADC.pde index 6f78acfdf1..3d4728d168 100644 --- a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/ADC.pde +++ b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/ADC.pde @@ -35,8 +35,6 @@ float read_adc(int select) #if PRINT_DEBUG != 0 Serial.print("!!!ADC:1,VAL:"); Serial.print (temp); - Serial.print (",TOW:"); - Serial.print (iTOW); Serial.println("***"); #endif #if PERFORMANCE_REPORTING == 1 @@ -50,8 +48,6 @@ float read_adc(int select) #if PRINT_DEBUG != 0 Serial.print("!!!ADC:2,VAL:"); Serial.print (temp); - Serial.print (",TOW:"); - Serial.print (iTOW); Serial.println("***"); #endif #if PERFORMANCE_REPORTING == 1 diff --git a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/DCM.pde b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/DCM.pde index 658a4e48a6..0583854932 100644 --- a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/DCM.pde +++ b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/DCM.pde @@ -29,8 +29,6 @@ void Normalize(void) Serial.print (renorm); Serial.print (",ERR:"); Serial.print (error); - Serial.print (",TOW:"); - Serial.print (iTOW); Serial.println("***"); #endif } else { @@ -43,8 +41,6 @@ void Normalize(void) Serial.print (renorm); Serial.print (",ERR:"); Serial.print (error); - Serial.print (",TOW:"); - Serial.print (iTOW); Serial.println("***"); #endif } @@ -63,8 +59,6 @@ void Normalize(void) Serial.print (renorm); Serial.print (",ERR:"); Serial.print (error); - Serial.print (",TOW:"); - Serial.print (iTOW); Serial.println("***"); #endif } else { @@ -77,8 +71,6 @@ void Normalize(void) Serial.print (renorm); Serial.print (",ERR:"); Serial.print (error); - Serial.print (",TOW:"); - Serial.print (iTOW); Serial.println("***"); #endif } @@ -97,8 +89,6 @@ void Normalize(void) Serial.print (renorm); Serial.print (",ERR:"); Serial.print (error); - Serial.print (",TOW:"); - Serial.print (iTOW); Serial.println("***"); #endif } else { @@ -109,8 +99,6 @@ void Normalize(void) #if PRINT_DEBUG != 0 Serial.print("???PRB:3,RNM:"); Serial.print (renorm); - Serial.print (",TOW:"); - Serial.print (iTOW); Serial.println("***"); #endif } @@ -201,8 +189,6 @@ void Drift_correction(void) Serial.print("!!!INT:1,MAG:"); Serial.print (ToDeg(Integrator_magnitude)); - Serial.print (",TOW:"); - Serial.print (iTOW); Serial.println("***"); #endif } diff --git a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/GPS_UBLOX.pde b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/GPS_UBLOX.pde index ad8d3a338b..1d2202782e 100644 --- a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/GPS_UBLOX.pde +++ b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/GPS_UBLOX.pde @@ -56,76 +56,40 @@ void parse_ubx_gps(){ gps_pos_fix_count++; #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){ - // 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((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); - 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); -#endif - } - + // Serial.print("Time von Arduino ;"); + // Serial.print(millis()); + 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 } diff --git a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/Output.pde b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/Output.pde index 5868f415e1..759d1ad4ac 100644 --- a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/Output.pde +++ b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/Output.pde @@ -197,10 +197,6 @@ void printdata(void){ #if PRINT_GPS == 1 if(gpsFixnew==1) { gpsFixnew=0; - Serial.print("LAT:"); - Serial.print(lat); - Serial.print(",LON:"); - Serial.print(lon); Serial.print(",ALT:"); Serial.print(alt_MSL/1000); // meters Serial.print(",COG:"); @@ -216,10 +212,6 @@ void printdata(void){ } #endif - //Serial.print("TOW:"); - //Serial.print(iTOW); - //Serial.println("***"); - #else // This section outputs binary data messages // Conforms to new binary message standard (12/31/09) @@ -270,10 +262,10 @@ void printdata(void){ IMU_buffer[14]=tempint&0xff; IMU_buffer[15]=(tempint>>8)&0xff; - IMU_buffer[16]=iTOW&0xff; - IMU_buffer[17]=(iTOW>>8)&0xff; - IMU_buffer[18]=(iTOW>>16)&0xff; - IMU_buffer[19]=(iTOW>>24)&0xff; + IMU_buffer[16]=0; + IMU_buffer[17]=0; + IMU_buffer[18]=0; + IMU_buffer[19]=0; IMU_buffer[20]=(imu_health>>8)&0xff; diff --git a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/arduimu_modified.pde b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/arduimu_modified.pde index 9424802332..e8c8e0ae28 100644 --- a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/arduimu_modified.pde +++ b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/arduimu_modified.pde @@ -181,10 +181,7 @@ union int_union { 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 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 iTOW=0; //GPS Millisecond Time of Week long alt=0; //Height above Ellipsoid in millimeters 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 @@ -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) 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 // GPS UBLOX byte ck_a=0; // Packet checksum diff --git a/sw/airborne/modules/ins/ins_arduimu_modified.c b/sw/airborne/modules/ins/ins_arduimu_modified.c index 0fe785acfb..3d9c836d31 100644 --- a/sw/airborne/modules/ins/ins_arduimu_modified.c +++ b/sw/airborne/modules/ins/ins_arduimu_modified.c @@ -60,10 +60,6 @@ void ArduIMU_init( void ) { ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT; } - -#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); \ @@ -71,49 +67,29 @@ void ArduIMU_init( void ) { _buf[_index+3] = (uint8_t) ((_value) >> 24); \ } - void ArduIMU_periodicGPS( void ) { - static uint8_t gps_data_status = GPS_DATA_MSG1; if (ardu_gps_trans.status != I2CTransDone) { return; } - if ( gps_data_status == GPS_DATA_MSG1 ) { - //posllh - GPS_Data [0] = gps_itow; - GPS_Data [1] = gps_lon; - GPS_Data [2] = gps_lat; - 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 [7] = gps_course * 100000; //Kurs - //status - GPS_Data [8] = gps_mode; //fix - GPS_Data [9] = gps_status_flags; //flags + //velned + GPS_Data [0] = gps_speed_3d; //speed 3D + GPS_Data [1] = gps_gspeed; //ground speed + GPS_Data [2] = gps_course * 100000; //Kurs + //alt + GPS_Data [3] = gps_alt; // height above elipsoid + GPS_Data [4] = gps_hmsl; // height above sea level + //status + GPS_Data [5] = gps_mode; //fix + GPS_Data [6] = gps_status_flags; //flags - 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 - 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; - } + FillBufWith32bit(ardu_gps_trans.buf, 0, GPS_Data[0]); + FillBufWith32bit(ardu_gps_trans.buf, 4, GPS_Data[1]); + FillBufWith32bit(ardu_gps_trans.buf, 8, GPS_Data[2]); + FillBufWith32bit(ardu_gps_trans.buf, 12, GPS_Data[3]); + FillBufWith32bit(ardu_gps_trans.buf, 16, GPS_Data[4]); + ardu_gps_trans.buf[20] = GPS_Data[5]; // status gps fix + ardu_gps_trans.buf[21] = GPS_Data[6]; // status flags + I2CTransmit(ARDUIMU_I2C_DEV, ardu_gps_trans, ArduIMU_SLAVE_ADDR, 22); }