From 39eca56ab54e3cd5ea865e566bb114cff285eb5c Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Wed, 2 Mar 2011 10:59:34 +0100 Subject: [PATCH] modified version of arduimu firmware and driver --- conf/modules/ins_arduimu.xml | 9 +- .../arduimu_modified/ADC.pde | 95 +++ .../arduimu_modified/Compass.pde | 74 ++ .../arduimu_modified/DCM.pde | 280 +++++++ .../arduimu_modified/GPS_EM406.pde | 204 +++++ .../arduimu_modified/GPS_NMEA.pde | 292 +++++++ .../arduimu_modified/GPS_UBLOX.pde | 177 +++++ .../arduimu_modified/Output.pde | 429 ++++++++++ .../arduimu_modified/Vector.pde | 40 + .../arduimu_modified/arduimu_modified.pde | 730 ++++++++++++++++++ .../arduimu_modified/matrix.pde | 22 + .../arduimu_modified/press_alt.pde | 244 ++++++ .../arduimu_modified/timing.pde | 23 + .../modules/ins/ins_arduimu_modified.c | 212 +++++ .../modules/ins/ins_arduimu_modified.h | 22 + 15 files changed, 2849 insertions(+), 4 deletions(-) create mode 100644 sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/ADC.pde create mode 100644 sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/Compass.pde create mode 100644 sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/DCM.pde create mode 100644 sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/GPS_EM406.pde create mode 100644 sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/GPS_NMEA.pde create mode 100644 sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/GPS_UBLOX.pde create mode 100644 sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/Output.pde create mode 100644 sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/Vector.pde create mode 100644 sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/arduimu_modified.pde create mode 100644 sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/matrix.pde create mode 100644 sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/press_alt.pde create mode 100644 sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/timing.pde create mode 100644 sw/airborne/modules/ins/ins_arduimu_modified.c create mode 100644 sw/airborne/modules/ins/ins_arduimu_modified.h diff --git a/conf/modules/ins_arduimu.xml b/conf/modules/ins_arduimu.xml index f3e4074b1d..a4bc6325b3 100644 --- a/conf/modules/ins_arduimu.xml +++ b/conf/modules/ins_arduimu.xml @@ -2,13 +2,14 @@
- +
- - + + + - + Output + #endif + + //************************************************************* + + + + Analog_Reference(EXTERNAL);//Using external analog reference + Analog_Init(); + + debug_print("Welcome..."); + + #if BOARD_VERSION == 1 + debug_print("You are using Hardware Version 1..."); + #endif + + #if BOARD_VERSION == 2 + debug_print("You are using Hardware Version 2..."); + #endif + + //Setup EM406 for SIRF binary mode at 38400bit/s + #if GPS_PROTOCOL == 2 + init_gps(); + #endif + + //Serial.print("ArduIMU: v"); + //Serial.println(SOFTWARE_VER); + debug_handler(0);//Printing version + + #if USE_MAGNETOMETER==1 + debug_handler(3); + I2C_Init(); + delay(100); + // Magnetometer initialization + Compass_Init(); + #endif + + // SETUP FOR SCP1000_D11 + #if USE_BAROMETER==1 + debug_handler(4); + setup_scp(); + #endif + + if(ENABLE_AIR_START){ + debug_handler(1); + startup_air(); + }else{ + debug_handler(2); + startup_ground(); + } + + + delay(250); + + Read_adc_raw(); // ADC initialization + timer=DIYmillis(); + delay(20); + +} + +//*************************************************************************************** +void loop() //Main Loop +{ + timeNow = millis(); + + if((timeNow-timer)>=20) // Main loop runs at 50Hz + { + timer_old = timer; + timer = timeNow; + +#if PERFORMANCE_REPORTING == 1 + mainLoop_count++; + if (timer-timer_old > G_Dt_max) G_Dt_max = timer-timer_old; +#endif + + G_Dt = (timer-timer_old)/1000.0; // Real time of loop run. We use this on the DCM algorithm (gyro integration time) + if(G_Dt > 1) + G_Dt = 0; //Something is wrong - keeps dt from blowing up, goes to zero to keep gyros from departing + + // *** DCM algorithm + + Read_adc_raw(); + + Matrix_update(); + + Normalize(); + + Drift_correction(); + + Euler_angles(); + + #if PRINT_BINARY == 1 + printdata(); //Send info via serial + #endif + + //Turn on the LED when you saturate any of the gyros. + if((abs(Gyro_Vector[0])>=ToRad(300))||(abs(Gyro_Vector[1])>=ToRad(300))||(abs(Gyro_Vector[2])>=ToRad(300))) + { + gyro_sat=1; +#if PERFORMANCE_REPORTING == 1 + gyro_sat_count++; +#endif + digitalWrite(5,HIGH); + } + + cycleCount++; + + // Do these things every 6th time through the main cycle + // This section gets called every 1000/(20*6) = 8 1/3 Hz + // doing it this way removes the need for another 'millis()' call + // and balances the processing load across main loop cycles. + switch (cycleCount) { + case(0): + decode_gps(); + break; + + case(1): + //Here we will check if we are getting a signal to ground start + if(digitalRead(GROUNDSTART_PIN) == LOW && groundstartDone == false) + startup_ground(); + break; + + case(2): + #if USE_BAROMETER==1 + ReadSCP1000(); // Read I2C absolute pressure sensor + press_filt = (press + 2l * press_filt) / 3l; //Light filtering + //temperature = (temperature * 9 + temp_unfilt) / 10; We will just use the ground temp for the altitude calculation + #endif + break; + + case(3): + #if USE_MAGNETOMETER==1 + Read_Compass(); // Read I2C magnetometer + Compass_Heading(); // Calculate magnetic heading + #endif + break; + + case(4): + // Display Status on LEDs + // GYRO Saturation indication + if(gyro_sat>=1) { + digitalWrite(5,HIGH); //Turn Red LED when gyro is saturated. + if(gyro_sat>=8) // keep the LED on for 8/10ths of a second + gyro_sat=0; + else + gyro_sat++; + } else { + digitalWrite(5,LOW); + } + + // YAW drift correction indication + if(ground_speed 20000) + { + printPerfData(timeNow-perf_mon_timer); + perf_mon_timer=timeNow; + } +#endif + + } + +} + +//******************************************************************************** +void startup_ground(void) +{ + uint16_t store=0; + int flashcount = 0; + + debug_handler(2); + for(int c=0; c= 10) { + flashcount = 0; +#if USE_BAROMETER==1 + ReadSCP1000(); + press_gnd = (press_gnd * 9l + press) / 10l; + temperature = (temperature * 9 + temp_unfilt) / 10; + +#endif + digitalWrite(7,HIGH); + digitalWrite(6,LOW); + digitalWrite(5,HIGH); + } + flashcount++; + + } + digitalWrite(5,LOW); + digitalWrite(6,LOW); + digitalWrite(7,LOW); + + AN_OFFSET[5]-=GRAVITY*SENSOR_SIGN[5]; + + for(int y=0; y<=5; y++) + { + Serial.println(AN_OFFSET[y]); + store = ((AN_OFFSET[y]-200.f)*100.0f); + eeprom_busy_wait(); + eeprom_write_word((uint16_t *) (y*2+2), store); + } + + while (gps_fix_count > 0 && USE_BAROMETER) { + decode_gps(); + // Serial.print(gpsFix); + // Serial.print(", "); + // Serial.println(gpsFixnew); + if (gpsFix == 0 && gpsFixnew == 1) { + gpsFixnew = 0; + gps_fix_count--; + } + } +#if USE_BAROMETER==1 + press_filt = press_gnd; + ground_alt = alt_MSL; + eeprom_busy_wait(); + eeprom_write_dword((uint32_t *)0x10, press_gnd); + eeprom_busy_wait(); + eeprom_write_word((uint16_t *)0x14, temperature); + eeprom_busy_wait(); + eeprom_write_word((uint16_t *)0x16, (ground_alt/1000)); +#endif + groundstartDone = true; + debug_handler(6); +} + +//************************************************************************************ +void startup_air(void) +{ + uint16_t temp=0; + + for(int y=0; y<=5; y++) + { + eeprom_busy_wait(); + temp = eeprom_read_word((uint16_t *) (y*2+2)); + AN_OFFSET[y] = temp/100.f+200.f; + Serial.println(AN_OFFSET[y]); + } +#if USE_BAROMETER==1 + eeprom_busy_wait(); + press_gnd = eeprom_read_dword((uint32_t *) 0x10); + press_filt = press_gnd; + eeprom_busy_wait(); + temperature = eeprom_read_word((uint16_t *) 0x14); + eeprom_busy_wait(); + ground_alt = eeprom_read_word((uint16_t *) 0x16); + ground_alt *= 1000; +#endif + Serial.println("***Air Start complete"); +} + + +void debug_print(char string[]) +{ + #if PRINT_DEBUG != 0 + Serial.print("???"); + Serial.print(string); + Serial.println("***"); + #endif +} + +void debug_handler(byte message) +{ + #if PRINT_DEBUG != 0 + + static unsigned long BAD_Checksum=0; + + switch(message) + { + case 0: + Serial.print("???Software Version "); + Serial.print(SOFTWARE_VER); + Serial.println("***"); + break; + + case 1: + Serial.println("???Air Start!***"); + break; + + case 2: + Serial.println("???Ground Start!***"); + break; + + case 3: + Serial.println("???Enabling Magneto...***"); + break; + + case 4: + Serial.println("???Enabling Pressure Altitude...***"); + break; + + case 5: + Serial.println("???Air Start complete"); + break; + + case 6: + Serial.println("???Ground Start complete"); + break; + + case 10: + BAD_Checksum++; + Serial.print("???GPS Bad Checksum: "); + Serial.print(BAD_Checksum); + Serial.println("...***"); + break; + + default: + Serial.println("???Invalid debug ID...***"); + break; + + } + #endif + +} + +/* +EEPROM memory map + +0 0x00 Unused +1 0x01 .. +2 0x02 AN_OFFSET[0] +3 0x03 .. +4 0x04 AN_OFFSET[1] +5 0x05 .. +6 0x06 AN_OFFSET[2] +7 0x07 .. +8 0x08 AN_OFFSET[3] +9 0x09 .. +10 0x0A AN_OFFSET[4] +11 0x0B .. +12 0x0C AN_OFFSET[5] +13 0x0D .. +14 0x0E Unused +15 0x0F .. +16 0x10 Ground Pressure +17 0x11 .. +18 0x12 .. +19 0x13 .. +20 0x14 Ground Temp +21 0x15 .. +22 0x16 Ground Altitude +23 0x17 .. +*/ diff --git a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/matrix.pde b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/matrix.pde new file mode 100644 index 0000000000..fd825e0e30 --- /dev/null +++ b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/matrix.pde @@ -0,0 +1,22 @@ +/**************************************************/ +//Multiply two 3x3 matrixs. This function developed by Jordi can be easily adapted to multiple n*n matrix's. (Pero me da flojera!). +void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3]) +{ + float op[3]; + for(int x=0; x<3; x++) + { + for(int y=0; y<3; y++) + { + for(int w=0; w<3; w++) + { + op[w]=a[x][w]*b[w][y]; + } + mat[x][y]=0; + mat[x][y]=op[0]+op[1]+op[2]; + + float test=mat[x][y]; + } + } +} + + diff --git a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/press_alt.pde b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/press_alt.pde new file mode 100644 index 0000000000..1af648e74e --- /dev/null +++ b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/press_alt.pde @@ -0,0 +1,244 @@ +/* VTI SCP1000-D11 I2C pressure sensor */ +/* Based on code by Chris Barnes February 2010 */ +/* Based on code by Jose Julio */ + +/* ATTENTION: SCP1000 is a 3v3 device */ + +#if USE_BAROMETER == 1 +void setup_scp() +{ + unsigned char temp_byte = 0; + delay(6); +// Init_Diagnostics(); // Initialise diagnostics output + + // If we know the starting altitude we could initialise it + // else just treat this as the altitude relative to the start + // init_alt(START_ALTITUDE); // altitude in m **************** + + + Wire.begin(); + TWIinit(); + SCP1000_StopAcquisition(); // Stop acquisition first - in case the sensor had already been put into a non-standby mode (e.g. Arduino s/w reset but sensor not) + delay(100); + SCP1000_StartAcquisition(); // SCP1000 only accepts new mode from standby - otherwise device will be left in previous mode +/* + Serial.print("Status: "); + temp_byte = SCP1000_GetStatus2(); + Serial.println((int)temp_byte); + DecodeStatus(temp_byte); + + Serial.print("Operation Mode: "); + Serial.println((int)SCP1000_GetOperation2()); +*/ + delay(100); +} + +/******************************************************************** + Altitude Calculation +*********************************************************************/ + +void alti(void) +{ + + double x; + double p = (double)press_gnd/(double)press; + double temp = (float)temperature/20.f + 273.15f; + x = log(p) * temp * 29271.267f; + //x = log(p) * temp * 29.271267 * 1000; + press_alt = x + ground_alt; + // Need to add comments for theory..... +} + +/******************************************************************** + Data Retrieval +*********************************************************************/ + void SCP1000_StartAcquisition() +{ + Wire.beginTransmission((uint8_t)PRESSURE_ADDR); + Wire.send((uint8_t)SNS_ADDR_POPERATION); // Start acquisition + Wire.send((uint8_t)SCP_MODE); + Wire.endTransmission(); +} + +void SCP1000_StopAcquisition() +{ + Wire.beginTransmission((uint8_t)PRESSURE_ADDR); + Wire.send((uint8_t)SNS_ADDR_POPERATION); + Wire.send(0x00); // Stop acquisition + Wire.endTransmission(); +} + +unsigned char SCP1000_GetStatus2() +{ + unsigned char ready[] = { + 2, WRITE_PRESSURE_ADDR, SNS_ADDR_PSTATUS, + 2, READ_PRESSURE_ADDR, 0, + 0 + }; + TWIdocmd(ready); + + return(ready[5]); +} + +unsigned char SCP1000_GetOperation2() +{ + unsigned char oper[] = { + 2, WRITE_PRESSURE_ADDR, SNS_ADDR_POPERATION, + 2, READ_PRESSURE_ADDR, 0, + 0 + }; + TWIdocmd(oper); + + return(oper[5]); +} + +// Return the raw temperature value provided by the SCP1000 +// This has units of degrees C x 20 +int SCP1000_GetTemp2() +{ + int temp; + + unsigned char getdatat[] = { + 2, WRITE_PRESSURE_ADDR, SNS_ADDR_PTEMP, + 3, READ_PRESSURE_ADDR, 0, 0, + 0 + }; + + TWIdocmd(getdatat); + + // check sign bit of temperature + if (0x20U & getdatat[5]) + { + // negative temperature - extend sign + getdatat[5] |= 0xC0; + } + temp = getdatat[5] << 8; // MSByte + temp |= getdatat[6]; // LSByte + + return(temp); +} + +// Return the raw pressure value provided by the SCP1000 +unsigned long SCP1000_GetPressure2() +{ + unsigned long press; + + unsigned char getdatap1[] = { + 2, WRITE_PRESSURE_ADDR, SNS_ADDR_DATARD8, + 2, READ_PRESSURE_ADDR, 0, + 0 + }; + unsigned char getdatap2[] = { + 2, WRITE_PRESSURE_ADDR, SNS_ADDR_PPRESSURE, + 3, READ_PRESSURE_ADDR, 0, 0, + 0 + }; + + TWIdocmd(getdatap1); + + press = 0x07 & getdatap1[5]; // MSByte (either 5 or 6 for the sort of altitude we are interested in) + + TWIdocmd(getdatap2); + + press <<= 8; + press |= getdatap2[5]; + press <<= 8; + press |= getdatap2[6]; + + return(press); +} + +// Decode bits from status byte and return TRUE is there is a measurement ready to be read +byte DecodeStatus(byte u8Status) +{ + static byte u8Starting = 0U; + + if (0U != (0x01 & u8Status)) + { + //SCP1000 Startup procedure is still running + if (u8Starting) + { + Serial.println("Starting"); + u8Starting = 1; // remember that we have already output this diagnostic + } + } + else + { + u8Starting = 0; // remember that we ahve seen the device out of starting mode + if (0U != (0x10 & u8Status)) + { + // Real Time Error - data was not read in time - clear by reading it + // Note - this "error" is to be expected on startup when the SCP1000 is already running + // as we will not have been reading out all of the measurements that have been made... + Serial.println("RTErr"); + } + if (0U != (0x20 & u8Status)) + { + // DRDY - new data ready + return (TRUE); + } + } + return (FALSE); +} + +void ReadSCP1000(void) +{ + temp_unfilt = SCP1000_GetTemp2(); + press = SCP1000_GetPressure2(); + +} + + + +/******************************************************************** + This Library is needed for the SCP1000-D11 + because I2C commands needs a RESTART between commands and not and STOP-START (like Wire library do) + This code is from http://harleyhacking.blogspot.com/ +*********************************************************************/ + + +#include + +// TWI operations +#define ENABTW ((1< +#include "modules/ins/ins_arduimu_modified.h" +#include "firmwares/fixedwing/main_fbw.h" +#include "mcu_periph/i2c.h" + +// test +#include "estimator.h" + +// für das Senden von GPS-Daten an den ArduIMU +#include "gps.h" +int32_t GPS_Data[14]; + +#ifndef ARDUIMU_I2C_DEV +#define ARDUIMU_I2C_DEV i2c0 +#endif + +// Adresse des I2C Slaves: 0001 0110 letztes Bit ist für Read/Write +// einzugebende Adresse im ArduIMU ist 0000 1011 +//da ArduIMU das Read/Write Bit selber anfügt. +#define ArduIMU_SLAVE_ADDR 0x22 + +#ifndef DOWNLINK_DEVICE +#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE +#endif + +#include "mcu_periph/uart.h" +#include "messages.h" +#include "downlink.h" + +struct i2c_transaction ardu_gps_trans; +struct i2c_transaction ardu_ins_trans; + +static int16_t recievedData[NB_DATA]; +float ArduIMU_data[NB_DATA]; + +float ins_roll_neutral; +float ins_pitch_neutral; + +void ArduIMU_init( void ) { + ardu_ins_trans.status = I2CTransDone; + ardu_gps_trans.status = I2CTransDone; + + ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT; + ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT; +} + + +#define GPS_DATA_MSG1 0 +#define GPS_DATA_MSG2 1 + +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; //höhe über elipsoid + GPS_Data [4] = gps_hmsl; //höhe über 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 + //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; + + //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); + + 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); + + gps_data_status = GPS_DATA_MSG1; + } + +} + +void ArduIMU_periodic( void ) { + //Frequence defined in conf/modules/ins_arduimu.xml + + if (ardu_ins_trans.status == I2CTransDone) { + I2CReceive(ARDUIMU_I2C_DEV, ardu_ins_trans, ArduIMU_SLAVE_ADDR, NB_DATA*2); + } + + /* + Buffer O: Roll + Buffer 1: Pitch + Buffer 2: Yaw + Buffer 3: Gyro X + Buffer 4: Gyro Y + Buffer 5: Gyro Z + Buffer 6: Accel X + Buffer 7: Accel Y + Buffer 8: Accel Z + */ + +} + +#include "math/pprz_algebra_int.h" + +void ArduIMU_event( void ) { + // Handle INS I2C event + if (ardu_ins_trans.status == I2CTransSuccess) { + // received data from I2C transaction + recievedData[0] = (ardu_ins_trans.buf[1]<<8) | ardu_ins_trans.buf[0]; + recievedData[1] = (ardu_ins_trans.buf[3]<<8) | ardu_ins_trans.buf[2]; + recievedData[2] = (ardu_ins_trans.buf[5]<<8) | ardu_ins_trans.buf[4]; + recievedData[3] = (ardu_ins_trans.buf[7]<<8) | ardu_ins_trans.buf[6]; + recievedData[4] = (ardu_ins_trans.buf[9]<<8) | ardu_ins_trans.buf[8]; + recievedData[5] = (ardu_ins_trans.buf[11]<<8) | ardu_ins_trans.buf[10]; + recievedData[6] = (ardu_ins_trans.buf[13]<<8) | ardu_ins_trans.buf[12]; + recievedData[7] = (ardu_ins_trans.buf[15]<<8) | ardu_ins_trans.buf[14]; + 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]); + + // 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; + 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]))); + } + } + else if (ardu_ins_trans.status == I2CTransFailed) { + ardu_ins_trans.status = I2CTransDone; + } + // Handle GPS I2C event + if (ardu_gps_trans.status == I2CTransSuccess || ardu_gps_trans.status == I2CTransFailed) { + ardu_gps_trans.status = I2CTransDone; + } +} + diff --git a/sw/airborne/modules/ins/ins_arduimu_modified.h b/sw/airborne/modules/ins/ins_arduimu_modified.h new file mode 100644 index 0000000000..cce43b66bd --- /dev/null +++ b/sw/airborne/modules/ins/ins_arduimu_modified.h @@ -0,0 +1,22 @@ +#ifndef ArduIMU_H +#define ArduIMU_H + +#include + +#define NB_DATA 9 + +extern float ArduIMU_data[NB_DATA]; + +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 ); +void ArduIMU_event( void ); + +#endif // ArduIMU_H