How to install Arduino Library

From Wiki
Revision as of 07:13, 27 October 2015 by Root (Talk | contribs) (→‎Introduction)

Jump to: navigation, search

Introduction

The link titleis a single-chip, single-supply low power 8-bit mediawiki:Formatting CMOS data acquisition device with four analog inputs, one analog output and a serial I 2 C-bus interface. Three address pins A0, A1 and A2 are used for programming the hardware address, allowing the use of up to eight devices connected to the I 2 C-bus without additionalhardware.Address,controlanddatatoandfrom the device are transferred serially via the two-line bidirectional I 2 C-bus. The functions of the device include analog input multiplexing, on-chip track and hold function, 8-bit analog-to-digital conversion and an 8-bit digital-to-analog conversion. The maximum conversion rate is given by the maximum speed of the I 2 C-bus.

The schematic diagram of the PCF8591 Module is shown as below: /* MultiWiiCopter by Alexandre Dubus www.multiwii.com March 2013 V2.2 This program 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 3 of the License, or any later version. see <http://www.gnu.org/licenses/> */ #include <avr/io.h> #include "config.h" #include "def.h" #include <avr/pgmspace.h> #define VERSION 220 /*********** RC alias *****************/ enum rc { ROLL, PITCH, YAW, THROTTLE, AUX1, AUX2, AUX3, AUX4 }; enum pid { PIDROLL, PIDPITCH, PIDYAW, PIDALT, PIDPOS, PIDPOSR, PIDNAVR, PIDLEVEL, PIDMAG, PIDVEL, // not used currently PIDITEMS }; const char pidnames[] PROGMEM = "ROLL;" "PITCH;" "YAW;" "ALT;" "Pos;" "PosR;" "NavR;" "LEVEL;" "MAG;" "VEL;" ; enum box { BOXARM, #if ACC BOXANGLE, BOXHORIZON, #endif #if BARO && (!defined(SUPPRESS_BARO_ALTHOLD)) BOXBARO, #endif #ifdef VARIOMETER BOXVARIO, #endif #if MAG BOXMAG, BOXHEADFREE, BOXHEADADJ, // acquire heading for HEADFREE mode #endif #if defined(SERVO_TILT) || defined(GIMBAL) || defined(SERVO_MIX_TILT) BOXCAMSTAB, #endif #if defined(CAMTRIG) BOXCAMTRIG, #endif #if GPS BOXGPSHOME, BOXGPSHOLD, #endif #if defined(FIXEDWING) || defined(HELICOPTER) BOXPASSTHRU, #endif #if defined(BUZZER) BOXBEEPERON, #endif #if defined(LED_FLASHER) BOXLEDMAX, // we want maximum illumination BOXLEDLOW, // low/no lights #endif #if defined(LANDING_LIGHTS_DDR) BOXLLIGHTS, // enable landing lights at any altitude #endif #ifdef INFLIGHT_ACC_CALIBRATION BOXCALIB, #endif #ifdef GOVERNOR_P BOXGOV, #endif #ifdef OSD_SWITCH BOXOSD, #endif CHECKBOXITEMS }; const char boxnames[] PROGMEM = // names for dynamic generation of config GUI "ARM;" #if ACC "ANGLE;" "HORIZON;" #endif #if BARO && (!defined(SUPPRESS_BARO_ALTHOLD)) "BARO;" #endif #ifdef VARIOMETER "VARIO;" #endif #if MAG "MAG;" "HEADFREE;" "HEADADJ;" #endif #if defined(SERVO_TILT) || defined(GIMBAL)|| defined(SERVO_MIX_TILT) "CAMSTAB;" #endif #if defined(CAMTRIG) "CAMTRIG;" #endif #if GPS "GPS HOME;" "GPS HOLD;" #endif #if defined(FIXEDWING) || defined(HELICOPTER) "PASSTHRU;" #endif #if defined(BUZZER) "BEEPER;" #endif #if defined(LED_FLASHER) "LEDMAX;" "LEDLOW;" #endif #if defined(LANDING_LIGHTS_DDR) "LLIGHTS;" #endif #ifdef INFLIGHT_ACC_CALIBRATION "CALIB;" #endif #ifdef GOVERNOR_P "GOVERNOR;" #endif #ifdef OSD_SWITCH "OSD SW;" #endif ; const uint8_t boxids[] PROGMEM = {// permanent IDs associated to boxes. This way, you can rely on an ID number to identify a BOX function. 0, //"ARM;" #if ACC 1, //"ANGLE;" 2, //"HORIZON;" #endif #if BARO && (!defined(SUPPRESS_BARO_ALTHOLD)) 3, //"BARO;" #endif #ifdef VARIOMETER 4, //"VARIO;" #endif #if MAG 5, //"MAG;" 6, //"HEADFREE;" 7, //"HEADADJ;" #endif #if defined(SERVO_TILT) || defined(GIMBAL)|| defined(SERVO_MIX_TILT) 8, //"CAMSTAB;" #endif #if defined(CAMTRIG) 9, //"CAMTRIG;" #endif #if GPS 10, //"GPS HOME;" 11, //"GPS HOLD;" #endif #if defined(FIXEDWING) || defined(HELICOPTER) 12, //"PASSTHRU;" #endif #if defined(BUZZER) 13, //"BEEPER;" #endif #if defined(LED_FLASHER) 14, //"LEDMAX;" 15, //"LEDLOW;" #endif #if defined(LANDING_LIGHTS_DDR) 16, //"LLIGHTS;" #endif #ifdef INFLIGHT_ACC_CALIBRATION 17, //"CALIB;" #endif #ifdef GOVERNOR_P 18, //"GOVERNOR;" #endif #ifdef OSD_SWITCH 19, //"OSD_SWITCH;" #endif }; static uint32_t currentTime = 0; static uint16_t previousTime = 0; static uint16_t cycleTime = 0; // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop static uint16_t calibratingA = 0; // the calibration is done in the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode. static uint16_t calibratingB = 0; // baro calibration = get new ground pressure value static uint16_t calibratingG; static uint16_t acc_1G; // this is the 1G measured acceleration static uint16_t acc_25deg; static int16_t gyroADC[3],accADC[3],accSmooth[3],magADC[3]; static int16_t heading,magHold,headFreeModeHold; // [-180;+180] static uint8_t vbat; // battery voltage in 0.1V steps static uint8_t vbatMin = VBATNOMINAL; // lowest battery voltage in 0.1V steps static uint8_t rcOptions[CHECKBOXITEMS]; static int32_t BaroAlt,EstAlt,AltHold; // in cm static int16_t BaroPID = 0; static int16_t errorAltitudeI = 0; static int16_t vario = 0; // variometer in cm/s #if defined(ARMEDTIMEWARNING) static uint32_t ArmedTimeWarningMicroSeconds = 0; #endif static int16_t debug[4]; static int16_t sonarAlt; //to think about the unit struct flags_struct { uint8_t OK_TO_ARM :1 ; uint8_t ARMED :1 ; uint8_t I2C_INIT_DONE :1 ; // For i2c gps we have to now when i2c init is done, so we can update parameters to the i2cgps from eeprom (at startup it is done in setup()) uint8_t ACC_CALIBRATED :1 ; uint8_t NUNCHUKDATA :1 ; uint8_t ANGLE_MODE :1 ; uint8_t HORIZON_MODE :1 ; uint8_t MAG_MODE :1 ; uint8_t BARO_MODE :1 ; uint8_t GPS_HOME_MODE :1 ; uint8_t GPS_HOLD_MODE :1 ; uint8_t HEADFREE_MODE :1 ; uint8_t PASSTHRU_MODE :1 ; uint8_t GPS_FIX :1 ; uint8_t GPS_FIX_HOME :1 ; uint8_t SMALL_ANGLES_25 :1 ; uint8_t CALIBRATE_MAG :1 ; uint8_t VARIO_MODE :1; } f; //for log #if defined(LOG_VALUES) || defined(LCD_TELEMETRY) static uint16_t cycleTimeMax = 0; // highest ever cycle timen static uint16_t cycleTimeMin = 65535; // lowest ever cycle timen static uint16_t powerMax = 0; // highest ever current; static int32_t BAROaltMax; // maximum value #endif #if defined(LOG_VALUES) || defined(LCD_TELEMETRY) || defined(ARMEDTIMEWARNING) || defined(LOG_PERMANENT) static uint32_t armedTime = 0; #endif static int16_t i2c_errors_count = 0; static int16_t annex650_overrun_count = 0; // ********************** //Automatic ACC Offset Calibration // ********************** #if defined(INFLIGHT_ACC_CALIBRATION) static uint16_t InflightcalibratingA = 0; static int16_t AccInflightCalibrationArmed; static uint16_t AccInflightCalibrationMeasurementDone = 0; static uint16_t AccInflightCalibrationSavetoEEProm = 0; static uint16_t AccInflightCalibrationActive = 0; #endif // ********************** // power meter // ********************** #if defined(POWERMETER) #define PMOTOR_SUM 8 // index into pMeter[] for sum static uint32_t pMeter[PMOTOR_SUM + 1]; // we use [0:7] for eight motors,one extra for sum static uint8_t pMeterV; // dummy to satisfy the paramStruct logic in ConfigurationLoop() static uint32_t pAlarm; // we scale the eeprom value from [0:255] to this value we can directly compare to the sum in pMeter[6] static uint16_t powerValue = 0; // last known current #endif static uint16_t intPowerMeterSum, intPowerTrigger1; // ********************** // telemetry // ********************** #if defined(LCD_TELEMETRY) static uint8_t telemetry = 0; static uint8_t telemetry_auto = 0; #endif #ifdef LCD_TELEMETRY_STEP static char telemetryStepSequence [] = LCD_TELEMETRY_STEP; static uint8_t telemetryStepIndex = 0; #endif // ****************** // rc functions // ****************** #define MINCHECK 1100 #define MAXCHECK 1900 #define ROL_LO (1<<(2*ROLL)) #define ROL_CE (3<<(2*ROLL)) #define ROL_HI (2<<(2*ROLL)) #define PIT_LO (1<<(2*PITCH)) #define PIT_CE (3<<(2*PITCH)) #define PIT_HI (2<<(2*PITCH)) #define YAW_LO (1<<(2*YAW)) #define YAW_CE (3<<(2*YAW)) #define YAW_HI (2<<(2*YAW)) #define THR_LO (1<<(2*THROTTLE)) #define THR_CE (3<<(2*THROTTLE)) #define THR_HI (2<<(2*THROTTLE)) static int16_t failsafeEvents = 0; volatile int16_t failsafeCnt = 0; static int16_t rcData[RC_CHANS]; // interval [1000;2000] static int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW static int16_t lookupPitchRollRC[6];// lookup table for expo & RC rate PITCH+ROLL static int16_t lookupThrottleRC[11];// lookup table for expo & mid THROTTLE static uint16_t rssi; // range: [0;1023] #if defined(SPEKTRUM) volatile uint8_t spekFrameFlags; volatile uint32_t spekTimeLast; #endif #if defined(OPENLRSv2MULTI) static uint8_t pot_P,pot_I; // OpenLRS onboard potentiometers for P and I trim or other usages #endif // ************** // gyro+acc IMU // ************** static int16_t gyroData[3] = {0,0,0}; static int16_t gyroZero[3] = {0,0,0}; static int16_t angle[2] = {0,0}; // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800 // ************************* // motor and servo functions // ************************* static int16_t axisPID[3]; static int16_t motor[NUMBER_MOTOR]; #if defined(SERVO) static int16_t servo[8] = {1500,1500,1500,1500,1500,1500,1500,1500}; #endif // ************************ // EEPROM Layout definition // ************************ static uint8_t dynP8[3], dynD8[3]; static struct { uint8_t currentSet; int16_t accZero[3]; int16_t magZero[3]; uint8_t checksum; // MUST BE ON LAST POSITION OF STRUCTURE ! } global_conf; static struct { uint8_t checkNewConf; uint8_t P8[PIDITEMS], I8[PIDITEMS], D8[PIDITEMS]; uint8_t rcRate8; uint8_t rcExpo8; uint8_t rollPitchRate; uint8_t yawRate; uint8_t dynThrPID; uint8_t thrMid8; uint8_t thrExpo8; int16_t angleTrim[2]; uint16_t activate[CHECKBOXITEMS]; uint8_t powerTrigger1; #ifdef FLYING_WING uint16_t wing_left_mid; uint16_t wing_right_mid; #endif #ifdef TRI uint16_t tri_yaw_middle; #endif #if defined HELICOPTER || defined(AIRPLANE)|| defined(SINGLECOPTER)|| defined(DUALCOPTER) int16_t servoTrim[8]; #endif #if defined(GYRO_SMOOTHING) uint8_t Smoothing[3]; #endif #if defined (FAILSAFE) int16_t failsafe_throttle; #endif #ifdef VBAT uint8_t vbatscale; uint8_t vbatlevel_warn1; uint8_t vbatlevel_warn2; uint8_t vbatlevel_crit; uint8_t no_vbat; #endif #ifdef POWERMETER uint16_t psensornull; uint16_t pleveldivsoft; uint16_t pleveldiv; uint8_t pint2ma; #endif #ifdef CYCLETIME_FIXATED uint16_t cycletime_fixated; #endif #ifdef MMGYRO uint8_t mmgyro; #endif #ifdef ARMEDTIMEWARNING uint16_t armedtimewarning; #endif int16_t minthrottle; #ifdef GOVERNOR_P int16_t governorP; int16_t governorD; int8_t governorR; #endif uint8_t checksum; // MUST BE ON LAST POSITION OF CONF STRUCTURE ! } conf; #ifdef LOG_PERMANENT static struct { uint16_t arm; // #arm events uint16_t disarm; // #disarm events uint16_t start; // #powercycle/reset/initialize events uint32_t armed_time ; // copy of armedTime @ disarm uint32_t lifetime; // sum (armed) lifetime in seconds uint16_t failsafe; // #failsafe state @ disarm uint16_t i2c; // #i2c errs state @ disarm uint8_t running; // toggle on arm & disarm to monitor for clean shutdown vs. powercut uint8_t checksum; // MUST BE ON LAST POSITION OF CONF STRUCTURE ! } plog; #endif // ********************** // GPS common variables // ********************** static int32_t GPS_coord[2]; static int32_t GPS_home[2]; static int32_t GPS_hold[2]; static uint8_t GPS_numSat; static uint16_t GPS_distanceToHome; // distance to home - unit: meter static int16_t GPS_directionToHome; // direction to home - unit: degree static uint16_t GPS_altitude; // GPS altitude - unit: meter static uint16_t GPS_speed; // GPS speed - unit: cm/s static uint8_t GPS_update = 0; // a binary toogle to distinct a GPS position update static int16_t GPS_angle[2] = { 0, 0}; // the angles that must be applied for GPS correction static uint16_t GPS_ground_course = 0; // - unit: degree*10 static uint8_t GPS_Present = 0; // Checksum from Gps serial static uint8_t GPS_Enable = 0; #define LAT 0 #define LON 1 // The desired bank towards North (Positive) or South (Negative) : latitude // The desired bank towards East (Positive) or West (Negative) : longitude static int16_t nav[2]; static int16_t nav_rated[2]; //Adding a rate controller to the navigation to make it smoother // default POSHOLD control gains #define POSHOLD_P .11 #define POSHOLD_I 0.0 #define POSHOLD_IMAX 20 // degrees #define POSHOLD_RATE_P 2.0 #define POSHOLD_RATE_I 0.08 // Wind control #define POSHOLD_RATE_D 0.045 // try 2 or 3 for POSHOLD_RATE 1 #define POSHOLD_RATE_IMAX 20 // degrees // default Navigation PID gains #define NAV_P 1.4 #define NAV_I 0.20 // Wind control #define NAV_D 0.08 // #define NAV_IMAX 20 // degrees ///////////////////////////////////////////////////////////////////////////////////////////////////////////// // Serial GPS only variables //navigation mode #define NAV_MODE_NONE 0 #define NAV_MODE_POSHOLD 1 #define NAV_MODE_WP 2 static uint8_t nav_mode = NAV_MODE_NONE; // Navigation mode static uint8_t alarmArray[16]; // array #if BARO static int32_t baroPressure; static int32_t baroTemperature; static int32_t baroPressureSum; #endif void annexCode() { // this code is excetuted at each loop and won't interfere with control loop if it lasts less than 650 microseconds static uint32_t calibratedAccTime; uint16_t tmp,tmp2; uint8_t axis,prop1,prop2; #define BREAKPOINT 1500 // PITCH & ROLL only dynamic PID adjustemnt, depending on throttle value if (rcData[THROTTLE]<BREAKPOINT) { prop2 = 100; } else { if (rcData[THROTTLE]<2000) { prop2 = 100 - (uint16_t)conf.dynThrPID*(rcData[THROTTLE]-BREAKPOINT)/(2000-BREAKPOINT); } else { prop2 = 100 - conf.dynThrPID; } } for(axis=0;axis<3;axis++) { tmp = min(abs(rcData[axis]-MIDRC),500); #if defined(DEADBAND) if (tmp>DEADBAND) { tmp -= DEADBAND; } else { tmp=0; } #endif if(axis!=2) { //ROLL & PITCH tmp2 = tmp/100; rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp-tmp2*100) * (lookupPitchRollRC[tmp2+1]-lookupPitchRollRC[tmp2]) / 100; prop1 = 100-(uint16_t)conf.rollPitchRate*tmp/500; prop1 = (uint16_t)prop1*prop2/100; } else { // YAW rcCommand[axis] = tmp; prop1 = 100-(uint16_t)conf.yawRate*tmp/500; } dynP8[axis] = (uint16_t)conf.P8[axis]*prop1/100; dynD8[axis] = (uint16_t)conf.D8[axis]*prop1/100; if (rcData[axis]<MIDRC) rcCommand[axis] = -rcCommand[axis]; } tmp = constrain(rcData[THROTTLE],MINCHECK,2000); tmp = (uint32_t)(tmp-MINCHECK)*1000/(2000-MINCHECK); // [MINCHECK;2000] -> [0;1000] tmp2 = tmp/100; rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp-tmp2*100) * (lookupThrottleRC[tmp2+1]-lookupThrottleRC[tmp2]) / 100; // [0;1000] -> expo -> [conf.minthrottle;MAXTHROTTLE] if(f.HEADFREE_MODE) { //to optimize float radDiff = (heading - headFreeModeHold) * 0.0174533f; // where PI/180 ~= 0.0174533 float cosDiff = cos(radDiff); float sinDiff = sin(radDiff); int16_t rcCommand_PITCH = rcCommand[PITCH]*cosDiff + rcCommand[ROLL]*sinDiff; rcCommand[ROLL] = rcCommand[ROLL]*cosDiff - rcCommand[PITCH]*sinDiff; rcCommand[PITCH] = rcCommand_PITCH; } #if defined(POWERMETER_HARD) uint16_t pMeterRaw; // used for current reading static uint16_t psensorTimer = 0; if (! (++psensorTimer % PSENSORFREQ)) { pMeterRaw = analogRead(PSENSORPIN); //lcdprint_int16(pMeterRaw); LCDcrlf(); powerValue = ( conf.psensornull > pMeterRaw ? conf.psensornull - pMeterRaw : pMeterRaw - conf.psensornull); // do not use abs(), it would induce implicit cast to uint and overrun if ( powerValue < 333) { // only accept reasonable values. 333 is empirical #ifdef LCD_TELEMETRY if (powerValue > powerMax) powerMax = powerValue; #endif } else { powerValue = 333; } pMeter[PMOTOR_SUM] += (uint32_t) powerValue; } #endif #if defined(BUZZER) #if defined(VBAT) static uint8_t vbatTimer = 0; static uint8_t ind = 0; uint16_t vbatRaw = 0; static uint16_t vbatRawArray[8]; if (! (++vbatTimer % VBATFREQ)) { vbatRawArray[(ind++)%8] = analogRead(V_BATPIN); for (uint8_t i=0;i<8;i++) vbatRaw += vbatRawArray[i]; vbat = (vbatRaw*2) / conf.vbatscale; // result is Vbatt in 0.1V steps } #endif alarmHandler(); // external buzzer routine that handles buzzer events globally now #endif #if defined(RX_RSSI) static uint8_t sig = 0; uint16_t rssiRaw = 0; static uint16_t rssiRawArray[8]; rssiRawArray[(sig++)%8] = analogRead(RX_RSSI_PIN); for (uint8_t i=0;i<8;i++) rssiRaw += rssiRawArray[i]; rssi = rssiRaw / 8; #endif if ( (calibratingA>0 && ACC ) || (calibratingG>0) ) { // Calibration phasis LEDPIN_TOGGLE; } else { if (f.ACC_CALIBRATED) {LEDPIN_OFF;} if (f.ARMED) {LEDPIN_ON;} } #if defined(LED_RING) static uint32_t LEDTime; if ( currentTime > LEDTime ) { LEDTime = currentTime + 50000; i2CLedRingState(); } #endif #if defined(LED_FLASHER) auto_switch_led_flasher(); #endif if ( currentTime > calibratedAccTime ) { if (! f.SMALL_ANGLES_25) { // the multi uses ACC and is not calibrated or is too much inclinated f.ACC_CALIBRATED = 0; LEDPIN_TOGGLE; calibratedAccTime = currentTime + 100000; } else { f.ACC_CALIBRATED = 1; } } #if !(defined(SPEKTRUM) && defined(PROMINI)) //Only one serial port on ProMini. Skip serial com if Spektrum Sat in use. Note: Spek code will auto-call serialCom if GUI data detected on serial0. #if defined(GPS_PROMINI) if(GPS_Enable == 0) {serialCom();} #else serialCom(); #endif #endif #if defined(POWERMETER) intPowerMeterSum = (pMeter[PMOTOR_SUM]/conf.pleveldiv); intPowerTrigger1 = conf.powerTrigger1 * PLEVELSCALE; #endif #ifdef LCD_TELEMETRY_AUTO static char telemetryAutoSequence [] = LCD_TELEMETRY_AUTO; static uint8_t telemetryAutoIndex = 0; static uint16_t telemetryAutoTimer = 0; if ( (telemetry_auto) && (! (++telemetryAutoTimer % LCD_TELEMETRY_AUTO_FREQ) ) ){ telemetry = telemetryAutoSequence[++telemetryAutoIndex % strlen(telemetryAutoSequence)]; LCDclear(); // make sure to clear away remnants } #endif #ifdef LCD_TELEMETRY static uint16_t telemetryTimer = 0; if (! (++telemetryTimer % LCD_TELEMETRY_FREQ)) { #if (LCD_TELEMETRY_DEBUG+0 > 0) telemetry = LCD_TELEMETRY_DEBUG; #endif if (telemetry) lcd_telemetry(); } #endif #if GPS & defined(GPS_LED_INDICATOR) // modified by MIS to use STABLEPIN LED for number of sattelites indication static uint32_t GPSLEDTime; // - No GPS FIX -> LED blink at speed of incoming GPS frames static uint8_t blcnt; // - Fix and sat no. bellow 5 -> LED off if(currentTime > GPSLEDTime) { // - Fix and sat no. >= 5 -> LED blinks, one blink for 5 sat, two blinks for 6 sat, three for 7 ... if(f.GPS_FIX && GPS_numSat >= 5) { if(++blcnt > 2*GPS_numSat) blcnt = 0; GPSLEDTime = currentTime + 150000; if(blcnt >= 10 && ((blcnt%2) == 0)) {STABLEPIN_ON;} else {STABLEPIN_OFF;} }else{ if((GPS_update == 1) && !f.GPS_FIX) {STABLEPIN_ON;} else {STABLEPIN_OFF;} blcnt = 0; } } #endif #if defined(LOG_VALUES) && (LOG_VALUES >= 2) if (cycleTime > cycleTimeMax) cycleTimeMax = cycleTime; // remember highscore if (cycleTime < cycleTimeMin) cycleTimeMin = cycleTime; // remember lowscore #endif if (f.ARMED) { #if defined(LCD_TELEMETRY) || defined(ARMEDTIMEWARNING) || defined(LOG_PERMANENT) armedTime += (uint32_t)cycleTime; #endif #if defined(VBAT) if ( (vbat > conf.no_vbat) && (vbat < vbatMin) ) vbatMin = vbat; #endif #ifdef LCD_TELEMETRY #if BARO if ( (BaroAlt > BAROaltMax) ) BAROaltMax = BaroAlt; #endif #endif } } void setup() { #if !defined(GPS_PROMINI) SerialOpen(0,SERIAL0_COM_SPEED); #if defined(PROMICRO) SerialOpen(1,SERIAL1_COM_SPEED); #endif #if defined(MEGA) SerialOpen(1,SERIAL1_COM_SPEED); SerialOpen(2,SERIAL2_COM_SPEED); SerialOpen(3,SERIAL3_COM_SPEED); #endif #endif LEDPIN_PINMODE; POWERPIN_PINMODE; BUZZERPIN_PINMODE; STABLEPIN_PINMODE; POWERPIN_OFF; initOutput(); #ifdef MULTIPLE_CONFIGURATION_PROFILES for(global_conf.currentSet=0; global_conf.currentSet<3; global_conf.currentSet++) { // check all settings integrity readEEPROM(); } #else global_conf.currentSet=0; readEEPROM(); #endif readGlobalSet(); readEEPROM(); // load current setting data blinkLED(2,40,global_conf.currentSet+1); configureReceiver(); #if defined (PILOTLAMP) PL_INIT; #endif #if defined(OPENLRSv2MULTI) initOpenLRS(); #endif initSensors(); #if defined(I2C_GPS) || defined(GPS_SERIAL) || defined(GPS_FROM_OSD) GPS_set_pids(); #endif previousTime = micros(); #if defined(GIMBAL) calibratingA = 512; #endif calibratingG = 512; calibratingB = 200; // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles #if defined(POWERMETER) for(uint8_t i=0;i<=PMOTOR_SUM;i++) pMeter[i]=0; #endif /************************************/ #if defined(GPS_SERIAL) GPS_SerialInit(); for(uint8_t i=0;i<=5;i++){ GPS_NewData(); LEDPIN_ON delay(20); LEDPIN_OFF delay(80); } if(!GPS_Present){ SerialEnd(GPS_SERIAL); SerialOpen(0,SERIAL0_COM_SPEED); } #if !defined(GPS_PROMINI) GPS_Present = 1; #endif GPS_Enable = GPS_Present; #endif /************************************/ #if defined(I2C_GPS) || defined(TINY_GPS) || defined(GPS_FROM_OSD) GPS_Enable = 1; #endif #if defined(LCD_ETPP) || defined(LCD_LCD03) || defined(OLED_I2C_128x64) || defined(LCD_TELEMETRY_STEP) initLCD(); #endif #ifdef LCD_TELEMETRY_DEBUG telemetry_auto = 1; #endif #ifdef LCD_CONF_DEBUG configurationLoop(); #endif #ifdef LANDING_LIGHTS_DDR init_landing_lights(); #endif ADCSRA |= _BV(ADPS2) ; ADCSRA &= ~_BV(ADPS1); ADCSRA &= ~_BV(ADPS0); // this speeds up analogRead without loosing too much resolution: http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1208715493/11 #if defined(LED_FLASHER) init_led_flasher(); led_flasher_set_sequence(LED_FLASHER_SEQUENCE); #endif f.SMALL_ANGLES_25=1; // important for gyro only conf #ifdef LOG_PERMANENT // read last stored set readPLog(); plog.lifetime += plog.armed_time / 1000000; plog.start++; // #powercycle/reset/initialize events // dump plog data to terminal #ifdef LOG_PERMANENT_SHOW_AT_STARTUP dumpPLog(0); #endif plog.armed_time = 0; // lifetime in seconds //plog.running = 0; // toggle on arm & disarm to monitor for clean shutdown vs. powercut #endif debugmsg_append_str("initialization completed\n"); } void go_arm() { if(calibratingG == 0 && f.ACC_CALIBRATED #if defined(FAILSAFE) && failsafeCnt < 2 #endif ) { if(!f.ARMED) { // arm now! f.ARMED = 1; headFreeModeHold = heading; #if defined(VBAT) if (vbat > conf.no_vbat) vbatMin = vbat; #endif #ifdef LCD_TELEMETRY // reset some values when arming #if BARO BAROaltMax = BaroAlt; #endif #endif #ifdef LOG_PERMANENT plog.arm++; // #arm events plog.running = 1; // toggle on arm & disarm to monitor for clean shutdown vs. powercut // write now. writePLog(); #endif } } else if(!f.ARMED) { blinkLED(2,255,1); alarmArray[8] = 1; } } void go_disarm() { if (f.ARMED) { f.ARMED = 0; #ifdef LOG_PERMANENT plog.disarm++; // #disarm events plog.armed_time = armedTime ; // lifetime in seconds if (failsafeEvents) plog.failsafe++; // #acitve failsafe @ disarm if (i2c_errors_count > 10) plog.i2c++; // #i2c errs @ disarm plog.running = 0; // toggle @ arm & disarm to monitor for clean shutdown vs. powercut // write now. writePLog(); #endif } } void servos2Neutral() { #ifdef TRI servo[5] = 1500; // we center the yaw servo in conf mode writeServos(); #endif #ifdef FLYING_WING servo[0] = conf.wing_left_mid; servo[1] = conf.wing_right_mid; writeServos(); #endif #ifdef AIRPLANE for(uint8_t i = 4; i<7 ;i++) servo[i] = 1500; writeServos(); #endif #ifdef HELICOPTER servo[5] = YAW_CENTER; servo[3] = servo[4] = servo[6] = 1500; writeServos(); #endif } // ******** Main Loop ********* void loop () { static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors static uint8_t rcSticks; // this hold sticks position for command combos uint8_t axis,i; int16_t error,errorAngle; int16_t delta,deltaSum; int16_t PTerm,ITerm,DTerm; int16_t PTermACC = 0 , ITermACC = 0 , PTermGYRO = 0 , ITermGYRO = 0; static int16_t lastGyro[3] = {0,0,0}; static int16_t delta1[3],delta2[3]; static int16_t errorGyroI[3] = {0,0,0}; static int16_t errorAngleI[2] = {0,0}; static uint32_t rcTime = 0; static int16_t initialThrottleHold; static uint32_t timestamp_fixated = 0; #if defined(SPEKTRUM) if (spekFrameFlags == 0x01) readSpektrum(); #endif #if defined(OPENLRSv2MULTI) Read_OpenLRS_RC(); #endif if (currentTime > rcTime ) { // 50Hz rcTime = currentTime + 20000; computeRC(); // Failsafe routine - added by MIS #if defined(FAILSAFE) if ( failsafeCnt > (5*FAILSAFE_DELAY) && f.ARMED) { // Stabilize, and set Throttle to specified level for(i=0; i<3; i++) rcData[i] = MIDRC; // after specified guard time after RC signal is lost (in 0.1sec) rcData[THROTTLE] = conf.failsafe_throttle; if (failsafeCnt > 5*(FAILSAFE_DELAY+FAILSAFE_OFF_DELAY)) { // Turn OFF motors after specified Time (in 0.1sec) go_disarm(); // This will prevent the copter to automatically rearm if failsafe shuts it down and prevents f.OK_TO_ARM = 0; // to restart accidentely by just reconnect to the tx - you will have to switch off first to rearm } failsafeEvents++; } if ( failsafeCnt > (5*FAILSAFE_DELAY) && !f.ARMED) { //Turn of "Ok To arm to prevent the motors from spinning after repowering the RX with low throttle and aux to arm go_disarm(); // This will prevent the copter to automatically rearm if failsafe shuts it down and prevents f.OK_TO_ARM = 0; // to restart accidentely by just reconnect to the tx - you will have to switch off first to rearm } failsafeCnt++; #endif // end of failsafe routine - next change is made with RcOptions setting // ------------------ STICKS COMMAND HANDLER -------------------- // checking sticks positions uint8_t stTmp = 0; for(i=0;i<4;i++) { stTmp >>= 2; if(rcData[i] > MINCHECK) stTmp |= 0x80; // check for MIN if(rcData[i] < MAXCHECK) stTmp |= 0x40; // check for MAX } if(stTmp == rcSticks) { if(rcDelayCommand<250) rcDelayCommand++; } else rcDelayCommand = 0; rcSticks = stTmp; // perform actions if (rcData[THROTTLE] <= MINCHECK) { // THROTTLE at minimum errorGyroI[ROLL] = 0; errorGyroI[PITCH] = 0; errorGyroI[YAW] = 0; errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0; if (conf.activate[BOXARM] > 0) { // Arming/Disarming via ARM BOX if ( rcOptions[BOXARM] && f.OK_TO_ARM ) go_arm(); else if (f.ARMED) go_disarm(); } } if(rcDelayCommand == 20) { if(f.ARMED) { // actions during armed #ifdef ALLOW_ARM_DISARM_VIA_TX_YAW if (conf.activate[BOXARM] == 0 && rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE) go_disarm(); // Disarm via YAW #endif #ifdef ALLOW_ARM_DISARM_VIA_TX_ROLL if (conf.activate[BOXARM] == 0 && rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_LO) go_disarm(); // Disarm via ROLL #endif } else { // actions during not armed i=0; if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) { // GYRO calibration calibratingG=512; #if GPS GPS_reset_home_position(); #endif #if BARO calibratingB=10; // calibrate baro to new ground level (10 * 25 ms = ~250 ms non blocking) #endif } #if defined(INFLIGHT_ACC_CALIBRATION) else if (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_HI) { // Inflight ACC calibration START/STOP if (AccInflightCalibrationMeasurementDone){ // trigger saving into eeprom after landing AccInflightCalibrationMeasurementDone = 0; AccInflightCalibrationSavetoEEProm = 1; }else{ AccInflightCalibrationArmed = !AccInflightCalibrationArmed; #if defined(BUZZER) if (AccInflightCalibrationArmed) alarmArray[0]=2; else alarmArray[0]=3; #endif } } #endif #ifdef MULTIPLE_CONFIGURATION_PROFILES if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_LO) i=1; // ROLL left -> Profile 1 else if (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_CE) i=2; // PITCH up -> Profile 2 else if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_HI) i=3; // ROLL right -> Profile 3 if(i) { global_conf.currentSet = i-1; writeGlobalSet(0); readEEPROM(); blinkLED(2,40,i); alarmArray[0] = i; } #endif if (rcSticks == THR_LO + YAW_HI + PIT_HI + ROL_CE) { // Enter LCD config #if defined(LCD_CONF) configurationLoop(); // beginning LCD configuration #endif previousTime = micros(); } #ifdef ALLOW_ARM_DISARM_VIA_TX_YAW else if (conf.activate[BOXARM] == 0 && rcSticks == THR_LO + YAW_HI + PIT_CE + ROL_CE) go_arm(); // Arm via YAW #endif #ifdef ALLOW_ARM_DISARM_VIA_TX_ROLL else if (conf.activate[BOXARM] == 0 && rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_HI) go_arm(); // Arm via ROLL #endif #ifdef LCD_TELEMETRY_AUTO else if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_LO) { // Auto telemetry ON/OFF if (telemetry_auto) { telemetry_auto = 0; telemetry = 0; } else telemetry_auto = 1; } #endif #ifdef LCD_TELEMETRY_STEP else if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_HI) { // Telemetry next step telemetry = telemetryStepSequence[++telemetryStepIndex % strlen(telemetryStepSequence)]; #ifdef OLED_I2C_128x64 if (telemetry != 0) i2c_OLED_init(); #endif LCDclear(); } #endif else if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE) calibratingA=512; // throttle=max, yaw=left, pitch=min else if (rcSticks == THR_HI + YAW_HI + PIT_LO + ROL_CE) f.CALIBRATE_MAG = 1; // throttle=max, yaw=right, pitch=min i=0; if (rcSticks == THR_HI + YAW_CE + PIT_HI + ROL_CE) {conf.angleTrim[PITCH]+=2; i=1;} else if (rcSticks == THR_HI + YAW_CE + PIT_LO + ROL_CE) {conf.angleTrim[PITCH]-=2; i=1;} else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_HI) {conf.angleTrim[ROLL] +=2; i=1;} else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_LO) {conf.angleTrim[ROLL] -=2; i=1;} if (i) { writeParams(1); rcDelayCommand = 0; // allow autorepetition #if defined(LED_RING) blinkLedRing(); #endif } } } #if defined(LED_FLASHER) led_flasher_autoselect_sequence(); #endif #if defined(INFLIGHT_ACC_CALIBRATION) if (AccInflightCalibrationArmed && f.ARMED && rcData[THROTTLE] > MINCHECK && !rcOptions[BOXARM] ){ // Copter is airborne and you are turning it off via boxarm : start measurement InflightcalibratingA = 50; AccInflightCalibrationArmed = 0; } if (rcOptions[BOXCALIB]) { // Use the Calib Option to activate : Calib = TRUE Meausrement started, Land and Calib = 0 measurement stored if (!AccInflightCalibrationActive && !AccInflightCalibrationMeasurementDone){ InflightcalibratingA = 50; } }else if(AccInflightCalibrationMeasurementDone && !f.ARMED){ AccInflightCalibrationMeasurementDone = 0; AccInflightCalibrationSavetoEEProm = 1; } #endif uint16_t auxState = 0; for(i=0;i<4;i++) auxState |= (rcData[AUX1+i]<1300)<<(3*i) | (1300<rcData[AUX1+i] && rcData[AUX1+i]<1700)<<(3*i+1) | (rcData[AUX1+i]>1700)<<(3*i+2); for(i=0;i<CHECKBOXITEMS;i++) rcOptions[i] = (auxState & conf.activate[i])>0; // note: if FAILSAFE is disable, failsafeCnt > 5*FAILSAFE_DELAY is always false #if ACC if ( rcOptions[BOXANGLE] || (failsafeCnt > 5*FAILSAFE_DELAY) ) { // bumpless transfer to Level mode if (!f.ANGLE_MODE) { errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0; f.ANGLE_MODE = 1; } } else { // failsafe support f.ANGLE_MODE = 0; } if ( rcOptions[BOXHORIZON] ) { f.ANGLE_MODE = 0; if (!f.HORIZON_MODE) { errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0; f.HORIZON_MODE = 1; } } else { f.HORIZON_MODE = 0; } #endif if (rcOptions[BOXARM] == 0) f.OK_TO_ARM = 1; #if !defined(GPS_LED_INDICATOR) if (f.ANGLE_MODE || f.HORIZON_MODE) {STABLEPIN_ON;} else {STABLEPIN_OFF;} #endif #if BARO #if (!defined(SUPPRESS_BARO_ALTHOLD)) if (rcOptions[BOXBARO]) { if (!f.BARO_MODE) { f.BARO_MODE = 1; AltHold = EstAlt; initialThrottleHold = rcCommand[THROTTLE]; errorAltitudeI = 0; BaroPID=0; } } else { f.BARO_MODE = 0; } #endif #ifdef VARIOMETER if (rcOptions[BOXVARIO]) { if (!f.VARIO_MODE) { f.VARIO_MODE = 1; } } else { f.VARIO_MODE = 0; } #endif #endif #if MAG if (rcOptions[BOXMAG]) { if (!f.MAG_MODE) { f.MAG_MODE = 1; magHold = heading; } } else { f.MAG_MODE = 0; } if (rcOptions[BOXHEADFREE]) { if (!f.HEADFREE_MODE) { f.HEADFREE_MODE = 1; } } else { f.HEADFREE_MODE = 0; } if (rcOptions[BOXHEADADJ]) { headFreeModeHold = heading; // acquire new heading } #endif #if GPS static uint8_t GPSNavReset = 1; if (f.GPS_FIX && GPS_numSat >= 5 ) { if (rcOptions[BOXGPSHOME]) { // if both GPS_HOME & GPS_HOLD are checked => GPS_HOME is the priority if (!f.GPS_HOME_MODE) { f.GPS_HOME_MODE = 1; f.GPS_HOLD_MODE = 0; GPSNavReset = 0; #if defined(I2C_GPS) GPS_I2C_command(I2C_GPS_COMMAND_START_NAV,0); //waypoint zero #else // SERIAL GPS_set_next_wp(&GPS_home[LAT],&GPS_home[LON]); nav_mode = NAV_MODE_WP; #endif } } else { f.GPS_HOME_MODE = 0; if (rcOptions[BOXGPSHOLD] && abs(rcCommand[ROLL])< AP_MODE && abs(rcCommand[PITCH]) < AP_MODE) { if (!f.GPS_HOLD_MODE) { f.GPS_HOLD_MODE = 1; GPSNavReset = 0; #if defined(I2C_GPS) GPS_I2C_command(I2C_GPS_COMMAND_POSHOLD,0); #else GPS_hold[LAT] = GPS_coord[LAT]; GPS_hold[LON] = GPS_coord[LON]; GPS_set_next_wp(&GPS_hold[LAT],&GPS_hold[LON]); nav_mode = NAV_MODE_POSHOLD; #endif } } else { f.GPS_HOLD_MODE = 0; // both boxes are unselected here, nav is reset if not already done if (GPSNavReset == 0 ) { GPSNavReset = 1; GPS_reset_nav(); } } } } else { f.GPS_HOME_MODE = 0; f.GPS_HOLD_MODE = 0; #if !defined(I2C_GPS) nav_mode = NAV_MODE_NONE; #endif } #endif #if defined(FIXEDWING) || defined(HELICOPTER) if (rcOptions[BOXPASSTHRU]) {f.PASSTHRU_MODE = 1;} else {f.PASSTHRU_MODE = 0;} #endif } else { // not in rc loop static uint8_t taskOrder=0; // never call all functions in the same loop, to avoid high delay spikes if(taskOrder>4) taskOrder-=5; switch (taskOrder) { case 0: taskOrder++; #if MAG if (Mag_getADC()) break; // max 350 µs (HMC5883) // only break when we actually did something #endif case 1: taskOrder++; #if BARO if (Baro_update() != 0 ) break; #endif case 2: taskOrder++; #if BARO if (getEstimatedAltitude() !=0) break; #endif case 3: taskOrder++; #if GPS if(GPS_Enable) GPS_NewData(); break; #endif case 4: taskOrder++; #if SONAR Sonar_update();debug[2] = sonarAlt; #endif #ifdef LANDING_LIGHTS_DDR auto_switch_landing_lights(); #endif #ifdef VARIOMETER if (f.VARIO_MODE) vario_signaling(); #endif break; } } computeIMU(); // Measure loop rate just afer reading the sensors currentTime = micros(); cycleTime = currentTime - previousTime; previousTime = currentTime; #ifdef CYCLETIME_FIXATED if (conf.cycletime_fixated) { if ((micros()-timestamp_fixated)>conf.cycletime_fixated) { } else { while((micros()-timestamp_fixated)<conf.cycletime_fixated) ; // waste away } timestamp_fixated=micros(); } #endif //*********************************** //**** Experimental FlightModes ***** //*********************************** #if defined(ACROTRAINER_MODE) if(f.ANGLE_MODE){ if (abs(rcCommand[ROLL]) + abs(rcCommand[PITCH]) >= ACROTRAINER_MODE ) { f.ANGLE_MODE=0; f.HORIZON_MODE=0; f.MAG_MODE=0; f.BARO_MODE=0; f.GPS_HOME_MODE=0; f.GPS_HOLD_MODE=0; } } #endif //*********************************** #if MAG if (abs(rcCommand[YAW]) <70 && f.MAG_MODE) { int16_t dif = heading - magHold; if (dif <= - 180) dif += 360; if (dif >= + 180) dif -= 360; if ( f.SMALL_ANGLES_25 ) rcCommand[YAW] -= dif*conf.P8[PIDMAG]>>5; } else magHold = heading; #endif #if BARO && (!defined(SUPPRESS_BARO_ALTHOLD)) if (f.BARO_MODE) { static uint8_t isAltHoldChanged = 0; #if defined(ALTHOLD_FAST_THROTTLE_CHANGE) if (abs(rcCommand[THROTTLE]-initialThrottleHold) > ALT_HOLD_THROTTLE_NEUTRAL_ZONE) { errorAltitudeI = 0; isAltHoldChanged = 1; rcCommand[THROTTLE] += (rcCommand[THROTTLE] > initialThrottleHold) ? -ALT_HOLD_THROTTLE_NEUTRAL_ZONE : ALT_HOLD_THROTTLE_NEUTRAL_ZONE; } else { if (isAltHoldChanged) { AltHold = EstAlt; isAltHoldChanged = 0; } rcCommand[THROTTLE] = initialThrottleHold + BaroPID; } #else static int16_t AltHoldCorr = 0; if (abs(rcCommand[THROTTLE]-initialThrottleHold)>ALT_HOLD_THROTTLE_NEUTRAL_ZONE) { // Slowly increase/decrease AltHold proportional to stick movement ( +100 throttle gives ~ +50 cm in 1 second with cycle time about 3-4ms) AltHoldCorr+= rcCommand[THROTTLE] - initialThrottleHold; if(abs(AltHoldCorr) > 500) { AltHold += AltHoldCorr/500; AltHoldCorr %= 500; } errorAltitudeI = 0; isAltHoldChanged = 1; } else if (isAltHoldChanged) { AltHold = EstAlt; isAltHoldChanged = 0; } rcCommand[THROTTLE] = initialThrottleHold + BaroPID; #endif } #endif #if GPS if ( (f.GPS_HOME_MODE || f.GPS_HOLD_MODE) && f.GPS_FIX_HOME ) { float sin_yaw_y = sin(heading*0.0174532925f); float cos_yaw_x = cos(heading*0.0174532925f); #if defined(NAV_SLEW_RATE) nav_rated[LON] += constrain(wrap_18000(nav[LON]-nav_rated[LON]),-NAV_SLEW_RATE,NAV_SLEW_RATE); nav_rated[LAT] += constrain(wrap_18000(nav[LAT]-nav_rated[LAT]),-NAV_SLEW_RATE,NAV_SLEW_RATE); GPS_angle[ROLL] = (nav_rated[LON]*cos_yaw_x - nav_rated[LAT]*sin_yaw_y) /10; GPS_angle[PITCH] = (nav_rated[LON]*sin_yaw_y + nav_rated[LAT]*cos_yaw_x) /10; #else GPS_angle[ROLL] = (nav[LON]*cos_yaw_x - nav[LAT]*sin_yaw_y) /10; GPS_angle[PITCH] = (nav[LON]*sin_yaw_y + nav[LAT]*cos_yaw_x) /10; #endif } else { GPS_angle[ROLL] = 0; GPS_angle[PITCH] = 0; } #endif //**** PITCH & ROLL & YAW PID **** int16_t prop; prop = min(max(abs(rcCommand[PITCH]),abs(rcCommand[ROLL])),500); // range [0;500] for(axis=0;axis<3;axis++) { if ((f.ANGLE_MODE || f.HORIZON_MODE) && axis<2 ) { // MODE relying on ACC // 50 degrees max inclination errorAngle = constrain((rcCommand[axis]<<1) + GPS_angle[axis],-500,+500) - angle[axis] + conf.angleTrim[axis]; //16 bits is ok here PTermACC = ((int32_t)errorAngle*conf.P8[PIDLEVEL])>>7; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result PTermACC = constrain(PTermACC,-conf.D8[PIDLEVEL]*5,+conf.D8[PIDLEVEL]*5); errorAngleI[axis] = constrain(errorAngleI[axis]+errorAngle,-10000,+10000); // WindUp //16 bits is ok here ITermACC = ((int32_t)errorAngleI[axis]*conf.I8[PIDLEVEL])>>12; // 32 bits is needed for calculation:10000*I8 could exceed 32768 16 bits is ok for result } if ( !f.ANGLE_MODE || f.HORIZON_MODE || axis == 2 ) { // MODE relying on GYRO or YAW axis if (abs(rcCommand[axis])<500) error = (rcCommand[axis]<<6)/conf.P8[axis] ; // 16 bits is needed for calculation: 500*64 = 32000 16 bits is ok for result if P8>5 (P>0.5) else error = ((int32_t)rcCommand[axis]<<6)/conf.P8[axis] ; // 32 bits is needed for calculation error -= gyroData[axis]; PTermGYRO = rcCommand[axis]; errorGyroI[axis] = constrain(errorGyroI[axis]+error,-16000,+16000); // WindUp 16 bits is ok here if (abs(gyroData[axis])>640) errorGyroI[axis] = 0; ITermGYRO = ((errorGyroI[axis]>>7)*conf.I8[axis])>>6; // 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000 } if ( f.HORIZON_MODE && axis<2) { PTerm = ((int32_t)PTermACC*(512-prop) + (int32_t)PTermGYRO*prop)>>9; // the real factor should be 500, but 512 is ok ITerm = ((int32_t)ITermACC*(512-prop) + (int32_t)ITermGYRO*prop)>>9; } else { if ( f.ANGLE_MODE && axis<2) { PTerm = PTermACC; ITerm = ITermACC; } else { PTerm = PTermGYRO; ITerm = ITermGYRO; } } PTerm -= ((int32_t)gyroData[axis]*dynP8[axis])>>6; // 32 bits is needed for calculation delta = gyroData[axis] - lastGyro[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 lastGyro[axis] = gyroData[axis]; deltaSum = delta1[axis]+delta2[axis]+delta; delta2[axis] = delta1[axis]; delta1[axis] = delta; DTerm = ((int32_t)deltaSum*dynD8[axis])>>5; // 32 bits is needed for calculation axisPID[axis] = PTerm + ITerm - DTerm; } mixTable(); writeServos(); writeMotors(); }


