From 39eca56ab54e3cd5ea865e566bb114cff285eb5c Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Wed, 2 Mar 2011 10:59:34 +0100 Subject: [PATCH 001/101] 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 From 17e8dfaa0dbe4739c42d55f15cca77ff246169e6 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Wed, 2 Mar 2011 17:18:53 +0100 Subject: [PATCH 002/101] clean arduino driver and firmware on modified version --- .../arduimu_modified/GPS_UBLOX.pde | 171 ++++++++---------- .../arduimu_modified/Output.pde | 22 +-- .../arduimu_modified/arduimu_modified.pde | 5 - .../modules/ins/ins_arduimu_modified.c | 134 ++++++-------- .../modules/ins/ins_arduimu_modified.h | 11 +- 5 files changed, 143 insertions(+), 200 deletions(-) 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 1624ab245c..ad8d3a338b 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 @@ -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 - } - - + } + + } 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 58a14a2752..5868f415e1 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 @@ -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++; 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 8ddcb0033b..9424802332 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 @@ -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 //************************************************************************************************************ diff --git a/sw/airborne/modules/ins/ins_arduimu_modified.c b/sw/airborne/modules/ins/ins_arduimu_modified.c index 166fc278dc..0fe785acfb 100644 --- a/sw/airborne/modules/ins/ins_arduimu_modified.c +++ b/sw/airborne/modules/ins/ins_arduimu_modified.c @@ -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; diff --git a/sw/airborne/modules/ins/ins_arduimu_modified.h b/sw/airborne/modules/ins/ins_arduimu_modified.h index cce43b66bd..3e64cf3125 100644 --- a/sw/airborne/modules/ins/ins_arduimu_modified.h +++ b/sw/airborne/modules/ins/ins_arduimu_modified.h @@ -2,18 +2,15 @@ #define ArduIMU_H #include +#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 ); From e7acfc6c8130feaea26199f9f506eeff373e2a71 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Sat, 5 Mar 2011 23:21:23 +0100 Subject: [PATCH 003/101] remove some of the data sent to arduimu --- .../arduimu_modified/ADC.pde | 4 - .../arduimu_modified/DCM.pde | 14 --- .../arduimu_modified/GPS_UBLOX.pde | 94 ++++++------------- .../arduimu_modified/Output.pde | 16 +--- .../arduimu_modified/arduimu_modified.pde | 14 --- .../modules/ins/ins_arduimu_modified.c | 60 ++++-------- 6 files changed, 51 insertions(+), 151 deletions(-) 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); } From a194dc17d8ac03a7dd236b662b2bf366de868cea Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Mon, 14 Mar 2011 18:29:35 +0100 Subject: [PATCH 004/101] remove i2c stop_after_transmit flag always do a stop after transmit (previous default behavior) --- sw/airborne/arch/lpc21/mcu_periph/i2c_arch.c | 39 ++++++-------- .../rotorcraft/actuators/actuators_mkk.c | 1 - sw/airborne/mcu_periph/i2c.h | 51 +++++++------------ sw/airborne/peripherals/ami601.c | 1 - sw/airborne/peripherals/hmc5843.c | 1 - sw/airborne/test/test_esc_mkk_simple.c | 1 - 6 files changed, 34 insertions(+), 60 deletions(-) diff --git a/sw/airborne/arch/lpc21/mcu_periph/i2c_arch.c b/sw/airborne/arch/lpc21/mcu_periph/i2c_arch.c index 378337c50b..88810f82e2 100644 --- a/sw/airborne/arch/lpc21/mcu_periph/i2c_arch.c +++ b/sw/airborne/arch/lpc21/mcu_periph/i2c_arch.c @@ -28,7 +28,6 @@ #include "interrupt_hw.h" #include BOARD_CONFIG - /////////////////// // I2C Automaton // /////////////////// @@ -56,22 +55,18 @@ __attribute__ ((always_inline)) static inline void I2cEndOfTransaction(struct i2 } } -__attribute__ ((always_inline)) static inline void I2cFinished(struct i2c_periph* p, struct i2c_transaction* t) { +__attribute__ ((always_inline)) static inline void I2cSendStop(struct i2c_periph* p, struct i2c_transaction* t) { + ((i2cRegs_t *)(p->reg_addr))->conset = _BV(STO); // transaction finished with success t->status = I2CTransSuccess; I2cEndOfTransaction(p); } -__attribute__ ((always_inline)) static inline void I2cSendStop(struct i2c_periph* p, struct i2c_transaction* t) { - ((i2cRegs_t *)(p->reg_addr))->conset = _BV(STO); - I2cFinished(p,t); -} - __attribute__ ((always_inline)) static inline void I2cFail(struct i2c_periph* p, struct i2c_transaction* t) { ((i2cRegs_t *)(p->reg_addr))->conset = _BV(STO); + // transaction failed t->status = I2CTransFailed; - p->status = I2CFailed; - // FIXME I2C should be reseted here + // FIXME I2C should be reseted here ? I2cEndOfTransaction(p); } @@ -122,13 +117,20 @@ __attribute__ ((always_inline)) static inline void I2cAutomaton(int32_t state, s I2cSendStop(p,trans); } break; + case I2C_MR_DATA_NACK: + if (p->idx_buf < trans->len_r) { + trans->buf[p->idx_buf] = ((i2cRegs_t *)(p->reg_addr))->dat; + } + I2cSendStop(p,trans); + break; case I2C_MR_SLA_ACK: /* At least one char */ /* Wait and reply with ACK or NACK */ I2cReceive(p->reg_addr,p->idx_buf < trans->len_r - 1); break; case I2C_MR_SLA_NACK: case I2C_MT_SLA_NACK: - I2cSendStart(p); + /* Slave is not responding, transaction is failed */ + I2cFail(p,trans); break; case I2C_MT_SLA_ACK: case I2C_MT_DATA_ACK: @@ -137,28 +139,17 @@ __attribute__ ((always_inline)) static inline void I2cAutomaton(int32_t state, s p->idx_buf++; } else { if (trans->type == I2CTransTxRx) { - trans->type = I2CTransRx; /* FIXME should not change type */ + //trans->type = I2CTransRx; /* FIXME should not change type */ p->idx_buf = 0; trans->slave_addr |= 1; I2cSendStart(p); } else { - if (trans->stop_after_transmit) { - I2cSendStop(p,trans); - } else { - I2cFinished(p,trans); - } + I2cSendStop(p,trans); } } break; - case I2C_MR_DATA_NACK: - if (p->idx_buf < trans->len_r) { - trans->buf[p->idx_buf] = ((i2cRegs_t *)(p->reg_addr))->dat; - } - I2cSendStop(p,trans); - break; default: - I2cSendStop(p,trans); - //I2cFail(p,trans); + I2cFail(p,trans); /* FIXME log error */ break; } diff --git a/sw/airborne/firmwares/rotorcraft/actuators/actuators_mkk.c b/sw/airborne/firmwares/rotorcraft/actuators/actuators_mkk.c index 90bb98f8f9..07602f1430 100644 --- a/sw/airborne/firmwares/rotorcraft/actuators/actuators_mkk.c +++ b/sw/airborne/firmwares/rotorcraft/actuators/actuators_mkk.c @@ -43,7 +43,6 @@ void actuators_init(void) { actuators_mkk.trans[i].type = I2CTransTx; actuators_mkk.trans[i].len_w = 1; actuators_mkk.trans[i].slave_addr = actuators_addr[i]; - actuators_mkk.trans[i].stop_after_transmit = TRUE; actuators_mkk.trans[i].status = I2CTransSuccess; } diff --git a/sw/airborne/mcu_periph/i2c.h b/sw/airborne/mcu_periph/i2c.h index 3495bbafec..93bd0f77ba 100644 --- a/sw/airborne/mcu_periph/i2c.h +++ b/sw/airborne/mcu_periph/i2c.h @@ -43,7 +43,6 @@ struct i2c_transaction { uint8_t slave_addr; uint16_t len_r; uint8_t len_w; - bool_t stop_after_transmit; volatile uint8_t buf[I2C_BUF_LEN]; volatile enum I2CTransactionStatus status; }; @@ -136,40 +135,28 @@ extern bool_t i2c_idle(struct i2c_periph* p); extern bool_t i2c_submit(struct i2c_periph* p, struct i2c_transaction* t); #define I2CReceive(_p, _t, _s_addr, _len) { \ - _t.type = I2CTransRx; \ - _t.slave_addr = _s_addr; \ - _t.len_r = _len; \ - _t.len_w = 0; \ - _t.stop_after_transmit = TRUE; \ - i2c_submit(&(_p),&(_t)); \ - } + _t.type = I2CTransRx; \ + _t.slave_addr = _s_addr; \ + _t.len_r = _len; \ + _t.len_w = 0; \ + i2c_submit(&(_p),&(_t)); \ +} #define I2CTransmit(_p, _t, _s_addr, _len) { \ - _t.type = I2CTransTx; \ - _t.slave_addr = _s_addr; \ - _t.len_r = 0; \ - _t.len_w = _len; \ - _t.stop_after_transmit = TRUE; \ - i2c_submit(&(_p),&(_t)); \ - } + _t.type = I2CTransTx; \ + _t.slave_addr = _s_addr; \ + _t.len_r = 0; \ + _t.len_w = _len; \ + i2c_submit(&(_p),&(_t)); \ +} -#define I2CTransmitNoStop(_p, _t, _s_addr, _len) { \ - _t.type = I2CTransTx; \ - _t.slave_addr = _s_addr; \ - _t.len_r = 0; \ - _t.len_w = _len; \ - _t.stop_after_transmit = FALSE; \ - i2c_submit(&(_p),&(_t)); \ - } - -#define I2CTransceive(_p, _t, _s_addr, _len_w, _len_r) { \ - _t.type = I2CTransTxRx; \ - _t.slave_addr = _s_addr; \ - _t.len_r = _len_r; \ - _t.len_w = _len_w; \ - _t.stop_after_transmit = TRUE; \ - i2c_submit(&(_p),&(_t)); \ - } +#define I2CTransceive(_p, _t, _s_addr, _len_w, _len_r) { \ + _t.type = I2CTransTxRx; \ + _t.slave_addr = _s_addr; \ + _t.len_r = _len_r; \ + _t.len_w = _len_w; \ + i2c_submit(&(_p),&(_t)); \ +} #endif /* I2C_H */ diff --git a/sw/airborne/peripherals/ami601.c b/sw/airborne/peripherals/ami601.c index 81b1e512a2..14d6f7b1c7 100644 --- a/sw/airborne/peripherals/ami601.c +++ b/sw/airborne/peripherals/ami601.c @@ -17,7 +17,6 @@ void ami601_init( void ) { } ami601_i2c_trans.status = I2CTransSuccess; ami601_i2c_trans.slave_addr = AMI601_SLAVE_ADDR; - ami601_i2c_trans.stop_after_transmit = TRUE; ami601_nb_err = 0; ami601_status = AMI601_IDLE; diff --git a/sw/airborne/peripherals/hmc5843.c b/sw/airborne/peripherals/hmc5843.c index 71050c442c..f02c148312 100644 --- a/sw/airborne/peripherals/hmc5843.c +++ b/sw/airborne/peripherals/hmc5843.c @@ -11,7 +11,6 @@ void hmc5843_init(void) { hmc5843.i2c_trans.status = I2CTransSuccess; hmc5843.i2c_trans.slave_addr = HMC5843_ADDR; - hmc5843.i2c_trans.stop_after_transmit = TRUE; hmc5843_arch_init(); } diff --git a/sw/airborne/test/test_esc_mkk_simple.c b/sw/airborne/test/test_esc_mkk_simple.c index 232890665a..983f19564b 100644 --- a/sw/airborne/test/test_esc_mkk_simple.c +++ b/sw/airborne/test/test_esc_mkk_simple.c @@ -59,7 +59,6 @@ static inline void main_periodic_task( void ) { trans.buf[0] = 0x04; trans.len_w = 1; trans.slave_addr = 0x58; - trans.stop_after_transmit = TRUE; i2c_submit(&ACTUATORS_MKK_DEV,&trans); LED_PERIODIC(); From ebcde0baed6f7f4818442fa9092beb47f68ca448 Mon Sep 17 00:00:00 2001 From: Allen Date: Mon, 14 Mar 2011 16:32:26 -0700 Subject: [PATCH 005/101] Add libopenstm32/libopencm3 to libraries for stm32 makefile --- conf/Makefile.stm32 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/conf/Makefile.stm32 b/conf/Makefile.stm32 index fb7fe7ea20..be9a360e33 100644 --- a/conf/Makefile.stm32 +++ b/conf/Makefile.stm32 @@ -138,7 +138,7 @@ else LDFLAGS = -D__thumb2__ -T$(LDSCRIPT) -nostartfiles -L$(GCC_LIB_DIR) -O$(OPT) --gc-sections endif LDFLAGS += -Wl,-Map=$(OBJDIR)/$(TARGET).map,--cref,--gc-sections -LDLIBS += -lc -lm -lgcc -lcmsis -lstm32 +LDLIBS += -lc -lm -lgcc -lcmsis -lstm32 -lopencm3_stm32 CPFLAGS = -j .isr_vector -j .text -j .data CPFLAGS_BIN = -Obinary From 541884fd106d4b364d2d500c7a8e356e21f403bf Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Tue, 15 Mar 2011 15:16:34 +0100 Subject: [PATCH 006/101] return a fail on i2c error --- sw/airborne/arch/lpc21/mcu_periph/i2c_arch.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sw/airborne/arch/lpc21/mcu_periph/i2c_arch.c b/sw/airborne/arch/lpc21/mcu_periph/i2c_arch.c index 88810f82e2..f8f02fc83e 100644 --- a/sw/airborne/arch/lpc21/mcu_periph/i2c_arch.c +++ b/sw/airborne/arch/lpc21/mcu_periph/i2c_arch.c @@ -114,7 +114,7 @@ __attribute__ ((always_inline)) static inline void I2cAutomaton(int32_t state, s } else { /* error , we should have got NACK */ - I2cSendStop(p,trans); + I2cFail(p,trans); } break; case I2C_MR_DATA_NACK: From 74ac232247f5f6728bf6887fd46e0fbf45874c30 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Tue, 15 Mar 2011 18:20:21 +0100 Subject: [PATCH 007/101] basic version of the arduimu firmware returns angles rates and accels gps velocities and status are send no baro or magneto support --- conf/modules/ins_arduimu.xml | 4 +- .../ADC.pde | 0 .../DCM.pde | 118 ++-- .../GPS_EM406.pde | 0 .../GPS_NMEA.pde | 0 .../arduimu_basic/GPS_pprz.pde | 64 ++ .../arduimu_basic/Output.pde | 200 +++++++ .../Vector.pde | 0 .../arduimu_basic.pde} | 551 ++++++------------ .../matrix.pde | 0 .../timing.pde | 0 .../arduimu_modified/Compass.pde | 74 --- .../arduimu_modified/GPS_UBLOX.pde | 120 ---- .../arduimu_modified/Output.pde | 419 ------------- .../arduimu_modified/press_alt.pde | 244 -------- ...arduimu_modified.c => ins_arduimu_basic.c} | 53 +- ...arduimu_modified.h => ins_arduimu_basic.h} | 0 17 files changed, 530 insertions(+), 1317 deletions(-) rename sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/{arduimu_modified => arduimu_basic}/ADC.pde (100%) rename sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/{arduimu_modified => arduimu_basic}/DCM.pde (75%) rename sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/{arduimu_modified => arduimu_basic}/GPS_EM406.pde (100%) rename sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/{arduimu_modified => arduimu_basic}/GPS_NMEA.pde (100%) create mode 100644 sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/GPS_pprz.pde create mode 100644 sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/Output.pde rename sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/{arduimu_modified => arduimu_basic}/Vector.pde (100%) rename sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/{arduimu_modified/arduimu_modified.pde => arduimu_basic/arduimu_basic.pde} (52%) rename sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/{arduimu_modified => arduimu_basic}/matrix.pde (100%) rename sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/{arduimu_modified => arduimu_basic}/timing.pde (100%) delete mode 100644 sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/Compass.pde delete mode 100644 sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/GPS_UBLOX.pde delete mode 100644 sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/Output.pde delete mode 100644 sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/press_alt.pde rename sw/airborne/modules/ins/{ins_arduimu_modified.c => ins_arduimu_basic.c} (79%) rename sw/airborne/modules/ins/{ins_arduimu_modified.h => ins_arduimu_basic.h} (100%) diff --git a/conf/modules/ins_arduimu.xml b/conf/modules/ins_arduimu.xml index ecbd865263..0fcd1c3d8b 100644 --- a/conf/modules/ins_arduimu.xml +++ b/conf/modules/ins_arduimu.xml @@ -2,14 +2,14 @@
- +
- + 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_basic/ADC.pde similarity index 100% rename from sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/ADC.pde rename to sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/ADC.pde 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_basic/DCM.pde similarity index 75% rename from sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/DCM.pde rename to sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/DCM.pde index 0583854932..305c2bac57 100644 --- a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/DCM.pde +++ b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/DCM.pde @@ -5,17 +5,17 @@ void Normalize(void) float temporary[3][3]; float renorm=0; boolean problem=FALSE; - + error= -Vector_Dot_Product(&DCM_Matrix[0][0],&DCM_Matrix[1][0])*.5; //eq.19 Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19 Vector_Scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq.19 - + Vector_Add(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]);//eq.19 Vector_Add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]);//eq.19 - + Vector_Cross_Product(&temporary[2][0],&temporary[0][0],&temporary[1][0]); // c= a x b //eq.20 - + renorm= Vector_Dot_Product(&temporary[0][0],&temporary[0][0]); if (renorm < 1.5625f && renorm > 0.64f) { renorm= .5 * (3-renorm); //eq.21 @@ -36,7 +36,7 @@ void Normalize(void) #if PERFORMANCE_REPORTING == 1 renorm_blowup_count++; #endif - #if PRINT_DEBUG != 0 +#if PRINT_DEBUG != 0 Serial.print("???PRB:1,RNM:"); Serial.print (renorm); Serial.print (",ERR:"); @@ -44,8 +44,8 @@ void Normalize(void) Serial.println("***"); #endif } - Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm); - + Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm); + renorm= Vector_Dot_Product(&temporary[1][0],&temporary[1][0]); if (renorm < 1.5625f && renorm > 0.64f) { renorm= .5 * (3-renorm); //eq.21 @@ -75,7 +75,7 @@ void Normalize(void) #endif } Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm); - + renorm= Vector_Dot_Product(&temporary[2][0],&temporary[2][0]); if (renorm < 1.5625f && renorm > 0.64f) { renorm= .5 * (3-renorm); //eq.21 @@ -103,18 +103,18 @@ void Normalize(void) #endif } Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm); - + if (problem) { // Our solution is blowing up and we will force back to initial condition. Hope we are not upside down! - DCM_Matrix[0][0]= 1.0f; - DCM_Matrix[0][1]= 0.0f; - DCM_Matrix[0][2]= 0.0f; - DCM_Matrix[1][0]= 0.0f; - DCM_Matrix[1][1]= 1.0f; - DCM_Matrix[1][2]= 0.0f; - DCM_Matrix[2][0]= 0.0f; - DCM_Matrix[2][1]= 0.0f; - DCM_Matrix[2][2]= 1.0f; - problem = FALSE; + DCM_Matrix[0][0]= 1.0f; + DCM_Matrix[0][1]= 0.0f; + DCM_Matrix[0][2]= 0.0f; + DCM_Matrix[1][0]= 0.0f; + DCM_Matrix[1][1]= 1.0f; + DCM_Matrix[1][2]= 0.0f; + DCM_Matrix[2][0]= 0.0f; + DCM_Matrix[2][1]= 0.0f; + DCM_Matrix[2][2]= 1.0f; + problem = FALSE; } } @@ -122,15 +122,13 @@ void Normalize(void) void Drift_correction(void) { //Compensation the Roll, Pitch and Yaw drift. - float mag_heading_x; - float mag_heading_y; static float Scaled_Omega_P[3]; static float Scaled_Omega_I[3]; float Accel_magnitude; float Accel_weight; float Integrator_magnitude; float tempfloat; - + //*****Roll and Pitch*************** // Calculate the magnitude of the accelerometer vector @@ -139,48 +137,35 @@ void Drift_correction(void) // Dynamic weighting of accelerometer info (reliability filter) // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0) Accel_weight = constrain(1 - 2*abs(1 - Accel_magnitude),0,1); // - - #if PERFORMANCE_REPORTING == 1 - tempfloat = ((Accel_weight - 0.5) * 256.0f); //amount added was determined to give imu_health a time constant about twice the time constant of the roll/pitch drift correction - imu_health += tempfloat; - imu_health = constrain(imu_health,129,65405); - #endif - + +#if PERFORMANCE_REPORTING == 1 + tempfloat = ((Accel_weight - 0.5) * 256.0f); //amount added was determined to give imu_health a time constant about twice the time constant of the roll/pitch drift correction + imu_health += tempfloat; + imu_health = constrain(imu_health,129,65405); +#endif + Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&DCM_Matrix[2][0]); //adjust the ground of reference Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight); - + Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight); Vector_Add(Omega_I,Omega_I,Scaled_Omega_I); - + //*****YAW*************** - - #if USE_MAGNETOMETER==1 - // We make the gyro YAW drift correction based on compass magnetic heading - mag_heading_x = cos(MAG_Heading); - mag_heading_y = sin(MAG_Heading); - errorCourse=(DCM_Matrix[0][0]*mag_heading_y) - (DCM_Matrix[1][0]*mag_heading_x); //Calculating YAW error - Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position. - - Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW); - Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional. - - Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW); - Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I - #else // Use GPS Ground course to correct yaw gyro drift + + // Use GPS Ground course to correct yaw gyro drift if(ground_speed>=SPEEDFILT) { COGX = cos(ToRad(ground_course)); COGY = sin(ToRad(ground_course)); errorCourse=(DCM_Matrix[0][0]*COGY) - (DCM_Matrix[1][0]*COGX); //Calculating YAW error Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position. - + Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW); Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional. - + Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW); Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I } - #endif // Here we will place a limit on the integrator so that the integrator cannot ever exceed half the saturation limit of the gyros Integrator_magnitude = sqrt(Vector_Dot_Product(Omega_I,Omega_I)); if (Integrator_magnitude > ToRad(300)) { @@ -192,14 +177,13 @@ void Drift_correction(void) Serial.println("***"); #endif } - - + } /**************************************************/ void Accel_adjust(void) { - Accel_Vector[1] += Accel_Scale(speed_3d*Omega[2]); // Centrifugal force on Acc_y = GPS_speed*GyroZ - Accel_Vector[2] -= Accel_Scale(speed_3d*Omega[1]); // Centrifugal force on Acc_z = GPS_speed*GyroY + Accel_Vector[1] += Accel_Scale(speed_3d*Omega[2]); // Centrifugal force on Acc_y = GPS_speed*GyroZ + Accel_Vector[2] -= Accel_Scale(speed_3d*Omega[1]); // Centrifugal force on Acc_z = GPS_speed*GyroY } /**************************************************/ @@ -208,17 +192,17 @@ void Matrix_update(void) Gyro_Vector[0]=Gyro_Scaled_X(read_adc(0)); //gyro x roll Gyro_Vector[1]=Gyro_Scaled_Y(read_adc(1)); //gyro y pitch Gyro_Vector[2]=Gyro_Scaled_Z(read_adc(2)); //gyro Z yaw - + Accel_Vector[0]=read_adc(3); // acc x Accel_Vector[1]=read_adc(4); // acc y Accel_Vector[2]=read_adc(5); // acc z - + Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]); //adding proportional term Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term Accel_adjust(); //Remove centrifugal acceleration. - - #if OUTPUTMODE==1 + +#if OUTPUTMODE==1 Update_Matrix[0][0]=0; Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y @@ -228,7 +212,7 @@ void Matrix_update(void) Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x Update_Matrix[2][2]=0; - #else // Uncorrected data (no drift correction) +#else // Uncorrected data (no drift correction) Update_Matrix[0][0]=0; Update_Matrix[0][1]=-G_Dt*Gyro_Vector[2];//-z Update_Matrix[0][2]=G_Dt*Gyro_Vector[1];//y @@ -238,7 +222,7 @@ void Matrix_update(void) Update_Matrix[2][0]=-G_Dt*Gyro_Vector[1]; Update_Matrix[2][1]=G_Dt*Gyro_Vector[0]; Update_Matrix[2][2]=0; - #endif +#endif Matrix_Multiply(DCM_Matrix,Update_Matrix,Temporary_Matrix); //a*b=c @@ -253,14 +237,14 @@ void Matrix_update(void) void Euler_angles(void) { - #if (OUTPUTMODE==2) // Only accelerometer info (debugging purposes) - roll = atan2(Accel_Vector[1],Accel_Vector[2]); // atan2(acc_y,acc_z) - pitch = -asin((Accel_Vector[0])/(double)GRAVITY); // asin(acc_x) - yaw = 0; - #else - pitch = -asin(DCM_Matrix[2][0]); - roll = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]); - yaw = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]); - #endif +#if (OUTPUTMODE==2) // Only accelerometer info (debugging purposes) + roll = atan2(Accel_Vector[1],Accel_Vector[2]); // atan2(acc_y,acc_z) + pitch = -asin((Accel_Vector[0])/(double)GRAVITY); // asin(acc_x) + yaw = 0; +#else + pitch = -asin(DCM_Matrix[2][0]); + roll = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]); + yaw = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]); +#endif } diff --git a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/GPS_EM406.pde b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/GPS_EM406.pde similarity index 100% rename from sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/GPS_EM406.pde rename to sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/GPS_EM406.pde diff --git a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/GPS_NMEA.pde b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/GPS_NMEA.pde similarity index 100% rename from sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/GPS_NMEA.pde rename to sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/GPS_NMEA.pde diff --git a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/GPS_pprz.pde b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/GPS_pprz.pde new file mode 100644 index 0000000000..6a2ddf704d --- /dev/null +++ b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/GPS_pprz.pde @@ -0,0 +1,64 @@ +/**************************************************************** + * Parse GPS data from a Paparazzi autopilot + ****************************************************************/ + // Code from Jordi, modified by Jose, modified by Gautier + + +void init_gps(void) +{ + //Serial.begin(38400); + pinMode(2,OUTPUT); //Serial Mux + digitalWrite(2,HIGH); //Serial Mux +} + +/**************************************************************** + * + ****************************************************************/ +void parse_pprz_gps() { + +#if PERFORMANCE_REPORTING == 1 + gps_pos_fix_count++; +#endif + + 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 + stGpsFix = Paparazzi_GPS_buffer[12]; + stFlags = Paparazzi_GPS_buffer[13]; + + 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); + +} + + +/**************************************************************** + * Join 4 bytes into a long + ****************************************************************/ +int32_t join_4_bytes(byte Buffer[]) +{ + longUnion.byte[0] = *Buffer; + longUnion.byte[1] = *(Buffer+1); + longUnion.byte[2] = *(Buffer+2); + longUnion.byte[3] = *(Buffer+3); + return(longUnion.dword); +} + +/**************************************************************** + * +void checksum(byte ubx_data) +{ + ck_a+=ubx_data; + ck_b+=ck_a; +} + + ****************************************************************/ diff --git a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/Output.pde b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/Output.pde new file mode 100644 index 0000000000..49170c774c --- /dev/null +++ b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/Output.pde @@ -0,0 +1,200 @@ + +//*****I2C Output************************************************************ + +void requestEvent(){ +#if PRINT_DEBUG != 0 + Serial.println("Sending IMU Data"); +#endif + + // Message Array : Roll; Pitch; YaW; GyroX; GyroY; GyroZ; ACCX; ACCY; ACCZ + // Float Number is multipited with 10000 and converted to an Integer, for sending via I2C. + // Resolution for angles: 12, rates: 12, accel:10 (from pprza BFP math lib) + I2C_Message_ar[0] = int(roll*(1<<12)); + I2C_Message_ar[1] = int(pitch*(1<<12)); + I2C_Message_ar[2] = int(yaw*(1<<12)); + 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((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, 18); + +} + +//******** GPS Data from Paparazzi AP ****************************************** + +void receiveEvent(int howMany){ +#if PRINT_DEBUG != 0 + Serial.print("Receiving GPS Bytes : "); + Serial.println(howMany); +#endif + + for(int i=0; i < howMany; i++){ + Paparazzi_GPS_buffer[i]=Wire.receive(); + } + + parse_pprz_gps(); // Parse new GPS packet... + GPS_timer=DIYmillis(); //Restarting timer... + + gpsDataReady=1; + +} + + +//************************************************************************************************************* + + +void printdata(void){ + +#if PRINT_I2C_MSG == 1 + Serial.print("Time "); + Serial.print(millis()); + Serial.print("; Roll "); + Serial.print(I2C_Message_ar[0]); + Serial.print("; Pitch "); + Serial.print(I2C_Message_ar[1]); + Serial.print("; YaW "); + Serial.print(I2C_Message_ar[2]); + Serial.print("; GyroX "); + Serial.print(I2C_Message_ar[3]); + Serial.print("; GyroY "); + Serial.print(I2C_Message_ar[4]); + Serial.print("; GyroZ "); + Serial.print(I2C_Message_ar[5]); + Serial.print("; ACCX "); + Serial.print(I2C_Message_ar[6]); + Serial.print("; ACCY "); + Serial.print(I2C_Message_ar[7]); + Serial.print("; ACCZ "); + Serial.println(I2C_Message_ar[8]); +#endif + + //Serial.print("!!!VER:"); + //Serial.print(SOFTWARE_VER); //output the software version + //Serial.print(","); + +#if PRINT_ANALOGS == 1 + Serial.print("AN0:"); + Serial.print(read_adc(0)); //Reversing the sign. + Serial.print(",AN1:"); + Serial.print(read_adc(1)); + Serial.print(",AN2:"); + Serial.print(read_adc(2)); + Serial.print(",AN3:"); + Serial.print(read_adc(3)); + Serial.print (",AN4:"); + Serial.print(read_adc(4)); + Serial.print (",AN5:"); + Serial.print(read_adc(5)); + Serial.print (","); +#endif + +#if PRINT_DCM == 1 + Serial.print ("EX0:"); + Serial.print(convert_to_dec(DCM_Matrix[0][0])); + Serial.print (",EX1:"); + Serial.print(convert_to_dec(DCM_Matrix[0][1])); + Serial.print (",EX2:"); + Serial.print(convert_to_dec(DCM_Matrix[0][2])); + Serial.print (",EX3:"); + Serial.print(convert_to_dec(DCM_Matrix[1][0])); + Serial.print (",EX4:"); + Serial.print(convert_to_dec(DCM_Matrix[1][1])); + Serial.print (",EX5:"); + Serial.print(convert_to_dec(DCM_Matrix[1][2])); + Serial.print (",EX6:"); + Serial.print(convert_to_dec(DCM_Matrix[2][0])); + Serial.print (",EX7:"); + Serial.print(convert_to_dec(DCM_Matrix[2][1])); + Serial.print (",EX8:"); + Serial.print(convert_to_dec(DCM_Matrix[2][2])); + Serial.print (","); +#endif + +#if PRINT_EULER == 1 + Serial.print("RLL:"); + Serial.print(ToDeg(roll)); + Serial.print(",PCH:"); + Serial.print(ToDeg(pitch)); + Serial.print(",YAW:"); + Serial.print(ToDeg(yaw)); + Serial.print(",IMUH:"); + Serial.print((imu_health>>8)&0xff); + Serial.print (","); +#endif + +#if PRINT_GPS == 1 + if(gpsFixnew==1) { + gpsFixnew=0; + Serial.print("COG:"); + Serial.print((ground_course)); + Serial.print(",SOG:"); + Serial.print(ground_speed); + Serial.print(",FIX:"); + Serial.print((int)gpsFix); + Serial.print (","); +#if PERFORMANCE_REPORTING == 1 + gps_messages_sent++; +#endif + } +#endif + +} + +void printPerfData(long time) +{ + Serial.print("PPP"); + Serial.print("pTm:"); + Serial.print(time,DEC); + Serial.print(",mLc:"); + Serial.print(mainLoop_count,DEC); + Serial.print(",DtM:"); + Serial.print(G_Dt_max,DEC); + Serial.print(",gsc:"); + Serial.print(gyro_sat_count,DEC); + Serial.print(",adc:"); + Serial.print(adc_constraints,DEC); + Serial.print(",rsc:"); + Serial.print(renorm_sqrt_count,DEC); + Serial.print(",rbc:"); + Serial.print(renorm_blowup_count,DEC); + Serial.print(",gpe:"); + Serial.print(gps_payload_error_count,DEC); + Serial.print(",gce:"); + Serial.print(gps_checksum_error_count,DEC); + Serial.print(",gpf:"); + Serial.print(gps_pos_fix_count,DEC); + Serial.print(",gnf:"); + Serial.print(gps_nav_fix_count,DEC); + Serial.print(",gms:"); + Serial.print(gps_messages_sent,DEC); + Serial.print(",imu:"); + Serial.print((imu_health>>8),DEC); + Serial.print(",***"); + + // Reset counters + mainLoop_count = 0; + G_Dt_max = 0; + gyro_sat_count = 0; + adc_constraints = 0; + renorm_sqrt_count = 0; + renorm_blowup_count = 0; + gps_payload_error_count = 0; + gps_checksum_error_count = 0; + gps_pos_fix_count = 0; + gps_nav_fix_count = 0; + gps_messages_sent = 0; + + +} + + +long convert_to_dec(float x) +{ + return x*10000000; +} + diff --git a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/Vector.pde b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/Vector.pde similarity index 100% rename from sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/Vector.pde rename to sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/Vector.pde 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_basic/arduimu_basic.pde similarity index 52% rename from sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/arduimu_modified.pde rename to sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/arduimu_basic.pde index e8c8e0ae28..df41fd5778 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_basic/arduimu_basic.pde @@ -2,6 +2,7 @@ // Code by Jordi Munoz and William Premerlani, Supported by Chris Anderson (Wired) and Nathan Sindle (SparkFun). // Version 1.0 for flat board updated by Doug Weibel and Jose Julio // Version 1.7 includes support for SCP1000 absolute pressure sensor +// Version modified by Gautier Hattenberger for use with Paparazzi autopilot only (no barometer, no compass) // Axis definition: X axis pointing forward, Y axis pointing to the right and Z axis pointing down. // Positive pitch : nose up @@ -19,9 +20,6 @@ // *** NOTE! Hardware version - Can be used for v1 (daughterboards) or v2 (flat) #define BOARD_VERSION 2 // 1 For V1 and 2 for V2 -// Ublox gps is recommended! -#define GPS_PROTOCOL 3 // 1 - NMEA, 2 - EM406, 3 - Ublox We have only tested with Ublox - // Enable Air Start uses Remove Before Fly flag - connection to pin 6 on ArduPilot #define ENABLE_AIR_START 1 // 1 if using airstart/groundstart signaling, 0 if not #define GROUNDSTART_PIN 8 // Pin number used for ground start signal (recommend 10 on v1 and 8 on v2 hardware) @@ -35,6 +33,7 @@ //OUTPUTMODE=1 will print the corrected data, 0 will print uncorrected data of the gyros (with drift), 2 will print accelerometer only data #define OUTPUTMODE 1 +#define PRINT_I2C_MSG 0 //Will print the I2C output buffer #define PRINT_DCM 0 //Will print the whole direction cosine matrix #define PRINT_ANALOGS 0 //Will print the analog raw data #define PRINT_EULER 0 //Will print the Euler angles Roll, Pitch and Yaw @@ -44,13 +43,8 @@ //**********I2C Parameter ******************************************** -#define PRINT_I2C_Data 1 //Will print GPS data int I2C_Message_ar[9]; - -#define GET_GPS_PAP 1 // Use GPS-DATA from Paparazzi Ublox - - //******************************************************************** @@ -60,13 +54,6 @@ int I2C_Message_ar[9]; // *** NOTE! Performance reporting is only supported for Ublox. Set to 0 for others #define PERFORMANCE_REPORTING 1 //Will include performance reports in the binary output ~ 1/2 min -/* Support for optional magnetometer (1 enabled, 0 dissabled) */ -#define USE_MAGNETOMETER 0 // use 1 if you want to make yaw gyro drift corrections using the optional magnetometer - -/* Support for optional barometer (1 enabled, 0 dissabled) */ -#define USE_BAROMETER 0 // use 1 if you want to get altitude using the optional absolute pressure sensor -#define ALT_MIX 50 // For binary messages: GPS or barometric altitude. 0 to 100 = % of barometric. For example 75 gives 25% GPS alt and 75% baro - //********************************************************************** // End of user parameters //********************************************************************** @@ -97,9 +84,6 @@ int I2C_Message_ar[9]; //#define Kp_YAW 2.5 //High yaw drift correction gain - use with caution! #define Ki_YAW 0.00005 -/*UBLOX Maximum payload length*/ -#define UBX_MAXPAYLOAD 56 - #define ADC_WARM_CYCLES 75 #define FALSE 0 @@ -125,10 +109,10 @@ float Omega_I[3]= {0,0,0};//Omega Integrator float Omega[3]= {0,0,0}; //Magnetometer variables -int magnetom_x; -int magnetom_y; -int magnetom_z; -float MAG_Heading; +//int magnetom_x; +//int magnetom_y; +//int magnetom_z; +//float MAG_Heading; // Euler angles float roll; @@ -144,79 +128,56 @@ float COGY=1; //Course overground Y axis unsigned int cycleCount=0; byte gyro_sat=0; -float DCM_Matrix[3][3]= { - { - 1,0,0 } - ,{ - 0,1,0 } - ,{ - 0,0,1 } -}; -float Update_Matrix[3][3]={{0,1,2},{3,4,5},{6,7,8}}; //Gyros here +float DCM_Matrix[3][3] = { + { 1,0,0 }, + { 0,1,0 }, + { 0,0,1 } +}; +float Update_Matrix[3][3] = { + { 0,1,2 }, + { 3,4,5 }, + { 6,7,8 } +}; //Gyros here float Temporary_Matrix[3][3]={ - { - 0,0,0 } - ,{ - 0,0,0 } - ,{ - 0,0,0 } + { 0,0,0 }, + { 0,0,0 }, + { 0,0,0 } }; - + //GPS //GPS stuff union long_union { - int32_t dword; - uint8_t byte[4]; + int32_t dword; + uint8_t byte[4]; } longUnion; union int_union { - int16_t word; - uint8_t byte[2]; + int16_t word; + uint8_t byte[2]; } intUnion; /*Flight GPS variables*/ 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 alt_MSL=0; //This is the altitude in millimeters -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 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; -#if GPS_PROTOCOL == 3 -// GPS UBLOX -byte ck_a=0; // Packet checksum -byte ck_b=0; -byte UBX_step=0; -byte UBX_class=0; -byte UBX_id=0; -byte UBX_payload_length_hi=0; -byte UBX_payload_length_lo=0; -byte UBX_payload_counter=0; -byte UBX_buffer[UBX_MAXPAYLOAD]; -byte UBX_ck_a=0; -byte UBX_ck_b=0; -#endif - - //***********************GPS PAPARAZZI************************************************************************ -#if GET_GPS_PAP -byte Paparazzi_GPS_buffer[UBX_MAXPAYLOAD]; +#define PPRZ_MAXPAYLOAD 32 +byte Paparazzi_GPS_buffer[PPRZ_MAXPAYLOAD]; int gpsDataReady = 0; // sind neuen GPS daten vorhanden ?? byte stGpsFix; byte stFlags; byte messageNr; -#endif //************************************************************************************************************ - - //ADC variables volatile uint8_t MuxSel=0; volatile uint8_t analog_reference = DEFAULT; @@ -224,71 +185,34 @@ volatile uint16_t analog_buffer[8]; volatile uint8_t analog_count[8]; - #if BOARD_VERSION == 1 - uint8_t sensors[6] = {0,2,1,3,5,4}; // Use these two lines for Hardware v1 (w/ daughterboards) - int SENSOR_SIGN[]= {1,-1,1,-1,1,-1,-1,-1,-1}; //Sensor: GYROX, GYROY, GYROZ, ACCELX, ACCELY, ACCELZ - #endif - - #if BOARD_VERSION == 2 - uint8_t sensors[6] = {6,7,3,0,1,2}; // For Hardware v2 flat - int SENSOR_SIGN[] = {1,-1,-1,1,-1,1,-1,-1,-1}; - #endif - - // Performance Monitoring variables - // Data collected and reported for ~1/2 minute intervals - #if PERFORMANCE_REPORTING == 1 - int mainLoop_count = 0; //Main loop cycles since last report - int G_Dt_max = 0.0; //Max main loop cycle time in milliseconds - byte gyro_sat_count = 0; - byte adc_constraints = 0; - byte renorm_sqrt_count = 0; - byte renorm_blowup_count = 0; - byte gps_payload_error_count = 0; - byte gps_checksum_error_count = 0; - byte gps_pos_fix_count = 0; - byte gps_nav_fix_count = 0; - byte gps_messages_sent = 0; - long perf_mon_timer = 0; - #endif - unsigned int imu_health = 65012; - -//********************************************************************** -// This section contains SCP1000_D11 PARAMETERS !!! -//********************************************************************** -#if USE_BAROMETER == 1 -#define SCP_MODE (9) // 9 = high speed mode, 10 = high resolution mode -#define PRESSURE_ADDR (0x11U) // IIC address of the SCP1000 -// ************ #define START_ALTITUDE (217U) // default initial altitude in m above sea level - -// When we have to manage data transfers via IIC directly we need to use the following addresses -// IIC address of the SCP1000 device forms the Top 7 bits of the address with the R/W bit as the LSB -#define READ_PRESSURE_ADDR (PRESSURE_ADDR<<1 | 1) -#define WRITE_PRESSURE_ADDR (PRESSURE_ADDR<<1) - -// SCP1000 Register addresses -#define SNS_ADDR_POPERATION (0x03U) // OPERATON register -#define SNS_ADDR_PSTATUS (0x07U) // STATUS register -#define SNS_ADDR_PPRESSURE (0x80U) // DATARD16 Register (pressure) -#define SNS_ADDR_DATARD8 (0x7FU) // DAYARD8 Register -#define SNS_ADDR_PTEMP (0x81U) // TEMPOUT Register (temperature) - -#ifndef TRUE -#define TRUE (0x01) -#endif -#ifndef FALSE -#define FALSE (0x00) +#if BOARD_VERSION == 1 +uint8_t sensors[6] = {0,2,1,3,5,4}; // Use these two lines for Hardware v1 (w/ daughterboards) +int SENSOR_SIGN[]= {1,-1,1,-1,1,-1,-1,-1,-1}; //Sensor: GYROX, GYROY, GYROZ, ACCELX, ACCELY, ACCELZ #endif -int temp_unfilt = 0; -int temperature = 0; -unsigned long press = 0; -unsigned long press_filt = 0; -unsigned long press_gnd = 0; -long ground_alt = 0; // Ground altitude in millimeters -long press_alt = 0; // Pressure altitude in millimeters - +#if BOARD_VERSION == 2 +uint8_t sensors[6] = {6,7,3,0,1,2}; // For Hardware v2 flat +int SENSOR_SIGN[] = {1,-1,-1,1,-1,1,-1,-1,-1}; #endif +// Performance Monitoring variables +// Data collected and reported for ~1/2 minute intervals +#if PERFORMANCE_REPORTING == 1 +int mainLoop_count = 0; //Main loop cycles since last report +int G_Dt_max = 0.0; //Max main loop cycle time in milliseconds +byte gyro_sat_count = 0; +byte adc_constraints = 0; +byte renorm_sqrt_count = 0; +byte renorm_blowup_count = 0; +byte gps_payload_error_count = 0; +byte gps_checksum_error_count = 0; +byte gps_pos_fix_count = 0; +byte gps_nav_fix_count = 0; +byte gps_messages_sent = 0; +long perf_mon_timer = 0; +#endif +unsigned int imu_health = 65012; + //****************************************************************** void setup() { @@ -301,96 +225,64 @@ void setup() pinMode(GROUNDSTART_PIN,INPUT); // Remove Before Fly flag (pin 6 on ArduPilot) digitalWrite(GROUNDSTART_PIN,HIGH); - //************Define I2C Output Handler***************************************************3 - #if PRINT_I2C_Data == 1 - Wire.begin(17); // join i2c bus with address #2 - Wire.onRequest(requestEvent); - #endif - - #if GET_GPS_PAP == 1 - // Muss noch sauber abgefangen werden wennn I2CDATA nicht definiert ist - //Wire.begin(17); // join i2c bus with address #2 - Wire.onReceive(receiveEvent); // register event --> Output - #endif - + //************Define I2C Output Handler************************ + Wire.begin(17); // join i2c bus with address #2 + Wire.onRequest(requestEvent); + Wire.onReceive(receiveEvent); // register event --> Output //************************************************************* - - - - Analog_Reference(EXTERNAL);//Using external analog reference + + Analog_Reference(EXTERNAL); //Using external analog reference Analog_Init(); - + debug_print("Welcome..."); - - #if BOARD_VERSION == 1 + +#if BOARD_VERSION == 1 debug_print("You are using Hardware Version 1..."); - #endif - - #if BOARD_VERSION == 2 +#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 - +#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(); + 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 - + 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(); @@ -398,12 +290,12 @@ void loop() //Main Loop Normalize(); Drift_correction(); - + Euler_angles(); - - #if PRINT_BINARY == 1 - printdata(); //Send info via serial - #endif + +#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))) @@ -414,79 +306,66 @@ void loop() //Main Loop #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 2000) { + digitalWrite(6, LOW); //If we don't receive any byte in two seconds turn off gps fix LED... + debug_print("No GPS data"); + gpsFix = 1; + } + 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): + // 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); + } + break; + + case(3): + // YAW drift correction indication + if(ground_speed 20000) - { + if (timeNow-perf_mon_timer > 20000) { printPerfData(timeNow-perf_mon_timer); perf_mon_timer=timeNow; } @@ -521,12 +400,6 @@ void startup_ground(void) Read_adc_raw(); for(int y=0; y<=5; y++) // Read first initial ADC values for offset. AN_OFFSET[y]=AN[y]; -#if USE_BAROMETER==1 - ReadSCP1000(); - press_gnd = press; - temperature = temp_unfilt; - delay(20); -#endif for(int i=0;i<400;i++) // We take some readings... { @@ -541,18 +414,11 @@ void startup_ground(void) } if(flashcount >= 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); @@ -560,6 +426,7 @@ void startup_ground(void) AN_OFFSET[5]-=GRAVITY*SENSOR_SIGN[5]; + // Store parameters in eeprom for(int y=0; y<=5; y++) { Serial.println(AN_OFFSET[y]); @@ -568,26 +435,6 @@ void startup_ground(void) 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); } @@ -604,83 +451,73 @@ void startup_air(void) 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"); + debug_handler(5); } void debug_print(char string[]) { - #if PRINT_DEBUG != 0 +#if PRINT_DEBUG != 0 Serial.print("???"); Serial.print(string); Serial.println("***"); - #endif +#endif } void debug_handler(byte message) { - #if PRINT_DEBUG != 0 - +#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 - + + 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 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_basic/matrix.pde similarity index 100% rename from sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/matrix.pde rename to sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/matrix.pde diff --git a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/timing.pde b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/timing.pde similarity index 100% rename from sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/timing.pde rename to sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/timing.pde diff --git a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/Compass.pde b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/Compass.pde deleted file mode 100644 index 3d9a8d2023..0000000000 --- a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/Compass.pde +++ /dev/null @@ -1,74 +0,0 @@ -/* ******************************************************* */ -/* I2C HMC5843 magnetometer */ -/* ******************************************************* */ - -// Local magnetic declination -// I use this web : http://www.ngdc.noaa.gov/geomagmodels/Declination.jsp -#define MAGNETIC_DECLINATION -6.0 // not used now -> magnetic bearing - -int CompassAddress = 0x1E; //0x3C //0x3D; //(0x42>>1); - -void I2C_Init() -{ - Wire.begin(); -} - - -void Compass_Init() -{ - Wire.beginTransmission(CompassAddress); - Wire.send(0x02); - Wire.send(0x00); // Set continouos mode (default to 10Hz) - Wire.endTransmission(); //end transmission -} - -void Read_Compass() -{ - int i = 0; - byte buff[6]; - - Wire.beginTransmission(CompassAddress); - Wire.send(0x03); //sends address to read from - Wire.endTransmission(); //end transmission - - //Wire.beginTransmission(CompassAddress); - Wire.requestFrom(CompassAddress, 6); // request 6 bytes from device - while(Wire.available()) // ((Wire.available())&&(i<6)) - { - buff[i] = Wire.receive(); // receive one byte - i++; - } - Wire.endTransmission(); //end transmission - - if (i==6) // All bytes received? - { - // MSB byte first, then LSB, X,Y,Z - magnetom_x = SENSOR_SIGN[6]*((((int)buff[2]) << 8) | buff[3]); // X axis (internal sensor y axis) - magnetom_y = SENSOR_SIGN[7]*((((int)buff[0]) << 8) | buff[1]); // Y axis (internal sensor x axis) - magnetom_z = SENSOR_SIGN[8]*((((int)buff[4]) << 8) | buff[5]); // Z axis - } - else - Serial.println("!ERR: Mag data"); -} - - -void Compass_Heading() -{ - float MAG_X; - float MAG_Y; - float cos_roll; - float sin_roll; - float cos_pitch; - float sin_pitch; - - cos_roll = cos(roll); - sin_roll = sin(roll); - cos_pitch = cos(pitch); - sin_pitch = sin(pitch); - // Tilt compensated Magnetic filed X: - MAG_X = magnetom_x*cos_pitch+magnetom_y*sin_roll*sin_pitch+magnetom_z*cos_roll*sin_pitch; - // Tilt compensated Magnetic filed Y: - MAG_Y = magnetom_y*cos_roll-magnetom_z*sin_roll; - // Magnetic Heading - MAG_Heading = atan2(-MAG_Y,MAG_X); -} 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 deleted file mode 100644 index 1d2202782e..0000000000 --- a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/GPS_UBLOX.pde +++ /dev/null @@ -1,120 +0,0 @@ -#if GPS_PROTOCOL == 3 - -/**************************************************************** - * Here you have all the parsing stuff for uBlox - ****************************************************************/ - // Code from Jordi, modified by Jose - - //You have to disable all the other string, only leave this ones: - - //NAV-POSLLH Geodetic Position Solution, PAGE 66 of datasheet - //NAV-VELNED Velocity Solution in NED, PAGE 71 of datasheet - //NAV-STATUS Receiver Navigation Status, PAGE 67 of datasheet - //NAV-SOL Navigation Solution Information, PAGE 72 of datasheet - - - // Baud Rate:38400 - -/* - GPSfix Type - - 0x00 = no fix - - 0x01 = dead reckonin - - 0x02 = 2D-fix - - 0x03 = 3D-fix - - 0x04 = GPS + dead re - - 0x05 = Time only fix - - 0x06..0xff = reserved*/ - - //Luckly uBlox has internal EEPROM so all the settings you change will remain forever. Not like the SIRF modules! - -void init_gps(void) -{ - //Serial.begin(38400); - pinMode(2,OUTPUT); //Serial Mux - digitalWrite(2,HIGH); //Serial Mux -} - -/**************************************************************** - * - ****************************************************************/ - -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; - } -} - -/**************************************************************** - * - ****************************************************************/ -void parse_ubx_gps(){ - -#if PERFORMANCE_REPORTING == 1 - gps_pos_fix_count++; -#endif - - 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((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 0 - // 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 - -} - - -/**************************************************************** - * - ****************************************************************/ - // Join 4 bytes into a long -int32_t join_4_bytes(byte Buffer[]) -{ - longUnion.byte[0] = *Buffer; - longUnion.byte[1] = *(Buffer+1); - longUnion.byte[2] = *(Buffer+2); - longUnion.byte[3] = *(Buffer+3); - return(longUnion.dword); -} - -/**************************************************************** - * - ****************************************************************/ -void checksum(byte ubx_data) -{ - ck_a+=ubx_data; - ck_b+=ck_a; -} - - -#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 deleted file mode 100644 index 759d1ad4ac..0000000000 --- a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/Output.pde +++ /dev/null @@ -1,419 +0,0 @@ - - -//*****I2C Output************************************************************ -// PRINT_I2C_Data -// #if PRINT_BINARY != 1 //Print either Ascii or binary messages - -void requestEvent(){ - // Message Array : Roll; Pitch; YaW; GyroX; GyroY; GyroZ; ACCX; ACCY; ACCZ - // Float Number is multipited with 10000 and converted to an Integer, for sending via I2C. - // Resolution for angles: 12, rates: 12, accel:10 (from pprza BFP math lib) - I2C_Message_ar[0] = int(roll*(1<<12)); - I2C_Message_ar[1] = int(pitch*(1<<12)); - I2C_Message_ar[2] = int(yaw*(1<<12)); - 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((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, 18); - - /* Uncomment for debug on serial link */ - - //Serial.println(); - /* - Serial.print("Time ;"); - Serial.print(millis()); - Serial.print("; Roll ;"); - Serial.print(I2C_Message_ar[0]); - Serial.print("; Pitch ;"); - Serial.print(I2C_Message_ar[1]); - Serial.print("; YaW ;"); - Serial.print(I2C_Message_ar[2]); - Serial.print("; GyroX ;"); - Serial.print(I2C_Message_ar[3]); - Serial.print("; GyroY: ;"); - Serial.print(I2C_Message_ar[4]); - Serial.print("; GyroZ ;"); - Serial.print(I2C_Message_ar[5]); - Serial.print("; ACCX ;"); - Serial.print(I2C_Message_ar[6]); - Serial.print("; ACCY: ;"); - Serial.print(I2C_Message_ar[7]); - Serial.print("; ACCZ ;"); - Serial.println(I2C_Message_ar[8]); - */ - -} - -//********GPS Data from PAPArazzi UBLOX********************************************************************** - -void receiveEvent(int howMany){ - Serial.print(" How Many Bytes GPS : "); - Serial.println(howMany); - - for(int i=0; i < howMany; i++){ - Paparazzi_GPS_buffer[i]=Wire.receive(); - } - - parse_ubx_gps(); // Parse new GPS packet... - GPS_timer=DIYmillis(); //Restarting timer... - - gpsDataReady=1; - -} - - -//************************************************************************************************************* - - -void printdata(void){ - - -#if PRINT_I2C_Data == 1 - /* - I2C_Message_ar[0] = int(roll*(1<<12)); - I2C_Message_ar[1] = int(pitch*(1<<12)); - I2C_Message_ar[2] = int(yaw*(1<<12)); - 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((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 ;"); - Serial.print(millis()); - Serial.print("; Roll ;"); - Serial.print(I2C_Message_ar[0]); - Serial.print("; Pitch ;"); - Serial.print(I2C_Message_ar[1]); - Serial.print("; YaW ;"); - Serial.print(I2C_Message_ar[2]); - Serial.print("; GyroX ;"); - Serial.print(I2C_Message_ar[3]); - Serial.print("; GyroY: ;"); - Serial.print(I2C_Message_ar[4]); - Serial.print("; GyroZ ;"); - Serial.print(I2C_Message_ar[5]); - Serial.print("; ACCX ;"); - Serial.print(I2C_Message_ar[6]); - Serial.print("; ACCY: ;"); - Serial.print(I2C_Message_ar[7]); - Serial.print("; ACCZ ;"); - Serial.println(I2C_Message_ar[8]); - */ -#endif - - - - - -#if PRINT_BINARY != 1 //Print either Ascii or binary messages - - //Serial.print("!!!VER:"); - //Serial.print(SOFTWARE_VER); //output the software version - //Serial.print(","); - - #if PRINT_ANALOGS==1 - Serial.print("AN0:"); - Serial.print(read_adc(0)); //Reversing the sign. - Serial.print(",AN1:"); - Serial.print(read_adc(1)); - Serial.print(",AN2:"); - Serial.print(read_adc(2)); - Serial.print(",AN3:"); - Serial.print(read_adc(3)); - Serial.print (",AN4:"); - Serial.print(read_adc(4)); - Serial.print (",AN5:"); - Serial.print(read_adc(5)); - Serial.print (","); - #endif - - #if PRINT_DCM == 1 - Serial.print ("EX0:"); - Serial.print(convert_to_dec(DCM_Matrix[0][0])); - Serial.print (",EX1:"); - Serial.print(convert_to_dec(DCM_Matrix[0][1])); - Serial.print (",EX2:"); - Serial.print(convert_to_dec(DCM_Matrix[0][2])); - Serial.print (",EX3:"); - Serial.print(convert_to_dec(DCM_Matrix[1][0])); - Serial.print (",EX4:"); - Serial.print(convert_to_dec(DCM_Matrix[1][1])); - Serial.print (",EX5:"); - Serial.print(convert_to_dec(DCM_Matrix[1][2])); - Serial.print (",EX6:"); - Serial.print(convert_to_dec(DCM_Matrix[2][0])); - Serial.print (",EX7:"); - Serial.print(convert_to_dec(DCM_Matrix[2][1])); - Serial.print (",EX8:"); - Serial.print(convert_to_dec(DCM_Matrix[2][2])); - Serial.print (","); - #endif - - #if PRINT_EULER == 1 - Serial.print("RLL:"); - Serial.print(ToDeg(roll)); - Serial.print(",PCH:"); - Serial.print(ToDeg(pitch)); - Serial.print(",YAW:"); - Serial.print(ToDeg(yaw)); - Serial.print(",IMUH:"); - Serial.print((imu_health>>8)&0xff); - Serial.print (","); - #endif - - #if USE_MAGNETOMETER == 1 - Serial.print("MGX:"); - Serial.print(magnetom_x); - Serial.print (",MGY:"); - Serial.print(magnetom_y); - Serial.print (",MGZ:"); - Serial.print(magnetom_z); - Serial.print (",MGH:"); - Serial.print(MAG_Heading); - Serial.print (","); - #endif - - #if USE_BAROMETER == 1 - Serial.print("Temp:"); - Serial.print(temp_unfilt/20.0); // Convert into degrees C - alti(); - Serial.print(",Pressure: "); - Serial.print(press); -// Serial.print(press>>2); // Convert into Pa - Serial.print(",Alt: "); - Serial.print(press_alt/1000); // Original floating point full solution in meters - Serial.print (","); - #endif - - #if PRINT_GPS == 1 - if(gpsFixnew==1) { - gpsFixnew=0; - Serial.print(",ALT:"); - Serial.print(alt_MSL/1000); // meters - Serial.print(",COG:"); - Serial.print((ground_course)); - Serial.print(",SOG:"); - Serial.print(ground_speed); - Serial.print(",FIX:"); - Serial.print((int)gpsFix); - Serial.print (","); - #if PERFORMANCE_REPORTING == 1 - gps_messages_sent++; - #endif - } - #endif - -#else - // This section outputs binary data messages - // Conforms to new binary message standard (12/31/09) - byte IMU_buffer[22]; - int tempint; - int ck; - long templong; - byte IMU_ck_a=0; - byte IMU_ck_b=0; - - // This section outputs the gps binary message when new gps data is available - if(gpsFixnew==1) { - #if PERFORMANCE_REPORTING == 1 - gps_messages_sent++; - #endif - gpsFixnew=0; - Serial.print("DIYd"); // This is the message preamble - IMU_buffer[0]=0x13; - ck=19; - IMU_buffer[1] = 0x03; - - templong = lon; //Longitude *10**7 in 4 bytes - IMU_buffer[2]=templong&0xff; - IMU_buffer[3]=(templong>>8)&0xff; - IMU_buffer[4]=(templong>>16)&0xff; - IMU_buffer[5]=(templong>>24)&0xff; - - templong = lat; //Latitude *10**7 in 4 bytes - IMU_buffer[6]=templong&0xff; - IMU_buffer[7]=(templong>>8)&0xff; - IMU_buffer[8]=(templong>>16)&0xff; - IMU_buffer[9]=(templong>>24)&0xff; - - #if USE_BAROMETER==0 - tempint=alt_MSL / 100; // Altitude MSL in meters * 10 in 2 bytes - #else - alti(); - tempint = (press_alt * ALT_MIX + alt_MSL * (100-ALT_MIX)) / 10000; //Blended GPS and pressure altitude - #endif - IMU_buffer[10]=tempint&0xff; - IMU_buffer[11]=(tempint>>8)&0xff; - - tempint=speed_3d*100; // Speed in M/S * 100 in 2 bytes - IMU_buffer[12]=tempint&0xff; - IMU_buffer[13]=(tempint>>8)&0xff; - - tempint=ground_course*100; // course in degreees * 100 in 2 bytes - IMU_buffer[14]=tempint&0xff; - IMU_buffer[15]=(tempint>>8)&0xff; - - IMU_buffer[16]=0; - IMU_buffer[17]=0; - IMU_buffer[18]=0; - IMU_buffer[19]=0; - - IMU_buffer[20]=(imu_health>>8)&0xff; - - for (int i=0;i>8)&0xff; - - tempint=ToDeg(pitch)*100; //Pitch (degrees) * 100 in 2 bytes - IMU_buffer[4]=tempint&0xff; - IMU_buffer[5]=(tempint>>8)&0xff; - - templong=(ToDeg(yaw)+gc_offset)*100; //Yaw (degrees) * 100 in 2 bytes - if(templong>18000) templong -=36000; - if(templong<-18000) templong +=36000; - tempint = templong; - IMU_buffer[6]=tempint&0xff; - IMU_buffer[7]=(tempint>>8)&0xff; - - for (int i=0;i>8)&0xff; - IMU_buffer[4]=(time>>16)&0xff; - IMU_buffer[5]=(time>>24)&0xff; - - IMU_buffer[6]=mainLoop_count&0xff; - IMU_buffer[7]=(mainLoop_count>>8)&0xff; - - IMU_buffer[8]=G_Dt_max&0xff; - IMU_buffer[9]=(G_Dt_max>>8)&0xff; - - IMU_buffer[10]=gyro_sat_count; - IMU_buffer[11]=adc_constraints; - IMU_buffer[12]=renorm_sqrt_count; - IMU_buffer[13]=renorm_blowup_count; - IMU_buffer[14]=gps_payload_error_count; - IMU_buffer[15]=gps_checksum_error_count; - IMU_buffer[16]=gps_pos_fix_count; - IMU_buffer[17]=gps_nav_fix_count; - IMU_buffer[18]=gps_messages_sent; - - for (int i=0;i>8),DEC); - Serial.print(",***"); - - */ -#endif - // Reset counters - mainLoop_count = 0; - G_Dt_max = 0; - gyro_sat_count = 0; - adc_constraints = 0; - renorm_sqrt_count = 0; - renorm_blowup_count = 0; - gps_payload_error_count = 0; - gps_checksum_error_count = 0; - gps_pos_fix_count = 0; - gps_nav_fix_count = 0; - gps_messages_sent = 0; - - -} - - -long convert_to_dec(float x) -{ - return x*10000000; -} - 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 deleted file mode 100644 index 1af648e74e..0000000000 --- a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/press_alt.pde +++ /dev/null @@ -1,244 +0,0 @@ -/* 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 "modules/ins/ins_arduimu_basic.h" +//#include "firmwares/fixedwing/main_fbw.h" #include "mcu_periph/i2c.h" // test @@ -15,7 +15,6 @@ Autoren@ZHAW: schmiemi // für das Senden von GPS-Daten an den ArduIMU #include "gps.h" -int32_t GPS_Data[10]; #define NB_DATA 9 @@ -71,25 +70,12 @@ void ArduIMU_periodicGPS( void ) { if (ardu_gps_trans.status != I2CTransDone) { return; } - //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 - - 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); + FillBufWith32bit(ardu_gps_trans.buf, 0, (int32_t)gps_speed_3d); // speed 3D + FillBufWith32bit(ardu_gps_trans.buf, 4, (int32_t)gps_gspeed); // ground speed + FillBufWith32bit(ardu_gps_trans.buf, 8, (int32_t)gps_course); // course + ardu_gps_trans.buf[12] = gps_mode; // status gps fix + ardu_gps_trans.buf[13] = gps_status_flags; // status flags + I2CTransmit(ARDUIMU_I2C_DEV, ardu_gps_trans, ArduIMU_SLAVE_ADDR, 14); } @@ -100,21 +86,20 @@ void ArduIMU_periodic( void ) { 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" +/* + 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 + */ void ArduIMU_event( void ) { // Handle INS I2C event diff --git a/sw/airborne/modules/ins/ins_arduimu_modified.h b/sw/airborne/modules/ins/ins_arduimu_basic.h similarity index 100% rename from sw/airborne/modules/ins/ins_arduimu_modified.h rename to sw/airborne/modules/ins/ins_arduimu_basic.h From 8eab13010020a1c9c61afb52c0134a63e6a77074 Mon Sep 17 00:00:00 2001 From: Allen Date: Tue, 15 Mar 2011 11:13:20 -0700 Subject: [PATCH 008/101] Use libopenstm32 RCC setup for lisa_m --- sw/airborne/arch/stm32/mcu_arch.c | 6 +++++- sw/airborne/boards/lisa_m_1.0.h | 2 ++ 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/sw/airborne/arch/stm32/mcu_arch.c b/sw/airborne/arch/stm32/mcu_arch.c index 3c8a01fd10..132eb8c059 100644 --- a/sw/airborne/arch/stm32/mcu_arch.c +++ b/sw/airborne/arch/stm32/mcu_arch.c @@ -34,7 +34,11 @@ #include BOARD_CONFIG void mcu_arch_init(void) { - +#ifdef USE_OPENCM3 + rcc_clock_setup_in_hse_12mhz_out_72mhz(); + NVIC_SetVectorTable(NVIC_VectTab_FLASH, 0x0); + return; +#endif #ifdef HSE_TYPE_EXT_CLK #warning Using external clock /* Setup the microcontroller system. diff --git a/sw/airborne/boards/lisa_m_1.0.h b/sw/airborne/boards/lisa_m_1.0.h index a0941c8737..611896757f 100644 --- a/sw/airborne/boards/lisa_m_1.0.h +++ b/sw/airborne/boards/lisa_m_1.0.h @@ -28,6 +28,8 @@ #define BOARD_HAS_BARO +#define USE_OPENCM3 + #define HSE_TYPE_EXT_CLK #define STM32_RCC_MODE RCC_HSE_ON #define STM32_PLL_MULT RCC_PLLMul_6 From 7420e6bc3362da508103cce284c4388b0fdffbd9 Mon Sep 17 00:00:00 2001 From: Allen Date: Tue, 15 Mar 2011 11:25:52 -0700 Subject: [PATCH 009/101] Include the opencm3 version of rcc.h when requested --- sw/airborne/arch/stm32/mcu_arch.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/sw/airborne/arch/stm32/mcu_arch.c b/sw/airborne/arch/stm32/mcu_arch.c index 132eb8c059..74c461d000 100644 --- a/sw/airborne/arch/stm32/mcu_arch.c +++ b/sw/airborne/arch/stm32/mcu_arch.c @@ -30,6 +30,9 @@ #include #include #include +#ifdef USE_OPENCM3 +#include +#endif #include BOARD_CONFIG From f46ff79a3aeac16fa3604648baed0cf60b3d7282 Mon Sep 17 00:00:00 2001 From: Allen Date: Tue, 15 Mar 2011 11:57:58 -0700 Subject: [PATCH 010/101] Include board config first in mcu_arch --- sw/airborne/arch/stm32/mcu_arch.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sw/airborne/arch/stm32/mcu_arch.c b/sw/airborne/arch/stm32/mcu_arch.c index 74c461d000..9a6fb255ec 100644 --- a/sw/airborne/arch/stm32/mcu_arch.c +++ b/sw/airborne/arch/stm32/mcu_arch.c @@ -24,6 +24,7 @@ #include "mcu.h" +#include BOARD_CONFIG #include #include @@ -34,7 +35,6 @@ #include #endif -#include BOARD_CONFIG void mcu_arch_init(void) { #ifdef USE_OPENCM3 From 972d4a72fdd2b4ea146fe72f0c981874b512405c Mon Sep 17 00:00:00 2001 From: Allen Date: Tue, 15 Mar 2011 14:58:28 -0700 Subject: [PATCH 011/101] Use usart_set_baudrate to get correct bitrate for lis/m --- sw/airborne/arch/stm32/mcu_periph/uart_arch.c | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/sw/airborne/arch/stm32/mcu_periph/uart_arch.c b/sw/airborne/arch/stm32/mcu_periph/uart_arch.c index 3edce0c1d1..134023b577 100644 --- a/sw/airborne/arch/stm32/mcu_periph/uart_arch.c +++ b/sw/airborne/arch/stm32/mcu_periph/uart_arch.c @@ -29,6 +29,14 @@ #include #include "std.h" +#include BOARD_CONFIG + +#ifdef USE_OPENCM3 +#define pprz_usart_set_baudrate(x, y) usart_set_baudrate(x, y) +#else +#define pprz_usart_set_baudrate(x, y) do { } while(0); +#endif + #ifdef USE_UART1 volatile uint16_t uart1_rx_insert_idx, uart1_rx_extract_idx; @@ -74,6 +82,9 @@ void uart1_init( void ) { USART_Init(USART1, &usart); /* Enable USART1 Receive interrupts */ USART_ITConfig(USART1, USART_IT_RXNE, ENABLE); + + pprz_usart_set_baudrate(USART1, UART1_BAUD); + /* Enable the USART1 */ USART_Cmd(USART1, ENABLE); @@ -197,6 +208,9 @@ void uart2_init( void ) { USART_Init(USART2, &usart); /* Enable USART2 Receive interrupts */ USART_ITConfig(USART2, USART_IT_RXNE, ENABLE); + + pprz_usart_set_baudrate(USART2, UART2_BAUD); + /* Enable the USART2 */ USART_Cmd(USART2, ENABLE); @@ -320,6 +334,9 @@ void uart3_init( void ) { USART_Init(USART3, &usart); /* Enable USART3 Receive interrupts */ USART_ITConfig(USART3, USART_IT_RXNE, ENABLE); + + pprz_usart_set_baudrate(USART3, UART3_BAUD); + /* Enable the USART3 */ USART_Cmd(USART3, ENABLE); From 660a802d828841d9db6f80d12927610279df21e7 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Wed, 16 Mar 2011 10:38:14 +0100 Subject: [PATCH 012/101] remove startup delay --- conf/airframes/ENAC/quadrotor/blender.xml | 3 --- 1 file changed, 3 deletions(-) diff --git a/conf/airframes/ENAC/quadrotor/blender.xml b/conf/airframes/ENAC/quadrotor/blender.xml index 72313de308..ee48120890 100644 --- a/conf/airframes/ENAC/quadrotor/blender.xml +++ b/conf/airframes/ENAC/quadrotor/blender.xml @@ -18,9 +18,6 @@ - - - From 59ae731649c04b9237a2f8359d12ce041b4eaf16 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Tue, 23 Nov 2010 16:08:41 +0100 Subject: [PATCH 013/101] update pressure board sensor and settings --- conf/settings/pbn.xml | 9 ++++++ .../modules/sensors/pressure_board_navarro.c | 32 ++++++++++++++++--- .../modules/sensors/pressure_board_navarro.h | 1 + 3 files changed, 37 insertions(+), 5 deletions(-) create mode 100644 conf/settings/pbn.xml diff --git a/conf/settings/pbn.xml b/conf/settings/pbn.xml new file mode 100644 index 0000000000..97f53f5f32 --- /dev/null +++ b/conf/settings/pbn.xml @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/sw/airborne/modules/sensors/pressure_board_navarro.c b/sw/airborne/modules/sensors/pressure_board_navarro.c index 987dc09f44..63e57f2d25 100644 --- a/sw/airborne/modules/sensors/pressure_board_navarro.c +++ b/sw/airborne/modules/sensors/pressure_board_navarro.c @@ -46,6 +46,15 @@ /* Weight for offset IIR filter */ #define PBN_OFFSET_FILTER 7 +/* Quadratic scale factor for airspeed */ +#ifndef PBN_AIRSPEED_SCALE +#define PBN_AIRSPEED_SCALE (1./0.54) +#endif + +/* Linear scale factor for altitude */ +#ifndef PBN_ALTITUDE_SCALE +#define PBN_ALTITUDE_SCALE 0.32 +#endif // Global variables uint16_t altitude_adc; @@ -61,6 +70,7 @@ uint16_t altitude_offset; uint16_t airspeed_offset; float pbn_altitude; float pbn_airspeed; +float airspeed_filter; uint16_t startup_delay; void pbn_init( void ) { @@ -73,6 +83,7 @@ void pbn_init( void ) { offset_cnt = OFFSET_NBSAMPLES_AVRG; pbn_airspeed = 0.; pbn_altitude = 0.; + airspeed_filter = PBN_OFFSET_FILTER; } @@ -106,20 +117,31 @@ void pbn_read_event( void ) { if (offset_cnt > 0) { // IIR filter to compute an initial offset +#ifndef PBN_AIRSPEED_OFFSET airspeed_offset = (PBN_OFFSET_FILTER * airspeed_offset + airspeed_adc) / (PBN_OFFSET_FILTER + 1); +#else + airspeed_offset = PBN_AIRSPEED_OFFSET; +#endif +#ifndef PBN_ALTITUDE_OFFSET altitude_offset = (PBN_OFFSET_FILTER * altitude_offset + altitude_adc) / (PBN_OFFSET_FILTER + 1); +#else + altitude_offset = PBN_ALTITUDE_OFFSET; +#endif // decrease init counter --offset_cnt; } else { // Compute airspeed and altitude - pbn_airspeed = (-4.45 + sqrtf(19.84-0.57*(float)(airspeed_offset-airspeed_adc)))/0.28; - pbn_altitude = 0.32*(float)(altitude_adc-altitude_offset); + //pbn_airspeed = (-4.45 + sqrtf(19.84-0.57*(float)(airspeed_offset-airspeed_adc)))/0.28; + uint16_t diff = Max(airspeed_adc-airspeed_offset, 0); + float tmp_airspeed = sqrtf((float)diff * PBN_AIRSPEED_SCALE); + pbn_altitude = PBN_ALTITUDE_SCALE*(float)(altitude_adc-altitude_offset); - //estimator_airspeed = (7*estimator_airspeed + pbn_airspeed ) / 8; - //estimator_airspeed = Max(estimator_airspeed, 0.); - //EstimatorSetAirspeed(pbn_airspeed); + pbn_airspeed = (airspeed_filter*pbn_airspeed + tmp_airspeed) / (airspeed_filter + 1.); +#ifdef USE_AIRSPEED + EstimatorSetAirspeed(pbn_airspeed); +#endif //alt_kalman(pbn_altitude); } diff --git a/sw/airborne/modules/sensors/pressure_board_navarro.h b/sw/airborne/modules/sensors/pressure_board_navarro.h index 2bd4edba89..5b1fe68bff 100644 --- a/sw/airborne/modules/sensors/pressure_board_navarro.h +++ b/sw/airborne/modules/sensors/pressure_board_navarro.h @@ -43,6 +43,7 @@ extern uint16_t airspeed_adc; extern uint16_t altitude_offset; extern uint16_t airspeed_offset; extern float pbn_altitude, pbn_airspeed; +extern float airspeed_filter; extern bool_t data_valid; extern struct i2c_transaction pbn_trans; From dab24945b8f73b2bf016a54ae862d3980c87dd74 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Wed, 16 Mar 2011 11:10:13 +0100 Subject: [PATCH 014/101] update adaptive control loops traj ref is not used by default airspeed ratio is used whith USE_AIRSPEED Conflicts: conf/settings/fw_ctl_n.xml --- conf/settings/fw_ctl_n.xml | 40 +++++++ conf/telemetry/fw_h_ctl_a.xml | 2 +- .../fixedwing/guidance/guidance_v_n.h | 2 - .../stabilization/stabilization_adaptive.c | 111 ++++++++++++------ .../stabilization/stabilization_adaptive.h | 2 + 5 files changed, 120 insertions(+), 37 deletions(-) diff --git a/conf/settings/fw_ctl_n.xml b/conf/settings/fw_ctl_n.xml index eb3a8ac9e7..5f2fdab3fa 100644 --- a/conf/settings/fw_ctl_n.xml +++ b/conf/settings/fw_ctl_n.xml @@ -4,7 +4,47 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/telemetry/fw_h_ctl_a.xml b/conf/telemetry/fw_h_ctl_a.xml index f8b3dc8d19..7ec85992a5 100644 --- a/conf/telemetry/fw_h_ctl_a.xml +++ b/conf/telemetry/fw_h_ctl_a.xml @@ -38,7 +38,7 @@ - + diff --git a/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.h b/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.h index 4a6261d35b..d020a8cbb2 100644 --- a/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.h +++ b/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.h @@ -39,9 +39,7 @@ extern float v_ctl_auto_pitch_dgain; extern uint8_t v_ctl_speed_mode; -#ifdef PITCH_TRIM extern float v_ctl_pitch_loiter_trim; extern float v_ctl_pitch_dash_trim; -#endif #endif /* FW_V_CTL_N_H */ diff --git a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c index 4b138cd8b0..7aff370af2 100644 --- a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c +++ b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c @@ -60,11 +60,11 @@ float h_ctl_ref_roll_accel; float h_ctl_ref_pitch_angle; float h_ctl_ref_pitch_rate; float h_ctl_ref_pitch_accel; -#define H_CTL_REF_W 2.5 +#define H_CTL_REF_W 5. #define H_CTL_REF_XI 0.85 -#define H_CTL_REF_MAX_P RadOfDeg(100.) +#define H_CTL_REF_MAX_P RadOfDeg(150.) #define H_CTL_REF_MAX_P_DOT RadOfDeg(500.) -#define H_CTL_REF_MAX_Q RadOfDeg(100.) +#define H_CTL_REF_MAX_Q RadOfDeg(150.) #define H_CTL_REF_MAX_Q_DOT RadOfDeg(500.) /* inner roll loop parameters */ @@ -96,6 +96,13 @@ float h_ctl_aileron_of_throttle; float h_ctl_elevator_of_roll; float h_ctl_pitch_of_roll; // Should be used instead of elevator_of_roll +bool_t use_airspeed_ratio; +float airspeed_ratio2; +#define AIRSPEED_RATIO_MIN 0.5 +#define AIRSPEED_RATIO_MAX 2. + +float v_ctl_pitch_loiter_trim; +float v_ctl_pitch_dash_trim; inline static void h_ctl_roll_loop( void ); inline static void h_ctl_pitch_loop( void ); @@ -148,6 +155,17 @@ void h_ctl_init( void ) { h_ctl_pitch_of_roll = H_CTL_PITCH_OF_ROLL; #endif + use_airspeed_ratio = FALSE; + airspeed_ratio2 = 1.; + +#ifdef PITCH_TRIM + v_ctl_pitch_loiter_trim = V_CTL_PITCH_LOITER_TRIM; + v_ctl_pitch_dash_trim = V_CTL_PITCH_DASH_TRIM; +#else + v_ctl_pitch_loiter_trim = 0.; + v_ctl_pitch_dash_trim = 0.; +#endif + } /** @@ -175,24 +193,27 @@ void h_ctl_course_loop ( void ) { BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint); } -static float airspeed_ratio2; - +#ifdef USE_AIRSPEED static inline void compute_airspeed_ratio( void ) { - float throttle_diff = v_ctl_throttle_setpoint / (float)MAX_PPRZ - v_ctl_auto_throttle_nominal_cruise_throttle; - float airspeed = NOMINAL_AIRSPEED; /* Estimated from the throttle */ - if (throttle_diff > 0) - airspeed += throttle_diff / (V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE - v_ctl_auto_throttle_nominal_cruise_throttle) * (MAXIMUM_AIRSPEED - NOMINAL_AIRSPEED); - else - airspeed += throttle_diff / (v_ctl_auto_throttle_nominal_cruise_throttle - V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE) * (NOMINAL_AIRSPEED - MINIMUM_AIRSPEED); - + if (use_airspeed_ratio) { + // low pass airspeed + static float airspeed = 0.; + airspeed = ( 15*airspeed + estimator_airspeed ) / 16; + // compute ratio float airspeed_ratio = airspeed / NOMINAL_AIRSPEED; - Bound(airspeed_ratio, 0.5, 2.); + Bound(airspeed_ratio, AIRSPEED_RATIO_MIN, AIRSPEED_RATIO_MAX); airspeed_ratio2 = airspeed_ratio*airspeed_ratio; + } else { + airspeed_ratio2 = 1.; + } } +#endif void h_ctl_attitude_loop ( void ) { if (!h_ctl_disabled) { - // compute_airspeed_ratio(); +#ifdef USE_AIRSPEED + compute_airspeed_ratio(); +#endif h_ctl_roll_loop(); h_ctl_pitch_loop(); } @@ -209,6 +230,7 @@ inline static void h_ctl_roll_loop( void ) { if (pprz_mode == PPRZ_MODE_MANUAL) h_ctl_roll_sum_err = 0; +#ifdef USE_ANGLE_REF // Update reference setpoints for roll h_ctl_ref_roll_angle += h_ctl_ref_roll_rate * H_CTL_REF_DT; h_ctl_ref_roll_rate += h_ctl_ref_roll_accel * H_CTL_REF_DT; @@ -223,6 +245,11 @@ inline static void h_ctl_roll_loop( void ) { h_ctl_ref_roll_rate = - H_CTL_REF_MAX_P; if (h_ctl_ref_roll_accel < 0.) h_ctl_ref_roll_accel = 0.; } +#else + h_ctl_ref_roll_angle = h_ctl_roll_setpoint; + h_ctl_ref_roll_rate = 0.; + h_ctl_ref_roll_accel = 0.; +#endif #ifdef USE_KFF_UPDATE // update Kff gains @@ -242,26 +269,28 @@ inline static void h_ctl_roll_loop( void ) { estimator_p = (err - last_err)/(1/60.); last_err = err; #endif - float d_err = (estimator_p - h_ctl_ref_roll_rate) / H_CTL_REF_DT; + float d_err = estimator_p - h_ctl_ref_roll_rate; - h_ctl_roll_sum_err += err * H_CTL_REF_DT; - BoundAbs(h_ctl_roll_sum_err, H_CTL_ROLL_SUM_ERR_MAX); + if (h_ctl_roll_igain < 0.) { + h_ctl_roll_sum_err += err * H_CTL_REF_DT; + BoundAbs(h_ctl_roll_sum_err, H_CTL_ROLL_SUM_ERR_MAX / h_ctl_roll_igain); + } - cmd_fb = h_ctl_roll_attitude_gain * err;// + h_ctl_roll_rate_gain * derr; + cmd_fb = h_ctl_roll_attitude_gain * err;// + h_ctl_roll_rate_gain * d_err; float cmd = h_ctl_roll_Kffa * h_ctl_ref_roll_accel + h_ctl_roll_Kffd * h_ctl_ref_roll_rate - cmd_fb - h_ctl_roll_rate_gain * d_err - h_ctl_roll_igain * h_ctl_roll_sum_err + v_ctl_throttle_setpoint * h_ctl_aileron_of_throttle; - //float cmd = h_ctl_roll_Kffp * h_ctl_ref_roll_accel - // + h_ctl_roll_Kffd * h_ctl_ref_roll_rate - // - h_ctl_roll_attitude_gain * err - // - h_ctl_roll_rate_gain * derr - // - h_ctl_roll_igain * h_ctl_roll_sum_err - // + v_ctl_throttle_setpoint * h_ctl_aileron_of_throttle; - - //x cmd /= airspeed_ratio2; +// float cmd = h_ctl_roll_Kffa * h_ctl_ref_roll_accel +// + h_ctl_roll_Kffd * h_ctl_ref_roll_rate +// - h_ctl_roll_attitude_gain * err +// - h_ctl_roll_rate_gain * d_err +// - h_ctl_roll_igain * h_ctl_roll_sum_err +// + v_ctl_throttle_setpoint * h_ctl_aileron_of_throttle; +// + cmd /= airspeed_ratio2; // Set aileron commands h_ctl_aileron_setpoint = TRIM_PPRZ(cmd); @@ -275,12 +304,16 @@ float v_ctl_auto_throttle_dash_trim = V_CTL_AUTO_THROTTLE_DASH_TRIM; #endif #ifdef PITCH_TRIM -float v_ctl_pitch_loiter_trim = V_CTL_PITCH_LOITER_TRIM; -float v_ctl_pitch_dash_trim = V_CTL_PITCH_DASH_TRIM; - inline static void loiter(void) { float pitch_trim; +#ifdef USE_AIRSPEED + if (estimator_airspeed > NOMINAL_AIRSPEED) { + pitch_trim = v_ctl_pitch_dash_trim * (airspeed_ratio2-1) / ((AIRSPEED_RATIO_MAX * AIRSPEED_RATIO_MAX) - 1); + } else { + pitch_trim = v_ctl_pitch_loiter_trim * (airspeed_ratio2-1) / ((AIRSPEED_RATIO_MIN * AIRSPEED_RATIO_MIN) - 1); + } +#else float throttle_diff = v_ctl_throttle_setpoint / (float)MAX_PPRZ - v_ctl_auto_throttle_nominal_cruise_throttle; if (throttle_diff > 0) { float max_diff = Max(V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE - v_ctl_auto_throttle_nominal_cruise_throttle, 0.1); @@ -289,6 +322,7 @@ inline static void loiter(void) { float max_diff = Max(v_ctl_auto_throttle_nominal_cruise_throttle - V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE, 0.1); pitch_trim = -throttle_diff / max_diff * v_ctl_pitch_loiter_trim; } +#endif h_ctl_pitch_loop_setpoint += pitch_trim; } @@ -309,10 +343,11 @@ inline static void h_ctl_pitch_loop( void ) { loiter(); #endif +#ifdef USE_ANGLE_REF // Update reference setpoints for pitch - h_ctl_ref_pitch_accel = H_CTL_REF_W*H_CTL_REF_W * (h_ctl_pitch_loop_setpoint - h_ctl_ref_pitch_angle) - 2*H_CTL_REF_XI*H_CTL_REF_W * h_ctl_ref_pitch_rate; - h_ctl_ref_pitch_rate += h_ctl_ref_pitch_accel * H_CTL_REF_DT; h_ctl_ref_pitch_angle += h_ctl_ref_pitch_rate * H_CTL_REF_DT; + h_ctl_ref_pitch_rate += h_ctl_ref_pitch_accel * H_CTL_REF_DT; + h_ctl_ref_pitch_accel = H_CTL_REF_W*H_CTL_REF_W * (h_ctl_pitch_loop_setpoint - h_ctl_ref_pitch_angle) - 2*H_CTL_REF_XI*H_CTL_REF_W * h_ctl_ref_pitch_rate; // Saturation on references BoundAbs(h_ctl_ref_pitch_accel, H_CTL_REF_MAX_Q_DOT); if (h_ctl_ref_pitch_rate > H_CTL_REF_MAX_Q) { @@ -323,14 +358,21 @@ inline static void h_ctl_pitch_loop( void ) { h_ctl_ref_pitch_rate = - H_CTL_REF_MAX_Q; if (h_ctl_ref_pitch_accel < 0.) h_ctl_ref_pitch_accel = 0.; } +#else + h_ctl_ref_pitch_angle = h_ctl_pitch_loop_setpoint; + h_ctl_ref_pitch_rate = 0.; + h_ctl_ref_pitch_accel = 0.; +#endif // Compute errors float err = estimator_theta - h_ctl_ref_pitch_angle; float d_err = (err - last_err)/H_CTL_REF_DT - h_ctl_ref_pitch_rate; last_err = err; - h_ctl_pitch_sum_err += err * H_CTL_REF_DT; - BoundAbs(h_ctl_pitch_sum_err, H_CTL_ROLL_SUM_ERR_MAX); + if (h_ctl_roll_igain < 0.) { + h_ctl_pitch_sum_err += err * H_CTL_REF_DT; + BoundAbs(h_ctl_pitch_sum_err, H_CTL_PITCH_SUM_ERR_MAX / h_ctl_pitch_igain); + } float cmd = h_ctl_pitch_Kffa * h_ctl_ref_pitch_accel + h_ctl_pitch_Kffd * h_ctl_ref_pitch_rate @@ -338,7 +380,8 @@ inline static void h_ctl_pitch_loop( void ) { + h_ctl_pitch_dgain * d_err + h_ctl_pitch_igain * h_ctl_pitch_sum_err; - // cmd /= airspeed_ratio2; + cmd /= airspeed_ratio2; h_ctl_elevator_setpoint = TRIM_PPRZ(cmd); } + diff --git a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.h b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.h index eb084c9811..37e1a55638 100644 --- a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.h +++ b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.h @@ -69,4 +69,6 @@ extern float h_ctl_ref_pitch_angle; extern float h_ctl_ref_pitch_rate; extern float h_ctl_ref_pitch_accel; +extern bool_t use_airspeed_ratio; + #endif /* FW_H_CTL_A_H */ From 7aab04c094d22e4ac3fd109c0d316dbe1f535c32 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 16 Mar 2011 23:20:58 +0100 Subject: [PATCH 015/101] only link against opencm3_stm32 for lisa/m right now --- conf/Makefile.stm32 | 2 +- conf/boards/lisa_m_1.0.makefile | 4 ++++ 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/conf/Makefile.stm32 b/conf/Makefile.stm32 index be9a360e33..fb7fe7ea20 100644 --- a/conf/Makefile.stm32 +++ b/conf/Makefile.stm32 @@ -138,7 +138,7 @@ else LDFLAGS = -D__thumb2__ -T$(LDSCRIPT) -nostartfiles -L$(GCC_LIB_DIR) -O$(OPT) --gc-sections endif LDFLAGS += -Wl,-Map=$(OBJDIR)/$(TARGET).map,--cref,--gc-sections -LDLIBS += -lc -lm -lgcc -lcmsis -lstm32 -lopencm3_stm32 +LDLIBS += -lc -lm -lgcc -lcmsis -lstm32 CPFLAGS = -j .isr_vector -j .text -j .data CPFLAGS_BIN = -Obinary diff --git a/conf/boards/lisa_m_1.0.makefile b/conf/boards/lisa_m_1.0.makefile index 0a05903f17..9bb238e36a 100644 --- a/conf/boards/lisa_m_1.0.makefile +++ b/conf/boards/lisa_m_1.0.makefile @@ -14,6 +14,10 @@ $(TARGET).ARCHDIR = $(ARCH) $(TARGET).OOCD_INTERFACE=flossjtag #$(TARGET).OOCD_INTERFACE=jtagkey-tiny +# ---------------------------------------------------------------------- +# add the opencm3_stm32 lib +LDLIBS += -lopencm3_stm32 + # ----------------------------------------------------------------------- ifndef FLASH_MODE From 57a81a203f7cbc1d8fb6937602f86f13b35abfc8 Mon Sep 17 00:00:00 2001 From: Allen Date: Wed, 16 Mar 2011 16:25:04 -0700 Subject: [PATCH 016/101] Be sure to enable GPIO banks relevant to uart during init (reported by antoine) --- sw/airborne/arch/stm32/mcu_periph/uart_arch.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/sw/airborne/arch/stm32/mcu_periph/uart_arch.c b/sw/airborne/arch/stm32/mcu_periph/uart_arch.c index 134023b577..8e12791e79 100644 --- a/sw/airborne/arch/stm32/mcu_periph/uart_arch.c +++ b/sw/airborne/arch/stm32/mcu_periph/uart_arch.c @@ -50,6 +50,7 @@ uint8_t uart1_tx_buffer[UART1_TX_BUFFER_SIZE]; void uart1_init( void ) { /* init RCC */ RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE); + RCC_APB2PeriphClockCmd(UART1_Periph, ENABLE); /* Enable USART1 interrupts */ NVIC_InitTypeDef nvic; @@ -176,6 +177,7 @@ uint8_t uart2_tx_buffer[UART2_TX_BUFFER_SIZE]; void uart2_init( void ) { /* init RCC */ RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE); + RCC_APB2PeriphClockCmd(UART2_Periph, ENABLE); /* Enable USART2 interrupts */ NVIC_InitTypeDef nvic; @@ -301,6 +303,7 @@ void uart3_init( void ) { /* init RCC */ RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE); RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE); + RCC_APB2PeriphClockCmd(UART3_Periph, ENABLE); /* Enable USART3 interrupts */ NVIC_InitTypeDef nvic; From e276182ab51ff151531700e5e4bb887d09ad60e1 Mon Sep 17 00:00:00 2001 From: Allen Date: Wed, 16 Mar 2011 17:10:03 -0700 Subject: [PATCH 017/101] Add declaration for set_baudrate function to silence warning for now --- sw/airborne/arch/stm32/mcu_periph/uart_arch.c | 1 + 1 file changed, 1 insertion(+) diff --git a/sw/airborne/arch/stm32/mcu_periph/uart_arch.c b/sw/airborne/arch/stm32/mcu_periph/uart_arch.c index 8e12791e79..e9efcafa93 100644 --- a/sw/airborne/arch/stm32/mcu_periph/uart_arch.c +++ b/sw/airborne/arch/stm32/mcu_periph/uart_arch.c @@ -32,6 +32,7 @@ #include BOARD_CONFIG #ifdef USE_OPENCM3 +void usart_set_baudrate(u32 usart, u32 baud); #define pprz_usart_set_baudrate(x, y) usart_set_baudrate(x, y) #else #define pprz_usart_set_baudrate(x, y) do { } while(0); From da30664b231f7c6fca049f2dd31c5a78b3daa581 Mon Sep 17 00:00:00 2001 From: Allen Date: Wed, 16 Mar 2011 17:10:58 -0700 Subject: [PATCH 018/101] Add support for UART5 (for lisa/m) --- sw/airborne/arch/stm32/mcu_periph/uart_arch.c | 125 ++++++++++++++++++ sw/airborne/arch/stm32/mcu_periph/uart_arch.h | 26 +++- sw/airborne/mcu_periph/uart.h | 20 +++ 3 files changed, 170 insertions(+), 1 deletion(-) diff --git a/sw/airborne/arch/stm32/mcu_periph/uart_arch.c b/sw/airborne/arch/stm32/mcu_periph/uart_arch.c index e9efcafa93..672b777770 100644 --- a/sw/airborne/arch/stm32/mcu_periph/uart_arch.c +++ b/sw/airborne/arch/stm32/mcu_periph/uart_arch.c @@ -413,6 +413,128 @@ void usart3_irq_handler(void) { #endif /* USE_UART3 */ +#ifdef USE_UART5 + +volatile uint16_t uart5_rx_insert_idx, uart5_rx_extract_idx; +uint8_t uart5_rx_buffer[UART5_RX_BUFFER_SIZE]; + +volatile uint16_t uart5_tx_insert_idx, uart5_tx_extract_idx; +volatile bool_t uart5_tx_running; +uint8_t uart5_tx_buffer[UART5_TX_BUFFER_SIZE]; + +void uart5_init( void ) { + + /* init RCC */ + RCC_APB1PeriphClockCmd(RCC_APB1Periph_UART5, ENABLE); + RCC_APB2PeriphClockCmd(UART5_PeriphTx, ENABLE); + RCC_APB2PeriphClockCmd(UART5_PeriphRx, ENABLE); + + /* Enable UART5 interrupts */ + NVIC_InitTypeDef nvic; + nvic.NVIC_IRQChannel = UART5_IRQn; + nvic.NVIC_IRQChannelPreemptionPriority = 2; + nvic.NVIC_IRQChannelSubPriority = 1; + nvic.NVIC_IRQChannelCmd = ENABLE; + NVIC_Init(&nvic); + + /* Init GPIOS */ + GPIO_InitTypeDef gpio; + /* GPIOC: GPIO_Pin_10 UART5 Tx push-pull */ + gpio.GPIO_Pin = UART5_TxPin; + gpio.GPIO_Mode = GPIO_Mode_AF_PP; + gpio.GPIO_Speed = GPIO_Speed_50MHz; + GPIO_Init(UART5_TxPort, &gpio); + /* GPIOC: GPIO_Pin_11 UART5 Rx pin as floating input */ + gpio.GPIO_Pin = UART5_RxPin; + gpio.GPIO_Mode = GPIO_Mode_IN_FLOATING; + GPIO_Init(UART5_RxPort, &gpio); + + /* Configure UART5 */ + USART_InitTypeDef usart; + usart.USART_BaudRate = UART5_BAUD; + usart.USART_WordLength = USART_WordLength_8b; + usart.USART_StopBits = USART_StopBits_1; + usart.USART_Parity = USART_Parity_No; + usart.USART_HardwareFlowControl = USART_HardwareFlowControl_None; + usart.USART_Mode = USART_Mode_Rx | USART_Mode_Tx; + USART_Init(USART5, &usart); + /* Enable UART5 Receive interrupts */ + USART_ITConfig(UART5, USART_IT_RXNE, ENABLE); + + pprz_usart_set_baudrate(UART5, UART5_BAUD); + + /* Enable the UART5 */ + USART_Cmd(UART5, ENABLE); + + // initialize the transmit data queue + uart5_tx_extract_idx = 0; + uart5_tx_insert_idx = 0; + uart5_tx_running = FALSE; + + // initialize the receive data queuenn + uart5_rx_extract_idx = 0; + uart5_rx_insert_idx = 0; + +} + +void uart5_transmit( uint8_t data ) { + + uint16_t temp = (uart5_tx_insert_idx + 1) % UART5_TX_BUFFER_SIZE; + + if (temp == uart5_tx_extract_idx) + return; // no room + + USART_ITConfig(USART5, USART_IT_TXE, DISABLE); + + // check if in process of sending data + if (uart5_tx_running) { // yes, add to queue + uart5_tx_buffer[uart5_tx_insert_idx] = data; + uart5_tx_insert_idx = temp; + } + else { // no, set running flag and write to output register + uart5_tx_running = TRUE; + USART_SendData(USART5, data); + } + USART_ITConfig(USART5, USART_IT_TXE, ENABLE); + +} + +bool_t uart5_check_free_space( uint8_t len) { + int16_t space = uart5_tx_extract_idx - uart5_tx_insert_idx; + if (space <= 0) + space += UART5_TX_BUFFER_SIZE; + return (uint16_t)(space - 1) >= len; +} + + +void usart5_irq_handler(void) { + + if(USART_GetITStatus(USART5, USART_IT_TXE) != RESET){ + // check if more data to send + if (uart5_tx_insert_idx != uart5_tx_extract_idx) { + USART_SendData(USART5,uart5_tx_buffer[uart5_tx_extract_idx]); + uart5_tx_extract_idx++; + uart5_tx_extract_idx %= UART5_TX_BUFFER_SIZE; + } + else { + uart5_tx_running = FALSE; // clear running flag + USART_ITConfig(USART5, USART_IT_TXE, DISABLE); + } + } + + if(USART_GetITStatus(USART5, USART_IT_RXNE) != RESET){ + uint16_t temp = (uart5_rx_insert_idx + 1) % UART5_RX_BUFFER_SIZE;; + uart5_rx_buffer[uart5_rx_insert_idx] = USART_ReceiveData(USART5); + // check for more room in queue + if (temp != uart5_rx_extract_idx) + uart5_rx_insert_idx = temp; // update insert index + } + +} + + +#endif /* USE_UART5 */ + void uart_init( void ) { #ifdef USE_UART1 @@ -424,6 +546,9 @@ void uart_init( void ) #ifdef USE_UART3 uart3_init(); #endif +#ifdef USE_UART5 + uart5_init(); +#endif } diff --git a/sw/airborne/arch/stm32/mcu_periph/uart_arch.h b/sw/airborne/arch/stm32/mcu_periph/uart_arch.h index e281700dba..620c94e894 100644 --- a/sw/airborne/arch/stm32/mcu_periph/uart_arch.h +++ b/sw/airborne/arch/stm32/mcu_periph/uart_arch.h @@ -44,6 +44,7 @@ #define UART1_TxPin GPIO_Pin_9 #define UART2_TxPin GPIO_Pin_2 #define UART3_TxPin GPIO_Pin_10 +#define UART5_TxPin GPIO_Pin_12 #define UART1_RxPin GPIO_Pin_10 #define UART2_RxPin GPIO_Pin_3 @@ -53,6 +54,7 @@ #define UART1_TxPort GPIOA #define UART2_TxPort GPIOA #define UART3_TxPort GPIOC +#define UART5_TxPort GPIOC #define UART1_RxPort GPIOA #define UART2_RxPort GPIOA @@ -62,7 +64,8 @@ #define UART1_Periph RCC_APB2Periph_GPIOA #define UART2_Periph RCC_APB2Periph_GPIOA #define UART3_Periph RCC_APB2Periph_GPIOC -#define UART5_Periph RCC_APB2Periph_GPIOD +#define UART5_PeriphTx RCC_APB2Periph_GPIOC +#define UART5_PeriphRx RCC_APB2Periph_GPIOD #define UART1_UartPeriph RCC_APB2Periph_USART1 #define UART2_UartPeriph RCC_APB1Periph_USART2 @@ -180,6 +183,27 @@ extern uint8_t uart3_tx_buffer[UART3_TX_BUFFER_SIZE]; #endif /* USE_UART3 */ +#ifdef USE_UART5 + +#define UART5_RX_BUFFER_SIZE 128 +#define UART5_TX_BUFFER_SIZE 128 + +extern volatile uint16_t uart5_rx_insert_idx, uart5_rx_extract_idx; +extern uint8_t uart5_rx_buffer[UART5_RX_BUFFER_SIZE]; + +extern volatile uint16_t uart5_tx_insert_idx, uart5_tx_extract_idx; +extern volatile bool_t uart5_tx_running; +extern uint8_t uart5_tx_buffer[UART5_TX_BUFFER_SIZE]; + +#define Uart5ChAvailable() (uart5_rx_insert_idx != uart5_rx_extract_idx) +#define Uart5Getch() ({ \ + uint8_t ret = uart5_rx_buffer[uart5_rx_extract_idx]; \ + uart5_rx_extract_idx = (uart5_rx_extract_idx + 1)%UART5_RX_BUFFER_SIZE; \ + ret; \ + }) + +#endif /* USE_UART5 */ + void uart_init( void ); diff --git a/sw/airborne/mcu_periph/uart.h b/sw/airborne/mcu_periph/uart.h index 32bc3588d2..848fcbad74 100644 --- a/sw/airborne/mcu_periph/uart.h +++ b/sw/airborne/mcu_periph/uart.h @@ -121,4 +121,24 @@ extern bool_t uart3_check_free_space( uint8_t len); #endif /* USE_UART3 */ +#ifdef USE_UART5 + +extern void uart5_init( void ); +extern void uart5_transmit( uint8_t data ); +extern bool_t uart5_check_free_space( uint8_t len); + +#define Uart5Init uart5_init +#define Uart5CheckFreeSpace(_x) uart5_check_free_space(_x) +#define Uart5Transmit(_x) uart5_transmit(_x) +#define Uart5SendMessage() {} + +#define UART5Init Uart5Init +#define UART5CheckFreeSpace Uart5CheckFreeSpace +#define UART5Transmit Uart5Transmit +#define UART5SendMessage Uart5SendMessage +#define UART5ChAvailable Uart5ChAvailable +#define UART5Getch Uart5Getch + +#endif /* USE_UART5 */ + #endif /* MCU_PERIPH_UART_H */ From 4ee1023c825e3c992043391dd6c1d99c066265d6 Mon Sep 17 00:00:00 2001 From: Allen Date: Wed, 16 Mar 2011 17:11:19 -0700 Subject: [PATCH 019/101] Add UART5 to lisa/m uart test program --- conf/autopilot/lisa_m_test_progs.makefile | 1 + sw/airborne/lisa/test_uart_lisam.c | 10 ++++++++++ 2 files changed, 11 insertions(+) diff --git a/conf/autopilot/lisa_m_test_progs.makefile b/conf/autopilot/lisa_m_test_progs.makefile index bc4448dfaa..9d7722ef67 100644 --- a/conf/autopilot/lisa_m_test_progs.makefile +++ b/conf/autopilot/lisa_m_test_progs.makefile @@ -92,6 +92,7 @@ test_uart_lisam.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c test_uart_lisam.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600 test_uart_lisam.CFLAGS += -DUSE_UART2 -DUART2_BAUD=B57600 test_uart_lisam.CFLAGS += -DUSE_UART3 -DUART3_BAUD=B57600 +test_uart_lisam.CFLAGS += -DUSE_UART5 -DUART5_BAUD=B57600 test_uart_lisam.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c diff --git a/sw/airborne/lisa/test_uart_lisam.c b/sw/airborne/lisa/test_uart_lisam.c index 4e898b0793..368b31bed5 100644 --- a/sw/airborne/lisa/test_uart_lisam.c +++ b/sw/airborne/lisa/test_uart_lisam.c @@ -57,6 +57,7 @@ static inline void main_periodic( void ) { Uart1Transmit('a'); Uart2Transmit('b'); Uart3Transmit('c'); + Uart5Transmit('d'); LED_OFF(1); LED_OFF(2); @@ -87,4 +88,13 @@ static inline void main_periodic( void ) { LED_ON(2); } } + + if (Uart5ChAvailable()) { + ch = Uart5Getch(); + if (ch == 'd') { + LED_ON(1); + } else { + LED_ON(2); + } + } } From 64a7cdff211cf8b67c20505a5cc770d455867e1e Mon Sep 17 00:00:00 2001 From: Allen Date: Wed, 16 Mar 2011 19:25:38 -0700 Subject: [PATCH 020/101] Use ACTUATORS_PWM_NB in test_servos (to deal with varying number of servos) --- sw/airborne/lisa/test_servos.c | 11 +++-------- 1 file changed, 3 insertions(+), 8 deletions(-) diff --git a/sw/airborne/lisa/test_servos.c b/sw/airborne/lisa/test_servos.c index 533feedc1a..98ae30c1f2 100644 --- a/sw/airborne/lisa/test_servos.c +++ b/sw/airborne/lisa/test_servos.c @@ -50,14 +50,9 @@ static inline void main_periodic( void ) { static float foo = 0.; foo += 0.0025; int32_t bar = 1500 + 500. * sin(foo); - // int32_t bar = 1450; - // int32_t bar = 1950; - actuators_pwm_values[0] = bar; - actuators_pwm_values[1] = bar; - actuators_pwm_values[2] = bar; - actuators_pwm_values[3] = bar; - actuators_pwm_values[4] = bar; - actuators_pwm_values[5] = bar; + for (int i = 0; i < ACTUATORS_PWM_NB; i++) { + actuators_pwm_values[i] = bar; + } actuators_pwm_commit(); LED_PERIODIC(); From c2e745b1f9c0e7bb7c37e42accc85fbeae7203d5 Mon Sep 17 00:00:00 2001 From: Allen Date: Wed, 16 Mar 2011 19:26:37 -0700 Subject: [PATCH 021/101] Enable test_servos for lisa_m --- conf/airframes/esden/lisa_m_pwm.xml | 4 +-- conf/autopilot/lisa_m_test_progs.makefile | 32 +++++++++++------------ 2 files changed, 18 insertions(+), 18 deletions(-) diff --git a/conf/airframes/esden/lisa_m_pwm.xml b/conf/airframes/esden/lisa_m_pwm.xml index 0545cff7a6..021ce2072a 100644 --- a/conf/airframes/esden/lisa_m_pwm.xml +++ b/conf/airframes/esden/lisa_m_pwm.xml @@ -226,8 +226,8 @@ - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/settings/fw_ctl_n.xml b/conf/settings/fw_ctl_n.xml index 5f2fdab3fa..df393b0711 100644 --- a/conf/settings/fw_ctl_n.xml +++ b/conf/settings/fw_ctl_n.xml @@ -4,53 +4,19 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + @@ -94,8 +60,6 @@ - - diff --git a/conf/settings/infrared.xml b/conf/settings/infrared.xml new file mode 100644 index 0000000000..ee2d434796 --- /dev/null +++ b/conf/settings/infrared.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/conf/settings/ins_basic.xml b/conf/settings/ins_basic.xml new file mode 100644 index 0000000000..386a15c7db --- /dev/null +++ b/conf/settings/ins_basic.xml @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/conf/settings/ir_i2c.xml b/conf/settings/ir_i2c.xml index 40304d0c2a..b386b4c6d9 100644 --- a/conf/settings/ir_i2c.xml +++ b/conf/settings/ir_i2c.xml @@ -3,6 +3,17 @@ + + + + + + + + + + + diff --git a/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c b/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c index 839a0da20b..02ecaaf826 100644 --- a/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c +++ b/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c @@ -57,7 +57,7 @@ float v_ctl_auto_throttle_pgain; float v_ctl_auto_throttle_igain; float v_ctl_auto_throttle_dgain; float v_ctl_auto_throttle_sum_err; -#define V_CTL_AUTO_THROTTLE_MAX_SUM_ERR 150 +#define V_CTL_AUTO_THROTTLE_MAX_SUM_ERR 0.3 float v_ctl_auto_throttle_pitch_of_vz_pgain; float v_ctl_auto_throttle_pitch_of_vz_dgain; @@ -70,14 +70,21 @@ float v_ctl_auto_pitch_pgain; float v_ctl_auto_pitch_dgain; float v_ctl_auto_pitch_igain; float v_ctl_auto_pitch_sum_err; -#define V_CTL_AUTO_PITCH_MAX_SUM_ERR 100 +#define V_CTL_AUTO_PITCH_MAX_SUM_ERR (RadOfDeg(10.)) + +#ifndef V_CTL_AUTO_PITCH_DGAIN +#define V_CTL_AUTO_PITCH_DGAIN 0. +#endif +#ifndef V_CTL_AUTO_PITCH_IGAIN +#define V_CTL_AUTO_PITCH_IGAIN 0. +#endif float controlled_throttle; pprz_t v_ctl_throttle_setpoint; pprz_t v_ctl_throttle_slewed; // Set higher than 2*V_CTL_ALTITUDE_MAX_CLIMB to disable -#define V_CTL_AUTO_CLIMB_LIMIT 0.5/4.0 // m/s/s +#define V_CTL_AUTO_CLIMB_LIMIT (0.5/4.0) // m/s/s uint8_t v_ctl_speed_mode; @@ -181,16 +188,21 @@ void v_ctl_altitude_loop( void ) { static inline void v_ctl_set_pitch ( void ) { static float last_err = 0.; + if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) + v_ctl_auto_pitch_sum_err = 0; + // Compute errors float err = estimator_z_dot - v_ctl_climb_setpoint; float d_err = err - last_err; last_err = err; - v_ctl_auto_pitch_sum_err += err*(1./60.); - BoundAbs(v_ctl_auto_pitch_sum_err, V_CTL_AUTO_PITCH_MAX_SUM_ERR); + if (v_ctl_auto_pitch_igain < 0.) { + v_ctl_auto_pitch_sum_err += err*(1./60.); + BoundAbs(v_ctl_auto_pitch_sum_err, V_CTL_AUTO_PITCH_MAX_SUM_ERR / v_ctl_auto_pitch_igain); + } // PI loop + feedforward ctl - nav_pitch = nav_pitch + nav_pitch = 0. //nav_pitch FIXME it really sucks ! + v_ctl_auto_throttle_pitch_of_vz_pgain * v_ctl_climb_setpoint + v_ctl_auto_pitch_pgain * err + v_ctl_auto_pitch_dgain * d_err @@ -201,13 +213,18 @@ static inline void v_ctl_set_pitch ( void ) { static inline void v_ctl_set_throttle( void ) { static float last_err = 0.; + if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) + v_ctl_auto_throttle_sum_err = 0; + // Compute errors float err = estimator_z_dot - v_ctl_climb_setpoint; float d_err = err - last_err; last_err = err; - v_ctl_auto_throttle_sum_err += err*(1./60.); - BoundAbs(v_ctl_auto_throttle_sum_err, V_CTL_AUTO_THROTTLE_MAX_SUM_ERR); + if (v_ctl_auto_throttle_igain < 0.) { + v_ctl_auto_throttle_sum_err += err*(1./60.); + BoundAbs(v_ctl_auto_throttle_sum_err, V_CTL_AUTO_THROTTLE_MAX_SUM_ERR / v_ctl_auto_throttle_igain); + } // PID loop + feedforward ctl controlled_throttle = v_ctl_auto_throttle_cruise_throttle @@ -269,8 +286,7 @@ void v_ctl_climb_loop ( void ) { } // Set Throttle output - float f_throttle = controlled_throttle; - v_ctl_throttle_setpoint = TRIM_UPPRZ(f_throttle * MAX_PPRZ); + v_ctl_throttle_setpoint = TRIM_UPPRZ(controlled_throttle * MAX_PPRZ); } @@ -279,7 +295,7 @@ void v_ctl_climb_loop ( void ) { #endif #ifndef V_CTL_THROTTLE_SLEW -#define V_CTL_THROTTLE_SLEW 1. +#define V_CTL_THROTTLE_SLEW (1.) #endif /** \brief Computes slewed throttle from throttle setpoint called at 20Hz diff --git a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c index 7aff370af2..29b3489cc5 100644 --- a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c +++ b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c @@ -107,17 +107,44 @@ float v_ctl_pitch_dash_trim; inline static void h_ctl_roll_loop( void ); inline static void h_ctl_pitch_loop( void ); +// Some default course gains +// H_CTL_COURSE_PGAIN needs to be define in airframe #ifndef H_CTL_COURSE_PRE_BANK_CORRECTION #define H_CTL_COURSE_PRE_BANK_CORRECTION 1. #endif - #ifndef H_CTL_COURSE_DGAIN #define H_CTL_COURSE_DGAIN 0. #endif +// Some default roll gains +// H_CTL_ROLL_ATTITUDE_GAIN needs to be define in airframe #ifndef H_CTL_ROLL_RATE_GAIN #define H_CTL_ROLL_RATE_GAIN 0. #endif +#ifndef H_CTL_ROLL_IGAIN +#define H_CTL_ROLL_IGAIN 0. +#endif +#ifndef H_CTL_ROLL_KFFA +#define H_CTL_ROLL_KFFA 0. +#endif +#ifndef H_CTL_ROLL_KFFD +#define H_CTL_ROLL_KFFD 0. +#endif + +// Some default pitch gains +// H_CTL_PITCH_PGAIN needs to be define in airframe +#ifndef H_CTL_PITCH_DGAIN +#define H_CTL_PITCH_DGAIN 0. +#endif +#ifndef H_CTL_PITCH_IGAIN +#define H_CTL_PITCH_IGAIN 0. +#endif +#ifndef H_CTL_PITCH_KFFA +#define H_CTL_PITCH_KFFA 0. +#endif +#ifndef H_CTL_PITCH_KFFD +#define H_CTL_PITCH_KFFD 0. +#endif void h_ctl_init( void ) { h_ctl_course_setpoint = 0.; @@ -158,7 +185,7 @@ void h_ctl_init( void ) { use_airspeed_ratio = FALSE; airspeed_ratio2 = 1.; -#ifdef PITCH_TRIM +#ifdef USE_PITCH_TRIM v_ctl_pitch_loiter_trim = V_CTL_PITCH_LOITER_TRIM; v_ctl_pitch_dash_trim = V_CTL_PITCH_DASH_TRIM; #else @@ -227,9 +254,6 @@ inline static void h_ctl_roll_loop( void ) { static float cmd_fb = 0.; - if (pprz_mode == PPRZ_MODE_MANUAL) - h_ctl_roll_sum_err = 0; - #ifdef USE_ANGLE_REF // Update reference setpoints for roll h_ctl_ref_roll_angle += h_ctl_ref_roll_rate * H_CTL_REF_DT; @@ -271,9 +295,16 @@ inline static void h_ctl_roll_loop( void ) { #endif float d_err = estimator_p - h_ctl_ref_roll_rate; - if (h_ctl_roll_igain < 0.) { - h_ctl_roll_sum_err += err * H_CTL_REF_DT; - BoundAbs(h_ctl_roll_sum_err, H_CTL_ROLL_SUM_ERR_MAX / h_ctl_roll_igain); + if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) { + h_ctl_roll_sum_err = 0.; + } + else { + if (h_ctl_roll_igain < 0.) { + h_ctl_roll_sum_err += err * H_CTL_REF_DT; + BoundAbs(h_ctl_roll_sum_err, (- H_CTL_ROLL_SUM_ERR_MAX / h_ctl_roll_igain)); + } else { + h_ctl_roll_sum_err = 0.; + } } cmd_fb = h_ctl_roll_attitude_gain * err;// + h_ctl_roll_rate_gain * d_err; @@ -303,7 +334,7 @@ float v_ctl_auto_throttle_loiter_trim = V_CTL_AUTO_THROTTLE_LOITER_TRIM; float v_ctl_auto_throttle_dash_trim = V_CTL_AUTO_THROTTLE_DASH_TRIM; #endif -#ifdef PITCH_TRIM +#ifdef USE_PITCH_TRIM inline static void loiter(void) { float pitch_trim; @@ -335,11 +366,8 @@ inline static void h_ctl_pitch_loop( void ) { if (h_ctl_pitch_of_roll <0.) h_ctl_pitch_of_roll = 0.; - if (pprz_mode == PPRZ_MODE_MANUAL) - h_ctl_pitch_sum_err = 0; - h_ctl_pitch_loop_setpoint = h_ctl_pitch_setpoint + h_ctl_pitch_of_roll * fabs(estimator_phi); -#ifdef PITCH_TRIM +#ifdef USE_PITCH_TRIM loiter(); #endif @@ -369,9 +397,16 @@ inline static void h_ctl_pitch_loop( void ) { float d_err = (err - last_err)/H_CTL_REF_DT - h_ctl_ref_pitch_rate; last_err = err; - if (h_ctl_roll_igain < 0.) { - h_ctl_pitch_sum_err += err * H_CTL_REF_DT; - BoundAbs(h_ctl_pitch_sum_err, H_CTL_PITCH_SUM_ERR_MAX / h_ctl_pitch_igain); + if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) { + h_ctl_pitch_sum_err = 0.; + } + else { + if (h_ctl_pitch_igain < 0.) { + h_ctl_pitch_sum_err += err * H_CTL_REF_DT; + BoundAbs(h_ctl_pitch_sum_err, (- H_CTL_PITCH_SUM_ERR_MAX / h_ctl_pitch_igain)); + } else { + h_ctl_pitch_sum_err = 0.; + } } float cmd = h_ctl_pitch_Kffa * h_ctl_ref_pitch_accel diff --git a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.h b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.h index 37e1a55638..4a7c082f22 100644 --- a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.h +++ b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.h @@ -46,16 +46,16 @@ extern float h_ctl_pitch_Kffa; extern float h_ctl_pitch_Kffd; extern float h_ctl_pitch_of_roll; -#define H_CTL_ROLL_SUM_ERR_MAX 100. -#define H_CTL_PITCH_SUM_ERR_MAX 100. +#define H_CTL_ROLL_SUM_ERR_MAX (MAX_PPRZ/2.) +#define H_CTL_PITCH_SUM_ERR_MAX (MAX_PPRZ/2.) #define fw_h_ctl_a_SetRollIGain(_gain) { \ - h_ctl_roll_sum_err = 0; \ + h_ctl_roll_sum_err = 0.; \ h_ctl_roll_igain = _gain; \ } #define fw_h_ctl_a_SetPitchIGain(_gain) { \ - h_ctl_pitch_sum_err = 0; \ + h_ctl_pitch_sum_err = 0.; \ h_ctl_pitch_igain = _gain; \ } From 4c804a1dcaa613c114c97fe79904720ca51ff04c Mon Sep 17 00:00:00 2001 From: Antoine Drouin Date: Tue, 22 Mar 2011 16:14:34 +0100 Subject: [PATCH 044/101] rotorcraft uses electrical subsystem like fixedwing - both for lpc21 and stm32 --- conf/autopilot/rotorcraft.makefile | 21 +- sw/airborne/arch/lpc21/mcu_periph/dac_arch.c | 6 + sw/airborne/arch/lpc21/mcu_periph/dac_arch.h | 12 + sw/airborne/boards/booz/baro_board.c | 14 +- sw/airborne/boards/booz/baro_board.h | 17 +- sw/airborne/boards/booz_1.0.h | 18 ++ sw/airborne/booz/arch/lpc21/booz2_analog_hw.c | 229 ------------------ sw/airborne/booz/arch/lpc21/booz2_analog_hw.h | 36 --- sw/airborne/booz/arch/sim/booz2_analog_hw.c | 29 --- sw/airborne/booz/arch/sim/booz2_analog_hw.h | 31 --- sw/airborne/booz/arch/stm32/booz2_analog_hw.h | 33 --- sw/airborne/booz/booz2_analog.c | 49 ---- sw/airborne/booz/booz2_analog.h | 47 ---- sw/airborne/firmwares/rotorcraft/main.c | 16 +- sw/airborne/firmwares/rotorcraft/telemetry.h | 66 ++--- sw/airborne/mcu.c | 6 + sw/airborne/mcu_periph/dac.h | 9 + 17 files changed, 119 insertions(+), 520 deletions(-) create mode 100644 sw/airborne/arch/lpc21/mcu_periph/dac_arch.c create mode 100644 sw/airborne/arch/lpc21/mcu_periph/dac_arch.h delete mode 100644 sw/airborne/booz/arch/lpc21/booz2_analog_hw.c delete mode 100644 sw/airborne/booz/arch/lpc21/booz2_analog_hw.h delete mode 100644 sw/airborne/booz/arch/sim/booz2_analog_hw.c delete mode 100644 sw/airborne/booz/arch/sim/booz2_analog_hw.h delete mode 100644 sw/airborne/booz/arch/stm32/booz2_analog_hw.h delete mode 100644 sw/airborne/booz/booz2_analog.c delete mode 100644 sw/airborne/booz/booz2_analog.h create mode 100644 sw/airborne/mcu_periph/dac.h diff --git a/conf/autopilot/rotorcraft.makefile b/conf/autopilot/rotorcraft.makefile index 8966f4bed8..69a38e4fa8 100644 --- a/conf/autopilot/rotorcraft.makefile +++ b/conf/autopilot/rotorcraft.makefile @@ -134,7 +134,7 @@ ap.srcs += $(SRC_BOOZ)/booz2_commands.c # ap.srcs += $(SRC_BOARD)/baro_board.c ifeq ($(BOARD), booz) -ap.CFLAGS += -DROTORCRAFT_BARO_LED=$(BARO_LED) -DBOOZ2_ANALOG_BARO_PERIOD='SYS_TICS_OF_SEC((1./100.))' +ap.CFLAGS += -DROTORCRAFT_BARO_LED=$(BARO_LED) else ifeq ($(BOARD), lisa_l) ap.CFLAGS += -DUSE_I2C2 endif @@ -142,18 +142,27 @@ endif # # Analog Backend # + ifeq ($(ARCH), lpc21) -ap.CFLAGS += -DBOOZ2_ANALOG_BATTERY_PERIOD='SYS_TICS_OF_SEC((1./10.))' -ap.srcs += $(SRC_FIRMWARE)/battery.c ap.CFLAGS += -DADC0_VIC_SLOT=2 ap.CFLAGS += -DADC1_VIC_SLOT=3 -ap.srcs += $(SRC_BOOZ)/booz2_analog.c \ - $(SRC_BOOZ_ARCH)/booz2_analog_hw.c +ap.CFLAGS += -DUSE_ADC +ap.srcs += $(SRC_ARCH)/mcu_periph/adc_arch.c +ap.srcs += subsystems/electrical.c +# baro has variable offset amplifier on booz board +ap.CFLAGS += -DUSE_DAC +ap.srcs += $(SRC_ARCH)/mcu_periph/dac_arch.c else ifeq ($(ARCH), stm32) -ap.srcs += lisa/lisa_analog_plug.c +#ap.srcs += lisa/lisa_analog_plug.c +ap.CFLAGS += -DUSE_ADC +ap.CFLAGS += -DUSE_AD1 -DUSE_AD1_1 -DUSE_AD1_2 -DUSE_AD1_3 -DUSE_AD1_4 +ap.CFLAGS += -DUSE_ADC1_2_IRQ_HANDLER +ap.srcs += $(SRC_ARCH)/mcu_periph/adc_arch.c +ap.srcs += subsystems/electrical.c endif + # # GPS choice # diff --git a/sw/airborne/arch/lpc21/mcu_periph/dac_arch.c b/sw/airborne/arch/lpc21/mcu_periph/dac_arch.c new file mode 100644 index 0000000000..adda46e12b --- /dev/null +++ b/sw/airborne/arch/lpc21/mcu_periph/dac_arch.c @@ -0,0 +1,6 @@ +#include "mcu_periph/dac.h" + +/* turn on DAC pins */ +void dac_init(void) { + PINSEL1 |= 2 << 18; +} diff --git a/sw/airborne/arch/lpc21/mcu_periph/dac_arch.h b/sw/airborne/arch/lpc21/mcu_periph/dac_arch.h new file mode 100644 index 0000000000..e27b929e34 --- /dev/null +++ b/sw/airborne/arch/lpc21/mcu_periph/dac_arch.h @@ -0,0 +1,12 @@ +#ifndef LPC21_MCU_PERIPH_DAC_ARCH_H +#define LPC21_MCU_PERIPH_DAC_ARCH_H + +#include "std.h" +#include "LPC21xx.h" + +static inline void DACSet(uint16_t x) { + DACR = x << 6; +} + + +#endif /* LPC21_MCU_PERIPH_DAC_ARCH_H */ diff --git a/sw/airborne/boards/booz/baro_board.c b/sw/airborne/boards/booz/baro_board.c index aa43c684a5..4277379a87 100644 --- a/sw/airborne/boards/booz/baro_board.c +++ b/sw/airborne/boards/booz/baro_board.c @@ -37,7 +37,10 @@ struct Baro baro; struct BaroBoard baro_board; + void baro_init( void ) { + + adc_buf_channel(ADC_CHANNEL_BARO, &baro_board.buf, DEFAULT_AV_NB_SAMPLE); baro.status = BS_UNINITIALIZED; baro.absolute = 0; @@ -53,7 +56,16 @@ void baro_init( void ) { #endif } -void baro_periodic(void) {} +void baro_periodic(void) { + + baro.absolute = baro_board.buf.sum/baro_board.buf.av_nb_sample; + baro_board.value_filtered = (3*baro_board.value_filtered + baro.absolute)/4; + if (baro.status == BS_UNINITIALIZED) { + RunOnceEvery(10, { baro_board_calibrate();}); + } + /* else */ + baro_board.data_available = TRUE; +} /* decrement offset until adc reading is over a threshold */ void baro_board_calibrate(void) { diff --git a/sw/airborne/boards/booz/baro_board.h b/sw/airborne/boards/booz/baro_board.h index f1fae16a48..f24568adae 100644 --- a/sw/airborne/boards/booz/baro_board.h +++ b/sw/airborne/boards/booz/baro_board.h @@ -4,14 +4,15 @@ #include "std.h" #include "subsystems/sensors/baro.h" -#include "booz/booz2_analog.h" +#include "mcu_periph/adc.h" +#include "booz/booz2_analog.h" // le DAC -/* we don't need that on this board */ struct BaroBoard { uint16_t offset; uint16_t value_filtered; bool_t data_available; + struct adc_buf buf; }; extern struct BaroBoard baro_board; @@ -30,17 +31,5 @@ static inline void baro_board_SetOffset(uint16_t _o) { Booz2AnalogSetDAC(_o); } -static inline void BoozBaroISRHandler(uint16_t _val) { - baro.absolute = _val; - baro_board.value_filtered = (3*baro_board.value_filtered + baro.absolute)/4; - if (baro.status == BS_UNINITIALIZED) { - RunOnceEvery(10, { baro_board_calibrate();}); - } - /* else */ - baro_board.data_available = TRUE; -} - - - #endif /* BOARDS_BOOZ_BARO_H */ diff --git a/sw/airborne/boards/booz_1.0.h b/sw/airborne/boards/booz_1.0.h index 9f44e89514..4d9dda8dc6 100644 --- a/sw/airborne/boards/booz_1.0.h +++ b/sw/airborne/boards/booz_1.0.h @@ -61,6 +61,24 @@ #define ANALOG_BARO_ADC 1 +/* battery */ +#define ADC_CHANNEL_VSUPPLY AdcBank0(2) +#ifndef USE_AD0 +#define USE_AD0 +#endif +#define USE_AD0_2 + +#define DefaultVoltageOfAdc(adc) (0.0183*adc) + + +/* baro */ +#define ADC_CHANNEL_BARO AdcBank1(2) +#ifndef USE_AD1 +#define USE_AD1 +#endif +#define USE_AD1_2 + + /* MS2100 on SSP, IMU connector */ #define MS2100_SS_PIN 28 diff --git a/sw/airborne/booz/arch/lpc21/booz2_analog_hw.c b/sw/airborne/booz/arch/lpc21/booz2_analog_hw.c deleted file mode 100644 index a991d3e3ed..0000000000 --- a/sw/airborne/booz/arch/lpc21/booz2_analog_hw.c +++ /dev/null @@ -1,229 +0,0 @@ -/* - * $Id$ - * - * Copyright (C) 2008-2009 Antoine Drouin - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - */ - -#include "booz2_analog.h" - -/* analog_arch includes baro ??? naaaa we don't want double references */ -#include "subsystems/sensors/baro.h" -#include "firmwares/rotorcraft/battery.h" - -#ifndef USE_EXTRA_ADC -/* Default mode - * Bust OFF - * Only one ADC can be read on each bank - * Baro and Bat are read on interrupt - */ - -#include "armVIC.h" -#include "sys_time.h" - -void ADC0_ISR ( void ) __attribute__((naked)); -void ADC1_ISR ( void ) __attribute__((naked)); - -void booz2_analog_init_hw( void ) { - - /* start ADC0 */ - /* select P0.29 as AD0.2 for bat meas*/ - PINSEL1 |= 0x01 << 26; - /* sample AD0.2 - PCLK/4 ( 3.75MHz) - ON */ - AD0CR = 1 << 2 | 0x03 << 8 | 1 << 21; - /* AD0 selected as IRQ */ - VICIntSelect &= ~VIC_BIT(VIC_AD0); - /* AD0 interrupt enabled */ - VICIntEnable = VIC_BIT(VIC_AD0); - /* AD0 interrupt as VIC2 */ - _VIC_CNTL(ADC0_VIC_SLOT) = VIC_ENABLE | VIC_AD0; - _VIC_ADDR(ADC0_VIC_SLOT) = (uint32_t)ADC0_ISR; - /* start convertion on T0M1 match */ - AD0CR |= 4 << 24; - - - /* clear match 1 */ - T0EMR &= ~TEMR_EM1; - /* set high on match 1 */ - T0EMR |= TEMR_EMC1_2; - /* first match in a while */ - T0MR1 = 1024; - - - /* start ADC1 */ - /* select P0.10 as AD1.2 for baro*/ - ANALOG_BARO_PINSEL |= ANALOG_BARO_PINSEL_VAL << ANALOG_BARO_PINSEL_BIT; - /* sample AD1.2 - PCLK/4 ( 3.75MHz) - ON */ - AD1CR = 1 << 2 | 0x03 << 8 | 1 << 21; - /* AD0 selected as IRQ */ - VICIntSelect &= ~VIC_BIT(VIC_AD1); - /* AD0 interrupt enabled */ - VICIntEnable = VIC_BIT(VIC_AD1); - /* AD0 interrupt as VIC2 */ - _VIC_CNTL(ADC1_VIC_SLOT) = VIC_ENABLE | VIC_AD1; - _VIC_ADDR(ADC1_VIC_SLOT) = (uint32_t)ADC1_ISR; - /* start convertion on T0M3 match */ - AD1CR |= 5 << 24; - - - /* clear match 2 */ - T0EMR &= ~TEMR_EM3; - /* set high on match 2 */ - T0EMR |= TEMR_EMC3_2; - /* first match in a while */ - T0MR3 = 512; - - /* turn on DAC pins */ - PINSEL1 |= 2 << 18; -} - - -void ADC0_ISR ( void ) { - ISR_ENTRY(); - uint32_t tmp = AD0GDR; - uint16_t tmp2 = (uint16_t)(tmp >> 6) & 0x03FF; - BatteryISRHandler(tmp2); - /* trigger next convertion */ - T0MR1 += BOOZ2_ANALOG_BATTERY_PERIOD; - /* lower clock */ - T0EMR &= ~TEMR_EM1; - VICVectAddr = 0x00000000; // clear this interrupt from the VIC - ISR_EXIT(); // recover registers and return -} - -void ADC1_ISR ( void ) { - ISR_ENTRY(); - uint32_t tmp = AD1GDR; - uint16_t tmp2 = (uint16_t)(tmp >> 6) & 0x03FF; - BoozBaroISRHandler(tmp2); - /* trigger next convertion */ - T0MR3 += BOOZ2_ANALOG_BARO_PERIOD; - /* lower clock */ - T0EMR &= ~TEMR_EM3; - VICVectAddr = 0x00000000; // clear this interrupt from the VIC - ISR_EXIT(); // recover registers and return -} - -#else // USE_EXTRA_ADC -/* Extra ADCs are read - * Bust ON - * Baro and Bat values are updated by hand - * Four ADCs can be configured - * ADC_1 is available on the cam connector - */ - -#include "LPC21xx.h" -#include "sys_time.h" - -uint16_t booz2_adc_1; -uint16_t booz2_adc_2; -uint16_t booz2_adc_3; -uint16_t booz2_adc_4; - -void booz2_analog_init_hw( void ) { - - /* AD0 */ - /* PCLK/4 ( 3.75MHz) - BURST - ON */ - AD0CR = 0x03 << 8 | 1 << 16 | 1 << 21; - /* disable global interrupt */ - ClearBit(AD0INTEN,8); - - /* AD1 */ - /* PCLK/4 ( 3.75MHz) - BURST - ON */ - AD1CR = 0x03 << 8 | 1 << 16 | 1 << 21; - /* disable global interrupt */ - ClearBit(AD1INTEN,8); - - /* select P0.29 as AD0.2 for bat meas*/ - PINSEL1 |= 0x01 << 26; - /* sample AD0.2 */ - AD0CR |= 1 << 2; - - - /* select P0.10 as AD1.2 for baro*/ - ANALOG_BARO_PINSEL |= ANALOG_BARO_PINSEL_VAL << ANALOG_BARO_PINSEL_BIT; - /* sample AD1.2 */ - AD1CR |= 1 << 2; - /* turn on DAC pins */ - PINSEL1 |= 2 << 18; - -#ifdef USE_ADC_1 - /* select P0.13 (ADC_SPARE) as AD1.4 adc 1 */ - PINSEL0 |= 0x03 << 26; - AD1CR |= 1 << 4; -#endif -#ifdef USE_ADC_2 - /* select P0.4 (SCK_0) as AD0.6 adc 2 */ - PINSEL0 |= 0x03 << 8; - AD0CR |= 1 << 6; -#endif -#ifdef USE_ADC_3 - /* select P0.5 (MISO_0) as AD0.7 adc 3 */ - PINSEL0 |= 0x03 << 10; - AD0CR |= 1 << 7; -#endif -#ifdef USE_ADC_4 - /* select P0.6 (MOSI_0) as AD1.0 adc 4 */ - PINSEL0 |= 0x03 << 12; - AD1CR |= 1 << 0; -#endif - - booz2_adc_1 = 0; - booz2_adc_2 = 0; - booz2_adc_3 = 0; - booz2_adc_4 = 0; -} - -void booz2_analog_baro_read(void) { - uint32_t tmp = AD1DR2; - uint16_t tmp2 = (uint16_t)(tmp >> 6) & 0x03FF; - BoozBaroISRHandler(tmp2); -} - -void booz2_analog_bat_read(void) { - uint32_t tmp = AD0DR2; - uint16_t tmp2 = (uint16_t)(tmp >> 6) & 0x03FF; - BatteryISRHandler(tmp2); -} - -void booz2_analog_extra_adc_read(void) { - uint32_t tmp,tmp2; -#ifdef USE_ADC_1 - tmp = AD1DR4; - tmp2 = (uint16_t)(tmp >> 6) & 0x03FF; - booz2_adc_1 = tmp2; -#endif -#ifdef USE_ADC_2 - tmp = AD0DR6; - tmp2 = (uint16_t)(tmp >> 6) & 0x03FF; - booz2_adc_2 = tmp2; -#endif -#ifdef USE_ADC_3 - tmp = AD0DR7; - tmp2 = (uint16_t)(tmp >> 6) & 0x03FF; - booz2_adc_3 = tmp2; -#endif -#ifdef USE_ADC_4 - tmp = AD1DR0; - tmp2 = (uint16_t)(tmp >> 6) & 0x03FF; - booz2_adc_4 = tmp2; -#endif -} - -#endif // USE_EXTRA_ADC diff --git a/sw/airborne/booz/arch/lpc21/booz2_analog_hw.h b/sw/airborne/booz/arch/lpc21/booz2_analog_hw.h deleted file mode 100644 index ae87ad0c84..0000000000 --- a/sw/airborne/booz/arch/lpc21/booz2_analog_hw.h +++ /dev/null @@ -1,36 +0,0 @@ -/* - * $Id$ - * - * Copyright (C) 2008-2009 Antoine Drouin - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - */ - -#ifndef BOOZ2_ANALOG_HW_H -#define BOOZ2_ANALOG_HW_H - -#include "LPC21xx.h" -#include "std.h" - -static inline void Booz2AnalogSetDAC(uint16_t x) { - DACR = x << 6; -} - -extern void booz2_analog_init_hw(void); - -#endif /* BOOZ2_ANALOG_HW_H */ diff --git a/sw/airborne/booz/arch/sim/booz2_analog_hw.c b/sw/airborne/booz/arch/sim/booz2_analog_hw.c deleted file mode 100644 index 25404f6ec4..0000000000 --- a/sw/airborne/booz/arch/sim/booz2_analog_hw.c +++ /dev/null @@ -1,29 +0,0 @@ -/* - * $Id$ - * - * Copyright (C) 2008-2009 Antoine Drouin - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - */ - -#include "booz2_analog.h" - -void booz2_analog_init_hw(void) { - -} - diff --git a/sw/airborne/booz/arch/sim/booz2_analog_hw.h b/sw/airborne/booz/arch/sim/booz2_analog_hw.h deleted file mode 100644 index 7e2c3231d9..0000000000 --- a/sw/airborne/booz/arch/sim/booz2_analog_hw.h +++ /dev/null @@ -1,31 +0,0 @@ -/* - * $Id$ - * - * Copyright (C) 2008-2009 Antoine Drouin - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - */ - -#ifndef BOOZ2_ANALOG_HW_H -#define BOOZ2_ANALOG_HW_H - -#define Booz2AnalogSetDAC(x) { } - -extern void booz2_analog_init_hw(void); - -#endif /* BOOZ2_ANALOG_HW_H */ diff --git a/sw/airborne/booz/arch/stm32/booz2_analog_hw.h b/sw/airborne/booz/arch/stm32/booz2_analog_hw.h deleted file mode 100644 index 9c074731f2..0000000000 --- a/sw/airborne/booz/arch/stm32/booz2_analog_hw.h +++ /dev/null @@ -1,33 +0,0 @@ -/* - * $Id: booz2_analog_hw.h 4172 2009-09-18 11:57:13Z flixr $ - * - * Copyright (C) 2008-2009 Antoine Drouin - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - */ - -#ifndef BOOZ2_ANALOG_HW_H -#define BOOZ2_ANALOG_HW_H - -#include "std.h" - -static inline void Booz2AnalogSetDAC(uint16_t x) {} - -extern void booz2_analog_init_hw(void); - -#endif /* BOOZ2_ANALOG_HW_H */ diff --git a/sw/airborne/booz/booz2_analog.c b/sw/airborne/booz/booz2_analog.c deleted file mode 100644 index 2e0f153b78..0000000000 --- a/sw/airborne/booz/booz2_analog.c +++ /dev/null @@ -1,49 +0,0 @@ -/* - * $Id$ - * - * Copyright (C) 2008-2009 Antoine Drouin - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - */ - -#include "booz2_analog.h" - -#include "std.h" - -// battery on AD0.3 on P0.30 -// baro on AD0.1 on P0.28 - -#define CHAN_BAT 3 -#define CHAN_BARO 1 - - -void booz2_analog_init( void ) { - - booz2_analog_init_hw(); - -} - -#ifdef USE_EXTRA_ADC -// Read manually baro (100Hz) and bat (10Hz) -void booz2_analog_periodic( void ) { - // baro - RunOnceEvery(5,booz2_analog_baro_read()); - // bat - RunOnceEvery(50,booz2_analog_bat_read()); -} -#endif diff --git a/sw/airborne/booz/booz2_analog.h b/sw/airborne/booz/booz2_analog.h deleted file mode 100644 index 1c4aa63481..0000000000 --- a/sw/airborne/booz/booz2_analog.h +++ /dev/null @@ -1,47 +0,0 @@ -/* - * $Id$ - * - * Copyright (C) 2008-2009 Antoine Drouin - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - */ - -#ifndef BOOZ2_ANALOG_H -#define BOOZ2_ANALOG_H - -extern void booz2_analog_init( void ); - -#ifdef USE_EXTRA_ADC -#include "std.h" - -extern uint16_t booz2_adc_1; -extern uint16_t booz2_adc_2; -extern uint16_t booz2_adc_3; -extern uint16_t booz2_adc_4; - -extern void booz2_analog_periodic( void ); - -extern void booz2_analog_baro_read(void); -extern void booz2_analog_bat_read(void); -extern void booz2_analog_extra_adc_read(void); -#endif - - -#include "booz2_analog_hw.h" - -#endif /* BOOZ2_ANALOG_H */ diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c index ad0bf0e3c0..fd6ce9a3ee 100644 --- a/sw/airborne/firmwares/rotorcraft/main.c +++ b/sw/airborne/firmwares/rotorcraft/main.c @@ -41,11 +41,10 @@ #include "subsystems/imu.h" #include "booz_gps.h" -#include "booz/booz2_analog.h" #include "subsystems/sensors/baro.h" #include "baro_board.h" -#include "firmwares/rotorcraft/battery.h" +#include "subsystems/electrical.h" // #include "booz_fms.h" // FIXME #include "firmwares/rotorcraft/autopilot.h" @@ -98,6 +97,7 @@ STATIC_INLINE void main_init( void ) { mcu_init(); sys_time_init(); + electrical_init(); actuators_init(); radio_control_init(); @@ -106,12 +106,8 @@ STATIC_INLINE void main_init( void ) { xbee_init(); #endif - booz2_analog_init(); baro_init(); - - battery_init(); imu_init(); - // booz_fms_init(); // FIXME autopilot_init(); nav_init(); @@ -156,7 +152,7 @@ STATIC_INLINE void main_periodic( void ) { /* booz_fms_periodic(); FIXME */ \ }, \ { \ - /*BoozControlSurfacesSetFromCommands();*/ \ + electrical_periodic(); \ }, \ { \ LED_PERIODIC(); \ @@ -172,16 +168,12 @@ STATIC_INLINE void main_periodic( void ) { } ); #ifdef USE_GPS - if (radio_control.status != RC_OK && \ + if (radio_control.status != RC_OK && \ autopilot_mode == AP_MODE_NAV && GpsIsLost()) \ autopilot_set_mode(AP_MODE_FAILSAFE); \ booz_gps_periodic(); #endif -#ifdef USE_EXTRA_ADC - booz2_analog_periodic(); -#endif - modules_periodic_task(); if (autopilot_in_flight) { diff --git a/sw/airborne/firmwares/rotorcraft/telemetry.h b/sw/airborne/firmwares/rotorcraft/telemetry.h index 1cf5801b80..e9cbe76fae 100644 --- a/sw/airborne/firmwares/rotorcraft/telemetry.h +++ b/sw/airborne/firmwares/rotorcraft/telemetry.h @@ -45,7 +45,7 @@ #define PERIODIC_SEND_ALIVE(_chan) DOWNLINK_SEND_ALIVE(_chan, 16, MD5SUM) -#include "firmwares/rotorcraft/battery.h" +#include "subsystems/electrical.h" #include "subsystems/imu.h" #include "booz_gps.h" #include "subsystems/ins.h" @@ -56,43 +56,43 @@ extern uint8_t telemetry_mode_Main_DefaultChannel; #ifdef USE_GPS -#define PERIODIC_SEND_ROTORCRAFT_STATUS(_chan) { \ - uint32_t imu_nb_err = 0; \ +#define PERIODIC_SEND_ROTORCRAFT_STATUS(_chan) { \ + uint32_t imu_nb_err = 0; \ uint8_t _twi_blmc_nb_err = 0; \ - DOWNLINK_SEND_ROTORCRAFT_STATUS(_chan, \ - &imu_nb_err, \ - &_twi_blmc_nb_err, \ - &radio_control.status, \ - &radio_control.frame_rate, \ - &booz_gps_state.fix, \ - &autopilot_mode, \ - &autopilot_in_flight, \ - &autopilot_motors_on, \ - &guidance_h_mode, \ - &guidance_v_mode, \ - &battery_voltage, \ - &cpu_time_sec \ - ); \ + DOWNLINK_SEND_ROTORCRAFT_STATUS(_chan, \ + &imu_nb_err, \ + &_twi_blmc_nb_err, \ + &radio_control.status, \ + &radio_control.frame_rate, \ + &booz_gps_state.fix, \ + &autopilot_mode, \ + &autopilot_in_flight, \ + &autopilot_motors_on, \ + &guidance_h_mode, \ + &guidance_v_mode, \ + &electrical.vsupply, \ + &cpu_time_sec \ + ); \ } #else /* !USE_GPS */ -#define PERIODIC_SEND_ROTORCRAFT_STATUS(_chan) { \ - uint32_t imu_nb_err = 0; \ +#define PERIODIC_SEND_ROTORCRAFT_STATUS(_chan) { \ + uint32_t imu_nb_err = 0; \ uint8_t twi_blmc_nb_err = 0; \ uint8_t fix = BOOZ2_GPS_FIX_NONE; \ - DOWNLINK_SEND_ROTORCRAFT_STATUS(_chan, \ - &imu_nb_err, \ - &twi_blmc_nb_err, \ - &radio_control.status, \ - &radio_control.frame_rate, \ - &fix, \ - &autopilot_mode, \ - &autopilot_in_flight, \ - &autopilot_motors_on, \ - &guidance_h_mode, \ - &guidance_v_mode, \ - &battery_voltage, \ - &cpu_time_sec \ - ); \ + DOWNLINK_SEND_ROTORCRAFT_STATUS(_chan, \ + &imu_nb_err, \ + &twi_blmc_nb_err, \ + &radio_control.status, \ + &radio_control.frame_rate, \ + &fix, \ + &autopilot_mode, \ + &autopilot_in_flight, \ + &autopilot_motors_on, \ + &guidance_h_mode, \ + &guidance_v_mode, \ + &electrical.vsupply, \ + &cpu_time_sec \ + ); \ } #endif /* USE_GPS */ diff --git a/sw/airborne/mcu.c b/sw/airborne/mcu.c index e0853e4bab..1e8fee07d7 100644 --- a/sw/airborne/mcu.c +++ b/sw/airborne/mcu.c @@ -48,6 +48,9 @@ #ifdef USE_SPI #include "mcu_periph/spi.h" #endif +#ifdef USE_DAC +#include "mcu_periph/dac.h" +#endif #endif /* PERIPHERALS_AUTO_INIT */ void mcu_init(void) { @@ -98,6 +101,9 @@ void mcu_init(void) { #ifdef USE_SPI spi_init(); #endif +#ifdef USE_DAC + dac_init(); +#endif #endif /* PERIPHERALS_AUTO_INIT */ } diff --git a/sw/airborne/mcu_periph/dac.h b/sw/airborne/mcu_periph/dac.h new file mode 100644 index 0000000000..f1a7d23b41 --- /dev/null +++ b/sw/airborne/mcu_periph/dac.h @@ -0,0 +1,9 @@ +#ifndef MCU_PERIPH_DAC_H +#define MCU_PERIPH_DAC_H + +#include "mcu_periph/dac_arch.h" + +extern void dac_init(void); + + +#endif /* MCU_PERIPH_DAC_H */ From 86fff9fb4c14ff994a227552163aab530922274e Mon Sep 17 00:00:00 2001 From: Antoine Drouin Date: Tue, 22 Mar 2011 16:24:15 +0100 Subject: [PATCH 045/101] servos on fixedwing lisa/M --- conf/autopilot/fixedwing.makefile | 3 +++ 1 file changed, 3 insertions(+) diff --git a/conf/autopilot/fixedwing.makefile b/conf/autopilot/fixedwing.makefile index d4e98b98c4..139a7ca919 100644 --- a/conf/autopilot/fixedwing.makefile +++ b/conf/autopilot/fixedwing.makefile @@ -37,6 +37,9 @@ ifeq ($(TARGET),$(ACTUATOR_TARGET)) ifeq ($(BOARD),lisa_l) include $(CFG_SHARED)/actuators_direct.makefile endif + ifeq ($(BOARD),lisa_m) + include $(CFG_SHARED)/actuators_direct.makefile + endif else include $(CFG_SHARED)/$(ACTUATORS).makefile From cd7bf6077ef401cff80fb153fec41e8dcf087544 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Tue, 22 Mar 2011 17:10:51 +0100 Subject: [PATCH 046/101] use the new DAC driver --- sw/airborne/boards/booz/baro_board.c | 7 ++----- sw/airborne/boards/booz/baro_board.h | 4 ++-- 2 files changed, 4 insertions(+), 7 deletions(-) diff --git a/sw/airborne/boards/booz/baro_board.c b/sw/airborne/boards/booz/baro_board.c index 4277379a87..482aa7944b 100644 --- a/sw/airborne/boards/booz/baro_board.c +++ b/sw/airborne/boards/booz/baro_board.c @@ -31,9 +31,6 @@ #define BOOZ_ANALOG_BARO_THRESHOLD 850 #endif -// pressure on AD0.1 on P0.28 -// offset on DAC on P0.25 - struct Baro baro; struct BaroBoard baro_board; @@ -47,7 +44,7 @@ void baro_init( void ) { baro.differential = 0; /* not handled on this board */ baro_board.offset = 1023; - Booz2AnalogSetDAC(baro_board.offset); + DACSet(baro_board.offset); baro_board.value_filtered = 0; baro_board.data_available = FALSE; @@ -74,7 +71,7 @@ void baro_board_calibrate(void) { baro_board.offset -= 15; else baro_board.offset--; - Booz2AnalogSetDAC(baro_board.offset); + DACSet(baro_board.offset); #ifdef ROTORCRAFT_BARO_LED LED_TOGGLE(ROTORCRAFT_BARO_LED); #endif diff --git a/sw/airborne/boards/booz/baro_board.h b/sw/airborne/boards/booz/baro_board.h index f24568adae..902aef2db1 100644 --- a/sw/airborne/boards/booz/baro_board.h +++ b/sw/airborne/boards/booz/baro_board.h @@ -5,7 +5,7 @@ #include "subsystems/sensors/baro.h" #include "mcu_periph/adc.h" -#include "booz/booz2_analog.h" // le DAC +#include "mcu_periph/dac.h" struct BaroBoard { @@ -28,7 +28,7 @@ extern void baro_board_calibrate(void); static inline void baro_board_SetOffset(uint16_t _o) { baro_board.offset = _o; - Booz2AnalogSetDAC(_o); + DACSet(_o); } From 3dd321ec11ce225cf9289655bb0c0e7c6f229387 Mon Sep 17 00:00:00 2001 From: Antoine Drouin Date: Tue, 22 Mar 2011 17:22:15 +0100 Subject: [PATCH 047/101] added a test program for persistent settings --- conf/airframes/Poine/test_settings.xml | 15 + conf/autopilot/lisa_l_test_progs.makefile | 355 ++++++++++-------- .../subsystems/fixedwing/autopilot.makefile | 1 + conf/settings/settings.dtd | 1 + sw/airborne/subsystems/settings.c | 17 + sw/airborne/subsystems/settings.h | 21 ++ sw/airborne/test/subsystems/test_settings.c | 100 +++++ sw/airborne/test/subsystems/test_settings.h | 11 + 8 files changed, 366 insertions(+), 155 deletions(-) create mode 100644 conf/airframes/Poine/test_settings.xml create mode 100644 sw/airborne/subsystems/settings.c create mode 100644 sw/airborne/subsystems/settings.h create mode 100644 sw/airborne/test/subsystems/test_settings.c create mode 100644 sw/airborne/test/subsystems/test_settings.h diff --git a/conf/airframes/Poine/test_settings.xml b/conf/airframes/Poine/test_settings.xml new file mode 100644 index 0000000000..72186562de --- /dev/null +++ b/conf/airframes/Poine/test_settings.xml @@ -0,0 +1,15 @@ + + + + + + + + + + + + + diff --git a/conf/autopilot/lisa_l_test_progs.makefile b/conf/autopilot/lisa_l_test_progs.makefile index e6cf380777..8a89e49b72 100644 --- a/conf/autopilot/lisa_l_test_progs.makefile +++ b/conf/autopilot/lisa_l_test_progs.makefile @@ -276,11 +276,11 @@ test_adc.ARCHDIR = $(ARCH) test_adc.CFLAGS = -I$(SRC_LISA) -I$(ARCH) -DPERIPHERALS_AUTO_INIT test_adc.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) -test_adc.srcs = $(SRC_AIRBORNE)/mcu.c \ +test_adc.srcs = $(SRC_LISA)/test_adc.c \ + $(SRC_AIRBORNE)/mcu.c \ $(SRC_ARCH)/mcu_arch.c \ - $(SRC_LISA)/test_adc.c \ - $(SRC_ARCH)/stm32_exceptions.c \ - $(SRC_ARCH)/stm32_vector_table.c + $(SRC_ARCH)/stm32_exceptions.c \ + $(SRC_ARCH)/stm32_vector_table.c test_adc.CFLAGS += -DUSE_LED test_adc.srcs += $(SRC_ARCH)/led_hw.c @@ -299,138 +299,175 @@ test_adc.srcs += downlink.c pprz_transport.c test_adc.srcs += $(SRC_ARCH)/mcu_periph/adc_arch.c test_adc.CFLAGS += -DUSE_AD1 -DUSE_AD1_1 -DUSE_AD1_2 -DUSE_AD1_3 -DUSE_AD1_4 +#test_adc.CFLAGS += -DUSE_AD1 -DUSE_AD1_3 test_adc.CFLAGS += -DUSE_ADC1_2_IRQ_HANDLER + + # -# test IMU b2 +# common test # # configuration # SYS_TIME_LED # MODEM_PORT # MODEM_BAUD # +PERIODIC_FREQUENCY = 512 + +COMMON_TEST_CFLAGS = -I$(SRC_FIRMWARE) -I$(ARCH) -DPERIPHERALS_AUTO_INIT +COMMON_TEST_CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) +COMMON_TEST_SRCS = $(SRC_AIRBORNE)/mcu.c \ + $(SRC_ARCH)/mcu_arch.c \ + $(SRC_ARCH)/stm32_exceptions.c \ + $(SRC_ARCH)/stm32_vector_table.c +COMMON_TEST_CFLAGS += -DUSE_LED +COMMON_TEST_SRCS += $(SRC_ARCH)/led_hw.c +COMMON_TEST_CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=$(SYS_TIME_LED) +COMMON_TEST_CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./$(PERIODIC_FREQUENCY).))' +COMMON_TEST_CFLAGS += -DPERIODIC_FREQUENCY=$(PERIODIC_FREQUENCY) +COMMON_TEST_SRCS += sys_time.c $(SRC_ARCH)/sys_time_hw.c +COMMON_TEST_CFLAGS += -DUSE_$(MODEM_PORT) -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD) +COMMON_TEST_SRCS += $(SRC_ARCH)/mcu_periph/uart_arch.c +COMMON_TEST_CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT) +COMMON_TEST_SRCS += downlink.c pprz_transport.c +COMMON_TEST_SRCS += math/pprz_trig_int.c + + +# +# test IMU b2 v1.1 +# +IMU_B2_CFLAGS = -DIMU_TYPE_H=\"subsystems/imu/imu_b2.h\" +IMU_B2_CFLAGS += -DIMU_B2_MAG_TYPE=IMU_B2_MAG_MS2100 -DIMU_B2_VERSION_1_1 +IMU_B2_SRCS = $(SRC_SUBSYSTEMS)/imu.c +IMU_B2_CFLAGS += -DMAX_1168_DRDY_PORT=$(MAX_1168_DRDY_PORT) +IMU_B2_CFLAGS += -DMAX_1168_DRDY_PORT_SOURCE=$(MAX_1168_DRDY_PORT_SOURCE) +IMU_B2_CFLAGS += -DUSE_SPI2 -DUSE_DMA1_C4_IRQ -DUSE_EXTI2_IRQ -DUSE_SPI2_IRQ +IMU_B2_SRCS += $(SRC_SUBSYSTEMS)/imu/imu_b2.c $(SRC_ARCH)/subsystems/imu/imu_b2_arch.c +IMU_B2_SRCS += peripherals/max1168.c $(SRC_ARCH)/peripherals/max1168_arch.c +IMU_B2_SRCS += peripherals/ms2100.c $(SRC_ARCH)/peripherals/ms2100_arch.c + test_imu_b2.ARCHDIR = $(ARCH) -test_imu_b2.CFLAGS = -I$(SRC_LISA) -I$(ARCH) -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH) -DPERIPHERALS_AUTO_INIT -test_imu_b2.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) -test_imu_b2.srcs += $(SRC_AIRBORNE)/mcu.c \ - $(SRC_ARCH)/mcu_arch.c \ - $(SRC_BOOZ_TEST)/booz_test_imu.c \ - $(SRC_ARCH)/stm32_exceptions.c \ - $(SRC_ARCH)/stm32_vector_table.c +test_imu_b2.srcs = test/subsystems/test_imu.c +test_imu_b2.CFLAGS = $(COMMON_TEST_CFLAGS) +test_imu_b2.srcs += $(COMMON_TEST_SRCS) +test_imu_b2.CFLAGS += $(IMU_B2_CFLAGS) +test_imu_b2.srcs += $(IMU_B2_SRCS) -test_imu_b2.CFLAGS += -DUSE_LED -test_imu_b2.srcs += $(SRC_ARCH)/led_hw.c -test_imu_b2.CFLAGS += -DUSE_SYS_TIME -test_imu_b2.CFLAGS += -DSYS_TIME_LED=$(SYS_TIME_LED) -test_imu_b2.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))' -test_imu_b2.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c -test_imu_b2.CFLAGS += -DUSE_$(MODEM_PORT) -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD) -test_imu_b2.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c - -test_imu_b2.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2 -test_imu_b2.srcs += downlink.c pprz_transport.c - -test_imu_b2.srcs += math/pprz_trig_int.c - -test_imu_b2.CFLAGS += -DIMU_TYPE_H=\"subsystems/imu/imu_b2.h\" -test_imu_b2.CFLAGS += -DIMU_B2_MAG_TYPE=IMU_B2_MAG_MS2100 -DIMU_B2_VERSION_1_1 -test_imu_b2.srcs += $(SRC_SUBSYSTEMS)/imu.c -test_imu_b2.CFLAGS += -DMAX_1168_DRDY_PORT=$(MAX_1168_DRDY_PORT) -test_imu_b2.CFLAGS += -DMAX_1168_DRDY_PORT_SOURCE=$(MAX_1168_DRDY_PORT_SOURCE) -test_imu_b2.CFLAGS += -DUSE_SPI2 -DUSE_DMA1_C4_IRQ -DUSE_EXTI2_IRQ -DUSE_SPI2_IRQ -test_imu_b2.srcs += $(SRC_SUBSYSTEMS)/imu/imu_b2.c $(SRC_ARCH)/subsystems/imu/imu_b2_arch.c -test_imu_b2.srcs += peripherals/max1168.c $(SRC_ARCH)/peripherals/max1168_arch.c -test_imu_b2.srcs += peripherals/ms2100.c $(SRC_ARCH)/peripherals/ms2100_arch.c # -# test IMU b2 1.2 -# -# configuration -# SYS_TIME_LED -# MODEM_PORT -# MODEM_BAUD +# test IMU b2 v1.2 # +IMU_B2_2_CFLAGS = -DIMU_TYPE_H=\"subsystems/imu/imu_b2.h\" +IMU_B2_2_CFLAGS += -DIMU_B2_MAG_TYPE=IMU_B2_MAG_HMC5843 -DIMU_B2_VERSION_1_2 +IMU_B2_2_SRCS = $(SRC_SUBSYSTEMS)/imu.c +IMU_B2_2_CFLAGS += -DMAX_1168_DRDY_PORT=$(MAX_1168_DRDY_PORT) +IMU_B2_2_CFLAGS += -DMAX_1168_DRDY_PORT_SOURCE=$(MAX_1168_DRDY_PORT_SOURCE) +IMU_B2_2_CFLAGS += -DUSE_SPI2 -DUSE_DMA1_C4_IRQ -DUSE_EXTI2_IRQ -DUSE_SPI2_IRQ +IMU_B2_2_SRCS += $(SRC_SUBSYSTEMS)/imu/imu_b2.c $(SRC_ARCH)/subsystems/imu/imu_b2_arch.c +IMU_B2_2_SRCS += peripherals/max1168.c $(SRC_ARCH)/peripherals/max1168_arch.c +IMU_B2_2_CFLAGS += -DUSE_I2C2 +IMU_B2_2_SRCS += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c +IMU_B2_2_SRCS += peripherals/hmc5843.c $(SRC_ARCH)/peripherals/hmc5843_arch.c +IMU_B2_2_CFLAGS += -DUSE_EXTI9_5_IRQ # Mag Int on PB5 + test_imu_b2_2.ARCHDIR = $(ARCH) -test_imu_b2_2.CFLAGS = -I$(SRC_LISA) -I$(ARCH) -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH) -DPERIPHERALS_AUTO_INIT -test_imu_b2_2.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) -test_imu_b2_2.srcs += $(SRC_AIRBORNE)/mcu.c \ - $(SRC_ARCH)/mcu_arch.c \ - $(SRC_BOOZ_TEST)/booz_test_imu.c \ - $(SRC_ARCH)/stm32_exceptions.c \ - $(SRC_ARCH)/stm32_vector_table.c +test_imu_b2_2.srcs = test/subsystems/test_imu.c +test_imu_b2_2.CFLAGS = $(COMMON_TEST_CFLAGS) +test_imu_b2_2.srcs += $(COMMON_TEST_SRCS) +test_imu_b2_2.CFLAGS += $(IMU_B2_2_CFLAGS) +test_imu_b2_2.srcs += $(IMU_B2_2_SRCS) -test_imu_b2_2.CFLAGS += -DUSE_LED -test_imu_b2_2.srcs += $(SRC_ARCH)/led_hw.c -test_imu_b2_2.CFLAGS += -DUSE_SYS_TIME -test_imu_b2_2.CFLAGS += -DSYS_TIME_LED=$(SYS_TIME_LED) -test_imu_b2_2.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))' -test_imu_b2_2.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c -test_imu_b2_2.CFLAGS += -DUSE_$(MODEM_PORT) -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD) -test_imu_b2_2.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c - -test_imu_b2_2.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2 -test_imu_b2_2.srcs += downlink.c pprz_transport.c - -test_imu_b2_2.srcs += math/pprz_trig_int.c - -test_imu_b2_2.CFLAGS += -DIMU_TYPE_H=\"subsystems/imu/imu_b2.h\" -test_imu_b2_2.CFLAGS += -DIMU_B2_MAG_TYPE=IMU_B2_MAG_HMC5843 -DIMU_B2_VERSION_1_2 -test_imu_b2_2.srcs += $(SRC_SUBSYSTEMS)/imu.c -test_imu_b2_2.CFLAGS += -DMAX_1168_DRDY_PORT=$(MAX_1168_DRDY_PORT) -test_imu_b2_2.CFLAGS += -DMAX_1168_DRDY_PORT_SOURCE=$(MAX_1168_DRDY_PORT_SOURCE) -test_imu_b2_2.CFLAGS += -DUSE_SPI2 -DUSE_DMA1_C4_IRQ -DUSE_EXTI2_IRQ -DUSE_SPI2_IRQ -test_imu_b2_2.srcs += $(SRC_SUBSYSTEMS)/imu/imu_b2.c $(SRC_ARCH)/subsystems/imu/imu_b2_arch.c -test_imu_b2_2.srcs += peripherals/max1168.c $(SRC_ARCH)/peripherals/max1168_arch.c -test_imu_b2_2.CFLAGS += -DUSE_I2C2 -test_imu_b2_2.srcs += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c -test_imu_b2_2.srcs += peripherals/hmc5843.c $(SRC_ARCH)/peripherals/hmc5843_arch.c -test_imu_b2_2.CFLAGS += -DUSE_EXTI9_5_IRQ # Mag Int on PB5 # # test IMU aspirin # +IMU_ASPIRIN_CFLAGS = -DIMU_TYPE_H=\"imu/imu_aspirin.h\" -DIMU_OVERRIDE_CHANNELS +IMU_ASPIRIN_SRCS = $(SRC_SUBSYSTEMS)/imu.c \ + $(SRC_SUBSYSTEMS)/imu/imu_aspirin.c \ + $(SRC_ARCH)/subsystems/imu/imu_aspirin_arch.c +IMU_ASPIRIN_SRCS += peripherals/hmc5843.c $(SRC_ARCH)/peripherals/hmc5843_arch.c +IMU_ASPIRIN_CFLAGS += -DUSE_I2C2 +IMU_ASPIRIN_SRCS += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c +IMU_ASPIRIN_CFLAGS += -DUSE_EXTI15_10_IRQ # Gyro Int on PC14 +IMU_ASPIRIN_CFLAGS += -DUSE_EXTI9_5_IRQ # Mag Int on PB5 +IMU_ASPIRIN_CFLAGS += -DUSE_EXTI2_IRQ # Accel Int on PD2 +IMU_ASPIRIN_CFLAGS += -DUSE_DMA1_C4_IRQ # SPI2 Rx DMA + test_imu_aspirin.ARCHDIR = $(ARCH) -test_imu_aspirin.CFLAGS = -I$(SRC_FIRMWARE) -I$(SRC_LISA) -I$(ARCH) -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH) -DPERIPHERALS_AUTO_INIT -test_imu_aspirin.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) -test_imu_aspirin.srcs += $(SRC_AIRBORNE)/mcu.c \ - $(SRC_ARCH)/mcu_arch.c \ - $(SRC_BOOZ_TEST)/booz_test_imu.c \ - $(SRC_ARCH)/stm32_exceptions.c \ - $(SRC_ARCH)/stm32_vector_table.c +test_imu_aspirin.srcs = test/subsystems/test_imu.c +test_imu_aspirin.CFLAGS = $(COMMON_TEST_CFLAGS) +test_imu_aspirin.srcs += $(COMMON_TEST_SRCS) +test_imu_aspirin.CFLAGS += $(IMU_ASPIRIN_CFLAGS) +test_imu_aspirin.srcs += $(IMU_ASPIRIN_SRCS) -test_imu_aspirin.CFLAGS += -DUSE_LED -test_imu_aspirin.srcs += $(SRC_ARCH)/led_hw.c -test_imu_aspirin.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=$(SYS_TIME_LED) -test_imu_aspirin.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))' -test_imu_aspirin.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c +# +# test AHRS +# +test_ahrs.ARCHDIR = $(ARCH) +test_ahrs.srcs = test/subsystems/test_ahrs.c +test_ahrs.CFLAGS = $(COMMON_TEST_CFLAGS) +test_ahrs.srcs += $(COMMON_TEST_SRCS) +test_ahrs.CFLAGS += $(IMU_ASPIRIN_CFLAGS) +test_ahrs.srcs += $(IMU_ASPIRIN_SRCS) -test_imu_aspirin.CFLAGS += -DUSE_$(MODEM_PORT) -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD) -test_imu_aspirin.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c +#AHRS = ice +AHRS = icq +#AHRS = flq +#AHRS = fcr +#AHRS = fcr2 +#AHRS = fcq -test_imu_aspirin.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT) -test_imu_aspirin.srcs += downlink.c pprz_transport.c +test_ahrs.srcs += subsystems/ahrs.c \ + subsystems/ahrs/ahrs_aligner.c -test_imu_aspirin.srcs += math/pprz_trig_int.c +ifeq ($(AHRS), ice) +test_ahrs.CFLAGS += -DFACE_REINJ_1=1024 +test_ahrs.CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_euler.h\" +test_ahrs.srcs += subsystems/ahrs/ahrs_int_cmpl_euler.c \ + lisa/plug_sys.c +endif -test_imu_aspirin.CFLAGS += -DIMU_TYPE_H=\"imu/imu_aspirin.h\" -DIMU_OVERRIDE_CHANNELS -test_imu_aspirin.srcs += $(SRC_SUBSYSTEMS)/imu.c \ - $(SRC_SUBSYSTEMS)/imu/imu_aspirin.c \ - $(SRC_ARCH)/subsystems/imu/imu_aspirin_arch.c -test_imu_aspirin.srcs += peripherals/hmc5843.c $(SRC_ARCH)/peripherals/hmc5843_arch.c +ifeq ($(AHRS), icq) +#test_ahrs.CFLAGS += -DAHRS_TYPE=\"ICQ\" +test_ahrs.CFLAGS += -DAHRS_PROPAGATE_FREQUENCY=512 +test_ahrs.CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl.h\" +test_ahrs.srcs +=subsystems/ahrs/ahrs_int_cmpl.c +endif -test_imu_aspirin.CFLAGS += -DUSE_I2C2 -test_imu_aspirin.srcs += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c -test_imu_aspirin.CFLAGS += -DUSE_EXTI15_10_IRQ # Gyro Int on PC14 -test_imu_aspirin.CFLAGS += -DUSE_EXTI9_5_IRQ # Mag Int on PB5 -test_imu_aspirin.CFLAGS += -DUSE_EXTI2_IRQ # Accel Int on PD2 -test_imu_aspirin.CFLAGS += -DUSE_DMA1_C4_IRQ # SPI2 Rx DMA +ifeq ($(AHRS), flq) +test_ahrs.CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_lkf_quat.h\" +test_ahrs.CFLAGS += -DAHRS_H_X=0.51562740288882 -DAHRS_H_Y=-0.05707735220832 -DAHRS_H_Z=0.85490967783446 +test_ahrs.srcs += subsystems/ahrs/ahrs_float_lkf_quat.c +endif +ifeq ($(AHRS), fcr) +test_ahrs.CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_dcm.h\" +test_ahrs.CFLAGS += -DINS_ROLL_NEUTRAL_DEFAULT=0 +test_ahrs.CFLAGS += -DINS_PITCH_NEUTRAL_DEFAULT=0 +test_ahrs.CFLAGS += -DAHRS_PROPAGATE_FREQUENCY=512 +test_ahrs.CFLAGS += -DDCM_UPDATE_AFTER_PROPAGATE +test_ahrs.srcs += subsystems/ahrs/ahrs_float_dcm.c +endif + +ifeq ($(AHRS), fcr2) +test_ahrs.CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl_rmat.h\" +test_ahrs.CFLAGS += -DAHRS_H_X=0.51562740288882 -DAHRS_H_Y=-0.05707735220832 -DAHRS_H_Z=0.85490967783446 +test_ahrs.CFLAGS += -DAHRS_PROPAGATE_FREQUENCY=512 +test_ahrs.srcs += subsystems/ahrs/ahrs_float_cmpl_rmat.c +endif + +ifeq ($(AHRS), fcq) +test_ahrs.CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl_rmat.h\" +test_ahrs.CFLAGS += -DAHRS_H_X=0.51562740288882 -DAHRS_H_Y=-0.05707735220832 -DAHRS_H_Z=0.85490967783446 +test_ahrs.CFLAGS += -DAHRS_PROPAGATE_FREQUENCY=512 +test_ahrs.srcs += subsystems/ahrs/ahrs_float_cmpl_quat.c +endif @@ -663,65 +700,73 @@ test_bmp085.srcs += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c + # -# Test manual : a simple test with rc and servos - I want to fly lisa/M +# tunnel sw # -test_manual.ARCHDIR = $(ARCH) -test_manual.CFLAGS = -I$(SRC_FIRMWARE) -I$(ARCH) -DPERIPHERALS_AUTO_INIT -test_manual.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) -test_manual.srcs = $(SRC_AIRBORNE)/mcu.c \ - $(SRC_ARCH)/mcu_arch.c \ - test/test_manual.c \ - $(SRC_ARCH)/stm32_exceptions.c \ - $(SRC_ARCH)/stm32_vector_table.c -test_manual.CFLAGS += -DUSE_LED -test_manual.srcs += $(SRC_ARCH)/led_hw.c -test_manual.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=$(SYS_TIME_LED) -test_manual.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)' -test_manual.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c - -test_manual.CFLAGS += -DUSE_$(MODEM_PORT) -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD) -test_manual.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c - -test_manual.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT) -test_manual.srcs += downlink.c pprz_transport.c - -test_manual.srcs += $(SRC_BOOZ)/booz2_commands.c - -test_manual.CFLAGS += -I$(SRC_FIRMWARE)/actuators/arch/$(ARCH) -#test_manual.srcs += $(SRC_FIRMWARE)/actuators/actuators_pwm.c -test_manual.srcs += $(SRC_FIRMWARE)/actuators/arch/$(ARCH)/actuators_pwm_arch.c -test_manual.srcs += $(SRC_FIRMWARE)/actuators/actuators_heli.c +tunnel_sw.ARCHDIR = $(ARCH) +tunnel_sw.CFLAGS += -I$(SRC_LISA) -I$(ARCH) -DPERIPHERALS_AUTO_INIT +tunnel_sw.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) +tunnel_sw.srcs += $(SRC_AIRBORNE)/mcu.c \ + $(SRC_ARCH)/mcu_arch.c \ + $(SRC_LISA)/tunnel_hw.c \ + $(SRC_ARCH)/stm32_exceptions.c \ + $(SRC_ARCH)/stm32_vector_table.c +tunnel_sw.CFLAGS += -DUSE_LED +tunnel_sw.srcs += $(SRC_ARCH)/led_hw.c +tunnel_sw.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=$(SYS_TIME_LED) +tunnel_sw.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)' +tunnel_sw.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c -test_manual.CFLAGS += -I$(SRC_BOOZ) -I$(SRC_BOOZ)/arch/$(ARCH) -test_manual.CFLAGS += -DRADIO_CONTROL -ifdef RADIO_CONTROL_LED -test_manual.CFLAGS += -DRADIO_CONTROL_LED=$(RADIO_CONTROL_LED) -endif -test_manual.CFLAGS += -DRADIO_CONTROL_BIND_IMPL_FUNC=radio_control_spektrum_try_bind -test_manual.CFLAGS += -DRADIO_CONTROL_TYPE_H=\"subsystems/radio_control/spektrum.h\" -test_manual.CFLAGS += -DRADIO_CONTROL_SPEKTRUM_PRIMARY_PORT=$(RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT) -test_manual.CFLAGS += -DOVERRIDE_$(RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT)_IRQ_HANDLER -DUSE_TIM6_IRQ -test_manual.srcs += $(SRC_SUBSYSTEMS)/radio_control.c \ - subsystems/radio_control/spektrum.c \ - $(SRC_ARCH)/subsystems/radio_control/spektrum_arch.c +# +# tunnel hw +# +tunnel_hw.ARCHDIR = $(ARCH) +tunnel_hw.CFLAGS += -I$(SRC_LISA) -I$(ARCH) -DPERIPHERALS_AUTO_INIT +tunnel_hw.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) +tunnel_hw.srcs += lisa/test/lisa_tunnel.c \ + $(SRC_AIRBORNE)/mcu.c \ + $(SRC_ARCH)/mcu_arch.c \ + $(SRC_ARCH)/stm32_exceptions.c \ + $(SRC_ARCH)/stm32_vector_table.c +tunnel_hw.CFLAGS += -DUSE_LED +tunnel_hw.srcs += $(SRC_ARCH)/led_hw.c +tunnel_hw.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=$(SYS_TIME_LED) +tunnel_hw.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)' +tunnel_hw.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c +tunnel_hw.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B38400 +tunnel_hw.CFLAGS += -DUSE_UART2 -DUART2_BAUD=B57600 +tunnel_hw.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c # -# tunnel +# test_settings : # -tunnel.ARCHDIR = $(ARCH) -tunnel.CFLAGS += -I$(SRC_LISA) -I$(ARCH) -DPERIPHERALS_AUTO_INIT -tunnel.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) -tunnel.srcs += $(SRC_AIRBORNE)/mcu.c \ - $(SRC_ARCH)/mcu_arch.c \ - $(SRC_LISA)/tunnel_hw.c \ - $(SRC_ARCH)/stm32_exceptions.c \ - $(SRC_ARCH)/stm32_vector_table.c -tunnel.CFLAGS += -DUSE_LED -tunnel.srcs += $(SRC_ARCH)/led_hw.c -tunnel.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=$(SYS_TIME_LED) -tunnel.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)' -tunnel.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c +# configuration +# MODEM_PORT : +# MODEM_BAUD : +# +test_settings.ARCHDIR = $(ARCH) +test_settings.CFLAGS += -I$(SRC_LISA) -I$(SRC_ARCH) -DPERIPHERALS_AUTO_INIT +test_settings.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) +test_settings.srcs = test/subsystems/test_settings.c \ + $(SRC_AIRBORNE)/mcu.c \ + $(SRC_ARCH)/mcu_arch.c \ + $(SRC_ARCH)/stm32_exceptions.c \ + $(SRC_ARCH)/stm32_vector_table.c +test_settings.CFLAGS += -DUSE_LED +test_settings.srcs += $(SRC_ARCH)/led_hw.c +test_settings.CFLAGS += -DUSE_SYS_TIME +test_settings.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)' +test_settings.CFLAGS += -DSYS_TIME_LED=$(SYS_TIME_LED) +test_settings.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c +test_settings.CFLAGS += -DUSE_$(MODEM_PORT) +test_settings.CFLAGS += -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD) +test_settings.srcs += downlink.c pprz_transport.c +test_settings.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT) +test_settings.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c +test_settings.CFLAGS += -DDATALINK=PPRZ -DPPRZ_UART=$(MODEM_PORT) +test_settings.srcs += subsystems/settings.c + diff --git a/conf/autopilot/subsystems/fixedwing/autopilot.makefile b/conf/autopilot/subsystems/fixedwing/autopilot.makefile index fe9ba0a275..4881ed55f9 100644 --- a/conf/autopilot/subsystems/fixedwing/autopilot.makefile +++ b/conf/autopilot/subsystems/fixedwing/autopilot.makefile @@ -142,6 +142,7 @@ ns_srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c #ifeq ($(ARCH), lpc21) ns_srcs += $(SRC_ARCH)/mcu_periph/adc_arch.c ifeq ($(ARCH), stm32) + ns_CFLAGS += -DUSE_AD1 -DUSE_AD1_1 -DUSE_AD1_2 -DUSE_AD1_3 -DUSE_AD1_4 ns_CFLAGS += -DUSE_ADC1_2_IRQ_HANDLER endif diff --git a/conf/settings/settings.dtd b/conf/settings/settings.dtd index 25cda6c3be..bde71f467e 100644 --- a/conf/settings/settings.dtd +++ b/conf/settings/settings.dtd @@ -36,6 +36,7 @@ unit CDATA #IMPLIED alt_unit CDATA #IMPLIED alt_unit_coef CDATA #IMPLIED values CDATA #IMPLIED +persistent CDATA #IMPLIED > Date: Tue, 22 Mar 2011 17:41:07 +0100 Subject: [PATCH 048/101] forgoten file for the setting test program --- conf/settings/settings_test.xml | 15 +++++++++++++++ 1 file changed, 15 insertions(+) create mode 100644 conf/settings/settings_test.xml diff --git a/conf/settings/settings_test.xml b/conf/settings/settings_test.xml new file mode 100644 index 0000000000..7dfaf715aa --- /dev/null +++ b/conf/settings/settings_test.xml @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + From d61179852146db5ad94555aac1d8b568bfc780f2 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Tue, 22 Mar 2011 19:18:53 +0100 Subject: [PATCH 049/101] remove unused GPS files and add module conf file for arduimu basic version --- conf/modules/ins_arduimu.xml | 9 +- conf/modules/ins_arduimu_basic.xml | 18 ++ .../arduimu_basic/GPS_EM406.pde | 204 ------------ .../arduimu_basic/GPS_NMEA.pde | 292 ------------------ 4 files changed, 22 insertions(+), 501 deletions(-) create mode 100644 conf/modules/ins_arduimu_basic.xml delete mode 100644 sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/GPS_EM406.pde delete mode 100644 sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/GPS_NMEA.pde diff --git a/conf/modules/ins_arduimu.xml b/conf/modules/ins_arduimu.xml index 0fcd1c3d8b..37d4fee9bc 100644 --- a/conf/modules/ins_arduimu.xml +++ b/conf/modules/ins_arduimu.xml @@ -2,14 +2,13 @@
- +
- - - + + - + diff --git a/conf/modules/ins_arduimu_basic.xml b/conf/modules/ins_arduimu_basic.xml new file mode 100644 index 0000000000..b1d4493404 --- /dev/null +++ b/conf/modules/ins_arduimu_basic.xml @@ -0,0 +1,18 @@ + + + +
+ +
+ + + + + + + + + + + + diff --git a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/GPS_EM406.pde b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/GPS_EM406.pde deleted file mode 100644 index b8b6f27dd9..0000000000 --- a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/GPS_EM406.pde +++ /dev/null @@ -1,204 +0,0 @@ -#if GPS_PROTOCOL == 2 - -#define BUF_LEN 100 - -// The input buffer -char gps_buffer[BUF_LEN]={ - -// Setup for SIRF binary at 38400 - $PSRF100,0,38400,8,1,0*3C - 0x24,0x50,0x53,0x52,0x46,0x31,0x30,0x30,0x2C,0x30,0x2C,0x33,0x38,0x34,0x30,0x30,0x2C,0x38,0x2C,0x31,0x2C,0x30,0x2A,0x33,0x43,0x0D,0x0A}; - -// Used to configure Sirf GPS -const byte gps_ender[]={0xB0,0xB3}; - -/**************************************************************** - Parsing stuff for SIRF binary protocol. - ****************************************************************/ -void init_gps(void) -{ - pinMode(2,OUTPUT); //Serial Mux - digitalWrite(2,HIGH); //Serial Mux - change_to_sirf_protocol(); - delay(100);//Waits fot the GPS to start_UP - configure_gps();//Function to configure GPS, to output only the desired msg's -} - -void decode_gps(void) -{ - static unsigned long GPS_timer = 0; - static byte gps_counter = 0; //Another gps counter for the buffer - static byte GPS_step = 0; - boolean gps_failure = false; - static byte gps_ok = 0;//Counter to verify the reciving info - const byte read_gps_header[]={0xA0,0xA2,0x00,0x5B,0x29};//Used to verify the payload msg header - - if(Serial.available() > 0)//Ok, let me see, the buffer is empty? - { - while(Serial.available() < 5){ } - switch(GPS_step) { - case 0: //This case will verify the header, to know when the payload will begin - while(Serial.available() > 0) //Loop if data available - { - if(Serial.read() == read_gps_header[gps_ok]){ //Checking if the head bytes are equal.. - //if yes increment 1 - gps_ok++; - }else{ - //Otherwise restart. - gps_ok = 0; - } - if(gps_ok >= 5) { - //Ohh 5 bytes are correct, that means jump to the next step, and break the loop - gps_ok = 0; - GPS_step++; - break; - } - } - break; - case 1: //Will receive all the payload and join the received bytes... - while(Serial.available() < 92){ - } - gps_counter = 0; - memset(gps_buffer,0,sizeof(gps_buffer)); - - while(Serial.available() > 0){ - //Read data and store it in the temp buffer - byte b1 = Serial.read(); - byte b2 = gps_buffer[gps_counter-1]; - // gps_ender[]={0xB0,0xB3}; - - if((b1 == gps_ender[1]) && (b2 == gps_ender[0])){ - GPS_step = 0; - gps_counter = 0; - gpsFix = gps_buffer[1]; - - if(gpsFix == 0x00){ - // GPS signal is error free - // ------------------------ - digitalWrite(6,HIGH); - GPS_timer = millis(); - gpsFixnew=1; //new information available flag for binary message - - //Parse the data - GPS_join_data(); - - } else { - // GPS has returned an error code - // ------------------------------ - gpsFix = 0x01; // In GPS language a good fix = 0, bad = 1 - digitalWrite(6,LOW); - } - - break; - }else{ - gps_buffer[gps_counter] = b1; - gps_counter++; - - if (gps_counter >= BUF_LEN){ - Serial.flush(); - break; - } - } - } - break; - } - } - - if(millis() - GPS_timer > 2000){ - digitalWrite(6, LOW); //If we don't receive any byte in two seconds turn off gps fix LED... - gpsFix = 0x01; - } -} - -void GPS_join_data(void) -{ - // Read bytes and combine them with Unions - // --------------------------------------- - byte j = 22; - longUnion.byte[3] = gps_buffer[j++]; - longUnion.byte[2] = gps_buffer[j++]; - longUnion.byte[1] = gps_buffer[j++]; - longUnion.byte[0] = gps_buffer[j++]; - lat = longUnion.dword; // latitude * 10,000,000 - - - longUnion.byte[3] = gps_buffer[j++]; - longUnion.byte[2] = gps_buffer[j++]; - longUnion.byte[1] = gps_buffer[j++]; - longUnion.byte[0] = gps_buffer[j++]; - lon = longUnion.dword; // longitude * 10,000,000 - - j = 34; - longUnion.byte[3] = gps_buffer[j++]; - longUnion.byte[2] = gps_buffer[j++]; - longUnion.byte[1] = gps_buffer[j++]; - longUnion.byte[0] = gps_buffer[j++]; - alt_MSL = longUnion.dword * 10; // alt in meters * 1000 - - j = 39; - intUnion.byte[1] = gps_buffer[j++]; - intUnion.byte[0] = gps_buffer[j++]; - speed_3d = (float)intUnion.word / 100.0; // meters/second We only get ground speed but store it as speed_3d for use in DCM - - //iTOW - - intUnion.byte[1] = gps_buffer[j++]; - intUnion.byte[0] = gps_buffer[j++]; - ground_course = (float)intUnion.word / 100.0; // degrees - ground_course = abs(ground_course); //The GPS has a BUG sometimes give you the correct value but negative, weird!! - - // clear buffer - // ------------- - memset(gps_buffer,0,sizeof(gps_buffer)); -} - - -void configure_gps(void) -{ - const byte gps_header[]={ - 0xA0,0xA2,0x00,0x08,0xA6,0x00 };//Used to configure Sirf GPS - const byte gps_payload[]={ - 0x02,0x04,0x07,0x09,0x1B };//Used to configure Sirf GPS - const byte gps_checksum[]={ - 0xA8,0xAA,0xAD,0xAF,0xC1 };//Used to configure Sirf GPS - const byte cero = 0x00;//Used to configure Sirf GPS - - for(int z=0; z<2; z++) - { - for(int x=0; x<5; x++)//Print all messages to setup GPS - { - for(int y=0; y<6; y++) - { - Serial.print(byte(gps_header[y]));//Prints the msg header, is the same header for all msg.. - } - Serial.print(byte(gps_payload[x]));//Prints the payload, is not the same for every msg - for(int y=0; y<6; y++) - { - Serial.print(byte(cero)); //Prints 6 zeros - } - Serial.print(byte(gps_checksum[x])); //Print the Checksum - Serial.print(byte(gps_ender[0])); //Print the Ender of the string, is same on all msg's. - Serial.print(byte(gps_ender[1])); //ender - } - } -} - -void change_to_sirf_protocol(void) -{ - Serial.begin(4800); //First try in 4800 - delay(300); - for (byte x=0; x<=28; x++) - { - Serial.print(byte(gps_buffer[x]));//Sending special bytes declared at the beginning - } - delay(300); - Serial.begin(9600); //Then try in 9600 - delay(300); - for (byte x=0; x<=28; x++) - { - Serial.print(byte(gps_buffer[x])); - } -Serial.begin(38400); //Universal Synchronous Asynchronous Recieving Transmiting -} - -#endif - diff --git a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/GPS_NMEA.pde b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/GPS_NMEA.pde deleted file mode 100644 index b03c2e7222..0000000000 --- a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/GPS_NMEA.pde +++ /dev/null @@ -1,292 +0,0 @@ -#if GPS_PROTOCOL == 1 - -//********************************************************************************************* -// You may need to insert parameters appropriate for your gps from this list into init_gps() -//GPS SIRF configuration strings... -#define SIRF_BAUD_RATE_4800 "$PSRF100,1,4800,8,1,0*0E\r\n" -#define SIRF_BAUD_RATE_9600 "$PSRF100,1,9600,8,1,0*0D\r\n" -#define SIRF_BAUD_RATE_19200 "$PSRF100,1,19200,8,1,0*38\r\n" -#define SIRF_BAUD_RATE_38400 "$PSRF100,1,38400,8,1,0*3D\r\n" -#define SIRF_BAUD_RATE_57600 "$PSRF100,1,57600,8,1,0*36\r\n" -#define GSA_ON "$PSRF103,2,0,1,1*27\r\n" // enable GSA -#define GSA_OFF "$PSRF103,2,0,0,1*26\r\n" // disable GSA -#define GSV_ON "$PSRF103,3,0,1,1*26\r\n" // enable GSV -#define GSV_OFF "$PSRF103,3,0,0,1*27\r\n" // disable GSV -#define USE_WAAS 1 //1 = Enable, 0 = Disable, good in USA, slower FIX... -#define WAAS_ON "$PSRF151,1*3F\r\n" // enable WAAS -#define WAAS_OFF "$PSRF151,0*3E\r\n" // disable WAAS - -//GPS Locosys configuration strings... -#define USE_SBAS 0 -#define SBAS_ON "$PMTK313,1*2E\r\n" -#define SBAS_OFF "$PMTK313,0*2F\r\n" - -#define NMEA_OUTPUT_5HZ "$PMTK314,0,5,0,5,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n" //Set GGA and RMC to 5HZ -#define NMEA_OUTPUT_4HZ "$PMTK314,0,4,0,4,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n" //Set GGA and RMC to 4HZ -#define NMEA_OUTPUT_3HZ "$PMTK314,0,3,0,3,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n" //Set GGA and RMC to 3HZ -#define NMEA_OUTPUT_2HZ "$PMTK314,0,2,0,2,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n" //Set GGA and RMC to 2HZ -#define NMEA_OUTPUT_1HZ "$PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n" //Set GGA and RMC to 1HZ - -#define LOCOSYS_REFRESH_RATE_200 "$PMTK220,200*2C" //200 milliseconds -#define LOCOSYS_REFRESH_RATE_250 "$PMTK220,250*29\r\n" //250 milliseconds - -#define LOCOSYS_BAUD_RATE_4800 "$PMTK251,4800*14\r\n" -#define LOCOSYS_BAUD_RATE_9600 "$PMTK251,9600*17\r\n" -#define LOCOSYS_BAUD_RATE_19200 "$PMTK251,19200*22\r\n" -#define LOCOSYS_BAUD_RATE_38400 "$PMTK251,38400*27\r\n" -//************************************************************************************************************** - - -/**************************************************************** - Parsing stuff for NMEA - ****************************************************************/ -#define BUF_LEN 200 - -// GPS Pointers -char *token; -char *search = ","; -char *brkb, *pEnd; -char gps_buffer[BUF_LEN]; //The traditional buffer. - -void init_gps(void) -{ - pinMode(2,OUTPUT); //Serial Mux - digitalWrite(2,HIGH); //Serial Mux - Serial.begin(9600); - delay(1000); - Serial.print(LOCOSYS_BAUD_RATE_38400); - Serial.begin(38400); - delay(500); - Serial.print(LOCOSYS_REFRESH_RATE_250); - delay(500); - Serial.print(NMEA_OUTPUT_4HZ); - delay(500); - Serial.print(SBAS_OFF); - - -/* EM406 example init - Serial.begin(4800); //Universal Sincronus Asyncronus Receiveing Transmiting - delay(1000); - Serial.print(SIRF_BAUD_RATE_9600); - - Serial.begin(9600); - delay(1000); - - Serial.print(GSV_OFF); - Serial.print(GSA_OFF); - - #if USE_WAAS == 1 - Serial.print(WAAS_ON); - #else - Serial.print(WAAS_OFF); - #endif -*/ - -} - -void decode_gps(void) -{ - const char head_rmc[]="GPRMC"; //GPS NMEA header to look for - const char head_gga[]="GPGGA"; //GPS NMEA header to look for - - static unsigned long GPS_timer = 0; //used to turn off the LED if no data is received. - - static byte unlock = 1; //some kind of event flag - static byte checksum = 0; //the checksum generated - static byte checksum_received = 0; //Checksum received - static byte counter = 0; //general counter - - //Temporary variables for some tasks, specially used in the GPS parsing part (Look at the NMEA_Parser tab) - unsigned long temp = 0; - unsigned long temp2 = 0; - unsigned long temp3 = 0; - - - while(Serial.available() > 0) - { - if(unlock == 0) - { - gps_buffer[0] = Serial.read();//puts a byte in the buffer - - if(gps_buffer[0]=='$')//Verify if is the preamble $ - { - counter = 0; - checksum = 0; - unlock = 1; - } - } else { - gps_buffer[counter] = Serial.read(); - - if(gps_buffer[counter] == 0x0A)//Looks for \F - { - unlock = 0; - - if (strncmp (gps_buffer, head_rmc, 5) == 0)//looking for rmc head.... - { - - /*Generating and parsing received checksum, */ - for(int x=0; x<100; x++) - { - if(gps_buffer[x]=='*') - { - checksum_received = strtol(&gps_buffer[x + 1], NULL, 16);//Parsing received checksum... - break; - } - else - { - checksum ^= gps_buffer[x]; //XOR the received data... - } - } - - if(checksum_received == checksum)//Checking checksum - { - /* Token will point to the data between comma "'", returns the data in the order received */ - /*THE GPRMC order is: UTC, UTC status , Lat, N/S indicator, Lon, E/W indicator, speed, course, date, mode, checksum*/ - token = strtok_r(gps_buffer, search, &brkb); //Contains the header GPRMC, not used - - token = strtok_r(NULL, search, &brkb); //UTC Time, not used - //time= atol (token); - token = strtok_r(NULL, search, &brkb); //Valid UTC data? maybe not used... - - - //Longitude in degrees, decimal minutes. (ej. 4750.1234 degrees decimal minutes = 47.835390 decimal degrees) - //Where 47 are degrees and 50 the minutes and .1234 the decimals of the minutes. - //To convert to decimal degrees, devide the minutes by 60 (including decimals), - //Example: "50.1234/60=.835390", then add the degrees, ex: "47+.835390 = 47.835390" decimal degrees - token = strtok_r(NULL, search, &brkb); //Contains Latitude in degrees decimal minutes... - - //taking only degrees, and minutes without decimals, - //strtol stop parsing till reach the decimal point "." result example 4750, eliminates .1234 - temp = strtol (token, &pEnd, 10); - - //takes only the decimals of the minutes - //result example 1234. - temp2 = strtol (pEnd + 1, NULL, 10); - - //joining degrees, minutes, and the decimals of minute, now without the point... - //Before was 4750.1234, now the result example is 47501234... - temp3 = (temp * 10000) + (temp2); - - - //modulo to leave only the decimal minutes, eliminating only the degrees.. - //Before was 47501234, the result example is 501234. - temp3 = temp3 % 1000000; - - - //Dividing to obtain only the de degrees, before was 4750 - //The result example is 47 (4750/100 = 47) - temp /= 100; - - //Joining everything and converting to * 10,000,000 ... - //First i convert the decimal minutes to degrees decimals stored in "temp3", example: 501234/600000 =.835390 - //Then i add the degrees stored in "temp" and add the result from the first step, example 47+.835390 = 47.835390 - //The result is stored in "lat" variable... - //**This is all changed in this case to be a long integer which is decimal degrees * 10**7 - - lat = (temp * 10000000) + ((temp3 *100) / 6); - - token = strtok_r(NULL, search, &brkb); //lat, north or south? - //If the char is equal to S (south), multiply the result by -1.. - if(*token == 'S'){ - lat *= -1; - } - - //This the same procedure use in lat, but now for Lon.... - token = strtok_r(NULL, search, &brkb); - temp = strtol (token,&pEnd, 10); - temp2 = strtol (pEnd + 1, NULL, 10); - temp3 = (temp * 10000) + (temp2); - temp3 = temp3%1000000; - temp/= 100; - lon = (temp * 10000000) + ((temp3 * 100) / 6); - - token = strtok_r(NULL, search, &brkb); //lon, east or west? - if(*token == 'W'){ - lon *= -1; - } - - token = strtok_r(NULL, search, &brkb); //Speed over ground - speed_3d = (float)atoi(token); // We only get ground speed but store it as speed_3d for use in DCM - - token = strtok_r(NULL, search, &brkb); //Course - ground_course = (float)atoi(token); - - gpsFixnew=1; //new information available flag for binary message - - } - checksum = 0; - }//End of the GPRMC parsing - - if (strncmp (gps_buffer, head_gga, 5) == 0)//now looking for GPGGA head.... - { - /*Generating and parsing received checksum, */ - for(int x = 0; x<100; x++) - { - if(gps_buffer[x]=='*') - { - checksum_received = strtol(&gps_buffer[x + 1], NULL, 16);//Parsing received checksum... - break; - } - else - { - checksum^= gps_buffer[x]; //XOR the received data... - } - } - - if(checksum_received== checksum)//Checking checksum - { - //strcpy(gps_GGA,gps_buffer); - - token = strtok_r(gps_buffer, search, &brkb);//GPGGA header, not used anymore - token = strtok_r(NULL, search, &brkb);//UTC, not used!! - token = strtok_r(NULL, search, &brkb);//lat, not used!! - token = strtok_r(NULL, search, &brkb);//north/south, nope... - token = strtok_r(NULL, search, &brkb);//lon, not used!! - token = strtok_r(NULL, search, &brkb);//wets/east, nope - token = strtok_r(NULL, search, &brkb);//Position fix, used!! - - if(atoi(token) >= 1){ - gpsFix = 0x00; // gpsFix = 0 means valid fix - }else{ - gpsFix = 0x01; - } - token = strtok_r(NULL, search, &brkb); //sats in use!! Nein... - token = strtok_r(NULL, search, &brkb);//HDOP, not needed - token = strtok_r(NULL, search, &brkb);//ALTITUDE, is the only meaning of this string.. in meters of course. - alt_MSL = abs(atoi(token)) * 1000; - - if(gpsFix == 0x00) digitalWrite(6, HIGH); //Status LED... - else digitalWrite(6, LOW); - } - checksum = 0; //Restarting the checksum - } - - for(int a = 0; a<= counter; a++)//restarting the buffer - { - gps_buffer[a]= 0; - } - counter = 0; //Restarting the counter - GPS_timer = millis(); //Restarting timer... - } - else - { - counter++; //Incrementing counter - if (counter >= 200) - { - //Serial.flush(); - counter = 0; - checksum = 0; - unlock = 0; - } - } - } - } - - if(millis() - GPS_timer > 2000){ - digitalWrite(6, LOW); //If we don't receive any byte in two seconds turn off gps fix LED... - gpsFix = 0x01; - gpsFixnew = 0; - } -} -#endif - From b4a6eb94276d0c39e71d3743ca51e5672b4ebb27 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Tue, 22 Mar 2011 20:48:36 +0100 Subject: [PATCH 050/101] removed booz2_unsimulated_peripherals and updated nps makefile to use electrical and adc instead of battery and booz2_analog. --- .../subsystems/rotorcraft/fdm_nps.makefile | 12 ++++--- .../arch/sim/booz2_unsimulated_peripherals.c | 31 ------------------- sw/simulator/nps/nps_autopilot_booz.c | 6 ++-- 3 files changed, 10 insertions(+), 39 deletions(-) delete mode 100644 sw/airborne/booz/arch/sim/booz2_unsimulated_peripherals.c diff --git a/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile b/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile index 1cd8ef6246..5c0fe15c78 100644 --- a/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile +++ b/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile @@ -65,7 +65,6 @@ sim.srcs += math/pprz_trig_int.c \ sim.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) -sim.srcs += $(SRC_BOOZ_SIM)/booz2_unsimulated_peripherals.c sim.srcs += firmwares/rotorcraft/main.c sim.srcs += mcu.c sim.srcs += $(SRC_ARCH)/mcu_arch.c @@ -90,13 +89,16 @@ sim.srcs += $(SRC_FIRMWARE)/datalink.c # -sim.CFLAGS += -DBOOZ2_ANALOG_BARO_LED=2 -DBOOZ2_ANALOG_BARO_PERIOD='SYS_TICS_OF_SEC((1./100.))' +sim.CFLAGS += -DROTORCRAFT_BARO_LED=2 sim.srcs += $(SRC_BOARD)/baro_board.c -sim.CFLAGS += -DBOOZ2_ANALOG_BATTERY_PERIOD='SYS_TICS_OF_SEC((1./10.))' -sim.srcs += $(SRC_FIRMWARE)/battery.c +sim.CFLAGS += -DUSE_ADC +sim.srcs += $(SRC_ARCH)/mcu_periph/adc_arch.c +sim.srcs += subsystems/electrical.c +# baro has variable offset amplifier on booz board +#sim.CFLAGS += -DUSE_DAC +#sim.srcs += $(SRC_ARCH)/mcu_periph/dac_arch.c -sim.srcs += $(SRC_BOOZ)/booz2_analog.c $(SRC_BOOZ_SIM)/booz2_analog_hw.c #sim.CFLAGS += -DIMU_TYPE_H=\"imu/imu_b2.h\" #sim.CFLAGS += -DIMU_B2_VERSION_1_1 diff --git a/sw/airborne/booz/arch/sim/booz2_unsimulated_peripherals.c b/sw/airborne/booz/arch/sim/booz2_unsimulated_peripherals.c deleted file mode 100644 index 5a86889e63..0000000000 --- a/sw/airborne/booz/arch/sim/booz2_unsimulated_peripherals.c +++ /dev/null @@ -1,31 +0,0 @@ -/* - * $Id$ - * - * Copyright (C) 2008-2009 Antoine Drouin - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - */ - -#include "mcu_periph/uart.h" -//#include "mcu_periph/i2c.h" - -void uart0_init( void ) {} -void uart1_init( void ) {} - -//void i2c0_hw_init( void ) {} -//void i2c1_hw_init( void ) {} diff --git a/sw/simulator/nps/nps_autopilot_booz.c b/sw/simulator/nps/nps_autopilot_booz.c index 80f3f0bbfd..c82a02c3bf 100644 --- a/sw/simulator/nps/nps_autopilot_booz.c +++ b/sw/simulator/nps/nps_autopilot_booz.c @@ -7,7 +7,7 @@ #include "subsystems/imu.h" #include "subsystems/sensors/baro.h" #include "baro_board.h" -#include "firmwares/rotorcraft/battery.h" +#include "subsystems/electrical.h" #include "actuators/supervision.h" @@ -25,9 +25,9 @@ void nps_autopilot_init(enum NpsRadioControlType type_rc, int num_rc_script, cha main_init(); #ifdef MAX_BAT_LEVEL - battery_voltage = MAX_BAT_LEVEL * 10; + electrical.vsupply = MAX_BAT_LEVEL * 10; #else - battery_voltage = 111; + electrical.vsupply = 111; #endif } From c9e9833850c0d24196885ba3065dd9ea0b52ccc0 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Wed, 23 Mar 2011 18:10:45 +0100 Subject: [PATCH 051/101] define ADC 0 to 3 in booz board header --- sw/airborne/boards/booz_1.0.h | 40 +++++++++++++++++++++++++++++------ 1 file changed, 34 insertions(+), 6 deletions(-) diff --git a/sw/airborne/boards/booz_1.0.h b/sw/airborne/boards/booz_1.0.h index 4d9dda8dc6..db7e8069b6 100644 --- a/sw/airborne/boards/booz_1.0.h +++ b/sw/airborne/boards/booz_1.0.h @@ -54,12 +54,41 @@ /* ADC */ -/* pressure : P0.10 AD1.2 */ -#define ANALOG_BARO_PINSEL PINSEL0 -#define ANALOG_BARO_PINSEL_VAL 0x03 -#define ANALOG_BARO_PINSEL_BIT 20 -#define ANALOG_BARO_ADC 1 +/* select P0.13 (ADC_SPARE) as AD1.4 for ADC_0 */ +#define ADC_0 AdcBank1(4) +#ifdef USE_ADC_0 +#ifndef USE_AD1 +#define USE_AD1 +#endif +#define USE_AD1_4 +#endif +/* select P0.4 (SCK_0) as AD0.6 for ADC_1 */ +#define ADC_1 AdcBank0(6) +#ifdef USE_ADC_1 +#ifndef USE_AD0 +#define USE_AD0 +#endif +#define USE_AD0_6 +#endif + +/* select P0.5 (MISO_0) as AD0.7 for ADC_2 */ +#define ADC_2 AdcBank0(7) +#ifdef USE_ADC_2 +#ifndef USE_AD0 +#define USE_AD0 +#endif +#define USE_AD0_7 +#endif + +/* select P0.6 (MOSI_0) as AD1.0 for ADC_3 */ +#define ADC_3 AdcBank1(0) +#ifdef USE_ADC_3 +#ifndef USE_AD1 +#define USE_AD1 +#endif +#define USE_AD1_0 +#endif /* battery */ #define ADC_CHANNEL_VSUPPLY AdcBank0(2) @@ -70,7 +99,6 @@ #define DefaultVoltageOfAdc(adc) (0.0183*adc) - /* baro */ #define ADC_CHANNEL_BARO AdcBank1(2) #ifndef USE_AD1 From b237bb17ae458c80b076eecd48a5e29f38333e98 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Thu, 24 Mar 2011 10:45:04 +0100 Subject: [PATCH 052/101] update maxbotix driver to the adc system for booz it should actually now work for any platform --- conf/airframes/ENAC/quadrotor/booz2_g1.xml | 4 +++- conf/modules/sonar_maxbotix_booz.xml | 23 ++++++++++++++----- sw/airborne/arch/lpc21/mcu_periph/adc_arch.h | 2 ++ ...sonar_maxbotix_booz.c => sonar_maxbotix.c} | 12 ++++++---- ...sonar_maxbotix_booz.h => sonar_maxbotix.h} | 7 +++--- sw/airborne/subsystems/ins.c | 10 ++++---- sw/airborne/subsystems/ins.h | 2 +- 7 files changed, 38 insertions(+), 22 deletions(-) rename sw/airborne/modules/sonar/{sonar_maxbotix_booz.c => sonar_maxbotix.c} (81%) rename sw/airborne/modules/sonar/{sonar_maxbotix_booz.h => sonar_maxbotix.h} (84%) diff --git a/conf/airframes/ENAC/quadrotor/booz2_g1.xml b/conf/airframes/ENAC/quadrotor/booz2_g1.xml index b1405b3931..b8acf7af5e 100644 --- a/conf/airframes/ENAC/quadrotor/booz2_g1.xml +++ b/conf/airframes/ENAC/quadrotor/booz2_g1.xml @@ -4,7 +4,9 @@ - + diff --git a/conf/modules/sonar_maxbotix_booz.xml b/conf/modules/sonar_maxbotix_booz.xml index ce23345d65..e28a1aa754 100644 --- a/conf/modules/sonar_maxbotix_booz.xml +++ b/conf/modules/sonar_maxbotix_booz.xml @@ -1,16 +1,27 @@ + +
- + +
- - - - - + + + + + + + + diff --git a/sw/airborne/arch/lpc21/mcu_periph/adc_arch.h b/sw/airborne/arch/lpc21/mcu_periph/adc_arch.h index 947003f64b..6e5fe748c9 100644 --- a/sw/airborne/arch/lpc21/mcu_periph/adc_arch.h +++ b/sw/airborne/arch/lpc21/mcu_periph/adc_arch.h @@ -25,6 +25,8 @@ #ifndef ADC_HW_H #define ADC_HW_H +#include BOARD_CONFIG + #define AdcBank0(x) (x) #define AdcBank1(x) (x+NB_ADC) diff --git a/sw/airborne/modules/sonar/sonar_maxbotix_booz.c b/sw/airborne/modules/sonar/sonar_maxbotix.c similarity index 81% rename from sw/airborne/modules/sonar/sonar_maxbotix_booz.c rename to sw/airborne/modules/sonar/sonar_maxbotix.c index fad7db7e8d..c339212cea 100644 --- a/sw/airborne/modules/sonar/sonar_maxbotix_booz.c +++ b/sw/airborne/modules/sonar/sonar_maxbotix.c @@ -1,5 +1,4 @@ /* - * $Id: demo_module.c 3079 2009-03-11 16:55:42Z gautier $ * * Copyright (C) 2010 Gautier Hattenberger * @@ -22,22 +21,25 @@ * */ -#include "sonar_maxbotix_booz.h" -#include "booz2_analog.h" +#include "modules/sonar/sonar_maxbotix.h" +#include "mcu_periph/adc.h" uint16_t sonar_meas; bool_t sonar_data_available; +static struct adc_buf sonar_adc; + void maxbotix_init(void) { sonar_meas = 0; sonar_data_available = FALSE; + + adc_buf_channel(ADC_CHANNEL_SONAR, &sonar_adc, DEFAULT_AV_NB_SAMPLE); } /** Read ADC value to update sonar measurement */ void maxbotix_read(void) { - booz2_analog_extra_adc_read(); - sonar_meas = booz2_adc_1; + sonar_meas = sonar_adc.sum / sonar_adc.av_nb_sample; sonar_data_available = TRUE; } diff --git a/sw/airborne/modules/sonar/sonar_maxbotix_booz.h b/sw/airborne/modules/sonar/sonar_maxbotix.h similarity index 84% rename from sw/airborne/modules/sonar/sonar_maxbotix_booz.h rename to sw/airborne/modules/sonar/sonar_maxbotix.h index 596a8e5a75..b692f0889a 100644 --- a/sw/airborne/modules/sonar/sonar_maxbotix_booz.h +++ b/sw/airborne/modules/sonar/sonar_maxbotix.h @@ -1,5 +1,4 @@ /* - * $Id: demo_module.h 3079 2009-03-11 16:55:42Z gautier $ * * Copyright (C) 2010 Gautier Hattenberger * @@ -22,9 +21,9 @@ * */ -/** \file sonar_maxbotix_booz.h +/** \file sonar_maxbotix.h * - * simple driver to deal with one maxbotix sensor on booz AP + * simple driver to deal with one maxbotix sensor */ #ifndef SONAR_MAXBOTIX_BOOZ_H @@ -39,7 +38,7 @@ extern bool_t sonar_data_available; extern void maxbotix_init(void); extern void maxbotix_read(void); -#include "subsystems/ins.h" // needed because ins is not a module +//#include "subsystems/ins.h" // needed because ins is not a module #define SonarEvent(_handler) { \ if (sonar_data_available) { \ diff --git a/sw/airborne/subsystems/ins.c b/sw/airborne/subsystems/ins.c index cd098c51b5..0e8e560273 100644 --- a/sw/airborne/subsystems/ins.c +++ b/sw/airborne/subsystems/ins.c @@ -70,7 +70,7 @@ bool_t ins_hf_realign; int32_t ins_qfe; bool_t ins_baro_initialised; int32_t ins_baro_alt; -#ifdef BOOZ2_SONAR +#ifdef USE_SONAR bool_t ins_update_on_agl; int32_t ins_sonar_offset; #endif @@ -190,7 +190,7 @@ void ins_update_baro() { if (ins_vf_realign) { ins_vf_realign = FALSE; ins_qfe = baro.absolute; -#ifdef BOOZ2_SONAR +#ifdef USE_SONAR ins_sonar_offset = sonar_meas; #endif vff_realign(0.); @@ -267,13 +267,13 @@ void ins_update_gps(void) { } void ins_update_sonar() { -#if defined BOOZ2_SONAR && defined USE_VFF +#if defined USE_SONAR && defined USE_VFF static int32_t sonar_filtered = 0; sonar_filtered = (sonar_meas + 2*sonar_filtered) / 3; /* update baro_qfe assuming a flat ground */ - if (ins_update_on_agl && booz2_analog_baro_status == BOOZ2_ANALOG_BARO_RUNNING) { + if (ins_update_on_agl && baro.status == BS_RUNNING) { int32_t d_sonar = (((int32_t)sonar_filtered - ins_sonar_offset) * INS_SONAR_SENS_NUM) / INS_SONAR_SENS_DEN; - ins_qfe = (int32_t)booz2_analog_baro_value + (d_sonar * (INS_BARO_SENS_DEN))/INS_BARO_SENS_NUM; + ins_qfe = baro.absolute + (d_sonar * (INS_BARO_SENS_DEN))/INS_BARO_SENS_NUM; } #endif } diff --git a/sw/airborne/subsystems/ins.h b/sw/airborne/subsystems/ins.h index d2d374c121..6353bbb553 100644 --- a/sw/airborne/subsystems/ins.h +++ b/sw/airborne/subsystems/ins.h @@ -37,7 +37,7 @@ extern struct NedCoor_i ins_gps_speed_cm_s_ned; extern int32_t ins_baro_alt; extern int32_t ins_qfe; extern bool_t ins_baro_initialised; -#ifdef BOOZ2_SONAR +#ifdef USE_SONAR extern bool_t ins_update_on_agl; /* use sonar to update agl if available */ extern int32_t ins_sonar_offset; #endif From 132191e5007583332c96249368051a1284f30fdb Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Thu, 24 Mar 2011 10:59:01 +0100 Subject: [PATCH 053/101] add a GPS_LLA message --- conf/messages.xml | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/conf/messages.xml b/conf/messages.xml index 2029c96743..7357d2027d 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -439,8 +439,19 @@ - - + + + + + + + + + + + + + From 52a0ae19ff987914624780700a407ed2dd363da1 Mon Sep 17 00:00:00 2001 From: Antoine Drouin Date: Thu, 24 Mar 2011 12:17:18 +0100 Subject: [PATCH 054/101] added a bias message --- conf/messages.xml | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/conf/messages.xml b/conf/messages.xml index 7357d2027d..57fcb9a2e5 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -1327,7 +1327,13 @@ - + + + + + + + From 3fbc18ecd3ad04777de2acf559f53418c7cd5918 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Thu, 24 Mar 2011 13:55:20 +0100 Subject: [PATCH 055/101] change generic adc channel to ADC_7 in example aiframe file, as ADC_1 collides with IR channel --- conf/airframes/twinjet_example.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/conf/airframes/twinjet_example.xml b/conf/airframes/twinjet_example.xml index 70fd68b0b7..b5974575d7 100644 --- a/conf/airframes/twinjet_example.xml +++ b/conf/airframes/twinjet_example.xml @@ -36,7 +36,7 @@ - + From e429af174a1b41dccf21051f387d3f61eaa69796 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Thu, 24 Mar 2011 15:55:02 +0100 Subject: [PATCH 056/101] updated nps makefile, added gls libs that are needed --- conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile b/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile index 5c0fe15c78..fbcf8060ec 100644 --- a/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile +++ b/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile @@ -22,7 +22,7 @@ sim.ARCHDIR = $(ARCH) sim.CFLAGS += -DSITL -DNPS sim.CFLAGS += `pkg-config glib-2.0 --cflags` -I /usr/include/meschach -sim.LDFLAGS += `pkg-config glib-2.0 --libs` -lm -lmeschach -lpcre -lglibivy +sim.LDFLAGS += `pkg-config glib-2.0 --libs` -lm -lpcre -lglibivy -lgsl -lgslcblas sim.CFLAGS += -I$(NPSDIR) -I$(SRC_FIRMWARE) -I$(SRC_BOOZ) -I$(SRC_BOOZ_SIM) -I$(SRC_BOARD) -I../simulator -I$(PAPARAZZI_HOME)/conf/simulator/nps # use the paparazzi-jsbsim package if it is installed, otherwise look for JSBsim under /opt/jsbsim From 7e241c42dca6656360a02472eeb8bfa7e39fe25a Mon Sep 17 00:00:00 2001 From: Piotr Esden-Tempski Date: Thu, 24 Mar 2011 16:55:30 -0700 Subject: [PATCH 057/101] Revert "Scale RC command for thrust based on MIN and MAX motor" Guidance should not care about motor scalings. This reverts commit dfa583cc39d3f431b2a291928642d6575389e806. --- sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c index 03a4b251b2..8dbdf59828 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c @@ -111,7 +111,7 @@ void guidance_v_read_rc(void) { // used in RC_DIRECT directly and as saturation in CLIMB and HOVER #ifndef USE_HELI - guidance_v_rc_delta_t = (int32_t)radio_control.values[RADIO_THROTTLE] * (SUPERVISION_MAX_MOTOR - SUPERVISION_MIN_MOTOR) / MAX_PPRZ + SUPERVISION_MIN_MOTOR; + guidance_v_rc_delta_t = (int32_t)radio_control.values[RADIO_THROTTLE] * 200 / MAX_PPRZ; #else guidance_v_rc_delta_t = (int32_t)radio_control.values[RADIO_THROTTLE] * 4 / 5; #endif From 1cd366db95026ac4f29f83821db3dc4d9848637b Mon Sep 17 00:00:00 2001 From: Piotr Esden-Tempski Date: Thu, 24 Mar 2011 18:37:39 -0700 Subject: [PATCH 058/101] Added scaling for throttle for pwm actuators. --- .../firmwares/rotorcraft/actuators/actuators_pwm_supervision.c | 1 + 1 file changed, 1 insertion(+) diff --git a/sw/airborne/firmwares/rotorcraft/actuators/actuators_pwm_supervision.c b/sw/airborne/firmwares/rotorcraft/actuators/actuators_pwm_supervision.c index 06fb68f5ad..9debad1cfc 100644 --- a/sw/airborne/firmwares/rotorcraft/actuators/actuators_pwm_supervision.c +++ b/sw/airborne/firmwares/rotorcraft/actuators/actuators_pwm_supervision.c @@ -46,6 +46,7 @@ void actuators_set(bool_t motors_on) { booz2_commands[COMMAND_PITCH] = booz2_commands[COMMAND_PITCH] * PWM_GAIN_SCALE; booz2_commands[COMMAND_ROLL] = booz2_commands[COMMAND_ROLL] * PWM_GAIN_SCALE; booz2_commands[COMMAND_YAW] = booz2_commands[COMMAND_YAW] * PWM_GAIN_SCALE; + booz2_commands[COMMAND_THRUST] = (booz2_commands[COMMAND_THRUST] * ((SUPERVISION_MAX_MOTOR - SUPERVISION_MIN_MOTOR) / 200)) + SUPERVISION_MIN_MOTOR; supervision_run(motors_on, FALSE, booz2_commands); From 76ddb40492761a1d30f1f80faac8281bd5c5b401 Mon Sep 17 00:00:00 2001 From: Piotr Esden-Tempski Date: Thu, 24 Mar 2011 18:44:12 -0700 Subject: [PATCH 059/101] Improved gains for lisa pwm aspirin quad. --- conf/airframes/esden/lisa_pwm_aspirin.xml | 25 ++++++++++++----------- 1 file changed, 13 insertions(+), 12 deletions(-) diff --git a/conf/airframes/esden/lisa_pwm_aspirin.xml b/conf/airframes/esden/lisa_pwm_aspirin.xml index 6b56c34a60..98f4515e8e 100644 --- a/conf/airframes/esden/lisa_pwm_aspirin.xml +++ b/conf/airframes/esden/lisa_pwm_aspirin.xml @@ -1,6 +1,6 @@ - + @@ -138,15 +138,15 @@ - - - + + + - - - + + + - + @@ -167,14 +167,15 @@ - - - + + + + - + From cd532aae6e8c62245331a7e4ed99b7f84b73262c Mon Sep 17 00:00:00 2001 From: Piotr Esden-Tempski Date: Thu, 24 Mar 2011 18:45:39 -0700 Subject: [PATCH 060/101] Changed mode 2 to ATTITUDE RC CLIMB. Now the throttle stick controls the climb/descend rate. --- conf/airframes/esden/lisa_pwm_aspirin.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/conf/airframes/esden/lisa_pwm_aspirin.xml b/conf/airframes/esden/lisa_pwm_aspirin.xml index 98f4515e8e..d8a3c92478 100644 --- a/conf/airframes/esden/lisa_pwm_aspirin.xml +++ b/conf/airframes/esden/lisa_pwm_aspirin.xml @@ -90,7 +90,7 @@
- +
From db30c0343fd83aa7168c99baf30ae5acb317eaf4 Mon Sep 17 00:00:00 2001 From: Piotr Esden-Tempski Date: Thu, 24 Mar 2011 19:47:00 -0700 Subject: [PATCH 061/101] Fixed baud rate for ublox6 on lisa_pwm_aspirin frame. --- conf/airframes/esden/lisa_pwm_aspirin.xml | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/conf/airframes/esden/lisa_pwm_aspirin.xml b/conf/airframes/esden/lisa_pwm_aspirin.xml index d8a3c92478..01bd57bcdf 100644 --- a/conf/airframes/esden/lisa_pwm_aspirin.xml +++ b/conf/airframes/esden/lisa_pwm_aspirin.xml @@ -221,7 +221,9 @@ - + + + From f5e8bd8736cbc501ef3c1f135216499122bcf87f Mon Sep 17 00:00:00 2001 From: Antoine Drouin Date: Fri, 25 Mar 2011 10:29:56 +0100 Subject: [PATCH 062/101] added flags to allow simulation --- conf/autopilot/subsystems/shared/ahrs_ic.makefile | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/conf/autopilot/subsystems/shared/ahrs_ic.makefile b/conf/autopilot/subsystems/shared/ahrs_ic.makefile index a683b25d66..bda8b08365 100644 --- a/conf/autopilot/subsystems/shared/ahrs_ic.makefile +++ b/conf/autopilot/subsystems/shared/ahrs_ic.makefile @@ -16,3 +16,7 @@ AHRS_SRCS += subsystems/ahrs/ahrs_aligner.c ap.CFLAGS += $(AHRS_CFLAGS) ap.srcs += $(AHRS_SRCS) + +sim.CFLAGS += $(AHRS_CFLAGS) +sim.srcs += $(AHRS_SRCS) + From c0f68a1f3ee9fea14cda947731fc66586bd81edb Mon Sep 17 00:00:00 2001 From: Antoine Drouin Date: Fri, 25 Mar 2011 10:33:07 +0100 Subject: [PATCH 063/101] added configuration file for ublox6 on rotorcraft firmware --- conf/gps/rotorcraft_ublox6.txt | 65 ++++++++++++++++++++++++++++++++++ 1 file changed, 65 insertions(+) create mode 100644 conf/gps/rotorcraft_ublox6.txt diff --git a/conf/gps/rotorcraft_ublox6.txt b/conf/gps/rotorcraft_ublox6.txt new file mode 100644 index 0000000000..d9963acc2f --- /dev/null +++ b/conf/gps/rotorcraft_ublox6.txt @@ -0,0 +1,65 @@ +MON-VER - 0A 04 46 00 37 2E 30 31 20 28 34 34 31 37 39 29 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 30 30 30 34 30 30 30 37 00 00 36 2E 30 32 20 28 33 36 30 32 33 29 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 +CFG-ANT - 06 13 04 00 1B 00 8B A9 +CFG-DAT - 06 06 02 00 00 00 +CFG-FXN - 06 0E 24 00 0C 00 00 00 00 00 00 00 00 00 00 00 10 27 00 00 10 27 00 00 D0 07 00 00 18 FC FF FF 00 00 00 00 00 00 00 00 +CFG-INF - 06 02 0A 00 00 00 00 00 00 00 00 00 00 00 +CFG-INF - 06 02 0A 00 01 00 00 00 87 87 87 87 87 87 +CFG-INF - 06 02 0A 00 03 00 00 00 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 01 01 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 01 02 00 05 00 00 00 00 +CFG-MSG - 06 01 08 00 01 03 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 01 04 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 01 06 00 01 00 00 00 00 +CFG-MSG - 06 01 08 00 01 11 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 01 12 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 01 20 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 01 21 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 01 22 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 01 30 00 04 00 00 00 00 +CFG-MSG - 06 01 08 00 01 31 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 01 32 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 02 20 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 02 23 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 0A 02 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 0A 05 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 0A 06 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 0A 07 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 0A 08 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 0A 09 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 0A 0A 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 0A 20 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 0A 21 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 0B 00 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 0B 30 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 0B 31 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 0B 32 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 0D 01 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 0D 03 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 F0 00 01 01 01 01 01 01 +CFG-MSG - 06 01 08 00 F0 01 01 01 01 01 01 01 +CFG-MSG - 06 01 08 00 F0 02 01 01 01 01 01 01 +CFG-MSG - 06 01 08 00 F0 03 01 01 01 01 01 01 +CFG-MSG - 06 01 08 00 F0 04 01 01 01 01 01 01 +CFG-MSG - 06 01 08 00 F0 05 01 01 01 01 01 01 +CFG-MSG - 06 01 08 00 F0 06 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 F0 07 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 F0 08 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 F0 09 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 F0 0A 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 F1 00 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 F1 03 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 F1 04 00 00 00 00 00 00 +CFG-NAV5 - 06 24 24 00 FF FF 08 02 00 00 00 00 10 27 00 00 05 00 FA 00 FA 00 64 00 2C 01 00 3C 00 00 00 00 00 00 00 00 00 00 00 00 +CFG-NAVX5 - 06 23 28 00 00 00 FF FF 03 00 00 00 03 02 03 10 07 00 00 01 00 00 43 06 00 00 00 00 01 01 00 00 00 64 78 00 00 00 00 00 00 00 00 00 +CFG-NMEA - 06 17 04 00 00 23 00 02 +CFG-PM - 06 32 18 00 00 06 00 00 04 90 00 00 E8 03 00 00 10 27 00 00 00 00 00 00 02 00 00 00 +CFG-PRT - 06 00 14 00 00 00 00 00 84 00 00 00 00 00 00 00 07 00 07 00 00 00 00 00 +CFG-PRT - 06 00 14 00 01 00 00 00 C0 08 00 00 00 E1 00 00 07 00 01 00 00 00 00 00 +CFG-PRT - 06 00 14 00 02 00 00 00 C0 08 00 00 80 25 00 00 00 00 00 00 00 00 00 00 +CFG-PRT - 06 00 14 00 03 00 00 00 00 00 00 00 00 00 00 00 07 00 07 00 00 00 00 00 +CFG-RATE - 06 08 06 00 7D 00 01 00 01 00 +CFG-RXM - 06 11 02 00 08 00 +CFG-SBAS - 06 16 08 00 03 03 03 00 51 08 00 00 +CFG-TP - 06 07 14 00 40 42 0F 00 A0 86 01 00 01 01 00 00 32 00 00 00 00 00 00 00 +CFG-TP5 - 06 31 20 00 00 E9 03 00 32 00 00 00 40 42 0F 00 40 42 0F 00 00 00 00 00 A0 86 01 00 00 00 00 00 F7 00 00 00 +CFG-USB - 06 1B 6C 00 46 15 A6 01 00 00 00 00 64 00 00 01 75 2D 62 6C 6F 78 20 41 47 20 2D 20 77 77 77 2E 75 2D 62 6C 6F 78 2E 63 6F 6D 00 00 00 00 00 00 75 2D 62 6C 6F 78 20 36 20 20 2D 20 20 47 50 53 20 52 65 63 65 69 76 65 72 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 From e1ffe4a590d3a786f3ad0e8421421cc913b537e4 Mon Sep 17 00:00:00 2001 From: Antoine Drouin Date: Fri, 25 Mar 2011 10:38:26 +0100 Subject: [PATCH 064/101] added permanent settings infrastructure on stm32 --- conf/autopilot/lisa_l_test_progs.makefile | 4 +- conf/settings/settings_test.xml | 2 +- .../arch/stm32/subsystems/settings_arch.c | 189 ++++++++++++++++++ .../arch/stm32/subsystems/settings_arch.h | 43 ++++ sw/airborne/subsystems/settings.c | 6 +- sw/airborne/subsystems/settings.h | 4 + sw/tools/gen_settings.ml | 56 +++++- 7 files changed, 296 insertions(+), 8 deletions(-) create mode 100644 sw/airborne/arch/stm32/subsystems/settings_arch.c create mode 100644 sw/airborne/arch/stm32/subsystems/settings_arch.h diff --git a/conf/autopilot/lisa_l_test_progs.makefile b/conf/autopilot/lisa_l_test_progs.makefile index 8a89e49b72..679478c7d5 100644 --- a/conf/autopilot/lisa_l_test_progs.makefile +++ b/conf/autopilot/lisa_l_test_progs.makefile @@ -735,7 +735,7 @@ tunnel_hw.srcs += $(SRC_ARCH)/led_hw.c tunnel_hw.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=$(SYS_TIME_LED) tunnel_hw.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)' tunnel_hw.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c -tunnel_hw.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B38400 +tunnel_hw.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600 tunnel_hw.CFLAGS += -DUSE_UART2 -DUART2_BAUD=B57600 tunnel_hw.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c @@ -769,4 +769,4 @@ test_settings.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK test_settings.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c test_settings.CFLAGS += -DDATALINK=PPRZ -DPPRZ_UART=$(MODEM_PORT) test_settings.srcs += subsystems/settings.c - +test_settings.srcs += $(SRC_ARCH)/subsystems/settings_arch.c diff --git a/conf/settings/settings_test.xml b/conf/settings/settings_test.xml index 7dfaf715aa..998d1fa561 100644 --- a/conf/settings/settings_test.xml +++ b/conf/settings/settings_test.xml @@ -7,7 +7,7 @@ - + diff --git a/sw/airborne/arch/stm32/subsystems/settings_arch.c b/sw/airborne/arch/stm32/subsystems/settings_arch.c new file mode 100644 index 0000000000..d25d80c1f6 --- /dev/null +++ b/sw/airborne/arch/stm32/subsystems/settings_arch.c @@ -0,0 +1,189 @@ +#include "subsystems/settings.h" + +#include +#include + +struct FlashInfo { + uint32_t addr; + uint32_t total_size; + uint32_t page_nr; + uint32_t page_size; +}; + + +static uint32_t pflash_checksum(uint32_t ptr, uint32_t size); +static int32_t flash_detect(struct FlashInfo* flash); +static int32_t pflash_program_bytes(struct FlashInfo* flash, + uint32_t src, + uint32_t size, + uint32_t chksum); + +#define FLASH_BEGIN 0x08000000 +#define FLASH_SIZE 0x1FFFF7E0 +#define FSIZ 8 +#define FCHK 4 + + +static uint32_t pflash_checksum(uint32_t ptr, uint32_t size) { + uint32_t i; + + /* reset crc */ + CRC_CR = CRC_CR_RESET; + + if (ptr % 4) { + /* calc in 8bit chunks */ + for (i=0; i<(size & ~3); i+=4) { + CRC_DR = (*(uint8_t*) (ptr+i)) | + (*(uint8_t*) (ptr+i+1)) << 8 | + (*(uint8_t*) (ptr+i+2)) << 16 | + (*(uint8_t*) (ptr+i+3)) << 24; + } + } else { + /* calc in 32bit */ + for (i=0; i<(size & ~3); i+=4) { + CRC_DR = *(uint32_t*) (ptr+i); + } + } + + /* remaining bytes */ + switch (size % 4) { + case 1: + CRC_DR = *(uint8_t*) (ptr+i); + break; + case 2: + CRC_DR = (*(uint8_t*) (ptr+i)) | + (*(uint8_t*) (ptr+i+1)) << 8; + break; + case 3: + CRC_DR = (*(uint8_t*) (ptr+i)) | + (*(uint8_t*) (ptr+i+1)) << 8 | + (*(uint8_t*) (ptr+i+2)) << 16; + break; + default: + break; + } + + return CRC_DR; +} + + + +static int32_t flash_detect(struct FlashInfo* flash) { + + flash->total_size = (MMIO32(FLASH_SIZE) * 0x400)&0x00FFFFFF; + + switch (flash->total_size) { + /* low density */ + case 0x00004000: /* 16 kBytes */ + case 0x00008000: /* 32 kBytes */ + /* medium density, e.g. STM32F103RBT6 (Olimex STM32-H103) */ + case 0x00010000: /* 64 kBytes */ + case 0x00200000: /* 128 kBytes */ + { + flash->page_size = 0x400; + break; + } + /* high density, e.g. STM32F103RE (Joby Lisa/M, Lisa/L) */ + case 0x00040000: /* 256 kBytes */ + case 0x00080000: /* 512 kBytes */ + /* XL density */ + case 0x000C0000: /* 768 kBytes */ + case 0x00100000: /* 1 MByte */ + { + flash->page_size = 0x800; + break; + } + default: {return -1;} + } + + flash->page_nr = (flash->total_size / flash->page_size) - 1; + flash->addr = FLASH_BEGIN + flash->page_nr * flash->page_size; + + return 0; +} + +// (gdb) p *flash +// $1 = {addr = 134739968, total_size = 524288, page_nr = 255, page_size = 2048} +// 0x807F800 0x80000 +static int32_t pflash_program_bytes(struct FlashInfo* flash, + uint32_t src, + uint32_t size, + uint32_t chksum) { + uint32_t i; + + /* erase */ + flash_unlock(); + flash_erase_page(flash->addr); + flash_lock(); + + /* verify erase */ + for (i=0; ipage_size; i+=4) { + if ((*(uint32_t*) (flash->addr + i)) != 0xFFFFFFFF) return -1; + } + + + flash_unlock(); + /* write full 16 bit words */ + for (i=0; i<(size & ~1); i+=2) { + flash_program_half_word(flash->addr+i, + (uint16_t)(*(uint8_t*)(src+i) | (*(uint8_t*)(src+i+1)) << 8)); + } + /* fill bytes with a zero */ + if (size & 1) { + flash_program_half_word(flash->addr+i, (uint16_t)(*(uint8_t*)(src+i))); + } + /* write size */ + flash_program_half_word(flash->addr+flash->page_size-FSIZ, + (uint16_t)(size & 0xFFFF)); + flash_program_half_word(flash->addr+flash->page_size-FSIZ+2, + (uint16_t)((size >> 16) & 0xFFFF)); + /* write checksum */ + flash_program_half_word(flash->addr+flash->page_size-FCHK, + (uint16_t)(chksum & 0xFFFF)); + flash_program_half_word(flash->addr+flash->page_size-FCHK+2, + (uint16_t)((chksum >> 16) & 0xFFFF)); + flash_lock(); + + /* verify data */ + for (i=0; iaddr+i)) != (*(uint8_t*) (src+i))) return -2; + } + if (*(uint32_t*) (flash->addr+flash->page_size-FSIZ) != size) return -3; + if (*(uint32_t*) (flash->addr+flash->page_size-FCHK) != chksum) return -4; + + return 0; +} + + +int32_t persistent_write(uint32_t ptr, uint32_t size) { + struct FlashInfo flash_info; + if (flash_detect(&flash_info)) return -1; + if ((size > flash_info.page_size-FSIZ) || (size == 0)) return -2; + + return pflash_program_bytes(&flash_info, + ptr, + size, + pflash_checksum(ptr, size)); +} + +int32_t persistent_read(uint32_t ptr, uint32_t size) { + struct FlashInfo flash; + uint32_t i; + + /* check parameters */ + if (flash_detect(&flash)) return -1; + if ((size > flash.page_size-FSIZ) || (size == 0)) return -2; + + /* check consistency */ + if (size != *(uint32_t*)(flash.addr+flash.page_size-FSIZ)) return -3; + if (pflash_checksum(flash.addr, size) != + *(uint32_t*)(flash.addr+flash.page_size-FCHK)) + return -4; + + /* copy data */ + for (i=0; i + * + * This file is part of Paparazzi. + * + * Paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * Paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + * + */ + + +/* + + TODO: + - remove last sector from usable flash memory in STM32 linker script + + flash data is located in the last page/sector of flash + + data_begin flash_info.addr + data_size flash_info.addr + flash_info.size - 8 + checksum flash_info.addr + flash_info.size - 4 +*/ + +#ifndef STM32_SUBSYSTEMS_SETTINGS_H +#define STM32_SUBSYSTEMS_SETTINGS_H + + + +#endif /* STM32_SUBSYSTEMS_SETTINGS_H */ diff --git a/sw/airborne/subsystems/settings.c b/sw/airborne/subsystems/settings.c index 452c0edcef..6453235c6e 100644 --- a/sw/airborne/subsystems/settings.c +++ b/sw/airborne/subsystems/settings.c @@ -4,14 +4,16 @@ struct PersistentSettings pers_settings; bool_t settings_store_now; + void settings_init(void) { - // READ SETTINGS FROM FLASH + if (persistent_read((uint32_t)&pers_settings, sizeof(struct PersistentSettings))) + return; // return -1 ? persitent_settings_load(); } void settings_store(void) { persitent_settings_store(); - // WRITE SETTINGS TO FLASH + persistent_write((uint32_t)&pers_settings, sizeof(struct PersistentSettings)); } diff --git a/sw/airborne/subsystems/settings.h b/sw/airborne/subsystems/settings.h index 16008c51fb..cadb3e8ac6 100644 --- a/sw/airborne/subsystems/settings.h +++ b/sw/airborne/subsystems/settings.h @@ -17,5 +17,9 @@ extern bool_t settings_store_now; #include "generated/settings.h" +/* implemented in arch dependant code */ +int32_t persistent_write(uint32_t ptr, uint32_t size); +int32_t persistent_read(uint32_t ptr, uint32_t size); + #endif /* SUBSYSTEMS_SETTINGS_H */ diff --git a/sw/tools/gen_settings.ml b/sw/tools/gen_settings.ml index 7bc25f6424..fdd5363cc8 100644 --- a/sw/tools/gen_settings.ml +++ b/sw/tools/gen_settings.ml @@ -134,11 +134,58 @@ let print_dl_settings = fun settings -> lprintf "default: return 0.;\n"; lprintf "}\n"; left (); - lprintf "}\n" - - + lprintf "}\n"; + left() + +(* + Blaaaaaa +*) +let print_persistent_settings = fun settings -> + let settings = flatten settings [] in + let pers_settings = + List.filter (fun x -> try let _ = Xml.attrib x "persistent" in true with _ -> false) settings in + (* structure declaration *) + if List.length pers_settings > 0 then begin + lprintf "\n/* Persistent Settings */\n"; + lprintf "struct PersistentSettings {\n"; + right(); + let idx = ref 0 in + List.iter + (fun s -> + let v = ExtXml.attrib s "var" in + lprintf "float s_%d; /* %s */\n" !idx v; incr idx) + pers_settings; + left(); + lprintf "};\n\n"; + lprintf "extern struct PersistentSettings pers_settings;\n\n"; + (* Inline function to store persistent settings *) + idx := 0; + lprintf "static inline void persitent_settings_store( void ) {\n"; + right(); + List.iter + (fun s -> + let v = ExtXml.attrib s "var" in + lprintf "pers_settings.s_%d = %s;\n" !idx v; incr idx) + pers_settings; + left(); + lprintf "};\n\n"; + (* Inline function to load persistent settings *) + idx := 0; + lprintf "static inline void persitent_settings_load( void ) {\n"; + right(); + List.iter + (fun s -> + let v = ExtXml.attrib s "var" in + lprintf "%s = pers_settings.s_%d;\n" v !idx; incr idx) + pers_settings; + left(); + lprintf "};\n" + end +(* + Blaaaaaa2 +*) let calib_mode_of_rc = function "gain_1_up" -> 1, "up" | "gain_1_down" -> 1, "down" @@ -233,6 +280,9 @@ let _ = left (); lprintf "}\n"; print_dl_settings dl_settings; + + print_persistent_settings dl_settings; + finish h_name with Xml.Error e -> prerr_endline (Xml.error e); exit 1 From 925e85e91557746acfdf446419353cb512bfa639 Mon Sep 17 00:00:00 2001 From: Antoine Drouin Date: Fri, 25 Mar 2011 11:40:07 +0100 Subject: [PATCH 065/101] enabled permanent setting on rotorcraft firmware.... beware --- conf/Makefile.stm32 | 2 +- conf/autopilot/rotorcraft.makefile | 2 ++ sw/airborne/arch/lpc21/subsystems/settings_arch.c | 10 ++++++++++ sw/airborne/firmwares/rotorcraft/main.c | 5 +++-- sw/airborne/subsystems/settings.c | 2 ++ sw/airborne/test/subsystems/test_settings.c | 12 +++++++++++- sw/tools/gen_settings.ml | 6 +++--- 7 files changed, 32 insertions(+), 7 deletions(-) create mode 100644 sw/airborne/arch/lpc21/subsystems/settings_arch.c diff --git a/conf/Makefile.stm32 b/conf/Makefile.stm32 index fb7fe7ea20..be9a360e33 100644 --- a/conf/Makefile.stm32 +++ b/conf/Makefile.stm32 @@ -138,7 +138,7 @@ else LDFLAGS = -D__thumb2__ -T$(LDSCRIPT) -nostartfiles -L$(GCC_LIB_DIR) -O$(OPT) --gc-sections endif LDFLAGS += -Wl,-Map=$(OBJDIR)/$(TARGET).map,--cref,--gc-sections -LDLIBS += -lc -lm -lgcc -lcmsis -lstm32 +LDLIBS += -lc -lm -lgcc -lcmsis -lstm32 -lopencm3_stm32 CPFLAGS = -j .isr_vector -j .text -j .data CPFLAGS_BIN = -Obinary diff --git a/conf/autopilot/rotorcraft.makefile b/conf/autopilot/rotorcraft.makefile index 69a38e4fa8..67742ac630 100644 --- a/conf/autopilot/rotorcraft.makefile +++ b/conf/autopilot/rotorcraft.makefile @@ -93,6 +93,8 @@ endif # or # include subsystems/rotorcraft/telemetry_xbee_api.makefile # +ap.srcs += subsystems/settings.c +ap.srcs += $(SRC_ARCH)/subsystems/settings_arch.c ap.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c # I2C is needed for speed controllers and barometers on lisa diff --git a/sw/airborne/arch/lpc21/subsystems/settings_arch.c b/sw/airborne/arch/lpc21/subsystems/settings_arch.c new file mode 100644 index 0000000000..517749b900 --- /dev/null +++ b/sw/airborne/arch/lpc21/subsystems/settings_arch.c @@ -0,0 +1,10 @@ +#include "subsystems/settings.h" + + +int32_t persistent_write(uint32_t ptr, uint32_t size) { + return 0; +} + +int32_t persistent_read(uint32_t ptr, uint32_t size) { + return 0; +} diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c index fd6ce9a3ee..1a960eda0c 100644 --- a/sw/airborne/firmwares/rotorcraft/main.c +++ b/sw/airborne/firmwares/rotorcraft/main.c @@ -32,6 +32,7 @@ #include "downlink.h" #include "firmwares/rotorcraft/telemetry.h" #include "datalink.h" +#include "subsystems/settings.h" #include "xbee.h" #include "booz2_commands.h" @@ -46,7 +47,6 @@ #include "subsystems/electrical.h" -// #include "booz_fms.h" // FIXME #include "firmwares/rotorcraft/autopilot.h" #include "firmwares/rotorcraft/stabilization.h" @@ -108,7 +108,6 @@ STATIC_INLINE void main_init( void ) { baro_init(); imu_init(); - // booz_fms_init(); // FIXME autopilot_init(); nav_init(); guidance_h_init(); @@ -126,6 +125,8 @@ STATIC_INLINE void main_init( void ) { modules_init(); + settings_init(); + mcu_int_enable(); } diff --git a/sw/airborne/subsystems/settings.c b/sw/airborne/subsystems/settings.c index 6453235c6e..1332f5831e 100644 --- a/sw/airborne/subsystems/settings.c +++ b/sw/airborne/subsystems/settings.c @@ -6,9 +6,11 @@ bool_t settings_store_now; void settings_init(void) { +#ifdef USE_PERMANENT_SETTINGS if (persistent_read((uint32_t)&pers_settings, sizeof(struct PersistentSettings))) return; // return -1 ? persitent_settings_load(); +#endif } diff --git a/sw/airborne/test/subsystems/test_settings.c b/sw/airborne/test/subsystems/test_settings.c index cf37af0782..1f33f4aad1 100644 --- a/sw/airborne/test/subsystems/test_settings.c +++ b/sw/airborne/test/subsystems/test_settings.c @@ -34,6 +34,7 @@ #include "mcu_periph/uart.h" #include "messages.h" +#include "my_debug_servo.h" static inline void main_init( void ); static inline void main_periodic( void ); @@ -61,13 +62,22 @@ static inline void main_init( void ) { mcu_init(); sys_time_init(); settings_init(); + // DEBUG_SERVO2_INIT(); + // LED_ON(1); + // LED_ON(2); + // DEBUG_S4_ON(); + // DEBUG_S5_ON(); + // DEBUG_S6_ON(); mcu_int_enable(); } static inline void main_periodic( void ) { - RunOnceEvery(100, {DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);}); + RunOnceEvery(100, { + DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM); + PeriodicSendDlValue(DefaultChannel); + }); } diff --git a/sw/tools/gen_settings.ml b/sw/tools/gen_settings.ml index fdd5363cc8..5619e088a1 100644 --- a/sw/tools/gen_settings.ml +++ b/sw/tools/gen_settings.ml @@ -138,14 +138,14 @@ let print_dl_settings = fun settings -> left() (* - Blaaaaaa + Generate code for persitent settings *) let print_persistent_settings = fun settings -> let settings = flatten settings [] in let pers_settings = List.filter (fun x -> try let _ = Xml.attrib x "persistent" in true with _ -> false) settings in (* structure declaration *) - if List.length pers_settings > 0 then begin +(* if List.length pers_settings > 0 then begin *) lprintf "\n/* Persistent Settings */\n"; lprintf "struct PersistentSettings {\n"; right(); @@ -180,7 +180,7 @@ let print_persistent_settings = fun settings -> pers_settings; left(); lprintf "};\n" - end +(* end *) (* From 78c629fcf0817a36b4f2dac4b0dde18f341ce9dd Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 25 Mar 2011 15:14:39 +0100 Subject: [PATCH 066/101] fix line endings, replace CRLF with LF only in rotorcraft_ublox6.txt --- conf/gps/rotorcraft_ublox6.txt | 130 ++++++++++++++++----------------- 1 file changed, 65 insertions(+), 65 deletions(-) diff --git a/conf/gps/rotorcraft_ublox6.txt b/conf/gps/rotorcraft_ublox6.txt index d9963acc2f..ed3c8389d5 100644 --- a/conf/gps/rotorcraft_ublox6.txt +++ b/conf/gps/rotorcraft_ublox6.txt @@ -1,65 +1,65 @@ -MON-VER - 0A 04 46 00 37 2E 30 31 20 28 34 34 31 37 39 29 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 30 30 30 34 30 30 30 37 00 00 36 2E 30 32 20 28 33 36 30 32 33 29 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 -CFG-ANT - 06 13 04 00 1B 00 8B A9 -CFG-DAT - 06 06 02 00 00 00 -CFG-FXN - 06 0E 24 00 0C 00 00 00 00 00 00 00 00 00 00 00 10 27 00 00 10 27 00 00 D0 07 00 00 18 FC FF FF 00 00 00 00 00 00 00 00 -CFG-INF - 06 02 0A 00 00 00 00 00 00 00 00 00 00 00 -CFG-INF - 06 02 0A 00 01 00 00 00 87 87 87 87 87 87 -CFG-INF - 06 02 0A 00 03 00 00 00 00 00 00 00 00 00 -CFG-MSG - 06 01 08 00 01 01 00 00 00 00 00 00 -CFG-MSG - 06 01 08 00 01 02 00 05 00 00 00 00 -CFG-MSG - 06 01 08 00 01 03 00 00 00 00 00 00 -CFG-MSG - 06 01 08 00 01 04 00 00 00 00 00 00 -CFG-MSG - 06 01 08 00 01 06 00 01 00 00 00 00 -CFG-MSG - 06 01 08 00 01 11 00 00 00 00 00 00 -CFG-MSG - 06 01 08 00 01 12 00 00 00 00 00 00 -CFG-MSG - 06 01 08 00 01 20 00 00 00 00 00 00 -CFG-MSG - 06 01 08 00 01 21 00 00 00 00 00 00 -CFG-MSG - 06 01 08 00 01 22 00 00 00 00 00 00 -CFG-MSG - 06 01 08 00 01 30 00 04 00 00 00 00 -CFG-MSG - 06 01 08 00 01 31 00 00 00 00 00 00 -CFG-MSG - 06 01 08 00 01 32 00 00 00 00 00 00 -CFG-MSG - 06 01 08 00 02 20 00 00 00 00 00 00 -CFG-MSG - 06 01 08 00 02 23 00 00 00 00 00 00 -CFG-MSG - 06 01 08 00 0A 02 00 00 00 00 00 00 -CFG-MSG - 06 01 08 00 0A 05 00 00 00 00 00 00 -CFG-MSG - 06 01 08 00 0A 06 00 00 00 00 00 00 -CFG-MSG - 06 01 08 00 0A 07 00 00 00 00 00 00 -CFG-MSG - 06 01 08 00 0A 08 00 00 00 00 00 00 -CFG-MSG - 06 01 08 00 0A 09 00 00 00 00 00 00 -CFG-MSG - 06 01 08 00 0A 0A 00 00 00 00 00 00 -CFG-MSG - 06 01 08 00 0A 20 00 00 00 00 00 00 -CFG-MSG - 06 01 08 00 0A 21 00 00 00 00 00 00 -CFG-MSG - 06 01 08 00 0B 00 00 00 00 00 00 00 -CFG-MSG - 06 01 08 00 0B 30 00 00 00 00 00 00 -CFG-MSG - 06 01 08 00 0B 31 00 00 00 00 00 00 -CFG-MSG - 06 01 08 00 0B 32 00 00 00 00 00 00 -CFG-MSG - 06 01 08 00 0D 01 00 00 00 00 00 00 -CFG-MSG - 06 01 08 00 0D 03 00 00 00 00 00 00 -CFG-MSG - 06 01 08 00 F0 00 01 01 01 01 01 01 -CFG-MSG - 06 01 08 00 F0 01 01 01 01 01 01 01 -CFG-MSG - 06 01 08 00 F0 02 01 01 01 01 01 01 -CFG-MSG - 06 01 08 00 F0 03 01 01 01 01 01 01 -CFG-MSG - 06 01 08 00 F0 04 01 01 01 01 01 01 -CFG-MSG - 06 01 08 00 F0 05 01 01 01 01 01 01 -CFG-MSG - 06 01 08 00 F0 06 00 00 00 00 00 00 -CFG-MSG - 06 01 08 00 F0 07 00 00 00 00 00 00 -CFG-MSG - 06 01 08 00 F0 08 00 00 00 00 00 00 -CFG-MSG - 06 01 08 00 F0 09 00 00 00 00 00 00 -CFG-MSG - 06 01 08 00 F0 0A 00 00 00 00 00 00 -CFG-MSG - 06 01 08 00 F1 00 00 00 00 00 00 00 -CFG-MSG - 06 01 08 00 F1 03 00 00 00 00 00 00 -CFG-MSG - 06 01 08 00 F1 04 00 00 00 00 00 00 -CFG-NAV5 - 06 24 24 00 FF FF 08 02 00 00 00 00 10 27 00 00 05 00 FA 00 FA 00 64 00 2C 01 00 3C 00 00 00 00 00 00 00 00 00 00 00 00 -CFG-NAVX5 - 06 23 28 00 00 00 FF FF 03 00 00 00 03 02 03 10 07 00 00 01 00 00 43 06 00 00 00 00 01 01 00 00 00 64 78 00 00 00 00 00 00 00 00 00 -CFG-NMEA - 06 17 04 00 00 23 00 02 -CFG-PM - 06 32 18 00 00 06 00 00 04 90 00 00 E8 03 00 00 10 27 00 00 00 00 00 00 02 00 00 00 -CFG-PRT - 06 00 14 00 00 00 00 00 84 00 00 00 00 00 00 00 07 00 07 00 00 00 00 00 -CFG-PRT - 06 00 14 00 01 00 00 00 C0 08 00 00 00 E1 00 00 07 00 01 00 00 00 00 00 -CFG-PRT - 06 00 14 00 02 00 00 00 C0 08 00 00 80 25 00 00 00 00 00 00 00 00 00 00 -CFG-PRT - 06 00 14 00 03 00 00 00 00 00 00 00 00 00 00 00 07 00 07 00 00 00 00 00 -CFG-RATE - 06 08 06 00 7D 00 01 00 01 00 -CFG-RXM - 06 11 02 00 08 00 -CFG-SBAS - 06 16 08 00 03 03 03 00 51 08 00 00 -CFG-TP - 06 07 14 00 40 42 0F 00 A0 86 01 00 01 01 00 00 32 00 00 00 00 00 00 00 -CFG-TP5 - 06 31 20 00 00 E9 03 00 32 00 00 00 40 42 0F 00 40 42 0F 00 00 00 00 00 A0 86 01 00 00 00 00 00 F7 00 00 00 -CFG-USB - 06 1B 6C 00 46 15 A6 01 00 00 00 00 64 00 00 01 75 2D 62 6C 6F 78 20 41 47 20 2D 20 77 77 77 2E 75 2D 62 6C 6F 78 2E 63 6F 6D 00 00 00 00 00 00 75 2D 62 6C 6F 78 20 36 20 20 2D 20 20 47 50 53 20 52 65 63 65 69 76 65 72 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 +MON-VER - 0A 04 46 00 37 2E 30 31 20 28 34 34 31 37 39 29 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 30 30 30 34 30 30 30 37 00 00 36 2E 30 32 20 28 33 36 30 32 33 29 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 +CFG-ANT - 06 13 04 00 1B 00 8B A9 +CFG-DAT - 06 06 02 00 00 00 +CFG-FXN - 06 0E 24 00 0C 00 00 00 00 00 00 00 00 00 00 00 10 27 00 00 10 27 00 00 D0 07 00 00 18 FC FF FF 00 00 00 00 00 00 00 00 +CFG-INF - 06 02 0A 00 00 00 00 00 00 00 00 00 00 00 +CFG-INF - 06 02 0A 00 01 00 00 00 87 87 87 87 87 87 +CFG-INF - 06 02 0A 00 03 00 00 00 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 01 01 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 01 02 00 05 00 00 00 00 +CFG-MSG - 06 01 08 00 01 03 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 01 04 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 01 06 00 01 00 00 00 00 +CFG-MSG - 06 01 08 00 01 11 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 01 12 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 01 20 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 01 21 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 01 22 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 01 30 00 04 00 00 00 00 +CFG-MSG - 06 01 08 00 01 31 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 01 32 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 02 20 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 02 23 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 0A 02 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 0A 05 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 0A 06 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 0A 07 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 0A 08 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 0A 09 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 0A 0A 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 0A 20 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 0A 21 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 0B 00 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 0B 30 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 0B 31 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 0B 32 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 0D 01 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 0D 03 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 F0 00 01 01 01 01 01 01 +CFG-MSG - 06 01 08 00 F0 01 01 01 01 01 01 01 +CFG-MSG - 06 01 08 00 F0 02 01 01 01 01 01 01 +CFG-MSG - 06 01 08 00 F0 03 01 01 01 01 01 01 +CFG-MSG - 06 01 08 00 F0 04 01 01 01 01 01 01 +CFG-MSG - 06 01 08 00 F0 05 01 01 01 01 01 01 +CFG-MSG - 06 01 08 00 F0 06 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 F0 07 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 F0 08 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 F0 09 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 F0 0A 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 F1 00 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 F1 03 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 F1 04 00 00 00 00 00 00 +CFG-NAV5 - 06 24 24 00 FF FF 08 02 00 00 00 00 10 27 00 00 05 00 FA 00 FA 00 64 00 2C 01 00 3C 00 00 00 00 00 00 00 00 00 00 00 00 +CFG-NAVX5 - 06 23 28 00 00 00 FF FF 03 00 00 00 03 02 03 10 07 00 00 01 00 00 43 06 00 00 00 00 01 01 00 00 00 64 78 00 00 00 00 00 00 00 00 00 +CFG-NMEA - 06 17 04 00 00 23 00 02 +CFG-PM - 06 32 18 00 00 06 00 00 04 90 00 00 E8 03 00 00 10 27 00 00 00 00 00 00 02 00 00 00 +CFG-PRT - 06 00 14 00 00 00 00 00 84 00 00 00 00 00 00 00 07 00 07 00 00 00 00 00 +CFG-PRT - 06 00 14 00 01 00 00 00 C0 08 00 00 00 E1 00 00 07 00 01 00 00 00 00 00 +CFG-PRT - 06 00 14 00 02 00 00 00 C0 08 00 00 80 25 00 00 00 00 00 00 00 00 00 00 +CFG-PRT - 06 00 14 00 03 00 00 00 00 00 00 00 00 00 00 00 07 00 07 00 00 00 00 00 +CFG-RATE - 06 08 06 00 7D 00 01 00 01 00 +CFG-RXM - 06 11 02 00 08 00 +CFG-SBAS - 06 16 08 00 03 03 03 00 51 08 00 00 +CFG-TP - 06 07 14 00 40 42 0F 00 A0 86 01 00 01 01 00 00 32 00 00 00 00 00 00 00 +CFG-TP5 - 06 31 20 00 00 E9 03 00 32 00 00 00 40 42 0F 00 40 42 0F 00 00 00 00 00 A0 86 01 00 00 00 00 00 F7 00 00 00 +CFG-USB - 06 1B 6C 00 46 15 A6 01 00 00 00 00 64 00 00 01 75 2D 62 6C 6F 78 20 41 47 20 2D 20 77 77 77 2E 75 2D 62 6C 6F 78 2E 63 6F 6D 00 00 00 00 00 00 75 2D 62 6C 6F 78 20 36 20 20 2D 20 20 47 50 53 20 52 65 63 65 69 76 65 72 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 From bec45ddddbefd2ea68c830dc4f753009481b3b27 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 25 Mar 2011 15:16:41 +0100 Subject: [PATCH 067/101] Revert "only link against opencm3_stm32 for lisa/m right now" This reverts commit 7aab04c094d22e4ac3fd109c0d316dbe1f535c32. --- conf/boards/lisa_m_1.0.makefile | 4 ---- 1 file changed, 4 deletions(-) diff --git a/conf/boards/lisa_m_1.0.makefile b/conf/boards/lisa_m_1.0.makefile index 9bb238e36a..0a05903f17 100644 --- a/conf/boards/lisa_m_1.0.makefile +++ b/conf/boards/lisa_m_1.0.makefile @@ -14,10 +14,6 @@ $(TARGET).ARCHDIR = $(ARCH) $(TARGET).OOCD_INTERFACE=flossjtag #$(TARGET).OOCD_INTERFACE=jtagkey-tiny -# ---------------------------------------------------------------------- -# add the opencm3_stm32 lib -LDLIBS += -lopencm3_stm32 - # ----------------------------------------------------------------------- ifndef FLASH_MODE From 817ee78ad0dd43a5f51f31358c877936d2f45f44 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 25 Mar 2011 15:21:16 +0100 Subject: [PATCH 068/101] removed AHRS_FIXED_POINT flag, as it is not used anymore --- .../subsystems/lisa_passthrough/ahrs_cmpl.makefile | 2 +- conf/autopilot/subsystems/rotorcraft/ahrs_cmpl.makefile | 7 +++---- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/conf/autopilot/subsystems/lisa_passthrough/ahrs_cmpl.makefile b/conf/autopilot/subsystems/lisa_passthrough/ahrs_cmpl.makefile index f13e52aeba..423f48203e 100644 --- a/conf/autopilot/subsystems/lisa_passthrough/ahrs_cmpl.makefile +++ b/conf/autopilot/subsystems/lisa_passthrough/ahrs_cmpl.makefile @@ -2,7 +2,7 @@ # Complementary filter for attitude estimation # -ap.CFLAGS += -DUSE_AHRS_CMPL -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED) -DAHRS_FIXED_POINT +ap.CFLAGS += -DUSE_AHRS_CMPL -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED) stm_passthrough.srcs += $(SRC_SUBSYSTEMS)/ahrs.c stm_passthrough.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_aligner.c stm_passthrough.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_int_cmpl_euler.c diff --git a/conf/autopilot/subsystems/rotorcraft/ahrs_cmpl.makefile b/conf/autopilot/subsystems/rotorcraft/ahrs_cmpl.makefile index 9482514737..457f08f575 100644 --- a/conf/autopilot/subsystems/rotorcraft/ahrs_cmpl.makefile +++ b/conf/autopilot/subsystems/rotorcraft/ahrs_cmpl.makefile @@ -3,15 +3,14 @@ # ifdef AHRS_ALIGNER_LED -ap.CFLAGS += -DUSE_AHRS_CMPL -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED) -DAHRS_FIXED_POINT -else -ap.CFLAGS += -DUSE_AHRS_CMPL -DAHRS_FIXED_POINT +ap.CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED) endif +ap.CFLAGS += -DUSE_AHRS_CMPL ap.srcs += $(SRC_SUBSYSTEMS)/ahrs.c ap.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_aligner.c ap.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_int_cmpl_euler.c -sim.CFLAGS += -DUSE_AHRS_CMPL -DAHRS_ALIGNER_LED=3 -DAHRS_FIXED_POINT +sim.CFLAGS += -DUSE_AHRS_CMPL -DAHRS_ALIGNER_LED=3 sim.srcs += $(SRC_SUBSYSTEMS)/ahrs.c sim.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_aligner.c sim.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_int_cmpl_euler.c From 8b160c6efd6d12e4e4878d800f37ba318dbc5502 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 25 Mar 2011 16:46:06 +0100 Subject: [PATCH 069/101] updated landing block for better alignment on a left landing, thx Reto. --- conf/flight_plans/EMAV2008.xml | 6 +++--- conf/flight_plans/IS.xml | 2 +- conf/flight_plans/basic.xml | 2 +- conf/flight_plans/ccc_gl.xml | 2 +- conf/flight_plans/fp_tp_auto.xml | 2 +- conf/flight_plans/grosslobke_demo.xml | 2 +- conf/flight_plans/joystick.xml | 2 +- conf/flight_plans/landing.xml | 2 +- conf/flight_plans/mav07.xml | 2 +- conf/flight_plans/mav08.xml | 2 +- conf/flight_plans/nordlys.xml | 2 +- conf/flight_plans/poles.xml | 2 +- conf/flight_plans/slayer_training.xml | 2 +- conf/flight_plans/snav.xml | 2 +- conf/flight_plans/standard.xml | 2 +- conf/flight_plans/tcas.xml | 2 +- conf/flight_plans/versatile.xml | 2 +- conf/flight_plans/xsens_cachejunction.xml | 2 +- 18 files changed, 20 insertions(+), 20 deletions(-) diff --git a/conf/flight_plans/EMAV2008.xml b/conf/flight_plans/EMAV2008.xml index 61457ec38c..2cadcfc0a2 100644 --- a/conf/flight_plans/EMAV2008.xml +++ b/conf/flight_plans/EMAV2008.xml @@ -84,7 +84,7 @@ - + @@ -130,7 +130,7 @@ - + @@ -149,7 +149,7 @@ - + diff --git a/conf/flight_plans/IS.xml b/conf/flight_plans/IS.xml index 9c264b7f37..dddd2b9cb5 100644 --- a/conf/flight_plans/IS.xml +++ b/conf/flight_plans/IS.xml @@ -148,7 +148,7 @@ diff --git a/conf/flight_plans/basic.xml b/conf/flight_plans/basic.xml index 57f3531d50..f9ead2281c 100644 --- a/conf/flight_plans/basic.xml +++ b/conf/flight_plans/basic.xml @@ -78,7 +78,7 @@ - + diff --git a/conf/flight_plans/ccc_gl.xml b/conf/flight_plans/ccc_gl.xml index 47a16f5a85..1a7656c6f6 100644 --- a/conf/flight_plans/ccc_gl.xml +++ b/conf/flight_plans/ccc_gl.xml @@ -112,7 +112,7 @@ - + diff --git a/conf/flight_plans/fp_tp_auto.xml b/conf/flight_plans/fp_tp_auto.xml index d9f0b4f940..3f61b05c1b 100644 --- a/conf/flight_plans/fp_tp_auto.xml +++ b/conf/flight_plans/fp_tp_auto.xml @@ -111,7 +111,7 @@ - + diff --git a/conf/flight_plans/grosslobke_demo.xml b/conf/flight_plans/grosslobke_demo.xml index 03e7dee1a1..92cecfe5f7 100755 --- a/conf/flight_plans/grosslobke_demo.xml +++ b/conf/flight_plans/grosslobke_demo.xml @@ -77,7 +77,7 @@ - + diff --git a/conf/flight_plans/joystick.xml b/conf/flight_plans/joystick.xml index 55b6b6e0cd..585a5542e4 100644 --- a/conf/flight_plans/joystick.xml +++ b/conf/flight_plans/joystick.xml @@ -80,7 +80,7 @@ - + diff --git a/conf/flight_plans/landing.xml b/conf/flight_plans/landing.xml index bcbc1cf8b0..953b26464b 100644 --- a/conf/flight_plans/landing.xml +++ b/conf/flight_plans/landing.xml @@ -18,7 +18,7 @@ - + diff --git a/conf/flight_plans/mav07.xml b/conf/flight_plans/mav07.xml index fb2a87092a..d4e3429ae0 100644 --- a/conf/flight_plans/mav07.xml +++ b/conf/flight_plans/mav07.xml @@ -130,7 +130,7 @@ - + diff --git a/conf/flight_plans/mav08.xml b/conf/flight_plans/mav08.xml index d418ee1cf7..084723612c 100644 --- a/conf/flight_plans/mav08.xml +++ b/conf/flight_plans/mav08.xml @@ -98,7 +98,7 @@ - + diff --git a/conf/flight_plans/nordlys.xml b/conf/flight_plans/nordlys.xml index 61fa5a0fa6..9e279fe850 100644 --- a/conf/flight_plans/nordlys.xml +++ b/conf/flight_plans/nordlys.xml @@ -85,7 +85,7 @@ - + diff --git a/conf/flight_plans/poles.xml b/conf/flight_plans/poles.xml index fad085ec5e..e1476ff1d8 100644 --- a/conf/flight_plans/poles.xml +++ b/conf/flight_plans/poles.xml @@ -70,7 +70,7 @@ - + diff --git a/conf/flight_plans/slayer_training.xml b/conf/flight_plans/slayer_training.xml index 16773c0183..92d84fe58c 100644 --- a/conf/flight_plans/slayer_training.xml +++ b/conf/flight_plans/slayer_training.xml @@ -75,7 +75,7 @@ - + diff --git a/conf/flight_plans/snav.xml b/conf/flight_plans/snav.xml index 866073c27e..d22bd1c962 100644 --- a/conf/flight_plans/snav.xml +++ b/conf/flight_plans/snav.xml @@ -55,7 +55,7 @@ - + diff --git a/conf/flight_plans/standard.xml b/conf/flight_plans/standard.xml index 906defbb70..ffa5e8dbda 100644 --- a/conf/flight_plans/standard.xml +++ b/conf/flight_plans/standard.xml @@ -68,7 +68,7 @@ - + diff --git a/conf/flight_plans/tcas.xml b/conf/flight_plans/tcas.xml index c0b327457c..9a3c2ce1ba 100644 --- a/conf/flight_plans/tcas.xml +++ b/conf/flight_plans/tcas.xml @@ -72,7 +72,7 @@ - + diff --git a/conf/flight_plans/versatile.xml b/conf/flight_plans/versatile.xml index c5d5e9becc..7544e5dda3 100644 --- a/conf/flight_plans/versatile.xml +++ b/conf/flight_plans/versatile.xml @@ -119,7 +119,7 @@ - + diff --git a/conf/flight_plans/xsens_cachejunction.xml b/conf/flight_plans/xsens_cachejunction.xml index 84aa6cf7bf..2179cf4094 100644 --- a/conf/flight_plans/xsens_cachejunction.xml +++ b/conf/flight_plans/xsens_cachejunction.xml @@ -89,7 +89,7 @@ - + From cfdc39f128c852a387702b1316a2287d7a196d8c Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 25 Mar 2011 17:07:06 +0100 Subject: [PATCH 070/101] fixed spelling of FLOAT_QUAT_NORMALISE to FLOAT_QUAT_NORMALIZE to be consistent --- doc/pprz_algebra/quaternion.tex | 4 ++-- .../stabilization/stabilization_attitude_quat_float.c | 2 +- .../stabilization/stabilization_attitude_ref_quat_float.c | 2 +- sw/airborne/fms/fms_spi_autopilot_msg.c | 2 +- sw/airborne/math/pprz_algebra_float.h | 8 ++++---- sw/airborne/subsystems/ahrs/ahrs_float_cmpl_rmat.c | 2 +- sw/airborne/subsystems/ahrs/ahrs_float_lkf.c | 2 +- sw/airborne/subsystems/imu.c | 2 +- sw/airborne/test/test_algebra.c | 8 ++++---- 9 files changed, 16 insertions(+), 16 deletions(-) diff --git a/doc/pprz_algebra/quaternion.tex b/doc/pprz_algebra/quaternion.tex index 7977222283..3cb937575f 100644 --- a/doc/pprz_algebra/quaternion.tex +++ b/doc/pprz_algebra/quaternion.tex @@ -162,8 +162,8 @@ It is also possible to directly normalise the quaternion \begin{equation} \quat{} := \frac{\quat{}}{\norm{\quat{}}} \end{equation} -\inHfile{INT32\_QUAT\_NORMALISE(q)}{pprz\_algebra\_int} -\inHfile{FLOAT\_QUAT\_NORMALISE(q)}{pprz\_algebra\_float} +\inHfile{INT32\_QUAT\_NORMALIZE(q)}{pprz\_algebra\_int} +\inHfile{FLOAT\_QUAT\_NORMALIZE(q)}{pprz\_algebra\_float} \subsection*{Making the real value positive} It is possible to invert the quaternion if its real value is negative diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c index 2680feef43..36f6dd9df7 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c @@ -200,7 +200,7 @@ void stabilization_attitude_run(bool_t enable_integrator) { scaled_att_err.qy = att_err.qy / IERROR_SCALE; scaled_att_err.qz = att_err.qz / IERROR_SCALE; FLOAT_QUAT_COMP_INV(new_sum_err, stabilization_att_sum_err_quat, scaled_att_err); - FLOAT_QUAT_NORMALISE(new_sum_err); + FLOAT_QUAT_NORMALIZE(new_sum_err); FLOAT_QUAT_COPY(stabilization_att_sum_err_quat, new_sum_err); FLOAT_EULERS_OF_QUAT(stabilization_att_sum_err_eulers, stabilization_att_sum_err_quat); } else { diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c index 7799d9995b..c6478d5b40 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c @@ -117,7 +117,7 @@ void stabilization_attitude_ref_update() { FLOAT_QUAT_DERIVATIVE(qdot, stab_att_ref_rate, stab_att_ref_quat); QUAT_SMUL(qdot, qdot, DT_UPDATE); QUAT_ADD(stab_att_ref_quat, qdot); - FLOAT_QUAT_NORMALISE(stab_att_ref_quat); + FLOAT_QUAT_NORMALIZE(stab_att_ref_quat); /* integrate reference rotational speeds */ struct FloatRates delta_rate; diff --git a/sw/airborne/fms/fms_spi_autopilot_msg.c b/sw/airborne/fms/fms_spi_autopilot_msg.c index 33c0a53ee1..8df37b8ba3 100644 --- a/sw/airborne/fms/fms_spi_autopilot_msg.c +++ b/sw/airborne/fms/fms_spi_autopilot_msg.c @@ -106,7 +106,7 @@ int spi_ap_link_init() FLOAT_QUAT_OF_AXIS_ANGLE(imuFloat.body_to_imu_quat, x_axis, QUAT_SETPOINT_HOVER_PITCH); #endif - FLOAT_QUAT_NORMALISE(imuFloat.body_to_imu_quat); + FLOAT_QUAT_NORMALIZE(imuFloat.body_to_imu_quat); FLOAT_EULERS_OF_QUAT(imuFloat.body_to_imu_eulers, imuFloat.body_to_imu_quat); FLOAT_RMAT_OF_QUAT(imuFloat.body_to_imu_rmat, imuFloat.body_to_imu_quat); diff --git a/sw/airborne/math/pprz_algebra_float.h b/sw/airborne/math/pprz_algebra_float.h index 4e07955bc2..5a31345f6c 100644 --- a/sw/airborne/math/pprz_algebra_float.h +++ b/sw/airborne/math/pprz_algebra_float.h @@ -494,7 +494,7 @@ static inline float float_rmat_reorthogonalize(struct FloatRMat* rm) { #define FLOAT_QUAT_NORM(_q) (sqrtf(SQUARE(_q.qi) + SQUARE(_q.qx)+ \ SQUARE(_q.qy) + SQUARE(_q.qz))) \ -#define FLOAT_QUAT_NORMALISE(q) { \ +#define FLOAT_QUAT_NORMALIZE(q) { \ float norm = FLOAT_QUAT_NORM(q); \ if (norm > FLT_MIN) { \ q.qi = q.qi / norm; \ @@ -515,7 +515,7 @@ static inline float float_rmat_reorthogonalize(struct FloatRMat* rm) { #define FLOAT_QUAT_COMP_NORM_SHORTEST(_a2c, _a2b, _b2c) { \ FLOAT_QUAT_COMP(_a2c, _a2b, _b2c); \ FLOAT_QUAT_WRAP_SHORTEST(_a2c); \ - FLOAT_QUAT_NORMALISE(_a2c); \ + FLOAT_QUAT_NORMALIZE(_a2c); \ } /* _a2c = _a2b comp _b2c , aka _a2c = _a2b * _b2c */ @@ -532,7 +532,7 @@ static inline float float_rmat_reorthogonalize(struct FloatRMat* rm) { #define FLOAT_QUAT_COMP_INV_NORM_SHORTEST(_a2b, _a2c, _b2c) { \ FLOAT_QUAT_COMP_INV(_a2b, _a2c, _b2c); \ FLOAT_QUAT_WRAP_SHORTEST(_a2b); \ - FLOAT_QUAT_NORMALISE(_a2b); \ + FLOAT_QUAT_NORMALIZE(_a2b); \ } /* _a2b = _a2c comp_inv _b2c , aka _a2b = _a2c * inv(_b2c) */ @@ -547,7 +547,7 @@ static inline float float_rmat_reorthogonalize(struct FloatRMat* rm) { #define FLOAT_QUAT_INV_COMP_NORM_SHORTEST(_b2c, _a2b, _a2c) { \ FLOAT_QUAT_INV_COMP(_b2c, _a2b, _a2c); \ FLOAT_QUAT_WRAP_SHORTEST(_b2c); \ - FLOAT_QUAT_NORMALISE(_b2c); \ + FLOAT_QUAT_NORMALIZE(_b2c); \ } /* _b2c = _a2b inv_comp _a2c , aka _b2c = inv(_a2b) * _a2c */ diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_rmat.c b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_rmat.c index 8e91105add..6eed92bc4d 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_rmat.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_rmat.c @@ -119,7 +119,7 @@ void ahrs_propagate(void) { #endif #ifdef AHRS_PROPAGATE_QUAT FLOAT_QUAT_INTEGRATE(ahrs_float.ltp_to_imu_quat, omega, dt); - FLOAT_QUAT_NORMALISE(ahrs_float.ltp_to_imu_quat); + FLOAT_QUAT_NORMALIZE(ahrs_float.ltp_to_imu_quat); compute_imu_rmat_and_euler_from_quat(); #endif compute_body_orientation_and_rates(); diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_lkf.c b/sw/airborne/subsystems/ahrs/ahrs_float_lkf.c index 50e2c6f6df..f1e781a36a 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_lkf.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_lkf.c @@ -315,7 +315,7 @@ void ahrs_propagate(void) { bafl_qnorm = FLOAT_QUAT_NORM(bafl_quat); //TODO check if broot force normalization is good, use lagrange normalization? - FLOAT_QUAT_NORMALISE(bafl_quat); + FLOAT_QUAT_NORMALIZE(bafl_quat); /* diff --git a/sw/airborne/subsystems/imu.c b/sw/airborne/subsystems/imu.c index d5412ff5f8..515adc6b38 100644 --- a/sw/airborne/subsystems/imu.c +++ b/sw/airborne/subsystems/imu.c @@ -64,7 +64,7 @@ void imu_float_init(struct ImuFloat* imuf) { EULERS_ASSIGN(imuf->body_to_imu_eulers, IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI); FLOAT_QUAT_OF_EULERS(imuf->body_to_imu_quat, imuf->body_to_imu_eulers); - FLOAT_QUAT_NORMALISE(imuf->body_to_imu_quat); + FLOAT_QUAT_NORMALIZE(imuf->body_to_imu_quat); FLOAT_RMAT_OF_EULERS(imuf->body_to_imu_rmat, imuf->body_to_imu_eulers); #else EULERS_ASSIGN(imuf->body_to_imu_eulers, 0., 0., 0.); diff --git a/sw/airborne/test/test_algebra.c b/sw/airborne/test/test_algebra.c index b8b7ce4d8b..ee67f3dffe 100644 --- a/sw/airborne/test/test_algebra.c +++ b/sw/airborne/test/test_algebra.c @@ -108,7 +108,7 @@ static void test_2(void) { struct Int32Quat quat_i; INT32_QUAT_OF_EULERS(quat_i, euler_i); DISPLAY_INT32_QUAT("quat_i", quat_i); - INT32_QUAT_NORMALISE(quat_i); + INT32_QUAT_NORMALIZE(quat_i); DISPLAY_INT32_QUAT("quat_i_n", quat_i); struct Int32Vect3 v2; @@ -155,7 +155,7 @@ static void test_3(void) { struct Int32Quat b2i_q; INT32_QUAT_OF_EULERS(b2i_q, b2i_e); DISPLAY_INT32_QUAT_AS_EULERS_DEG("b2i_q", b2i_q); - // INT32_QUAT_NORMALISE(b2i_q); + // INT32_QUAT_NORMALIZE(b2i_q); // DISPLAY_INT32_QUAT_AS_EULERS_DEG("b2i_q_n", b2i_q); /* Compute BODY to IMU rotation matrix */ @@ -232,7 +232,7 @@ static void test_4_int(void) { struct Int32Quat _q; INT32_QUAT_OF_EULERS(_q, _e); DISPLAY_INT32_QUAT_AS_EULERS_DEG("quat1 ", _q); - // INT32_QUAT_NORMALISE(_q); + // INT32_QUAT_NORMALIZE(_q); // DISPLAY_INT32_QUAT_2("_q_n", _q); /* back to eulers */ @@ -257,7 +257,7 @@ static void test_4_float(void) { struct FloatQuat q; FLOAT_QUAT_OF_EULERS(q, e); // DISPLAY_FLOAT_QUAT("q", q); - FLOAT_QUAT_NORMALISE(q); + FLOAT_QUAT_NORMALIZE(q); DISPLAY_FLOAT_QUAT("q_n", q); DISPLAY_FLOAT_QUAT_AS_INT("q_n as int", q); /* back to eulers */ From 5e2ad34b332f14e83322355c9207d7c78a836733 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 25 Mar 2011 17:51:48 +0100 Subject: [PATCH 071/101] fixed telemetry_transparent_usb: moved include of usb_serial.h from datalink.c to datalink.h --- sw/airborne/downlink.h | 3 +++ sw/airborne/firmwares/fixedwing/datalink.c | 4 ---- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/sw/airborne/downlink.h b/sw/airborne/downlink.h index 17052669ed..26dcab2988 100644 --- a/sw/airborne/downlink.h +++ b/sw/airborne/downlink.h @@ -51,6 +51,9 @@ #include "pprz_transport.h" #include "modem.h" #include "xbee.h" +#ifdef USE_USB_SERIAL +#include "mcu_periph/usb_serial.h" +#endif #endif /** !SITL */ #ifndef DefaultChannel diff --git a/sw/airborne/firmwares/fixedwing/datalink.c b/sw/airborne/firmwares/fixedwing/datalink.c index 89e9c735b7..8900d5447c 100644 --- a/sw/airborne/firmwares/fixedwing/datalink.c +++ b/sw/airborne/firmwares/fixedwing/datalink.c @@ -47,10 +47,6 @@ #include "joystick.h" #endif -#ifdef USE_USB_SERIAL -#include "mcu_periph/usb_serial.h" -#endif - #ifdef HITL #include "gps.h" #endif From 7da4b519afaeb0509eea9f3775e27b2ba235ee91 Mon Sep 17 00:00:00 2001 From: Antoine Drouin Date: Fri, 25 Mar 2011 18:16:24 +0100 Subject: [PATCH 072/101] added support for handler in persistent settings - not sure if it's the right semantic - changed enabling flag to USE_PERSISTENT_SETTINGS --- conf/autopilot/lisa_l_test_progs.makefile | 1 + conf/settings/settings_test.xml | 2 +- sw/airborne/subsystems/settings.c | 2 +- sw/airborne/subsystems/settings.h | 7 +------ sw/tools/gen_settings.ml | 13 +++++++++++-- 5 files changed, 15 insertions(+), 10 deletions(-) diff --git a/conf/autopilot/lisa_l_test_progs.makefile b/conf/autopilot/lisa_l_test_progs.makefile index 679478c7d5..95aef22c04 100644 --- a/conf/autopilot/lisa_l_test_progs.makefile +++ b/conf/autopilot/lisa_l_test_progs.makefile @@ -770,3 +770,4 @@ test_settings.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c test_settings.CFLAGS += -DDATALINK=PPRZ -DPPRZ_UART=$(MODEM_PORT) test_settings.srcs += subsystems/settings.c test_settings.srcs += $(SRC_ARCH)/subsystems/settings_arch.c +test_settings.CFLAGS += -DUSE_PERSISTENT_SETTINGS diff --git a/conf/settings/settings_test.xml b/conf/settings/settings_test.xml index 998d1fa561..d23db3776a 100644 --- a/conf/settings/settings_test.xml +++ b/conf/settings/settings_test.xml @@ -7,7 +7,7 @@ - + diff --git a/sw/airborne/subsystems/settings.c b/sw/airborne/subsystems/settings.c index 1332f5831e..04754e2ead 100644 --- a/sw/airborne/subsystems/settings.c +++ b/sw/airborne/subsystems/settings.c @@ -6,7 +6,7 @@ bool_t settings_store_now; void settings_init(void) { -#ifdef USE_PERMANENT_SETTINGS +#ifdef USE_PERSISTENT_SETTINGS if (persistent_read((uint32_t)&pers_settings, sizeof(struct PersistentSettings))) return; // return -1 ? persitent_settings_load(); diff --git a/sw/airborne/subsystems/settings.h b/sw/airborne/subsystems/settings.h index cadb3e8ac6..8c39b680eb 100644 --- a/sw/airborne/subsystems/settings.h +++ b/sw/airborne/subsystems/settings.h @@ -8,12 +8,7 @@ extern void settings_store(void); extern bool_t settings_store_now; -#define settings_StoreSettings(_v) { \ - if (_v) { \ - settings_store(); \ - settings_store_now = FALSE; \ - } \ - } +#define settings_StoreSettings(_v) { settings_store_now = _v; settings_store(); } #include "generated/settings.h" diff --git a/sw/tools/gen_settings.ml b/sw/tools/gen_settings.ml index 5619e088a1..a537587827 100644 --- a/sw/tools/gen_settings.ml +++ b/sw/tools/gen_settings.ml @@ -168,7 +168,7 @@ let print_persistent_settings = fun settings -> lprintf "pers_settings.s_%d = %s;\n" !idx v; incr idx) pers_settings; left(); - lprintf "};\n\n"; + lprintf "}\n\n"; (* Inline function to load persistent settings *) idx := 0; lprintf "static inline void persitent_settings_load( void ) {\n"; @@ -176,7 +176,16 @@ let print_persistent_settings = fun settings -> List.iter (fun s -> let v = ExtXml.attrib s "var" in - lprintf "%s = pers_settings.s_%d;\n" v !idx; incr idx) + begin + try + let h = ExtXml.attrib s "handler" and + m = ExtXml.attrib s "module" in + lprintf "%s_%s( pers_settings.s_%d );\n" (Filename.basename m) h !idx ; +(* lprintf "%s = pers_settings.s_%d;\n" v !idx *) (* do we want to set the value too or just call the handler ? *) + with + ExtXml.Error e -> lprintf "%s = pers_settings.s_%d;\n" v !idx + end; + incr idx) pers_settings; left(); lprintf "};\n" From b8e644c3de3cf04b1834becdaea6ea1ebdd6a5db Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 25 Mar 2011 19:32:05 +0100 Subject: [PATCH 073/101] stop compilation with an error message when trying to use telemetry_transparent_usb on any arch other than lpc21 --- .../subsystems/fixedwing/telemetry_transparent_usb.makefile | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/conf/autopilot/subsystems/fixedwing/telemetry_transparent_usb.makefile b/conf/autopilot/subsystems/fixedwing/telemetry_transparent_usb.makefile index fdc2d31baf..1bf60fe476 100644 --- a/conf/autopilot/subsystems/fixedwing/telemetry_transparent_usb.makefile +++ b/conf/autopilot/subsystems/fixedwing/telemetry_transparent_usb.makefile @@ -1,9 +1,13 @@ #serial USB (e.g. /dev/ttyACM0) +ifeq ($(ARCH), lpc21) ap.CFLAGS += -DDOWNLINK -DDOWNLINK_FBW_DEVICE=UsbS -DDOWNLINK_AP_DEVICE=UsbS -DPPRZ_UART=UsbS ap.CFLAGS += -DDOWNLINK_TRANSPORT=PprzTransport -DDATALINK=PPRZ -DUSE_USB_SERIAL -DUSE_USB_HIGH_PCLK ap.srcs += $(SRC_FIXEDWING)/downlink.c $(SRC_FIRMWARE)/datalink.c $(SRC_FIXEDWING)/pprz_transport.c ap.srcs += $(SRC_ARCH)/usb_ser_hw.c $(SRC_ARCH)/lpcusb/usbhw_lpc.c $(SRC_ARCH)/lpcusb/usbcontrol.c ap.srcs += $(SRC_ARCH)/lpcusb/usbstdreq.c $(SRC_ARCH)/lpcusb/usbinit.c +else +$(error telemetry_transparent_usb currently only implemented for the lpc21) +endif From b0fa9aafc1c0e46de7114e876e6c0e588991e844 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 25 Mar 2011 19:36:41 +0100 Subject: [PATCH 074/101] stop compilation with an error message when trying to use usb_tunnel on any arch other than lpc21 --- conf/autopilot/setup.makefile | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/conf/autopilot/setup.makefile b/conf/autopilot/setup.makefile index b71b547167..bd1757dbb9 100644 --- a/conf/autopilot/setup.makefile +++ b/conf/autopilot/setup.makefile @@ -23,6 +23,7 @@ tunnel.srcs += mcu.c $(SRC_ARCH)/mcu_arch.c # for the usb_tunnel we need to set PCLK higher with the flag USE_USB_HIGH_PCLK # a configuration program to access both uart through usb +ifeq ($(ARCH), lpc21) usb_tunnel_0.CFLAGS += -DUSE_UART0 -DUART0_BAUD=B115200 -DPERIPHERALS_AUTO_INIT usb_tunnel_0.CFLAGS += -DUSE_USB_LINE_CODING -DUSE_USB_SERIAL -DUSE_LED -DUSE_USB_HIGH_PCLK usb_tunnel_0.srcs += $(SRC_ARCH)/usb_tunnel.c $(SRC_ARCH)/usb_ser_hw.c $(SRC_ARCH)/mcu_periph/uart_arch.c @@ -38,6 +39,9 @@ usb_tunnel_1.srcs += $(SRC_ARCH)/lpcusb/usbhw_lpc.c $(SRC_ARCH)/lpcusb/usbinit.c usb_tunnel_1.srcs += $(SRC_ARCH)/lpcusb/usbcontrol.c $(SRC_ARCH)/lpcusb/usbstdreq.c usb_tunnel_1.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c usb_tunnel_1.srcs += mcu.c $(SRC_ARCH)/mcu_arch.c +else +$(error usb_tunnel currently only implemented for the lpc21) +endif From 77d3cd265ef8e22fa8245898a8b389df54eed037 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 25 Mar 2011 22:03:52 +0100 Subject: [PATCH 075/101] added telemetry_transparent_usb for rotorcrafts --- .../rotorcraft/telemetry_transparent_usb.makefile | 14 ++++++++++++++ 1 file changed, 14 insertions(+) create mode 100644 conf/autopilot/subsystems/rotorcraft/telemetry_transparent_usb.makefile diff --git a/conf/autopilot/subsystems/rotorcraft/telemetry_transparent_usb.makefile b/conf/autopilot/subsystems/rotorcraft/telemetry_transparent_usb.makefile new file mode 100644 index 0000000000..ec1277c2c5 --- /dev/null +++ b/conf/autopilot/subsystems/rotorcraft/telemetry_transparent_usb.makefile @@ -0,0 +1,14 @@ + +#serial USB (e.g. /dev/ttyACM0) + +ifeq ($(ARCH), lpc21) +ap.CFLAGS += -DDOWNLINK -DDOWNLINK_DEVICE=UsbS -DPPRZ_UART=UsbS +ap.CFLAGS += -DDOWNLINK_TRANSPORT=PprzTransport -DDATALINK=PPRZ -DUSE_USB_SERIAL -DUSE_USB_HIGH_PCLK +ap.srcs += downlink.c pprz_transport.c +ap.srcs += $(SRC_FIRMWARE)/datalink.c $(SRC_FIRMWARE)/telemetry.c +ap.srcs += $(SRC_ARCH)/usb_ser_hw.c $(SRC_ARCH)/lpcusb/usbhw_lpc.c $(SRC_ARCH)/lpcusb/usbcontrol.c +ap.srcs += $(SRC_ARCH)/lpcusb/usbstdreq.c $(SRC_ARCH)/lpcusb/usbinit.c +else +$(error telemetry_transparent_usb currently only implemented for the lpc21) +endif + From bdad84b55fa7e9be808b991be2b8acd19bb83cad Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sat, 26 Mar 2011 00:31:05 +0100 Subject: [PATCH 076/101] fix booz test_usb target --- conf/autopilot/booz2_test_progs.makefile | 1 + 1 file changed, 1 insertion(+) diff --git a/conf/autopilot/booz2_test_progs.makefile b/conf/autopilot/booz2_test_progs.makefile index ec7cc603ac..83896fb47d 100644 --- a/conf/autopilot/booz2_test_progs.makefile +++ b/conf/autopilot/booz2_test_progs.makefile @@ -189,6 +189,7 @@ test_usb.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))' # -DTIME_LED=1 test_usb.CFLAGS += -DUSE_LED test_usb.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c +test_usb.srcs += mcu.c $(SRC_ARCH)/mcu_arch.c #test_usb.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600 #test_usb.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c From 67ae8c5fd9e00b6358d1a6a500c1cb1b0376b3f0 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sat, 26 Mar 2011 00:38:40 +0100 Subject: [PATCH 077/101] USE_USB_HIGH_PCLK is not needed for booz board/rotorcrafts --- .../subsystems/rotorcraft/telemetry_transparent_usb.makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/conf/autopilot/subsystems/rotorcraft/telemetry_transparent_usb.makefile b/conf/autopilot/subsystems/rotorcraft/telemetry_transparent_usb.makefile index ec1277c2c5..7100d4212e 100644 --- a/conf/autopilot/subsystems/rotorcraft/telemetry_transparent_usb.makefile +++ b/conf/autopilot/subsystems/rotorcraft/telemetry_transparent_usb.makefile @@ -3,7 +3,7 @@ ifeq ($(ARCH), lpc21) ap.CFLAGS += -DDOWNLINK -DDOWNLINK_DEVICE=UsbS -DPPRZ_UART=UsbS -ap.CFLAGS += -DDOWNLINK_TRANSPORT=PprzTransport -DDATALINK=PPRZ -DUSE_USB_SERIAL -DUSE_USB_HIGH_PCLK +ap.CFLAGS += -DDOWNLINK_TRANSPORT=PprzTransport -DDATALINK=PPRZ -DUSE_USB_SERIAL ap.srcs += downlink.c pprz_transport.c ap.srcs += $(SRC_FIRMWARE)/datalink.c $(SRC_FIRMWARE)/telemetry.c ap.srcs += $(SRC_ARCH)/usb_ser_hw.c $(SRC_ARCH)/lpcusb/usbhw_lpc.c $(SRC_ARCH)/lpcusb/usbcontrol.c From 12a389973cd0859f8f2e12bc40afcd836d802da0 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sat, 26 Mar 2011 02:32:36 +0100 Subject: [PATCH 078/101] added include airframe.h to downlink so you can specify TELEMETRY_MODE_x anywhere in the aiframe file --- sw/airborne/downlink.c | 1 + 1 file changed, 1 insertion(+) diff --git a/sw/airborne/downlink.c b/sw/airborne/downlink.c index dbd07749aa..003b816beb 100644 --- a/sw/airborne/downlink.c +++ b/sw/airborne/downlink.c @@ -29,6 +29,7 @@ #include "std.h" +#include "generated/airframe.h" #ifdef FBW #ifndef TELEMETRY_MODE_FBW From 2c590d81761c75832d4bf1fac9a1ae3b5f03fa56 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sat, 26 Mar 2011 02:16:02 +0100 Subject: [PATCH 079/101] added dummy actuators for testing usb/i2c --- conf/airframes/booz2_flixr.xml | 17 +-- .../rotorcraft/actuators_dummy.makefile | 5 + conf/telemetry/telemetry_booz2_flixr.xml | 124 ------------------ .../rotorcraft/actuators/actuators_dummy.c | 35 +++++ 4 files changed, 45 insertions(+), 136 deletions(-) create mode 100644 conf/autopilot/subsystems/rotorcraft/actuators_dummy.makefile delete mode 100644 conf/telemetry/telemetry_booz2_flixr.xml create mode 100644 sw/airborne/firmwares/rotorcraft/actuators/actuators_dummy.c diff --git a/conf/airframes/booz2_flixr.xml b/conf/airframes/booz2_flixr.xml index 38255e1429..f4aabaae9e 100644 --- a/conf/airframes/booz2_flixr.xml +++ b/conf/airframes/booz2_flixr.xml @@ -4,28 +4,21 @@ - - + - - + + - - - - - + + - - - diff --git a/conf/autopilot/subsystems/rotorcraft/actuators_dummy.makefile b/conf/autopilot/subsystems/rotorcraft/actuators_dummy.makefile new file mode 100644 index 0000000000..5c6b93839a --- /dev/null +++ b/conf/autopilot/subsystems/rotorcraft/actuators_dummy.makefile @@ -0,0 +1,5 @@ +# +# empty dummy actuators for testing +# + +ap.srcs += $(SRC_FIRMWARE)/actuators/actuators_dummy.c diff --git a/conf/telemetry/telemetry_booz2_flixr.xml b/conf/telemetry/telemetry_booz2_flixr.xml deleted file mode 100644 index 441e1d228f..0000000000 --- a/conf/telemetry/telemetry_booz2_flixr.xml +++ /dev/null @@ -1,124 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sw/airborne/firmwares/rotorcraft/actuators/actuators_dummy.c b/sw/airborne/firmwares/rotorcraft/actuators/actuators_dummy.c new file mode 100644 index 0000000000..3204727e9f --- /dev/null +++ b/sw/airborne/firmwares/rotorcraft/actuators/actuators_dummy.c @@ -0,0 +1,35 @@ +/* + * $Id$ + * + * Copyright (C) 2008-2009 Antoine Drouin + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +#include "firmwares/rotorcraft/actuators.h" + + + + +void actuators_init(void) { + +} + + +void actuators_set(bool_t motors_on) { +} From 48ab3b383d08c787b332cb6b15568b326b73f75b Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sat, 26 Mar 2011 14:01:45 +0100 Subject: [PATCH 080/101] use VSupplyOfAdc for scaling of supply voltage, defaults to VoltageOfAdc. With this you can set a different scaling for the supply voltage than for the rest of the adcs. --- conf/airframes/booz2_flixr.xml | 6 +++--- sw/airborne/subsystems/electrical.c | 5 ++++- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/conf/airframes/booz2_flixr.xml b/conf/airframes/booz2_flixr.xml index f4aabaae9e..1aafdff6d0 100644 --- a/conf/airframes/booz2_flixr.xml +++ b/conf/airframes/booz2_flixr.xml @@ -205,9 +205,9 @@
- - - + + +
diff --git a/sw/airborne/subsystems/electrical.c b/sw/airborne/subsystems/electrical.c index 125ccf5699..cc1bb55125 100644 --- a/sw/airborne/subsystems/electrical.c +++ b/sw/airborne/subsystems/electrical.c @@ -21,6 +21,9 @@ static struct { #ifndef VoltageOfAdc #define VoltageOfAdc(adc) DefaultVoltageOfAdc(adc) #endif +#ifndef VSupplyOfAdc +#define VSupplyOfAdc(adc) DefaultVoltageOfAdc(adc) +#endif #ifndef MilliAmpereOfAdc #define MilliAmpereOfAdc(adc) DefaultMilliAmpereOfAdc(adc) #endif @@ -45,7 +48,7 @@ void electrical_init(void) { void electrical_periodic(void) { #ifndef SITL - electrical.vsupply = VoltageOfAdc((10*(electrical_priv.vsupply_adc_buf.sum/electrical_priv.vsupply_adc_buf.av_nb_sample))); + electrical.vsupply = VSupplyOfAdc((10*(electrical_priv.vsupply_adc_buf.sum/electrical_priv.vsupply_adc_buf.av_nb_sample))); #endif #ifdef ADC_CHANNEL_CURRENT From 6110311dc9f39ab9078a149fd3404951895b4891 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sat, 26 Mar 2011 14:05:52 +0100 Subject: [PATCH 081/101] default VSupplyOfAdc to VoltageOfAdc instead of DefaultVoltageOfAdc --- sw/airborne/subsystems/electrical.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sw/airborne/subsystems/electrical.c b/sw/airborne/subsystems/electrical.c index cc1bb55125..6e3df7aa40 100644 --- a/sw/airborne/subsystems/electrical.c +++ b/sw/airborne/subsystems/electrical.c @@ -22,7 +22,7 @@ static struct { #define VoltageOfAdc(adc) DefaultVoltageOfAdc(adc) #endif #ifndef VSupplyOfAdc -#define VSupplyOfAdc(adc) DefaultVoltageOfAdc(adc) +#define VSupplyOfAdc(adc) VoltageOfAdc(adc) #endif #ifndef MilliAmpereOfAdc #define MilliAmpereOfAdc(adc) DefaultMilliAmpereOfAdc(adc) From 60d4140366b177dab0b66916e9a802088a55549f Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sat, 26 Mar 2011 14:07:50 +0100 Subject: [PATCH 082/101] removed obsolete battery and lisa_anlog_plug files --- conf/autopilot/rotorcraft.makefile | 1 - sw/airborne/firmwares/rotorcraft/battery.c | 30 --------------- sw/airborne/firmwares/rotorcraft/battery.h | 43 ---------------------- sw/airborne/lisa/lisa_analog_plug.c | 10 ----- 4 files changed, 84 deletions(-) delete mode 100644 sw/airborne/firmwares/rotorcraft/battery.c delete mode 100644 sw/airborne/firmwares/rotorcraft/battery.h delete mode 100644 sw/airborne/lisa/lisa_analog_plug.c diff --git a/conf/autopilot/rotorcraft.makefile b/conf/autopilot/rotorcraft.makefile index 67742ac630..c3506ccafa 100644 --- a/conf/autopilot/rotorcraft.makefile +++ b/conf/autopilot/rotorcraft.makefile @@ -155,7 +155,6 @@ ap.srcs += subsystems/electrical.c ap.CFLAGS += -DUSE_DAC ap.srcs += $(SRC_ARCH)/mcu_periph/dac_arch.c else ifeq ($(ARCH), stm32) -#ap.srcs += lisa/lisa_analog_plug.c ap.CFLAGS += -DUSE_ADC ap.CFLAGS += -DUSE_AD1 -DUSE_AD1_1 -DUSE_AD1_2 -DUSE_AD1_3 -DUSE_AD1_4 ap.CFLAGS += -DUSE_ADC1_2_IRQ_HANDLER diff --git a/sw/airborne/firmwares/rotorcraft/battery.c b/sw/airborne/firmwares/rotorcraft/battery.c deleted file mode 100644 index 357cc448d2..0000000000 --- a/sw/airborne/firmwares/rotorcraft/battery.c +++ /dev/null @@ -1,30 +0,0 @@ -/* - * $Id$ - * - * Copyright (C) 2008-2009 Antoine Drouin - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - */ - -#include "firmwares/rotorcraft/battery.h" - -uint8_t battery_voltage; - -void battery_init(void) { - battery_voltage = 0; -} diff --git a/sw/airborne/firmwares/rotorcraft/battery.h b/sw/airborne/firmwares/rotorcraft/battery.h deleted file mode 100644 index 41aa171692..0000000000 --- a/sw/airborne/firmwares/rotorcraft/battery.h +++ /dev/null @@ -1,43 +0,0 @@ -/* - * $Id$ - * - * Copyright (C) 2008-2009 Antoine Drouin - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - */ - -#ifndef BATTERY_H -#define BATTERY_H - -#include "std.h" - -#include "generated/airframe.h" - -/* decivolts */ -extern uint8_t battery_voltage; - -static inline void BatteryISRHandler(uint16_t _val) { - uint32_t cal_v = (uint32_t)(_val) * BATTERY_SENS_NUM / BATTERY_SENS_DEN; - uint32_t sum = (uint32_t)battery_voltage + cal_v; - battery_voltage = (uint8_t)(sum/2); -} - - -extern void battery_init(void); - -#endif /* BATTERY_H */ diff --git a/sw/airborne/lisa/lisa_analog_plug.c b/sw/airborne/lisa/lisa_analog_plug.c deleted file mode 100644 index 3e49d689d9..0000000000 --- a/sw/airborne/lisa/lisa_analog_plug.c +++ /dev/null @@ -1,10 +0,0 @@ -#include "std.h" - -uint8_t battery_voltage; - -void booz2_analog_init(void); -void battery_init(void); - -void booz2_analog_init(void) {} -void battery_init(void) {} - From b4a886538a2984309fc1f76eceee0ae2d29e483d Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sat, 26 Mar 2011 15:06:28 +0100 Subject: [PATCH 083/101] Revert "default VSupplyOfAdc to VoltageOfAdc instead of DefaultVoltageOfAdc" This reverts commit 6110311dc9f39ab9078a149fd3404951895b4891. --- sw/airborne/subsystems/electrical.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sw/airborne/subsystems/electrical.c b/sw/airborne/subsystems/electrical.c index 6e3df7aa40..cc1bb55125 100644 --- a/sw/airborne/subsystems/electrical.c +++ b/sw/airborne/subsystems/electrical.c @@ -22,7 +22,7 @@ static struct { #define VoltageOfAdc(adc) DefaultVoltageOfAdc(adc) #endif #ifndef VSupplyOfAdc -#define VSupplyOfAdc(adc) VoltageOfAdc(adc) +#define VSupplyOfAdc(adc) DefaultVoltageOfAdc(adc) #endif #ifndef MilliAmpereOfAdc #define MilliAmpereOfAdc(adc) DefaultMilliAmpereOfAdc(adc) From d315265fc9ee0a28e7547a2e4b51216661df9bb1 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sat, 26 Mar 2011 15:06:56 +0100 Subject: [PATCH 084/101] Revert "use VSupplyOfAdc for scaling of supply voltage, defaults to VoltageOfAdc. With this you can set a different scaling for the supply voltage than for the rest of the adcs." This reverts commit 48ab3b383d08c787b332cb6b15568b326b73f75b. --- conf/airframes/booz2_flixr.xml | 6 +++--- sw/airborne/subsystems/electrical.c | 5 +---- 2 files changed, 4 insertions(+), 7 deletions(-) diff --git a/conf/airframes/booz2_flixr.xml b/conf/airframes/booz2_flixr.xml index 1aafdff6d0..f4aabaae9e 100644 --- a/conf/airframes/booz2_flixr.xml +++ b/conf/airframes/booz2_flixr.xml @@ -205,9 +205,9 @@
- - - + + +
diff --git a/sw/airborne/subsystems/electrical.c b/sw/airborne/subsystems/electrical.c index cc1bb55125..125ccf5699 100644 --- a/sw/airborne/subsystems/electrical.c +++ b/sw/airborne/subsystems/electrical.c @@ -21,9 +21,6 @@ static struct { #ifndef VoltageOfAdc #define VoltageOfAdc(adc) DefaultVoltageOfAdc(adc) #endif -#ifndef VSupplyOfAdc -#define VSupplyOfAdc(adc) DefaultVoltageOfAdc(adc) -#endif #ifndef MilliAmpereOfAdc #define MilliAmpereOfAdc(adc) DefaultMilliAmpereOfAdc(adc) #endif @@ -48,7 +45,7 @@ void electrical_init(void) { void electrical_periodic(void) { #ifndef SITL - electrical.vsupply = VSupplyOfAdc((10*(electrical_priv.vsupply_adc_buf.sum/electrical_priv.vsupply_adc_buf.av_nb_sample))); + electrical.vsupply = VoltageOfAdc((10*(electrical_priv.vsupply_adc_buf.sum/electrical_priv.vsupply_adc_buf.av_nb_sample))); #endif #ifdef ADC_CHANNEL_CURRENT From e0ecaa6055e8090f64a1ab0bc6f3bfa92bf3807a Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sat, 26 Mar 2011 15:08:27 +0100 Subject: [PATCH 085/101] define VoltageOfAdc in my airframe for batter voltage --- conf/airframes/booz2_flixr.xml | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/conf/airframes/booz2_flixr.xml b/conf/airframes/booz2_flixr.xml index f4aabaae9e..42d128ef2f 100644 --- a/conf/airframes/booz2_flixr.xml +++ b/conf/airframes/booz2_flixr.xml @@ -206,8 +206,7 @@ - - +
From 6fa80f971e0157bbe1fb6aba62d80e64f41a60f2 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sat, 26 Mar 2011 23:19:36 +0100 Subject: [PATCH 086/101] added info about the VIC slots used on rotorcrafts --- .../arch/lpc21/vic_slots_rotorcrafts.txt | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) create mode 100644 sw/airborne/arch/lpc21/vic_slots_rotorcrafts.txt diff --git a/sw/airborne/arch/lpc21/vic_slots_rotorcrafts.txt b/sw/airborne/arch/lpc21/vic_slots_rotorcrafts.txt new file mode 100644 index 0000000000..637d324ff2 --- /dev/null +++ b/sw/airborne/arch/lpc21/vic_slots_rotorcrafts.txt @@ -0,0 +1,18 @@ +VIC slots used for rotorcrafts with the LPC2148 + +define name slot used for +------------------------------------------------------------------ +TIMER0_VIC_SLOT 1 system timer +AD0_VIC_SLOT 2 was for adc battery (not needed anymore?) +AD1_VIC_SLOT 4 was for adc baro (not needed anymore?) +UART0_VIC_SLOT 5 uart_arch, e.g. gps +UART1_VIC_SLOT 6 uart_arch, e.g. modem +I2C0_VIC_SLOT 10 actuators_acstec, actuators_acstec_v2, actuators_mkk +I2C1_VIC_SLOT 11 ami601 in imu_b2_v1.0, imu_crista +MAX1168_EOC_VIC_SLOT 8 imu_b2_v1.0, imu_b2_v1.1, imu_b2_v1.2 +SSP_VIC_SLOT 9 imu_b2_v1.0, imu_b2_v1.1, imu_b2_v1.2 +MS2100_DRDY_VIC_SLOT 11 ms2100 mag in imu_b2_v1.1 + +hardcoded, no define 7 SPI1 in mcu_periph/spi_arch.c, imu_crista_arch +hardcoded, no define 10 usb, e.g. telemetry_transparent_usb + From a699e847728f54c88539e949a4939a51d856b19b Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sat, 26 Mar 2011 23:38:00 +0100 Subject: [PATCH 087/101] changed some VIC slots for rotorcrafts to free slot 10 used by USB (changed I2C used for mags and actuators) --- conf/airframes/booz2_flixr.xml | 4 ++-- .../autopilot/subsystems/rotorcraft/actuators_asctec.makefile | 2 +- .../subsystems/rotorcraft/actuators_asctec_v2.makefile | 2 +- conf/autopilot/subsystems/rotorcraft/actuators_mkk.makefile | 2 +- conf/autopilot/subsystems/rotorcraft/imu_b2_v1.0.makefile | 2 +- conf/autopilot/subsystems/rotorcraft/imu_b2_v1.1.makefile | 2 +- conf/autopilot/subsystems/rotorcraft/imu_b2_v1.2.makefile | 2 +- conf/autopilot/subsystems/rotorcraft/imu_crista.makefile | 2 +- 8 files changed, 9 insertions(+), 9 deletions(-) diff --git a/conf/airframes/booz2_flixr.xml b/conf/airframes/booz2_flixr.xml index 42d128ef2f..a7336cffbe 100644 --- a/conf/airframes/booz2_flixr.xml +++ b/conf/airframes/booz2_flixr.xml @@ -16,7 +16,7 @@ - + @@ -206,7 +206,7 @@ - +
diff --git a/conf/autopilot/subsystems/rotorcraft/actuators_asctec.makefile b/conf/autopilot/subsystems/rotorcraft/actuators_asctec.makefile index be29427092..e31987e894 100644 --- a/conf/autopilot/subsystems/rotorcraft/actuators_asctec.makefile +++ b/conf/autopilot/subsystems/rotorcraft/actuators_asctec.makefile @@ -14,7 +14,7 @@ endif # Simulator sim.srcs += $(SRC_FIRMWARE)/actuators/actuators_asctec.c -sim.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=150 -DI2C0_SCLH=150 -DI2C0_VIC_SLOT=10 +sim.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=150 -DI2C0_SCLH=150 -DI2C0_VIC_SLOT=11 sim.srcs += mcu_periph/i2c.c sim.srcs += $(SRC_ARCH)/mcu_periph/i2c_arch.c diff --git a/conf/autopilot/subsystems/rotorcraft/actuators_asctec_v2.makefile b/conf/autopilot/subsystems/rotorcraft/actuators_asctec_v2.makefile index 6b9d300c2e..30451deebb 100644 --- a/conf/autopilot/subsystems/rotorcraft/actuators_asctec_v2.makefile +++ b/conf/autopilot/subsystems/rotorcraft/actuators_asctec_v2.makefile @@ -22,7 +22,7 @@ ap.srcs += $(SRC_FIRMWARE)/actuators/actuators_asctec.c ifeq ($(ARCH), lpc21) ap.CFLAGS += -DACTUATORS_ASCTEC_DEVICE=i2c0 -ap.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=150 -DI2C0_SCLH=150 -DI2C0_VIC_SLOT=10 +ap.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=150 -DI2C0_SCLH=150 -DI2C0_VIC_SLOT=11 endif ifeq ($(ARCH), stm32) diff --git a/conf/autopilot/subsystems/rotorcraft/actuators_mkk.makefile b/conf/autopilot/subsystems/rotorcraft/actuators_mkk.makefile index 3529c03923..a55d0e4313 100644 --- a/conf/autopilot/subsystems/rotorcraft/actuators_mkk.makefile +++ b/conf/autopilot/subsystems/rotorcraft/actuators_mkk.makefile @@ -41,7 +41,7 @@ ap.srcs += $(SRC_FIRMWARE)/actuators/actuators_mkk.c ifeq ($(ARCH), lpc21) ap.CFLAGS += -DACTUATORS_MKK_DEVICE=i2c0 -ap.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=$(MKK_I2C_SCL_TIME) -DI2C0_SCLH=$(MKK_I2C_SCL_TIME) -DI2C0_VIC_SLOT=10 +ap.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=$(MKK_I2C_SCL_TIME) -DI2C0_SCLH=$(MKK_I2C_SCL_TIME) -DI2C0_VIC_SLOT=11 else ifeq ($(ARCH), stm32) ap.CFLAGS += -DACTUATORS_MKK_DEVICE=i2c1 ap.CFLAGS += -DUSE_I2C1 diff --git a/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.0.makefile b/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.0.makefile index f3074ca2bb..0e435f803d 100644 --- a/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.0.makefile +++ b/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.0.makefile @@ -50,7 +50,7 @@ imu_CFLAGS += -DMAX1168_EOC_VIC_SLOT=8 imu_CFLAGS += -DUSE_AMI601 imu_srcs += peripherals/ami601.c -imu_CFLAGS += -DUSE_I2C1 -DI2C1_SCLL=150 -DI2C1_SCLH=150 -DI2C1_VIC_SLOT=11 +imu_CFLAGS += -DUSE_I2C1 -DI2C1_SCLL=150 -DI2C1_SCLH=150 -DI2C1_VIC_SLOT=12 # Keep CFLAGS/Srcs for imu in separate expression so we can assign it to other targets diff --git a/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.1.makefile b/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.1.makefile index 18db231194..afea1237dd 100644 --- a/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.1.makefile +++ b/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.1.makefile @@ -57,7 +57,7 @@ imu_srcs += $(SRC_ARCH)/peripherals/ms2100_arch.c ifeq ($(ARCH), lpc21) imu_CFLAGS += -DSSP_VIC_SLOT=9 imu_CFLAGS += -DMAX1168_EOC_VIC_SLOT=8 -imu_CFLAGS += -DMS2100_DRDY_VIC_SLOT=11 +imu_CFLAGS += -DMS2100_DRDY_VIC_SLOT=12 else ifeq ($(ARCH), stm32) imu_CFLAGS += -DUSE_SPI2 -DUSE_DMA1_C4_IRQ -DUSE_EXTI2_IRQ -DUSE_SPI2_IRQ imu_CFLAGS += -DMAX_1168_DRDY_PORT=$(MAX_1168_DRDY_PORT) diff --git a/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.2.makefile b/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.2.makefile index e208c41664..91ba3d16d4 100644 --- a/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.2.makefile +++ b/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.2.makefile @@ -58,7 +58,7 @@ ifeq ($(ARCH), lpc21) imu_CFLAGS += -DSSP_VIC_SLOT=9 imu_CFLAGS += -DMAX1168_EOC_VIC_SLOT=8 #FIXME ms2100 not used on this imu -imu_CFLAGS += -DMS2100_DRDY_VIC_SLOT=11 +imu_CFLAGS += -DMS2100_DRDY_VIC_SLOT=12 else ifeq ($(ARCH), stm32) imu_CFLAGS += -DUSE_SPI2 -DUSE_DMA1_C4_IRQ -DUSE_EXTI2_IRQ -DUSE_SPI2_IRQ imu_CFLAGS += -DMAX_1168_DRDY_PORT=$(MAX_1168_DRDY_PORT) diff --git a/conf/autopilot/subsystems/rotorcraft/imu_crista.makefile b/conf/autopilot/subsystems/rotorcraft/imu_crista.makefile index 449da2f909..aa1dfb9cc9 100644 --- a/conf/autopilot/subsystems/rotorcraft/imu_crista.makefile +++ b/conf/autopilot/subsystems/rotorcraft/imu_crista.makefile @@ -70,7 +70,7 @@ imu_srcs += peripherals/ami601.c imu_CFLAGS += -DUSE_I2C1 ifeq ($(ARCH), lpc21) -imu_CFLAGS += -DI2C1_SCLL=150 -DI2C1_SCLH=150 -DI2C1_VIC_SLOT=11 -DI2C1_BUF_LEN=16 +imu_CFLAGS += -DI2C1_SCLL=150 -DI2C1_SCLH=150 -DI2C1_VIC_SLOT=12 -DI2C1_BUF_LEN=16 else ifeq ($(ARCH), stm32) #FIXME endif From 395ad19aee6753b62628c4090e46d39d1f0aa412 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sat, 26 Mar 2011 23:46:14 +0100 Subject: [PATCH 088/101] updated info about VIC slots on rotorcraft after changing some --- sw/airborne/arch/lpc21/vic_slots_rotorcrafts.txt | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/sw/airborne/arch/lpc21/vic_slots_rotorcrafts.txt b/sw/airborne/arch/lpc21/vic_slots_rotorcrafts.txt index 637d324ff2..ccd16bc361 100644 --- a/sw/airborne/arch/lpc21/vic_slots_rotorcrafts.txt +++ b/sw/airborne/arch/lpc21/vic_slots_rotorcrafts.txt @@ -7,12 +7,11 @@ AD0_VIC_SLOT 2 was for adc battery (not needed anymore?) AD1_VIC_SLOT 4 was for adc baro (not needed anymore?) UART0_VIC_SLOT 5 uart_arch, e.g. gps UART1_VIC_SLOT 6 uart_arch, e.g. modem -I2C0_VIC_SLOT 10 actuators_acstec, actuators_acstec_v2, actuators_mkk -I2C1_VIC_SLOT 11 ami601 in imu_b2_v1.0, imu_crista MAX1168_EOC_VIC_SLOT 8 imu_b2_v1.0, imu_b2_v1.1, imu_b2_v1.2 -SSP_VIC_SLOT 9 imu_b2_v1.0, imu_b2_v1.1, imu_b2_v1.2 -MS2100_DRDY_VIC_SLOT 11 ms2100 mag in imu_b2_v1.1 - hardcoded, no define 7 SPI1 in mcu_periph/spi_arch.c, imu_crista_arch +SSP_VIC_SLOT 9 imu_b2_v1.0, imu_b2_v1.1, imu_b2_v1.2 hardcoded, no define 10 usb, e.g. telemetry_transparent_usb +I2C0_VIC_SLOT 11 actuators_acstec, actuators_acstec_v2, actuators_mkk +I2C1_VIC_SLOT 12 ami601 in imu_b2_v1.0, imu_crista +MS2100_DRDY_VIC_SLOT 12 ms2100 mag in imu_b2_v1.1 From 62a64efd66f6ca6304ecfa5280a3fbce94c2a10b Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sun, 27 Mar 2011 17:43:20 +0200 Subject: [PATCH 089/101] updated info on vic slots --- sw/airborne/arch/lpc21/vic_slots.txt | 15 ----------- sw/airborne/arch/lpc21/vic_slots_fw.txt | 22 +++++++++++++++ .../arch/lpc21/vic_slots_rotorcrafts.txt | 27 ++++++++++--------- 3 files changed, 36 insertions(+), 28 deletions(-) delete mode 100644 sw/airborne/arch/lpc21/vic_slots.txt create mode 100644 sw/airborne/arch/lpc21/vic_slots_fw.txt diff --git a/sw/airborne/arch/lpc21/vic_slots.txt b/sw/airborne/arch/lpc21/vic_slots.txt deleted file mode 100644 index c75e66ef1e..0000000000 --- a/sw/airborne/arch/lpc21/vic_slots.txt +++ /dev/null @@ -1,15 +0,0 @@ - 1 TIMER0_VIC_SLOT - 2 AD0_VIC_SLOT - 4 AD1_VIC_SLOT - 5 UART0_VIC_SLOT - 6 UART1_VIC_SLOT - 9 I2C0_VIC_SLOT - I2C1_VIC_SLOT -11 SSP_VIC_SLOT -11 MS2100_DRDY_VIC_SLOT - -12 MICROMAG_DRDY_VIC_SLOT -12 MAX11040_DRDY_VIC_SLOT - -?? MAX1167_EOC_VIC_SLOT - diff --git a/sw/airborne/arch/lpc21/vic_slots_fw.txt b/sw/airborne/arch/lpc21/vic_slots_fw.txt new file mode 100644 index 0000000000..6c94aa7bfe --- /dev/null +++ b/sw/airborne/arch/lpc21/vic_slots_fw.txt @@ -0,0 +1,22 @@ +VIC slots used for fixedwings with the LPC2148 + + +define name slot (default) used for +------------------------------------------------------------------ +TIMER0_VIC_SLOT 1 (1) system timer +AD0_VIC_SLOT 2 (2) was for adc battery (not needed anymore?) +hardcoded, no define 3 PWM_ISR in servos_4015 +AD1_VIC_SLOT 4 (4) was for adc baro (not needed anymore?) +UART0_VIC_SLOT 5 (5) uart_arch, e.g. gps +UART1_VIC_SLOT 6 (6) uart_arch, e.g. modem +hardcoded, no define 7 SPI1 in mcu_periph/spi_arch.c, imu_crista_arch, max3100 module, baro_scp module, lcd_dogm module +MAX1168_EOC_VIC_SLOT 8 imu_b2_v1.0, imu_b2_v1.1, imu_b2_v1.2 +hardcoded, no define 8 EXTINT in max3100 module +I2C0_VIC_SLOT ? (9) (9 is default in mcu_periph/i2c_arch.c) +SSP_VIC_SLOT 9 imu_b2_v1.0, imu_b2_v1.1, imu_b2_v1.2 +MICROMAG_DRDY_VIC_SLOT 9 micromag +hardcoded, no define 10 usb, e.g. telemetry_transparent_usb +hardcoded, no define 11 EXTINT in baro_scp module +I2C1_VIC_SLOT 12 ami601 in imu_b2_v1.0, imu_crista +MS2100_DRDY_VIC_SLOT 12 ms2100 mag in imu_b2_v1.1 +MAX11040_DRDY_VIC_SLOT ? max11040 adc module diff --git a/sw/airborne/arch/lpc21/vic_slots_rotorcrafts.txt b/sw/airborne/arch/lpc21/vic_slots_rotorcrafts.txt index ccd16bc361..0acf1429f4 100644 --- a/sw/airborne/arch/lpc21/vic_slots_rotorcrafts.txt +++ b/sw/airborne/arch/lpc21/vic_slots_rotorcrafts.txt @@ -1,17 +1,18 @@ VIC slots used for rotorcrafts with the LPC2148 -define name slot used for +define name slot (default) used for ------------------------------------------------------------------ -TIMER0_VIC_SLOT 1 system timer -AD0_VIC_SLOT 2 was for adc battery (not needed anymore?) -AD1_VIC_SLOT 4 was for adc baro (not needed anymore?) -UART0_VIC_SLOT 5 uart_arch, e.g. gps -UART1_VIC_SLOT 6 uart_arch, e.g. modem -MAX1168_EOC_VIC_SLOT 8 imu_b2_v1.0, imu_b2_v1.1, imu_b2_v1.2 -hardcoded, no define 7 SPI1 in mcu_periph/spi_arch.c, imu_crista_arch -SSP_VIC_SLOT 9 imu_b2_v1.0, imu_b2_v1.1, imu_b2_v1.2 -hardcoded, no define 10 usb, e.g. telemetry_transparent_usb -I2C0_VIC_SLOT 11 actuators_acstec, actuators_acstec_v2, actuators_mkk -I2C1_VIC_SLOT 12 ami601 in imu_b2_v1.0, imu_crista -MS2100_DRDY_VIC_SLOT 12 ms2100 mag in imu_b2_v1.1 +TIMER0_VIC_SLOT 1 (1) system timer +AD0_VIC_SLOT 2 (2) was for adc battery (not needed anymore?) + +AD1_VIC_SLOT 4 (4) was for adc baro (not needed anymore?) +UART0_VIC_SLOT 5 (5) uart_arch, e.g. gps +UART1_VIC_SLOT 6 (6) uart_arch, e.g. modem +hardcoded, no define 7 SPI1 in mcu_periph/spi_arch.c, imu_crista_arch +MAX1168_EOC_VIC_SLOT 8 imu_b2_v1.0, imu_b2_v1.1, imu_b2_v1.2 +SSP_VIC_SLOT 9 imu_b2_v1.0, imu_b2_v1.1, imu_b2_v1.2 +hardcoded, no define 10 usb, e.g. telemetry_transparent_usb +I2C0_VIC_SLOT 11 (9) actuators_acstec, actuators_acstec_v2, actuators_mkk, (9 is default in mcu_periph/i2c_arch.c) +I2C1_VIC_SLOT 12 ami601 in imu_b2_v1.0, imu_crista +MS2100_DRDY_VIC_SLOT 12 ms2100 mag in imu_b2_v1.1 From dbe81a167b89ea373c537883b51a70e4c5aa94de Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sun, 27 Mar 2011 18:52:02 +0200 Subject: [PATCH 090/101] update airframe file --- conf/airframes/booz2_flixr.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/conf/airframes/booz2_flixr.xml b/conf/airframes/booz2_flixr.xml index a7336cffbe..b132b6649a 100644 --- a/conf/airframes/booz2_flixr.xml +++ b/conf/airframes/booz2_flixr.xml @@ -13,7 +13,7 @@ - + From 565406f0d484e143ecadee51fa1ceae7bcf4025f Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sun, 27 Mar 2011 19:00:02 +0200 Subject: [PATCH 091/101] removed manual assignment of uart VIC slots for lpc based rotorcrafts, default assignments in uart arch used --- conf/autopilot/subsystems/rotorcraft/gps_skytraq.makefile | 5 ----- conf/autopilot/subsystems/rotorcraft/gps_ublox.makefile | 4 ---- .../subsystems/rotorcraft/telemetry_transparent.makefile | 4 ---- .../subsystems/rotorcraft/telemetry_xbee_api.makefile | 4 ---- 4 files changed, 17 deletions(-) diff --git a/conf/autopilot/subsystems/rotorcraft/gps_skytraq.makefile b/conf/autopilot/subsystems/rotorcraft/gps_skytraq.makefile index e3f8faf488..9a06840e50 100644 --- a/conf/autopilot/subsystems/rotorcraft/gps_skytraq.makefile +++ b/conf/autopilot/subsystems/rotorcraft/gps_skytraq.makefile @@ -10,10 +10,5 @@ ifneq ($(GPS_LED),none) ap.CFLAGS += -DGPS_LED=$(GPS_LED) endif -ifeq ($(ARCH), lpc21) -ap.CFLAGS += -D$(GPS_PORT)_VIC_SLOT=5 -endif - - sim.CFLAGS += -DUSE_GPS sim.srcs += $(SRC_BOOZ)/booz_gps.c diff --git a/conf/autopilot/subsystems/rotorcraft/gps_ublox.makefile b/conf/autopilot/subsystems/rotorcraft/gps_ublox.makefile index 1e476c2c29..784099f03a 100644 --- a/conf/autopilot/subsystems/rotorcraft/gps_ublox.makefile +++ b/conf/autopilot/subsystems/rotorcraft/gps_ublox.makefile @@ -10,9 +10,5 @@ ifneq ($(GPS_LED),none) ap.CFLAGS += -DGPS_LED=$(GPS_LED) endif -ifeq ($(ARCH), lpc21) -ap.CFLAGS += -D$(GPS_PORT)_VIC_SLOT=5 -endif - sim.CFLAGS += -DUSE_GPS sim.srcs += $(SRC_BOOZ)/booz_gps.c diff --git a/conf/autopilot/subsystems/rotorcraft/telemetry_transparent.makefile b/conf/autopilot/subsystems/rotorcraft/telemetry_transparent.makefile index fbc1b89c2e..920b031d66 100644 --- a/conf/autopilot/subsystems/rotorcraft/telemetry_transparent.makefile +++ b/conf/autopilot/subsystems/rotorcraft/telemetry_transparent.makefile @@ -12,7 +12,3 @@ ap.CFLAGS += -DDOWNLINK -DDOWNLINK_DEVICE=$(MODEM_PORT) -DPPRZ_UART=$(MODEM_PORT ap.CFLAGS += -DDOWNLINK_TRANSPORT=PprzTransport -DDATALINK=PPRZ ap.srcs += downlink.c pprz_transport.c ap.srcs += $(SRC_FIRMWARE)/datalink.c $(SRC_FIRMWARE)/telemetry.c - -ifeq ($(ARCH), lpc21) -ap.CFLAGS += -D$(MODEM_PORT)_VIC_SLOT=6 -endif diff --git a/conf/autopilot/subsystems/rotorcraft/telemetry_xbee_api.makefile b/conf/autopilot/subsystems/rotorcraft/telemetry_xbee_api.makefile index a10f3fa95e..d083461928 100644 --- a/conf/autopilot/subsystems/rotorcraft/telemetry_xbee_api.makefile +++ b/conf/autopilot/subsystems/rotorcraft/telemetry_xbee_api.makefile @@ -13,7 +13,3 @@ ap.CFLAGS += -DDOWNLINK -DDOWNLINK_DEVICE=$(MODEM_PORT) -DXBEE_UART=$(MODEM_PORT ap.CFLAGS += -DDOWNLINK_TRANSPORT=XBeeTransport -DDATALINK=XBEE ap.srcs += downlink.c xbee.c ap.srcs += $(SRC_FIRMWARE)/datalink.c $(SRC_FIRMWARE)/telemetry.c - -ifeq ($(ARCH), lpc21) -ap.CFLAGS += -D$(MODEM_PORT)_VIC_SLOT=6 -endif From 5c68dfda0a0205ec7fbb523a97ab8894294f3319 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sun, 27 Mar 2011 23:36:10 +0200 Subject: [PATCH 092/101] add settings_arch for sim, and add it to rotorcraft sim --- conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile | 2 ++ sw/airborne/arch/sim/subsystems/settings_arch.c | 10 ++++++++++ 2 files changed, 12 insertions(+) create mode 100644 sw/airborne/arch/sim/subsystems/settings_arch.c diff --git a/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile b/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile index fbcf8060ec..592c196ea3 100644 --- a/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile +++ b/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile @@ -74,6 +74,8 @@ sim.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))' #sim.CFLAGS += -DUSE_LED sim.srcs += sys_time.c +sim.srcs += subsystems/settings.c +sim.srcs += $(SRC_ARCH)/subsystems/settings_arch.c sim.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=IvyTransport sim.srcs += $(SRC_FIRMWARE)/telemetry.c \ diff --git a/sw/airborne/arch/sim/subsystems/settings_arch.c b/sw/airborne/arch/sim/subsystems/settings_arch.c new file mode 100644 index 0000000000..517749b900 --- /dev/null +++ b/sw/airborne/arch/sim/subsystems/settings_arch.c @@ -0,0 +1,10 @@ +#include "subsystems/settings.h" + + +int32_t persistent_write(uint32_t ptr, uint32_t size) { + return 0; +} + +int32_t persistent_read(uint32_t ptr, uint32_t size) { + return 0; +} From ab897b7f55bb00a6f858140cb151a000122f6173 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Mon, 28 Mar 2011 14:10:50 +0200 Subject: [PATCH 093/101] added usb tunnels to booz2_test_progs --- conf/airframes/booz2_flixr.xml | 2 ++ conf/autopilot/booz2_test_progs.makefile | 23 +++++++++++++++++++++++ 2 files changed, 25 insertions(+) diff --git a/conf/airframes/booz2_flixr.xml b/conf/airframes/booz2_flixr.xml index b132b6649a..c92b5a846f 100644 --- a/conf/airframes/booz2_flixr.xml +++ b/conf/airframes/booz2_flixr.xml @@ -26,6 +26,8 @@ + + diff --git a/conf/autopilot/booz2_test_progs.makefile b/conf/autopilot/booz2_test_progs.makefile index 83896fb47d..ff3604c885 100644 --- a/conf/autopilot/booz2_test_progs.makefile +++ b/conf/autopilot/booz2_test_progs.makefile @@ -131,6 +131,29 @@ tunnel_bb.CFLAGS += -DUSE_LED tunnel_bb.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c +# +# usb tunnels +# +usb_tunnel_0.ARCHDIR = $(ARCH) +usb_tunnel_0.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) +usb_tunnel_0.CFLAGS += -DUSE_UART0 -DUART0_BAUD=B38400 -DPERIPHERALS_AUTO_INIT +usb_tunnel_0.CFLAGS += -DUSE_USB_LINE_CODING -DUSE_USB_SERIAL -DUSE_LED +usb_tunnel_0.srcs += $(SRC_ARCH)/usb_tunnel.c $(SRC_ARCH)/usb_ser_hw.c $(SRC_ARCH)/mcu_periph/uart_arch.c +usb_tunnel_0.srcs += $(SRC_ARCH)/lpcusb/usbhw_lpc.c $(SRC_ARCH)/lpcusb/usbinit.c +usb_tunnel_0.srcs += $(SRC_ARCH)/lpcusb/usbcontrol.c $(SRC_ARCH)/lpcusb/usbstdreq.c +usb_tunnel_0.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c +usb_tunnel_0.srcs += mcu.c $(SRC_ARCH)/mcu_arch.c + +usb_tunnel_1.ARCHDIR = $(ARCH) +usb_tunnel_1.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) +usb_tunnel_1.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B38400 -DPERIPHERALS_AUTO_INIT +usb_tunnel_1.CFLAGS += -DUSE_USB_LINE_CODING -DUSE_USB_SERIAL -DUSE_LED +usb_tunnel_1.srcs += $(SRC_ARCH)/usb_tunnel.c $(SRC_ARCH)/usb_ser_hw.c $(SRC_ARCH)/mcu_periph/uart_arch.c +usb_tunnel_1.srcs += $(SRC_ARCH)/lpcusb/usbhw_lpc.c $(SRC_ARCH)/lpcusb/usbinit.c +usb_tunnel_1.srcs += $(SRC_ARCH)/lpcusb/usbcontrol.c $(SRC_ARCH)/lpcusb/usbstdreq.c +usb_tunnel_1.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c +usb_tunnel_1.srcs += mcu.c $(SRC_ARCH)/mcu_arch.c + # # test GPS From a448cebf83055adbe2d8d8ca65bb5b83d8e0c43a Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Mon, 28 Mar 2011 16:22:14 +0200 Subject: [PATCH 094/101] renamed BOOZ2_[GYRO|ACCEL|MAG] message to IMU_[GYRO|ACCEL|MAG]_SCALED --- conf/messages.xml | 6 +++--- conf/telemetry/telemetry_booz2.xml | 6 +++--- sw/airborne/booz/test/booz2_test_crista.c | 4 ++-- sw/airborne/booz/test/booz_test_imu.c | 6 +++--- sw/airborne/csc/mercury_xsens.h | 12 ++++++------ sw/airborne/firmwares/beth/main_overo.c | 4 ++-- sw/airborne/firmwares/beth/main_stm32.c | 6 +++--- sw/airborne/firmwares/rotorcraft/telemetry.h | 12 ++++++------ sw/airborne/test/subsystems/test_ahrs.c | 6 +++--- sw/airborne/test/subsystems/test_imu.c | 6 +++--- 10 files changed, 34 insertions(+), 34 deletions(-) diff --git a/conf/messages.xml b/conf/messages.xml index 57fcb9a2e5..27a0c43441 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -834,19 +834,19 @@ - + - + - + diff --git a/conf/telemetry/telemetry_booz2.xml b/conf/telemetry/telemetry_booz2.xml index ee9f58aac8..4852249e86 100644 --- a/conf/telemetry/telemetry_booz2.xml +++ b/conf/telemetry/telemetry_booz2.xml @@ -40,9 +40,9 @@ - - - + + + diff --git a/sw/airborne/booz/test/booz2_test_crista.c b/sw/airborne/booz/test/booz2_test_crista.c index 82f990afe4..1231b7e6ab 100644 --- a/sw/airborne/booz/test/booz2_test_crista.c +++ b/sw/airborne/booz/test/booz2_test_crista.c @@ -108,11 +108,11 @@ static inline void on_imu_event(void) { &imu_accel_unscaled.z); } else if (cnt == 7) { - DOWNLINK_SEND_BOOZ2_GYRO(&imu_gyro.x, + DOWNLINK_SEND_IMU_GYRO_SCALED(&imu_gyro.x, &imu_gyro.y, &imu_gyro.z); - DOWNLINK_SEND_BOOZ2_ACCEL(&imu_accel.x, + DOWNLINK_SEND_IMU_ACCEL_SCALED(&imu_accel.x, &imu_accel.y, &imu_accel.z); } diff --git a/sw/airborne/booz/test/booz_test_imu.c b/sw/airborne/booz/test/booz_test_imu.c index 262013388a..19146b63cb 100644 --- a/sw/airborne/booz/test/booz_test_imu.c +++ b/sw/airborne/booz/test/booz_test_imu.c @@ -118,7 +118,7 @@ static inline void on_accel_event(void) { &imu.accel_unscaled.z); } else if (cnt == 7) { - DOWNLINK_SEND_BOOZ2_ACCEL(DefaultChannel, + DOWNLINK_SEND_IMU_ACCEL_SCALED(DefaultChannel, &imu.accel.x, &imu.accel.y, &imu.accel.z); @@ -140,7 +140,7 @@ static inline void on_gyro_accel_event(void) { &imu.gyro_unscaled.r); } else if (cnt == 7) { - DOWNLINK_SEND_BOOZ2_GYRO(DefaultChannel, + DOWNLINK_SEND_IMU_GYRO_SCALED(DefaultChannel, &imu.gyro.p, &imu.gyro.q, &imu.gyro.r); @@ -155,7 +155,7 @@ static inline void on_mag_event(void) { if (cnt > 10) cnt = 0; if (cnt == 0) { - DOWNLINK_SEND_BOOZ2_MAG(DefaultChannel, + DOWNLINK_SEND_IMU_MAG_SCALED(DefaultChannel, &imu.mag.x, &imu.mag.y, &imu.mag.z); diff --git a/sw/airborne/csc/mercury_xsens.h b/sw/airborne/csc/mercury_xsens.h index 9b04d79737..a2f1857a97 100644 --- a/sw/airborne/csc/mercury_xsens.h +++ b/sw/airborne/csc/mercury_xsens.h @@ -64,20 +64,20 @@ extern int xsens_setzero; #include "subsystems/ahrs.h" -#define PERIODIC_SEND_BOOZ2_GYRO() { \ - DOWNLINK_SEND_BOOZ2_GYRO(&imu.gyro.p, \ +#define PERIODIC_SEND_IMU_GYRO_SCALED() { \ + DOWNLINK_SEND_IMU_GYRO_SCALED(&imu.gyro.p, \ &imu.gyro.q, \ &imu.gyro.r); \ } -#define PERIODIC_SEND_BOOZ2_ACCEL() { \ - DOWNLINK_SEND_BOOZ2_ACCEL(&imu.accel.x, \ +#define PERIODIC_SEND_IMU_ACCEL_SCALED() { \ + DOWNLINK_SEND_IMU_ACCEL_SCALED(&imu.accel.x, \ &imu.accel.y, \ &imu.accel.z); \ } -#define PERIODIC_SEND_BOOZ2_MAG() { \ - DOWNLINK_SEND_BOOZ2_MAG(&imu.mag.x, \ +#define PERIODIC_SEND_IMU_MAG_SCALED() { \ + DOWNLINK_SEND_IMU_MAG_SCALED(&imu.mag.x, \ &imu.mag.y, \ &imu.mag.z); \ } diff --git a/sw/airborne/firmwares/beth/main_overo.c b/sw/airborne/firmwares/beth/main_overo.c index 6a52bc090a..b24958321f 100644 --- a/sw/airborne/firmwares/beth/main_overo.c +++ b/sw/airborne/firmwares/beth/main_overo.c @@ -159,13 +159,13 @@ static void main_periodic(int my_sig_num) { RunOnceEvery(10, {DOWNLINK_SEND_IMU_ACCEL_RAW(gcs_com.udp_transport, //&msg_in.payload.msg_up.accel.x,&msg_in.payload.msg_up.accel.y,&msg_in.payload.msg_up.accel.z &imu.accel_unscaled.x,&imu.accel_unscaled.y,&imu.accel_unscaled.z);}) - RunOnceEvery(50, {DOWNLINK_SEND_BOOZ2_GYRO(gcs_com.udp_transport, + RunOnceEvery(50, {DOWNLINK_SEND_IMU_GYRO_SCALED(gcs_com.udp_transport, //&msg_in.payload.msg_up.gyro.p,&msg_in.payload.msg_up.gyro.q,&msg_in.payload.msg_up.gyro.r) &imu.gyro.p,&imu.gyro.q,&imu.gyro.r);}); RunOnceEvery(50, {DOWNLINK_SEND_AHRS_EULER(gcs_com.udp_transport, &estimator.tilt, &estimator.elevation, &estimator.azimuth );}); - RunOnceEvery(50, {DOWNLINK_SEND_BOOZ2_ACCEL(DefaultChannel, + RunOnceEvery(50, {DOWNLINK_SEND_IMU_ACCEL_SCALED(DefaultChannel, //&msg_in.payload.msg_up.accel.x,&msg_in.payload.msg_up.accel.y,&msg_in.payload.msg_up.accel.z &imu.accel.x,&imu.accel.y,&imu.accel.z);});*/ diff --git a/sw/airborne/firmwares/beth/main_stm32.c b/sw/airborne/firmwares/beth/main_stm32.c index ee5aca10d2..af09ab6a0a 100644 --- a/sw/airborne/firmwares/beth/main_stm32.c +++ b/sw/airborne/firmwares/beth/main_stm32.c @@ -176,12 +176,12 @@ static inline void on_gyro_accel_event(void) { &imu.accel_unscaled.z); } else if (cnt == 7) { - DOWNLINK_SEND_BOOZ2_GYRO(DefaultChannel, + DOWNLINK_SEND_IMU_GYRO_SCALED(DefaultChannel, &imu.gyro.p, &imu.gyro.q, &imu.gyro.r); - DOWNLINK_SEND_BOOZ2_ACCEL(DefaultChannel, + DOWNLINK_SEND_IMU_ACCEL_SCALED(DefaultChannel, &imu.accel.x, &imu.accel.y, &imu.accel.z); @@ -196,7 +196,7 @@ static inline void on_mag_event(void) { if (cnt > 1) cnt = 0; if (cnt%2) { - DOWNLINK_SEND_BOOZ2_MAG(DefaultChannel, + DOWNLINK_SEND_IMU_MAG_SCALED(DefaultChannel, &imu.mag.x, &imu.mag.y, &imu.mag.z); diff --git a/sw/airborne/firmwares/rotorcraft/telemetry.h b/sw/airborne/firmwares/rotorcraft/telemetry.h index e9cbe76fae..65a755cbb6 100644 --- a/sw/airborne/firmwares/rotorcraft/telemetry.h +++ b/sw/airborne/firmwares/rotorcraft/telemetry.h @@ -130,22 +130,22 @@ extern uint8_t telemetry_mode_Main_DefaultChannel; #define PERIODIC_SEND_PPM(_chan) {} #endif -#define PERIODIC_SEND_BOOZ2_GYRO(_chan) { \ - DOWNLINK_SEND_BOOZ2_GYRO(_chan, \ +#define PERIODIC_SEND_IMU_GYRO_SCALED(_chan) { \ + DOWNLINK_SEND_IMU_GYRO_SCALED(_chan, \ &imu.gyro.p, \ &imu.gyro.q, \ &imu.gyro.r); \ } -#define PERIODIC_SEND_BOOZ2_ACCEL(_chan) { \ - DOWNLINK_SEND_BOOZ2_ACCEL(_chan, \ +#define PERIODIC_SEND_IMU_ACCEL_SCALED(_chan) { \ + DOWNLINK_SEND_IMU_ACCEL_SCALED(_chan, \ &imu.accel.x, \ &imu.accel.y, \ &imu.accel.z); \ } -#define PERIODIC_SEND_BOOZ2_MAG(_chan) { \ - DOWNLINK_SEND_BOOZ2_MAG(_chan, \ +#define PERIODIC_SEND_IMU_MAG_SCALED(_chan) { \ + DOWNLINK_SEND_IMU_MAG_SCALED(_chan, \ &imu.mag.x, \ &imu.mag.y, \ &imu.mag.z); \ diff --git a/sw/airborne/test/subsystems/test_ahrs.c b/sw/airborne/test/subsystems/test_ahrs.c index 26456a7bb7..ac42d4b344 100644 --- a/sw/airborne/test/subsystems/test_ahrs.c +++ b/sw/airborne/test/subsystems/test_ahrs.c @@ -136,20 +136,20 @@ static inline void main_report(void) { &imu.mag_unscaled.z); }, { - DOWNLINK_SEND_BOOZ2_ACCEL(DefaultChannel, + DOWNLINK_SEND_IMU_ACCEL_SCALED(DefaultChannel, &imu.accel.x, &imu.accel.y, &imu.accel.z); }, { - DOWNLINK_SEND_BOOZ2_GYRO(DefaultChannel, + DOWNLINK_SEND_IMU_GYRO_SCALED(DefaultChannel, &imu.gyro.p, &imu.gyro.q, &imu.gyro.r); }, { - DOWNLINK_SEND_BOOZ2_MAG(DefaultChannel, + DOWNLINK_SEND_IMU_MAG_SCALED(DefaultChannel, &imu.mag.x, &imu.mag.y, &imu.mag.z); diff --git a/sw/airborne/test/subsystems/test_imu.c b/sw/airborne/test/subsystems/test_imu.c index 012d16865d..9591438b63 100644 --- a/sw/airborne/test/subsystems/test_imu.c +++ b/sw/airborne/test/subsystems/test_imu.c @@ -108,7 +108,7 @@ static inline void on_accel_event(void) { &imu.accel_unscaled.z); } else if (cnt == 7) { - DOWNLINK_SEND_BOOZ2_ACCEL(DefaultChannel, + DOWNLINK_SEND_IMU_ACCEL_SCALED(DefaultChannel, &imu.accel.x, &imu.accel.y, &imu.accel.z); @@ -129,7 +129,7 @@ static inline void on_gyro_accel_event(void) { &imu.gyro_unscaled.r); } else if (cnt == 7) { - DOWNLINK_SEND_BOOZ2_GYRO(DefaultChannel, + DOWNLINK_SEND_IMU_GYRO_SCALED(DefaultChannel, &imu.gyro.p, &imu.gyro.q, &imu.gyro.r); @@ -144,7 +144,7 @@ static inline void on_mag_event(void) { if (cnt > 10) cnt = 0; if (cnt == 0) { - DOWNLINK_SEND_BOOZ2_MAG(DefaultChannel, + DOWNLINK_SEND_IMU_MAG_SCALED(DefaultChannel, &imu.mag.x, &imu.mag.y, &imu.mag.z); From 075e3659db1bba699af31af08f911a7877d00315 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Tue, 29 Mar 2011 18:22:11 +0200 Subject: [PATCH 095/101] expression parser now allows to use '->' --- sw/ground_segment/joystick/input2ivy.ml | 3 ++- sw/lib/ocaml/expr_lexer.mll | 1 + sw/lib/ocaml/expr_parser.mly | 6 ++++-- sw/lib/ocaml/expr_syntax.ml | 4 ++++ sw/lib/ocaml/expr_syntax.mli | 1 + sw/lib/ocaml/papget.ml | 2 ++ sw/tools/fp_proc.ml | 3 ++- 7 files changed, 16 insertions(+), 4 deletions(-) diff --git a/sw/ground_segment/joystick/input2ivy.ml b/sw/ground_segment/joystick/input2ivy.ml index df2a3b8fcb..2caae99c23 100644 --- a/sw/ground_segment/joystick/input2ivy.ml +++ b/sw/ground_segment/joystick/input2ivy.ml @@ -247,7 +247,8 @@ let eval_expr = fun buttons axis inputs expr -> | Syntax.Call (ident, exprs) | Syntax.CallOperator (ident, exprs) -> eval_call ident (List.map eval exprs) | Syntax.Index _ -> failwith "eval_expr: index" - | Syntax.Field _ -> failwith "eval_expr: Field" in + | Syntax.Field _ -> failwith "eval_expr: Field" + | Syntax.Deref _ -> failwith "eval_expr: deref" in eval expr diff --git a/sw/lib/ocaml/expr_lexer.mll b/sw/lib/ocaml/expr_lexer.mll index 695409d5a9..dbfa4fab10 100644 --- a/sw/lib/ocaml/expr_lexer.mll +++ b/sw/lib/ocaml/expr_lexer.mll @@ -42,6 +42,7 @@ rule token = parse | '}' { RC } | '[' { LB } | ']' { RB } + | "->" { DEREF } | "==" { EQ } | "&&" { AND } | "||" { OR } diff --git a/sw/lib/ocaml/expr_parser.mly b/sw/lib/ocaml/expr_parser.mly index c2f63b4a00..7aa963ce37 100644 --- a/sw/lib/ocaml/expr_parser.mly +++ b/sw/lib/ocaml/expr_parser.mly @@ -29,7 +29,7 @@ open Expr_syntax %token FLOAT %token IDENT %token EOF -%token DOT COMMA SEMICOLON LP RP LC RC LB RB AND COLON OR +%token DOT COMMA SEMICOLON LP RP LC RC LB RB DEREF AND COLON OR %token EQ GT ASSIGN GEQ NOT %token PLUS MINUS %token MULT DIV MOD @@ -39,7 +39,8 @@ open Expr_syntax %left PLUS MINUS %left MULT DIV MOD %nonassoc NOT -%nonassoc UMINUS /* highest precedence */ +%nonassoc UMINUS +%left DEREF /* highest precedence */ %start expression /* the entry point */ %type expression @@ -63,6 +64,7 @@ expression: | FLOAT { Float $1 } | IDENT { Ident $1 } | IDENT DOT IDENT { Field ($1,$3) } + | expression DEREF IDENT { Deref($1, $3) } | IDENT LP Args RP { Call ($1, $3) } | LP expression RP { $2 } | IDENT LB expression RB { Index ($1, $3) } diff --git a/sw/lib/ocaml/expr_syntax.ml b/sw/lib/ocaml/expr_syntax.ml index be15604c8d..5f8f5034b5 100644 --- a/sw/lib/ocaml/expr_syntax.ml +++ b/sw/lib/ocaml/expr_syntax.ml @@ -37,6 +37,7 @@ type expression = | CallOperator of ident * (expression list) | Index of ident * expression | Field of ident * ident + | Deref of expression * ident let c_var_of_ident = fun x -> "_var_" ^ x @@ -55,6 +56,7 @@ let rec sprint = function sprintf "%s(%s)" i (String.concat "," ses) | Index (i,e) -> sprintf "%s[%s]" i (sprint e) | Field (i,f) -> sprintf "%s.%s" i f + | Deref (e,f) -> sprintf "(%s)->%s" (sprint e) f (* Valid functions : FIXME *) let functions = [ @@ -113,3 +115,5 @@ let rec check_expression = fun e -> | Field (i, _field) -> if not (List.mem i variables) then unexpected "ident" i + | Deref (e, _field) -> + check_expression e diff --git a/sw/lib/ocaml/expr_syntax.mli b/sw/lib/ocaml/expr_syntax.mli index 0d266c2354..7cb9619094 100644 --- a/sw/lib/ocaml/expr_syntax.mli +++ b/sw/lib/ocaml/expr_syntax.mli @@ -34,6 +34,7 @@ type expression = | CallOperator of ident * expression list | Index of ident * expression | Field of ident * ident + | Deref of expression * ident val c_var_of_ident : ident -> string (** Encapsulate a user ident into a C variable *) diff --git a/sw/lib/ocaml/papget.ml b/sw/lib/ocaml/papget.ml index 23ed642836..586f428724 100644 --- a/sw/lib/ocaml/papget.ml +++ b/sw/lib/ocaml/papget.ml @@ -92,6 +92,7 @@ let hash_vars = fun expr -> | E.Int _ | E.Float _ -> () | E.Call (_id, list) | E.CallOperator (_id, list) -> List.iter loop list | E.Index (_id, e) -> loop e + | E.Deref (_e, _f) as deref -> fprintf stderr "Warning: Deref operator is not allowed in Papgets expressions (%s)" (E.sprint deref) | E.Field (i, f) -> if not (Hashtbl.mem htable (i,f)) then let msg_obj = new message_field i f in @@ -121,6 +122,7 @@ let eval_expr = fun (extra_functions:(string * (string list -> string)) list) h | E.Call (ident, _l) | E.CallOperator (ident, _l) -> failwith (sprintf "Papget.eval_expr '%s(...)'" ident) | E.Index (ident, _e) -> failwith (sprintf "Papget.eval_expr '%s[...]'" ident) + | E.Deref (_e, _f) as deref -> failwith (sprintf "Papget.eval_expr Deref operator is not allowed in Papgets expressions (%s)" (E.sprint deref)) | E.Field (i, f) -> try (Hashtbl.find h (i,f))#last_value diff --git a/sw/tools/fp_proc.ml b/sw/tools/fp_proc.ml index 2a2e70dec4..93f37bab4c 100644 --- a/sw/tools/fp_proc.ml +++ b/sw/tools/fp_proc.ml @@ -59,7 +59,8 @@ let subst_expression = fun env e -> | Int _ | Float _ | Field _ -> e | Call (i, es) -> Call (i, List.map sub es) | CallOperator (i, es) -> CallOperator (i, List.map sub es) - | Index (i,e) -> Index (i,sub e) in + | Index (i,e) -> Index (i,sub e) + | Deref (e,f) -> Deref (sub e, f) in sub e From 400fd674837c30426637966b747be9ba5a4f8a5f Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Tue, 29 Mar 2011 20:05:55 +0200 Subject: [PATCH 096/101] don't abort with error when using telemetry_transparent_usb in sim --- conf/airframes/booz2_flixr.xml | 2 +- .../subsystems/fixedwing/telemetry_transparent_usb.makefile | 2 ++ .../subsystems/rotorcraft/telemetry_transparent_usb.makefile | 2 ++ 3 files changed, 5 insertions(+), 1 deletion(-) diff --git a/conf/airframes/booz2_flixr.xml b/conf/airframes/booz2_flixr.xml index c92b5a846f..323dc620f4 100644 --- a/conf/airframes/booz2_flixr.xml +++ b/conf/airframes/booz2_flixr.xml @@ -13,7 +13,7 @@ - + diff --git a/conf/autopilot/subsystems/fixedwing/telemetry_transparent_usb.makefile b/conf/autopilot/subsystems/fixedwing/telemetry_transparent_usb.makefile index 1bf60fe476..4221fce81d 100644 --- a/conf/autopilot/subsystems/fixedwing/telemetry_transparent_usb.makefile +++ b/conf/autopilot/subsystems/fixedwing/telemetry_transparent_usb.makefile @@ -8,6 +8,8 @@ ap.srcs += $(SRC_FIXEDWING)/downlink.c $(SRC_FIRMWARE)/datalink.c $(SRC_FIXEDWIN ap.srcs += $(SRC_ARCH)/usb_ser_hw.c $(SRC_ARCH)/lpcusb/usbhw_lpc.c $(SRC_ARCH)/lpcusb/usbcontrol.c ap.srcs += $(SRC_ARCH)/lpcusb/usbstdreq.c $(SRC_ARCH)/lpcusb/usbinit.c else +ifneq ($(ARCH), sim) $(error telemetry_transparent_usb currently only implemented for the lpc21) endif +endif diff --git a/conf/autopilot/subsystems/rotorcraft/telemetry_transparent_usb.makefile b/conf/autopilot/subsystems/rotorcraft/telemetry_transparent_usb.makefile index 7100d4212e..a15920babb 100644 --- a/conf/autopilot/subsystems/rotorcraft/telemetry_transparent_usb.makefile +++ b/conf/autopilot/subsystems/rotorcraft/telemetry_transparent_usb.makefile @@ -9,6 +9,8 @@ ap.srcs += $(SRC_FIRMWARE)/datalink.c $(SRC_FIRMWARE)/telemetry.c ap.srcs += $(SRC_ARCH)/usb_ser_hw.c $(SRC_ARCH)/lpcusb/usbhw_lpc.c $(SRC_ARCH)/lpcusb/usbcontrol.c ap.srcs += $(SRC_ARCH)/lpcusb/usbstdreq.c $(SRC_ARCH)/lpcusb/usbinit.c else +ifneq ($(ARCH), sim) $(error telemetry_transparent_usb currently only implemented for the lpc21) endif +endif From 8ac9a5f2f0ec502bba700b724d4f6d501038e26c Mon Sep 17 00:00:00 2001 From: Antoine Drouin Date: Tue, 29 Mar 2011 23:43:58 +0300 Subject: [PATCH 097/101] couple of gain changes --- conf/airframes/Poine/booz2_a7.xml | 104 ++++++++++++++++++++---------- 1 file changed, 70 insertions(+), 34 deletions(-) diff --git a/conf/airframes/Poine/booz2_a7.xml b/conf/airframes/Poine/booz2_a7.xml index 091401ebed..671a6fc9c4 100644 --- a/conf/airframes/Poine/booz2_a7.xml +++ b/conf/airframes/Poine/booz2_a7.xml @@ -61,14 +61,6 @@ - @@ -77,7 +69,7 @@
- +
@@ -124,7 +116,7 @@ - + + --> + + + + + + + + + + + + + + + + + +
+
+ + + + +
+ +
- +
@@ -155,21 +173,29 @@ + + + + + + - +
- + - + +
@@ -182,15 +208,19 @@
- + +--> - - + + + + + + @@ -201,29 +231,35 @@ - + + + + + - - - - - - - - - - - - - + + + + + + + + + + + + + + + From ac8b9d9926e3396ed97282e25f46e93cda5ccf0c Mon Sep 17 00:00:00 2001 From: Antoine Drouin Date: Wed, 30 Mar 2011 00:34:54 +0300 Subject: [PATCH 098/101] no pers settings for the demo --- conf/airframes/Poine/booz2_a7.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/conf/airframes/Poine/booz2_a7.xml b/conf/airframes/Poine/booz2_a7.xml index 671a6fc9c4..61428ed284 100644 --- a/conf/airframes/Poine/booz2_a7.xml +++ b/conf/airframes/Poine/booz2_a7.xml @@ -215,7 +215,7 @@ --> - + From 1aafda872af6679e369f51f65db70bf16e938c53 Mon Sep 17 00:00:00 2001 From: Allen Date: Thu, 31 Mar 2011 21:15:29 -0700 Subject: [PATCH 099/101] Start of skeleton for BMP085 baro sensor on lisa/m --- sw/airborne/boards/lisa_m/baro_board.c | 105 +++++++++++++++---------- sw/airborne/boards/lisa_m/baro_board.h | 43 ++++------ 2 files changed, 80 insertions(+), 68 deletions(-) diff --git a/sw/airborne/boards/lisa_m/baro_board.c b/sw/airborne/boards/lisa_m/baro_board.c index 11829b5acf..4d87ea5003 100644 --- a/sw/airborne/boards/lisa_m/baro_board.c +++ b/sw/airborne/boards/lisa_m/baro_board.c @@ -9,12 +9,11 @@ struct i2c_transaction baro_trans; static inline void baro_board_write_to_register(uint8_t baro_addr, uint8_t reg_addr, uint8_t val_msb, uint8_t val_lsb); static inline void baro_board_read_from_register(uint8_t baro_addr, uint8_t reg_addr); static inline void baro_board_set_current_register(uint8_t baro_addr, uint8_t reg_addr); -static inline void baro_board_read_from_current_register(uint8_t baro_addr); +static inline void baro_board_read(void); -// absolute -#define BARO_ABS_ADDR 0x90 -// differential -#define BARO_DIFF_ADDR 0x92 + +#define BMP085_SAMPLE_PERIOD_MS (3 + (2 << BMP085_OSS) * 3) +#define BMP085_SAMPLE_PERIOD (BMP075_SAMPLE_PERIOD_MS >> 1) void baro_init(void) { baro.status = BS_UNINITIALIZED; @@ -23,6 +22,22 @@ void baro_init(void) { baro_board.status = LBS_UNINITIALIZED; } +static inline void bmp085_write_reg(uint8_t addr, uint8_t value) +{ + baro_trans.type = I2CTransTx; + baro_trans.slave_addr = BMP085_ADDR; + baro_trans.len_w = 2; + baro_trans.buf[0] = addr; + baro_trans.buf[1] = value; + i2c_submit(&i2c2, &baro_trans); + while (baro_trans.status == I2CTransPending || baro_trans.status == I2CTransRunning); +} + +static inline void bmp085_request_pressure(void) +{ + bmp085_write_reg(0xF4, 0x34 + (BMP085_OSS << 6)); +} + void baro_periodic(void) { // check i2c_done @@ -33,31 +48,22 @@ void baro_periodic(void) { baro_board.status = LBS_RESETED; break; case LBS_RESETED: - baro_board_send_config_abs(); - baro_board.status = LBS_INITIALIZING_ABS; + baro_board_send_config(); + baro_board.status = LBS_INITIALIZING; break; - case LBS_INITIALIZING_ABS: - baro_board_set_current_register(BARO_ABS_ADDR, 0x00); - baro_board.status = LBS_INITIALIZING_ABS_1; + case LBS_INITIALIZING: + baro_board_set_current_register(BMP085_ADDR, 0x00); + baro_board.status = LBS_INITIALIZING_1; break; - case LBS_INITIALIZING_ABS_1: - baro_board_send_config_diff(); - baro_board.status = LBS_INITIALIZING_DIFF; - break; - case LBS_INITIALIZING_DIFF: - baro_board_set_current_register(BARO_DIFF_ADDR, 0x00); - baro_board.status = LBS_INITIALIZING_DIFF_1; - // baro_board.status = LBS_UNINITIALIZED; - break; - case LBS_INITIALIZING_DIFF_1: + case LBS_INITIALIZING_1: baro.status = BS_RUNNING; - case LBS_READ_DIFF: - baro_board_read_from_current_register(BARO_ABS_ADDR); - baro_board.status = LBS_READING_ABS; + case LBS_REQUEST: + bmp085_request_pressure(); + baro_board.status = LBS_READ; break; - case LBS_READ_ABS: - baro_board_read_from_current_register(BARO_DIFF_ADDR); - baro_board.status = LBS_READING_DIFF; + case LBS_READ: + baro_board_read(); + baro_board.status = LBS_READING; break; default: break; @@ -66,12 +72,9 @@ void baro_periodic(void) { } -void baro_board_send_config_abs(void) { - baro_board_write_to_register(BARO_ABS_ADDR, 0x01, 0x86, 0x83); -} - -void baro_board_send_config_diff(void) { - baro_board_write_to_register(BARO_DIFF_ADDR, 0x01, 0x84, 0x83); +void baro_board_send_config(void) { + /* maybe we should read factory calibration here */ + //baro_board_write_to_register(BMP085_ADDR, 0x01, 0x86, 0x83); } void baro_board_send_reset(void) { @@ -82,6 +85,19 @@ void baro_board_send_reset(void) { i2c_submit(&i2c2,&baro_trans); } +static inline void bmp085_read_reg24(uint8_t addr) +{ + baro_trans.type = I2CTransTxRx; + baro_trans.slave_addr = BMP085_ADDR; + baro_trans.len_w = 1; + baro_trans.len_r = 3; + baro_trans.buf[0] = addr; + i2c_submit(&i2c2, &baro_trans); + //while (baro_trans.status == I2CTransPending || baro_trans.status == I2CTransRunning); + + //return (baro_trans.buf[0] << 16) | (baro_trans.buf[1] >> 8) | (baro_trans.buf[2]); +} + static inline void baro_board_write_to_register(uint8_t baro_addr, uint8_t reg_addr, uint8_t val_msb, uint8_t val_lsb) { baro_trans.type = I2CTransTx; baro_trans.slave_addr = baro_addr; @@ -99,8 +115,6 @@ static inline void baro_board_read_from_register(uint8_t baro_addr, uint8_t reg_ baro_trans.len_r = 2; baro_trans.buf[0] = reg_addr; i2c_submit(&i2c2,&baro_trans); - // i2c2.buf[0] = reg_addr; - // i2c2_transceive(baro_addr, 1, 2, &baro_board.i2c_done); } static inline void baro_board_set_current_register(uint8_t baro_addr, uint8_t reg_addr) { @@ -109,14 +123,21 @@ static inline void baro_board_set_current_register(uint8_t baro_addr, uint8_t re baro_trans.len_w = 1; baro_trans.buf[0] = reg_addr; i2c_submit(&i2c2,&baro_trans); - // i2c2.buf[0] = reg_addr; - // i2c2_transmit(baro_addr, 1, &baro_board.i2c_done); } -static inline void baro_board_read_from_current_register(uint8_t baro_addr) { - baro_trans.type = I2CTransRx; - baro_trans.slave_addr = baro_addr; - baro_trans.len_r = 2; - i2c_submit(&i2c2,&baro_trans); - // i2c2_receive(baro_addr, 2, &baro_board.i2c_done); + +static inline void bmp085_read_pressure(void) +{ + bmp085_read_reg24(0xF6); +} + +static inline void baro_board_read() +{ + //int32_t x; + //bmp085_request_pressure(); + bmp085_read_pressure(); + //baro_trans.type = I2CTransRx; + //baro_trans.slave_addr = BMP085_ADDR; + //baro_trans.len_r = 2; + //i2c_submit(&i2c2,&baro_trans); } diff --git a/sw/airborne/boards/lisa_m/baro_board.h b/sw/airborne/boards/lisa_m/baro_board.h index e2d61a79db..374f9d30ae 100644 --- a/sw/airborne/boards/lisa_m/baro_board.h +++ b/sw/airborne/boards/lisa_m/baro_board.h @@ -4,24 +4,25 @@ * */ -#ifndef BOARDS_LISA_L_BARO_H -#define BOARDS_LISA_L_BARO_H +#ifndef BOARDS_LISA_M_BARO_H +#define BOARDS_LISA_M_BARO_H #include "std.h" #include "mcu_periph/i2c.h" +// absolute +#define BMP085_ADDR 0xEE +#define BMP085_OSS 3 + enum LisaBaroStatus { LBS_UNINITIALIZED, LBS_RESETED, - LBS_INITIALIZING_ABS, - LBS_INITIALIZING_ABS_1, - LBS_INITIALIZING_DIFF, - LBS_INITIALIZING_DIFF_1, + LBS_INITIALIZING, + LBS_INITIALIZING_1, LBS_IDLE, - LBS_READING_ABS, - LBS_READ_ABS, - LBS_READING_DIFF, - LBS_READ_DIFF + LBS_REQUEST, + LBS_READING, + LBS_READ, }; struct BaroBoard { @@ -32,31 +33,21 @@ extern struct BaroBoard baro_board; extern struct i2c_transaction baro_trans; extern void baro_board_send_reset(void); -extern void baro_board_send_config_abs(void); -extern void baro_board_send_config_diff(void); +extern void baro_board_send_config(void); #define BaroEvent(_b_abs_handler, _b_diff_handler) { \ - if (baro_board.status == LBS_READING_ABS && \ + if (baro_board.status == LBS_READING && \ baro_trans.status != I2CTransPending) { \ - baro_board.status = LBS_READ_ABS; \ + baro_board.status = LBS_REQUEST; \ if (baro_trans.status == I2CTransSuccess) { \ - int16_t tmp = baro_trans.buf[0]<<8 | baro_trans.buf[1]; \ - baro.absolute = tmp; \ + int32_t tmp = (baro_trans.buf[0]<<16) | (baro_trans.buf[1] << 8) | baro_trans.buf[0]; \ + baro.absolute = tmp >> ( 8 - BMP085_OSS); \ _b_abs_handler(); \ } \ } \ - else if (baro_board.status == LBS_READING_DIFF && \ - baro_trans.status != I2CTransPending) { \ - baro_board.status = LBS_READ_DIFF; \ - if (baro_trans.status == I2CTransSuccess) { \ - int16_t tmp = baro_trans.buf[0]<<8 | baro_trans.buf[1]; \ - baro.differential = tmp; \ - _b_diff_handler(); \ - } \ - } \ } -#endif /* BOARDS_LISA_L_BARO_H */ +#endif /* BOARDS_LISA_M_BARO_H */ From 81d95eec98acd1a52e2fae935e788e4f4e602013 Mon Sep 17 00:00:00 2001 From: Allen Date: Thu, 31 Mar 2011 21:50:28 -0700 Subject: [PATCH 100/101] Add/enable lisa_m test_baro test program --- conf/airframes/esden/lisa_m_pwm.xml | 4 +- conf/autopilot/lisa_m_test_progs.makefile | 42 ++++----- sw/airborne/boards/lisa_m/test_baro.c | 108 ++++++++++++++++++++++ 3 files changed, 131 insertions(+), 23 deletions(-) create mode 100644 sw/airborne/boards/lisa_m/test_baro.c diff --git a/conf/airframes/esden/lisa_m_pwm.xml b/conf/airframes/esden/lisa_m_pwm.xml index 20ef5ac15f..2d93394455 100644 --- a/conf/airframes/esden/lisa_m_pwm.xml +++ b/conf/airframes/esden/lisa_m_pwm.xml @@ -230,8 +230,8 @@ -