#include "board.h" #include "mw.h" // June 2013 V2.2-dev flags_t f; int16_t debug[4]; uint8_t toggleBeep = 0; uint32_t currentTime = 0; uint32_t previousTime = 0; 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 int16_t headFreeModeHold; uint8_t vbat; // battery voltage in 0.1V steps int16_t telemTemperature1; // gyro sensor temperature int16_t failsafeCnt = 0; int16_t failsafeEvents = 0; int16_t rcData[RC_CHANS]; // interval [1000;2000] int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW int16_t lookupPitchRollRC[PITCH_LOOKUP_LENGTH]; // lookup table for expo & RC rate PITCH+ROLL int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH]; // lookup table for expo & mid THROTTLE uint16_t rssi; // range: [0;1023] rcReadRawDataPtr rcReadRawFunc = NULL; // receive data from default (pwm/ppm) or additional (spek/sbus/?? receiver drivers) static void pidMultiWii(void); static void pidRewrite(void); pidControllerFuncPtr pid_controller = pidMultiWii; // which pid controller are we using, defaultMultiWii uint8_t dynP8[3], dynI8[3], dynD8[3]; uint8_t rcOptions[CHECKBOXITEMS]; int16_t axisPID[3]; // ********************** // GPS // ********************** int32_t GPS_coord[2]; int32_t GPS_home[2]; int32_t GPS_hold[2]; uint8_t GPS_numSat; uint16_t GPS_distanceToHome; // distance to home point in meters int16_t GPS_directionToHome; // direction to home or hol point in degrees uint16_t GPS_altitude, GPS_speed; // altitude in 0.1m and speed in 0.1m/s uint8_t GPS_update = 0; // it's a binary toogle to distinct a GPS position update int16_t GPS_angle[2] = { 0, 0 }; // it's the angles that must be applied for GPS correction uint16_t GPS_ground_course = 0; // degrees * 10 int16_t nav[2]; int16_t nav_rated[2]; // Adding a rate controller to the navigation to make it smoother int8_t nav_mode = NAV_MODE_NONE; // Navigation mode uint8_t GPS_numCh; // Number of channels uint8_t GPS_svinfo_chn[16]; // Channel number uint8_t GPS_svinfo_svid[16]; // Satellite ID uint8_t GPS_svinfo_quality[16]; // Bitfield Qualtity uint8_t GPS_svinfo_cno[16]; // Carrier to Noise Ratio (Signal Strength) // Automatic ACC Offset Calibration uint16_t InflightcalibratingA = 0; int16_t AccInflightCalibrationArmed; uint16_t AccInflightCalibrationMeasurementDone = 0; uint16_t AccInflightCalibrationSavetoEEProm = 0; uint16_t AccInflightCalibrationActive = 0; // Battery monitoring stuff uint8_t batteryCellCount = 3; // cell count uint16_t batteryWarningVoltage; // annoying buzzer after this one, battery ready to be dead void blinkLED(uint8_t num, uint8_t wait, uint8_t repeat) { uint8_t i, r; for (r = 0; r < repeat; r++) { for (i = 0; i < num; i++) { LED0_TOGGLE; // switch LEDPIN state BEEP_ON; delay(wait); BEEP_OFF; } delay(60); } } #define BREAKPOINT 1500 void annexCode(void) { static uint32_t calibratedAccTime; int32_t tmp, tmp2; int32_t axis, prop1, prop2; static uint8_t buzzerFreq; // delay between buzzer ring // vbat shit static uint8_t vbatTimer = 0; static uint8_t ind = 0; uint16_t vbatRaw = 0; static uint16_t vbatRawArray[8]; int i; // 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) cfg.dynThrPID * (rcData[THROTTLE] - BREAKPOINT) / (2000 - BREAKPOINT); } else { prop2 = 100 - cfg.dynThrPID; } } for (axis = 0; axis < 3; axis++) { tmp = min(abs(rcData[axis] - mcfg.midrc), 500); if (axis != 2) { // ROLL & PITCH if (cfg.deadband) { if (tmp > cfg.deadband) { tmp -= cfg.deadband; } else { tmp = 0; } } tmp2 = tmp / 100; rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100; prop1 = 100 - (uint16_t) cfg.rollPitchRate * tmp / 500; prop1 = (uint16_t) prop1 *prop2 / 100; } else { // YAW if (cfg.yawdeadband) { if (tmp > cfg.yawdeadband) { tmp -= cfg.yawdeadband; } else { tmp = 0; } } rcCommand[axis] = tmp * -mcfg.yaw_control_direction; prop1 = 100 - (uint16_t)cfg.yawRate * abs(tmp) / 500; } dynP8[axis] = (uint16_t)cfg.P8[axis] * prop1 / 100; dynI8[axis] = (uint16_t)cfg.I8[axis] * prop1 / 100; dynD8[axis] = (uint16_t)cfg.D8[axis] * prop1 / 100; if (rcData[axis] < mcfg.midrc) rcCommand[axis] = -rcCommand[axis]; } tmp = constrain(rcData[THROTTLE], mcfg.mincheck, 2000); tmp = (uint32_t) (tmp - mcfg.mincheck) * 1000 / (2000 - mcfg.mincheck); // [MINCHECK;2000] -> [0;1000] tmp2 = tmp / 100; rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE] if (f.HEADFREE_MODE) { float radDiff = (heading - headFreeModeHold) * M_PI / 180.0f; float cosDiff = cosf(radDiff); float sinDiff = sinf(radDiff); int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff; rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff; rcCommand[PITCH] = rcCommand_PITCH; } if (feature(FEATURE_VBAT)) { if (!(++vbatTimer % VBATFREQ)) { vbatRawArray[(ind++) % 8] = adcGetChannel(ADC_BATTERY); for (i = 0; i < 8; i++) vbatRaw += vbatRawArray[i]; vbat = batteryAdcToVoltage(vbatRaw / 8); } if ((vbat > batteryWarningVoltage) || (vbat < mcfg.vbatmincellvoltage)) { // VBAT ok, buzzer off buzzerFreq = 0; } else buzzerFreq = 4; // low battery } buzzer(buzzerFreq); // external buzzer routine that handles buzzer events globally now if ((calibratingA > 0 && sensors(SENSOR_ACC)) || (calibratingG > 0)) { // Calibration phasis LED0_TOGGLE; } else { if (f.ACC_CALIBRATED) LED0_OFF; if (f.ARMED) LED0_ON; // This will switch to/from 9600 or 115200 baud depending on state. Of course, it should only do it on changes. With telemetry_softserial>0 telemetry is always enabled, also see updateTelemetryState() if (feature(FEATURE_TELEMETRY)) updateTelemetryState(); } #ifdef LEDRING if (feature(FEATURE_LED_RING)) { static uint32_t LEDTime; if ((int32_t)(currentTime - LEDTime) >= 0) { LEDTime = currentTime + 50000; ledringState(); } } #endif if ((int32_t)(currentTime - calibratedAccTime) >= 0) { if (!f.SMALL_ANGLES_25) { f.ACC_CALIBRATED = 0; // the multi uses ACC and is not calibrated or is too much inclinated LED0_TOGGLE; calibratedAccTime = currentTime + 500000; } else { f.ACC_CALIBRATED = 1; } } serialCom(); if (sensors(SENSOR_GPS)) { static uint32_t GPSLEDTime; if ((int32_t)(currentTime - GPSLEDTime) >= 0 && (GPS_numSat >= 5)) { GPSLEDTime = currentTime + 150000; LED1_TOGGLE; } } // Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities. if (gyro.temperature) gyro.temperature(&telemTemperature1); else { // TODO MCU temp } } uint16_t pwmReadRawRC(uint8_t chan) { uint16_t data; data = pwmRead(mcfg.rcmap[chan]); if (data < 750 || data > 2250) data = mcfg.midrc; return data; } void computeRC(void) { static int16_t rcData4Values[8][4], rcDataMean[8]; static uint8_t rc4ValuesIndex = 0; uint8_t chan, a; rc4ValuesIndex++; for (chan = 0; chan < 8; chan++) { rcData4Values[chan][rc4ValuesIndex % 4] = rcReadRawFunc(chan); rcDataMean[chan] = 0; for (a = 0; a < 4; a++) rcDataMean[chan] += rcData4Values[chan][a]; rcDataMean[chan] = (rcDataMean[chan] + 2) / 4; if (rcDataMean[chan] < rcData[chan] - 3) rcData[chan] = rcDataMean[chan] + 2; if (rcDataMean[chan] > rcData[chan] + 3) rcData[chan] = rcDataMean[chan] - 2; } } static void mwArm(void) { if (calibratingG == 0 && f.ACC_CALIBRATED) { // TODO: feature(FEATURE_FAILSAFE) && failsafeCnt < 2 // TODO: && ( !feature || ( feature && ( failsafecnt > 2) ) if (!f.ARMED) { // arm now! f.ARMED = 1; headFreeModeHold = heading; } } else if (!f.ARMED) { blinkLED(2, 255, 1); } } static void mwDisarm(void) { if (f.ARMED) f.ARMED = 0; } static void mwVario(void) { } static int32_t errorGyroI[3] = { 0, 0, 0 }; static int32_t errorAngleI[2] = { 0, 0 }; static void pidMultiWii(void) { int axis, prop; int32_t error, errorAngle; int32_t PTerm, ITerm, PTermACC = 0, ITermACC = 0, PTermGYRO = 0, ITermGYRO = 0, DTerm; static int16_t lastGyro[3] = { 0, 0, 0 }; static int32_t delta1[3], delta2[3]; int32_t deltaSum; int32_t delta; // **** PITCH & ROLL & YAW PID **** prop = max(abs(rcCommand[PITCH]), abs(rcCommand[ROLL])); // 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(2 * rcCommand[axis] + GPS_angle[axis], -500, +500) - angle[axis] + cfg.angleTrim[axis]; PTermACC = errorAngle * cfg.P8[PIDLEVEL] / 100; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result PTermACC = constrain(PTermACC, -cfg.D8[PIDLEVEL] * 5, +cfg.D8[PIDLEVEL] * 5); errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); // WindUp ITermACC = (errorAngleI[axis] * cfg.I8[PIDLEVEL]) >> 12; } if (!f.ANGLE_MODE || f.HORIZON_MODE || axis == 2) { // MODE relying on GYRO or YAW axis error = (int32_t)rcCommand[axis] * 10 * 8 / cfg.P8[axis]; error -= gyroData[axis]; PTermGYRO = rcCommand[axis]; errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp if (abs(gyroData[axis]) > 640) errorGyroI[axis] = 0; ITermGYRO = (errorGyroI[axis] / 125 * cfg.I8[axis]) >> 6; } if (f.HORIZON_MODE && axis < 2) { PTerm = (PTermACC * (500 - prop) + PTermGYRO * prop) / 500; ITerm = (ITermACC * (500 - prop) + ITermGYRO * prop) / 500; } else { if (f.ANGLE_MODE && axis < 2) { PTerm = PTermACC; ITerm = ITermACC; } else { PTerm = PTermGYRO; ITerm = ITermGYRO; } } PTerm -= (int32_t)gyroData[axis] * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation delta = gyroData[axis] - lastGyro[axis]; lastGyro[axis] = gyroData[axis]; deltaSum = delta1[axis] + delta2[axis] + delta; delta2[axis] = delta1[axis]; delta1[axis] = delta; DTerm = (deltaSum * dynD8[axis]) / 32; axisPID[axis] = PTerm + ITerm - DTerm; } } #define GYRO_I_MAX 256 static void pidRewrite(void) { int32_t errorAngle = 0; int axis; int32_t delta, deltaSum; static int32_t delta1[3], delta2[3]; int32_t PTerm, ITerm, DTerm; static int32_t lastError[3] = { 0, 0, 0 }; int32_t AngleRateTmp, RateError; // ----------PID controller---------- for (axis = 0; axis < 3; axis++) { // -----Get the desired angle rate depending on flight mode if ((f.ANGLE_MODE || f.HORIZON_MODE) && axis < 2 ) { // MODE relying on ACC // calculate error and limit the angle to 50 degrees max inclination errorAngle = constrain((rcCommand[axis] << 1) + GPS_angle[axis], -500, +500) - angle[axis] + cfg.angleTrim[axis]; // 16 bits is ok here } if (axis == 2) { // YAW is always gyro-controlled (MAG correction is applied to rcCommand) AngleRateTmp = (((int32_t)(cfg.yawRate + 27) * rcCommand[2]) >> 5); } else { if (!f.ANGLE_MODE) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID AngleRateTmp = ((int32_t) (cfg.rollPitchRate + 27) * rcCommand[axis]) >> 4; if (f.HORIZON_MODE) { // mix up angle error to desired AngleRateTmp to add a little auto-level feel AngleRateTmp += (errorAngle * cfg.I8[PIDLEVEL]) >> 8; } } else { // it's the ANGLE mode - control is angle based, so control loop is needed AngleRateTmp = (errorAngle * cfg.P8[PIDLEVEL]) >> 4; } } // --------low-level gyro-based PID. ---------- // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes // -----calculate scaled error.AngleRates // multiplication of rcCommand corresponds to changing the sticks scaling here RateError = AngleRateTmp - gyroData[axis]; // -----calculate P component PTerm = (RateError * cfg.P8[axis]) >> 7; // -----calculate I component // there should be no division before accumulating the error to integrator, because the precision would be reduced. // Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used. // Time correction (to avoid different I scaling for different builds based on average cycle time) // is normalized to cycle time = 2048. errorGyroI[axis] = errorGyroI[axis] + ((RateError * cycleTime) >> 11) * cfg.I8[axis]; // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated. // I coefficient (I8) moved before integration to make limiting independent from PID settings errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t)-GYRO_I_MAX << 13, (int32_t)+GYRO_I_MAX << 13); ITerm = errorGyroI[axis] >> 13; //-----calculate D-term delta = RateError - lastError[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 lastError[axis] = RateError; // Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference // would be scaled by different dt each time. Division by dT fixes that. delta = (delta * ((uint16_t)0xFFFF / (cycleTime >> 4))) >> 6; // add moving average here to reduce noise deltaSum = delta1[axis] + delta2[axis] + delta; delta2[axis] = delta1[axis]; delta1[axis] = delta; DTerm = (deltaSum * cfg.D8[axis]) >> 8; // -----calculate total PID output axisPID[axis] = PTerm + ITerm + DTerm; } } void setPIDController(int type) { switch (type) { case 0: default: pid_controller = pidMultiWii; break; case 1: pid_controller = pidRewrite; break; } } void loop(void) { 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 stTmp = 0; int i; static uint32_t rcTime = 0; #ifdef BARO static int16_t initialThrottleHold; #endif static uint32_t loopTime; uint16_t auxState = 0; static uint8_t GPSNavReset = 1; bool isThrottleLow = false; bool rcReady = false; // calculate rc stuff from serial-based receivers (spek/sbus) if (feature(FEATURE_SERIALRX)) { switch (mcfg.serialrx_type) { case SERIALRX_SPEKTRUM1024: case SERIALRX_SPEKTRUM2048: rcReady = spektrumFrameComplete(); break; case SERIALRX_SBUS: rcReady = sbusFrameComplete(); break; } } if (((int32_t)(currentTime - rcTime) >= 0) || rcReady) { // 50Hz or data driven rcReady = false; rcTime = currentTime + 20000; computeRC(); // in 3D mode, we need to be able to disarm by switch at any time if (feature(FEATURE_3D)) { if (!rcOptions[BOXARM]) mwDisarm(); } // Read value of AUX channel as rssi // 0 is disable, 1-4 is AUX{1..4} if (mcfg.rssi_aux_channel > 0) { const int16_t rssiChannelData = rcData[AUX1 + mcfg.rssi_aux_channel - 1]; // Range of rssiChannelData is [1000;2000]. rssi should be in [0;1023]; rssi = (uint16_t)((constrain(rssiChannelData - 1000, 0, 1000) / 1000.0f) * 1023.0f); } // Failsafe routine if (feature(FEATURE_FAILSAFE)) { if (failsafeCnt > (5 * cfg.failsafe_delay) && f.ARMED) { // Stabilize, and set Throttle to specified level for (i = 0; i < 3; i++) rcData[i] = mcfg.midrc; // after specified guard time after RC signal is lost (in 0.1sec) rcData[THROTTLE] = cfg.failsafe_throttle; if (failsafeCnt > 5 * (cfg.failsafe_delay + cfg.failsafe_off_delay)) { // Turn OFF motors after specified Time (in 0.1sec) mwDisarm(); // 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 * cfg.failsafe_delay) && !f.ARMED) { // Turn off "Ok To arm to prevent the motors from spinning after repowering the RX with low throttle and aux to arm mwDisarm(); // 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++; } // end of failsafe routine - next change is made with RcOptions setting // ------------------ STICKS COMMAND HANDLER -------------------- // checking sticks positions for (i = 0; i < 4; i++) { stTmp >>= 2; if (rcData[i] > mcfg.mincheck) stTmp |= 0x80; // check for MIN if (rcData[i] < mcfg.maxcheck) stTmp |= 0x40; // check for MAX } if (stTmp == rcSticks) { if (rcDelayCommand < 250) rcDelayCommand++; } else rcDelayCommand = 0; rcSticks = stTmp; // perform actions if (feature(FEATURE_3D) && (rcData[THROTTLE] > (mcfg.midrc - mcfg.deadband3d_throttle) && rcData[THROTTLE] < (mcfg.midrc + mcfg.deadband3d_throttle))) isThrottleLow = true; else if (!feature(FEATURE_3D) && (rcData[THROTTLE] < mcfg.mincheck)) isThrottleLow = true; if (isThrottleLow) { errorGyroI[ROLL] = 0; errorGyroI[PITCH] = 0; errorGyroI[YAW] = 0; errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0; if (cfg.activate[BOXARM] > 0) { // Arming/Disarming via ARM BOX if (rcOptions[BOXARM] && f.OK_TO_ARM) mwArm(); else if (f.ARMED) mwDisarm(); } } if (rcDelayCommand == 20) { if (f.ARMED) { // actions during armed // Disarm on throttle down + yaw if (cfg.activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE)) mwDisarm(); // Disarm on roll (only when retarded_arm is enabled) if (mcfg.retarded_arm && cfg.activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_LO)) mwDisarm(); } else { // actions during not armed i = 0; // GYRO calibration if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) { calibratingG = CALIBRATING_GYRO_CYCLES; if (feature(FEATURE_GPS)) GPS_reset_home_position(); if (sensors(SENSOR_BARO)) calibratingB = 10; // calibrate baro to new ground level (10 * 25 ms = ~250 ms non blocking) if (!sensors(SENSOR_MAG)) heading = 0; // reset heading to zero after gyro calibration // Inflight ACC Calibration } else if (feature(FEATURE_INFLIGHT_ACC_CAL) && (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_HI)) { if (AccInflightCalibrationMeasurementDone) { // trigger saving into eeprom after landing AccInflightCalibrationMeasurementDone = 0; AccInflightCalibrationSavetoEEProm = 1; } else { AccInflightCalibrationArmed = !AccInflightCalibrationArmed; if (AccInflightCalibrationArmed) { toggleBeep = 2; } else { toggleBeep = 3; } } } // Multiple configuration profiles if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_LO) // ROLL left -> Profile 1 i = 1; else if (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_CE) // PITCH up -> Profile 2 i = 2; else if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_HI) // ROLL right -> Profile 3 i = 3; if (i) { mcfg.current_profile = i - 1; writeEEPROM(0, false); blinkLED(2, 40, i); // TODO alarmArray[0] = i; } // Arm via YAW if (cfg.activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_HI + PIT_CE + ROL_CE)) mwArm(); // Arm via ROLL else if (mcfg.retarded_arm && cfg.activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_HI)) mwArm(); // Calibrating Acc else if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE) calibratingA = CALIBRATING_ACC_CYCLES; // Calibrating Mag else if (rcSticks == THR_HI + YAW_HI + PIT_LO + ROL_CE) f.CALIBRATE_MAG = 1; i = 0; // Acc Trim if (rcSticks == THR_HI + YAW_CE + PIT_HI + ROL_CE) { cfg.angleTrim[PITCH] += 2; i = 1; } else if (rcSticks == THR_HI + YAW_CE + PIT_LO + ROL_CE) { cfg.angleTrim[PITCH] -= 2; i = 1; } else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_HI) { cfg.angleTrim[ROLL] += 2; i = 1; } else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_LO) { cfg.angleTrim[ROLL] -= 2; i = 1; } if (i) { writeEEPROM(1, true); rcDelayCommand = 0; // allow autorepetition } } } if (feature(FEATURE_INFLIGHT_ACC_CAL)) { if (AccInflightCalibrationArmed && f.ARMED && rcData[THROTTLE] > mcfg.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; } } // Check AUX switches 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 & cfg.activate[i]) > 0; // note: if FAILSAFE is disable, failsafeCnt > 5 * FAILSAVE_DELAY is always false if ((rcOptions[BOXANGLE] || (failsafeCnt > 5 * cfg.failsafe_delay)) && (sensors(SENSOR_ACC))) { // bumpless transfer to Level mode if (!f.ANGLE_MODE) { errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0; f.ANGLE_MODE = 1; } } else { f.ANGLE_MODE = 0; // failsave support } 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; } if ((rcOptions[BOXARM]) == 0) f.OK_TO_ARM = 1; if (f.ANGLE_MODE || f.HORIZON_MODE) { LED1_ON; } else { LED1_OFF; } #ifdef BARO if (sensors(SENSOR_BARO)) { // Baro alt hold activate 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; } // Vario signalling activate if (feature(FEATURE_VARIO)) { if (rcOptions[BOXVARIO]) { if (!f.VARIO_MODE) { f.VARIO_MODE = 1; } } else { f.VARIO_MODE = 0; } } } #endif #ifdef MAG if (sensors(SENSOR_ACC) || sensors(SENSOR_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 (sensors(SENSOR_GPS)) { if (f.GPS_FIX && GPS_numSat >= 5) { // if both GPS_HOME & GPS_HOLD are checked => GPS_HOME is the priority if (rcOptions[BOXGPSHOME]) { if (!f.GPS_HOME_MODE) { f.GPS_HOME_MODE = 1; f.GPS_HOLD_MODE = 0; GPSNavReset = 0; GPS_set_next_wp(&GPS_home[LAT], &GPS_home[LON]); nav_mode = NAV_MODE_WP; } } else { f.GPS_HOME_MODE = 0; if (rcOptions[BOXGPSHOLD] && abs(rcCommand[ROLL]) < cfg.ap_mode && abs(rcCommand[PITCH]) < cfg.ap_mode) { if (!f.GPS_HOLD_MODE) { f.GPS_HOLD_MODE = 1; GPSNavReset = 0; 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; } } 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; nav_mode = NAV_MODE_NONE; } } if (rcOptions[BOXPASSTHRU]) { f.PASSTHRU_MODE = 1; } else { f.PASSTHRU_MODE = 0; } if (mcfg.mixerConfiguration == MULTITYPE_FLYING_WING || mcfg.mixerConfiguration == MULTITYPE_AIRPLANE) { f.HEADFREE_MODE = 0; } } else { // not in rc loop static int taskOrder = 0; // never call all function in the same loop, to avoid high delay spikes if (taskOrder > 4) taskOrder -= 5; switch (taskOrder) { case 0: taskOrder++; #ifdef MAG if (sensors(SENSOR_MAG) && Mag_getADC()) break; #endif case 1: taskOrder++; #ifdef BARO if (sensors(SENSOR_BARO) && Baro_update()) break; #endif case 2: taskOrder++; #ifdef BARO if (sensors(SENSOR_BARO) && getEstimatedAltitude()) break; #endif case 3: // if GPS feature is enabled, gpsThread() will be called at some intervals to check for stuck // hardware, wrong baud rates, init GPS if needed, etc. Don't use SENSOR_GPS here as gpsThread() can and will // change this based on available hardware taskOrder++; if (feature(FEATURE_GPS)) { gpsThread(); break; } case 4: taskOrder++; #ifdef SONAR if (sensors(SENSOR_SONAR)) { Sonar_update(); } #endif if (feature(FEATURE_VARIO) && f.VARIO_MODE) mwVario(); break; } } currentTime = micros(); if (mcfg.looptime == 0 || (int32_t)(currentTime - loopTime) >= 0) { loopTime = currentTime + mcfg.looptime; computeIMU(); annexCode(); // Measure loop rate just afer reading the sensors currentTime = micros(); cycleTime = (int32_t)(currentTime - previousTime); previousTime = currentTime; #ifdef MAG if (sensors(SENSOR_MAG)) { if (abs(rcCommand[YAW]) < 70 && f.MAG_MODE) { int16_t dif = heading - magHold; if (dif <= -180) dif += 360; if (dif >= +180) dif -= 360; dif *= -mcfg.yaw_control_direction; if (f.SMALL_ANGLES_25) rcCommand[YAW] -= dif * cfg.P8[PIDMAG] / 30; // 18 deg } else magHold = heading; } #endif #ifdef BARO if (sensors(SENSOR_BARO)) { if (f.BARO_MODE) { static uint8_t isAltHoldChanged = 0; static int16_t AltHoldCorr = 0; if (cfg.alt_hold_fast_change) { // rapid alt changes if (abs(rcCommand[THROTTLE] - initialThrottleHold) > cfg.alt_hold_throttle_neutral) { errorAltitudeI = 0; isAltHoldChanged = 1; rcCommand[THROTTLE] += (rcCommand[THROTTLE] > initialThrottleHold) ? -cfg.alt_hold_throttle_neutral : cfg.alt_hold_throttle_neutral; } else { if (isAltHoldChanged) { AltHold = EstAlt; isAltHoldChanged = 0; } rcCommand[THROTTLE] = initialThrottleHold + BaroPID; } } else { // slow alt changes for apfags if (abs(rcCommand[THROTTLE] - initialThrottleHold) > cfg.alt_hold_throttle_neutral) { // 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; AltHold += AltHoldCorr / 2000; AltHoldCorr %= 2000; isAltHoldChanged = 1; } else if (isAltHoldChanged) { AltHold = EstAlt; AltHoldCorr = 0; isAltHoldChanged = 0; } rcCommand[THROTTLE] = initialThrottleHold + BaroPID; rcCommand[THROTTLE] = constrain(rcCommand[THROTTLE], mcfg.minthrottle + 150, mcfg.maxthrottle); } } } #endif if (cfg.throttle_angle_correction && (f.ANGLE_MODE || f.HORIZON_MODE)) { rcCommand[THROTTLE] += throttleAngleCorrection; } if (sensors(SENSOR_GPS)) { if ((f.GPS_HOME_MODE || f.GPS_HOLD_MODE) && f.GPS_FIX_HOME) { float sin_yaw_y = sinf(heading * 0.0174532925f); float cos_yaw_x = cosf(heading * 0.0174532925f); if (cfg.nav_slew_rate) { nav_rated[LON] += constrain(wrap_18000(nav[LON] - nav_rated[LON]), -cfg.nav_slew_rate, cfg.nav_slew_rate); // TODO check this on uint8 nav_rated[LAT] += constrain(wrap_18000(nav[LAT] - nav_rated[LAT]), -cfg.nav_slew_rate, cfg.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; } } } // PID - note this is function pointer set by setPIDController() pid_controller(); mixTable(); writeServos(); writeMotors(); } }