Pin Function

Example.jpg

Addressing

EachPCF8591deviceinanI 2 C-bussystemisactivatedby sending a valid address to the device. The address consists of a fixed part and a programmable part. The programmable part must be set according to the address pins A0, A1 and A2. The address always has to be sent as the first byte after the start condition in the I 2 C-bus protocol. The last bit of the address byte is the read/write-bit which sets the direction of the following data transfer (see Figs 4).

  • Figs 4

Control byte

The second byte sent to a PCF8591 device will be stored in its control register and is required to control the device function.Theuppernibbleofthecontrolregisterisusedfor enabling the analog output, and for programming the analog inputs as single-ended or differential inputs. The lower nibble selects one of the analog input channels defined by the upper nibble (see Fig.5). If the auto-increment flag is set, the channel number is incremented automatically after each A/D conversion.
If the auto-increment mode is desired in applications where the internal oscillator is used, the analog output enable flag in the control byte (bit 6) should be set. This allows the internal oscillator to run continuously, thereby preventing conversion errors resulting from oscillator start-up delay. The analog output enable flag may be reset at other times to reduce quiescent power consumption. The selection of a non-existing input channel results in the highest available channel number being allocated. Therefore, if the auto-increment flag is set, the next selected channel will be always channel 0. The most significant bits of both nibbles are reserved for future functions and have to be set to logic 0. After a Power-on reset condition all bits of the control register are reset to logic 0. The D/A converter and the oscillator are disabled for power saving. The analog output is switched to a high-impedance state.

  • Fig.5
    • D/A conversion

The third byte sent to a PCF8591 device is stored in the DAC data register and is converted to the corresponding analog voltage using the on-chip D/A converter. This D/A converter consists of a resistor divider chain connected to the external reference voltage with 256 taps and selection switches. The tap-decoder switches one of these taps to the DAC output line (see Fig.6). The analog output voltage is buffered by an auto-zeroed unity gain amplifier. This buffer amplifier may be switched on or off by setting the analog output enable flag of thecontrol register. In the active state the output voltage is held until a further data byte is sent. The on-chip D/A converter is also used for successive approximation A/D conversion. In order to release the DAC for an A/D conversion cycle the unity gain amplifier is equippedwithatrackandholdcircuit.Thiscircuitholdsthe output voltage while executing the A/D conversion. The output voltage supplied to the analog output AOUT is given by the formula shown in Fig.7. The waveforms of a D/A conversion sequence are shown in Fig.8.

  • Fig.6
    • Fig.7
      • Fig.8

The A/D converter makes use of the successive approximation conversion technique. The on-chip D/A converter and a high-gain comparator are used temporarily during an A/D conversion cycle. An A/D conversion cycle is always started after sending a valid read mode address to a PCF8591 device. The A/D conversion cycle is triggered at the trailing edge of the acknowledge clock pulse and is executed while transmitting the result of the previous conversion (see Fig.9). Once a conversion cycle is triggered an input voltage sample of the selected channel is stored on the chip and is converted to the corresponding 8-bit binary code.Samples picked up from differential inputs are converted to an 8-bit twos complement code (see Figs 10 and 11). The conversion result is stored in the ADC data register and awaits transmission. If the auto-increment flag is set the next channel is selected. The first byte transmitted in a read cycle contains the conversion result code of the previous read cycle. After a Power-on reset condition the first byte read is a hexadecimal 80. The maximum A/D conversion rate is given by the actual speed of the I 2 C-bus.

  1. Fig.9
    • Fig.10
    • Fig.11
Reference voltage
For the D/A and A/D conversion either a stable external voltage reference or the supply voltage has to be applied to the resistor divider chain (pins V REF and AGND). The AGND pin has to be connected to the system analog ground and may have a DC off-set with reference to V SS . A low frequency may be applied to the V REF and AGND pins. This allows the use of the D/A converter as a one-quadrant multiplier; see Chapter 15 and Fig.7. The A/D converter may also be used as a one or two quadrant analog divider. The analog input voltage is divided by the reference voltage. The result is converted to a binary code. In this application the user has to keep the reference voltage stable during the conversion cycle.

Oscillator

An on-chip oscillator generates the clock signal required for the A/D conversion cycle and for refreshing the auto-zeroed buffer amplifier. When using this oscillator the EXT pin has to be connected to V SS . At the OSC pin the oscillator frequency is available. If the EXT pin is connected to V DD the oscillator output OSC is switched to a high-impedance state allowing the user to feed an external clock signal to OSC.

Retrieved from "http://wiki.sunfounder.cc/index.php?title=How_to_install_Arduino_Library&oldid=83"