示例#1
0
void supervisorInitComplete(void) {
    supervisorDisarm();
}
示例#2
0
void supervisorTaskCode(void *unused) {
    unsigned long lastAqCounter = 0;  // used for idle time calc
    uint32_t count = 0;

    AQ_NOTICE("Supervisor task started\n");

    // wait for ADC vIn data
    while (analogData.batCellCount == 0)
	yield(100);

    supervisorData.vInLPF = analogData.vIn;

    if (analogData.extAmp > 0.0f)
	supervisorData.currentSenseValPtr = &analogData.extAmp;
    else if (canSensorsData.values[CAN_SENSORS_PDB_BATA] > 0.0f)
	supervisorData.currentSenseValPtr = &canSensorsData.values[CAN_SENSORS_PDB_BATA];
    else
	supervisorData.aOutLPF = SUPERVISOR_INVALID_AMPSOUT_VALUE;

    if (supervisorData.currentSenseValPtr)
	supervisorData.aOutLPF = *supervisorData.currentSenseValPtr;

    while (1) {
	yield(1000/SUPERVISOR_RATE);

	if (supervisorData.state & STATE_CALIBRATION) {
	    int i;

	    // try to indicate completion percentage
	    i = constrainInt(20*((calibData.percentComplete)/(100.0f/3.0f)), 1, 21);
	    if (i > 20)
		digitalHi(supervisorData.readyLed);
	    else if (!(count % i))
		digitalTogg(supervisorData.readyLed);

#ifdef SUPERVISOR_DEBUG_PORT
	    i = constrainInt(20*((calibData.percentComplete-100.0f/3.0f)/(100.0f/3.0f)), 1, 21);
	    if (i > 20)
		digitalHi(supervisorData.debugLed);
	    else if (!(count % i))
		digitalTogg(supervisorData.debugLed);
#endif //SUPERVISOR_DEBUG_PORT

	    i = constrainInt(20*((calibData.percentComplete-100.0f/3.0f*2.0f)/(100.0f/3.0f)), 1, 21);
	    if (i > 20)
		digitalHi(supervisorData.gpsLed);
	    else if (!(count % i))
		digitalTogg(supervisorData.gpsLed);

	    // user looking to go back to DISARMED mode?
	    if (RADIO_THROT < p[CTRL_MIN_THROT] && RADIO_RUDD < -500) {
		if (!supervisorData.armTime) {
		    supervisorData.armTime = timerMicros();
		}
		else if ((timerMicros() - supervisorData.armTime) > SUPERVISOR_DISARM_TIME) {
		    supervisorDisarm();
		    supervisorData.armTime = 0;
		}
	    }
	    else {
		supervisorData.armTime = 0;
	    }
	}  // end if calibrating
	else if (supervisorData.state & STATE_DISARMED) {

#ifdef SUPERVISOR_DEBUG_PORT
	    // 0.5 Hz blink debug LED if config file could be found on uSD card
	    if (supervisorData.configRead && (!(count % SUPERVISOR_RATE))) {
		// only for first 15s
		if (timerMicros() > 15000000) {
		    supervisorData.configRead = 0;
		    digitalLo(supervisorData.debugLed);
		}
		else {
		    digitalTogg(supervisorData.debugLed);
		}
	    }
#endif // SUPERVISOR_DEBUG_PORT

	    // 1 Hz blink if disarmed, 5 Hz if writing to uSD card
	    if (!(count % ((supervisorData.diskWait) ? SUPERVISOR_RATE/10 : SUPERVISOR_RATE/2)))
		digitalTogg(supervisorData.readyLed);

	    // Attempt to arm if throttle down and yaw full right for 2sec.
	    if (RADIO_VALID && RADIO_THROT < p[CTRL_MIN_THROT] && RADIO_RUDD > +500) {
		if (!supervisorData.armTime) {
		    supervisorData.armTime = timerMicros();
		}
		else if ((timerMicros() - supervisorData.armTime) > SUPERVISOR_DISARM_TIME) {
		    supervisorArm();
		    supervisorData.armTime = 0;
		}
	    }
	    else {
		supervisorData.armTime = 0;
	    }

	    // various functions
	    if (RADIO_VALID && RADIO_THROT < p[CTRL_MIN_THROT] && RADIO_RUDD < -500) {
		if (!supervisorData.stickCmdTimer) {
		    supervisorData.stickCmdTimer = timerMicros();
		} else if ((timerMicros() - supervisorData.stickCmdTimer) > SUPERVISOR_STICK_CMD_TIME) {

#ifdef HAS_DIGITAL_IMU
		    // tare function (lower left)
		    if (RADIO_ROLL < -500 && RADIO_PITCH > +500) {
			supervisorTare();
			supervisorData.stickCmdTimer = 0;
		    }
		    else
#endif // HAS_DIGITAL_IMU
		    // config write (upper right)
		    if (RADIO_ROLL > +500 && RADIO_PITCH < -500) {
			supervisorLEDsOn();
			configSaveParamsToFlash();
#ifdef DIMU_HAVE_EEPROM
			dIMURequestCalibWrite();
#endif
			supervisorLEDsOff();
			supervisorData.stickCmdTimer = 0;
		    }
		    // calibration mode (upper left)
		    else if (RADIO_ROLL < -500 && RADIO_PITCH < -500) {
			supervisorCalibrate();
			supervisorData.stickCmdTimer = 0;
		    }
		    // clear waypoints (lower right with WP Record switch active)
		    else if (RADIO_ROLL > 500 && RADIO_PITCH > 500 && rcIsSwitchActive(NAV_CTRL_WP_REC)) {
			navClearWaypoints();
			supervisorData.stickCmdTimer = 0;
		    }
		} // end stick timer check
	    }
	    // no stick commands detected
	    else
		supervisorData.stickCmdTimer = 0;

	} // end if disarmed
	else if (supervisorData.state & STATE_ARMED) {
	    // Disarm only if in manual mode
	    if (RADIO_THROT < p[CTRL_MIN_THROT] && RADIO_RUDD < -500 && navData.mode == NAV_STATUS_MANUAL) {
		if (!supervisorData.armTime) {
		    supervisorData.armTime = timerMicros();
		}
		else if ((timerMicros() - supervisorData.armTime) > SUPERVISOR_DISARM_TIME) {
		    supervisorDisarm();
		    supervisorData.armTime = 0;
		}
	    }
	    else {
		supervisorData.armTime = 0;
	    }
	}
	// end of calibrating/disarmed/armed mode checks

	// radio loss
	if ((supervisorData.state & STATE_FLYING) && (navData.mode < NAV_STATUS_MISSION || (supervisorData.state & STATE_RADIO_LOSS2))) {
	    // regained?
	    if (RADIO_QUALITY > 1.0f) {
		supervisorData.lastGoodRadioMicros = timerMicros();

		if (supervisorData.state & STATE_RADIO_LOSS1)
		    AQ_NOTICE("Warning: radio signal regained\n");

		navData.spvrModeOverride = 0;
		supervisorData.state &= ~(STATE_RADIO_LOSS1 | STATE_RADIO_LOSS2);
	    }
	    // loss 1
	    else if (!(supervisorData.state & STATE_RADIO_LOSS1) && (timerMicros() - supervisorData.lastGoodRadioMicros) > SUPERVISOR_RADIO_LOSS1) {
		supervisorData.state |= STATE_RADIO_LOSS1;
		AQ_NOTICE("Warning: Radio loss stage 1 detected\n");

		// hold position
		navData.spvrModeOverride = NAV_STATUS_POSHOLD;
		RADIO_PITCH = 0;    // center sticks
		RADIO_ROLL = 0;
		RADIO_RUDD = 0;
		RADIO_THROT = RADIO_MID_THROTTLE;  // center throttle
	    }
	    // loss 2
	    else if (!(supervisorData.state & STATE_RADIO_LOSS2) && (timerMicros() - supervisorData.lastGoodRadioMicros) > SUPERVISOR_RADIO_LOSS2) {
		supervisorData.state |= STATE_RADIO_LOSS2;
		AQ_NOTICE("Warning: Radio loss stage 2! Initiating recovery.\n");

		// only available with GPS
		if (navData.navCapable) {
		    navMission_t *wp;
		    uint8_t wpi = 0;
		    uint8_t fsOption = (uint8_t)p[SPVR_FS_RAD_ST2];

		    navClearWaypoints();
		    wp = navGetWaypoint(wpi++);

		    if (fsOption > SPVR_OPT_FS_RAD_ST2_LAND && navCalcDistance(gpsData.lat, gpsData.lon, navData.homeLeg.targetLat, navData.homeLeg.targetLon) > SUPERVISOR_HOME_POS_DETECT_RADIUS) {
			float targetAltitude;

			// ascend
			if (fsOption == SPVR_OPT_FS_RAD_ST2_ASCEND && ALTITUDE < navData.homeLeg.targetAlt + p[SPVR_FS_ADD_ALT]) {
			    // the home leg's altitude is recorded without pressure offset
			    targetAltitude = navData.homeLeg.targetAlt + p[SPVR_FS_ADD_ALT] + navData.presAltOffset;

			    wp->type = NAV_LEG_GOTO;
			    wp->relativeAlt = 0;
			    wp->targetAlt = targetAltitude;
			    wp->targetLat = gpsData.lat;
			    wp->targetLon = gpsData.lon;
			    wp->targetRadius = SUPERVISOR_HOME_ALT_DETECT_MARGIN;
			    wp->maxHorizSpeed = navData.homeLeg.maxHorizSpeed;
			    wp->maxVertSpeed = navData.homeLeg.maxVertSpeed;
			    wp->poiHeading = navData.homeLeg.poiHeading;
			    wp->loiterTime = 0;
			    wp->poiAltitude = 0.0f;

			    wp = navGetWaypoint(wpi++);
			}
			else {
			    // the greater of our current altitude or home's altitude
			    targetAltitude = ((ALTITUDE > navData.homeLeg.targetAlt) ? ALTITUDE : navData.homeLeg.targetAlt) + navData.presAltOffset;
			}

			// go home with previously determined altitude
			wp->type = NAV_LEG_GOTO;
			wp->relativeAlt = 0;
			wp->targetAlt = targetAltitude;
			wp->targetLat = navData.homeLeg.targetLat;
			wp->targetLon = navData.homeLeg.targetLon;
			wp->targetRadius = SUPERVISOR_HOME_POS_DETECT_RADIUS;
			wp->maxHorizSpeed = navData.homeLeg.maxHorizSpeed;
			wp->maxVertSpeed = navData.homeLeg.maxVertSpeed;
			wp->poiHeading = navData.homeLeg.poiHeading;
			wp->loiterTime = 0;
			wp->poiAltitude = 0.0f;

			wp = navGetWaypoint(wpi++);

			// decend to home
			wp->type = NAV_LEG_HOME;
			wp->targetRadius = SUPERVISOR_HOME_POS_DETECT_RADIUS;
			wp->loiterTime = 0;
			wp->poiAltitude = 0.0f;

			wp = navGetWaypoint(wpi++);
		    }

		    // land
		    wp->type = NAV_LEG_LAND;
		    wp->maxVertSpeed = NAV_DFLT_LND_SPEED;
		    wp->maxHorizSpeed = 0.0f;
		    wp->poiAltitude = 0.0f;

		    // go
		    navData.missionLeg = 0;
		    navData.tempMissionLoaded = 1;
		    navData.spvrModeOverride = NAV_STATUS_MISSION;
		}
		// no GPS, slow decent in PH mode
		else {
		    navData.spvrModeOverride = NAV_STATUS_POSHOLD;
		    RADIO_PITCH = 0;    // center sticks
		    RADIO_ROLL = 0;
		    RADIO_RUDD = 0;
		    RADIO_THROT = RADIO_MID_THROTTLE * 3 / 4;  // 1/4 max decent
		}
	    }
	}
	// end radio loss check

	// calculate idle time
	supervisorData.idlePercent = (counter - lastAqCounter) * minCycles * 100.0f / ((1e6f / SUPERVISOR_RATE) * rccClocks.SYSCLK_Frequency / 1e6f);
	lastAqCounter = counter;

	// smooth vIn readings
	supervisorData.vInLPF += (analogData.vIn - supervisorData.vInLPF) * (0.1f / SUPERVISOR_RATE);

	// smooth current flow readings, if any
	if (supervisorData.currentSenseValPtr)
	    supervisorData.aOutLPF += (*supervisorData.currentSenseValPtr - supervisorData.aOutLPF) * (0.1f / SUPERVISOR_RATE);

	//calculate remaining battery % based on configured low batt stg 2 level -- ASSumes 4.2v/cell maximum
	supervisorData.battRemainingPrct = (supervisorData.vInLPF - p[SPVR_LOW_BAT2] * analogData.batCellCount) / ((4.2f - p[SPVR_LOW_BAT2]) * analogData.batCellCount) * 100;

	// low battery
	if (!(supervisorData.state & STATE_LOW_BATTERY1) && supervisorData.vInLPF < (p[SPVR_LOW_BAT1]*analogData.batCellCount)) {
	    supervisorData.state |= STATE_LOW_BATTERY1;
	    AQ_NOTICE("Warning: Low battery stage 1\n");

	    // TODO: something
	}
	else if (!(supervisorData.state & STATE_LOW_BATTERY2) && supervisorData.vInLPF < (p[SPVR_LOW_BAT2]*analogData.batCellCount)) {
	    supervisorData.state |= STATE_LOW_BATTERY2;
	    AQ_NOTICE("Warning: Low battery stage 2\n");

	    // TODO: something
	}
	// end battery level checks

	supervisorSetSystemStatus();

	if (supervisorData.state & STATE_FLYING) {
	    // count flight time in seconds
	    supervisorData.flightTime += (1.0f / SUPERVISOR_RATE);

	    // rapidly flash Ready LED if we are critically low on power
	    if (supervisorData.state & STATE_LOW_BATTERY2)
		digitalTogg(supervisorData.readyLed);
	}
	else if (supervisorData.state & STATE_ARMED) {
	    digitalHi(supervisorData.readyLed);
	}

#ifdef SUPERVISOR_DEBUG_PORT
	// DEBUG LED to indicate radio RX state
	if (!supervisorData.configRead && RADIO_INITIALIZED && supervisorData.state != STATE_CALIBRATION) {
	    // packet received in the last 50ms?
	    if (RADIO_VALID) {
		digitalHi(supervisorData.debugLed);
	    }
	    else {
		if (RADIO_BINDING)
		    digitalTogg(supervisorData.debugLed);
		else
		    digitalLo(supervisorData.debugLed);
	    }
	}
#endif

	count++;

#ifdef USE_SIGNALING
	signalingEvent();
#endif
    }
}
示例#3
0
void navNavigate(void) {
    unsigned long currentTime = IMU_LASTUPD;
    unsigned char leg = navData.missionLeg;
    navMission_t *curLeg = &navData.missionLegs[leg];
    int reqFlightMode;  // requested flight mode based on user controls or other factors
    float tmp;

    navSetFixType();

    // is there sufficient position accuracy to navigate?
    if (navData.fixType == 3)
        navData.navCapable = 1;
    // do not drop out of mission due to (hopefully) temporary GPS degradation
    else if (navData.mode < NAV_STATUS_POSHOLD)
        navData.navCapable = 0;

    bool hasViableMission = (navData.navCapable && ((navData.mode != NAV_STATUS_MISSION && leg < NAV_MAX_MISSION_LEGS && curLeg->type) || (navData.mode == NAV_STATUS_MISSION && navData.hasMissionLeg)));

    // this defines the hierarchy of available flight modes in case of failsafe override or conflicting controls being active
    if (navData.spvrModeOverride)
	reqFlightMode = navData.spvrModeOverride;
    else if (rcIsSwitchActive(NAV_CTRL_MISN))
	if (hasViableMission)
	    reqFlightMode = NAV_STATUS_MISSION;
	else
	    reqFlightMode = NAV_STATUS_POSHOLD;
    else if (rcIsSwitchActive(NAV_CTRL_PH)) {
	reqFlightMode = NAV_STATUS_POSHOLD;
    }
    else if (rcIsSwitchActive(NAV_CTRL_AH))
	reqFlightMode = NAV_STATUS_ALTHOLD;
    // always default to manual
    else
	reqFlightMode = NAV_STATUS_MANUAL;

    // Can we navigate && do we want to be in mission mode?
    if ((supervisorData.state & STATE_ARMED) && reqFlightMode == NAV_STATUS_MISSION && hasViableMission) {
        //  are we currently in position hold mode && do we have a clear mission ahead of us?
        if (navData.mode == NAV_STATUS_POSHOLD || navData.mode == NAV_STATUS_DVH) {
            curLeg = navLoadLeg(leg);
            navData.mode = NAV_STATUS_MISSION;
            AQ_NOTICE("Mission mode active\n");
        }
    }
    // do we want to be in position hold mode?
    else if ((supervisorData.state & STATE_ARMED) && reqFlightMode > NAV_STATUS_MANUAL) {

	// check for DVH
	if (reqFlightMode == NAV_STATUS_POSHOLD && (RADIO_PITCH > p[CTRL_DEAD_BAND] || RADIO_PITCH < -p[CTRL_DEAD_BAND] || RADIO_ROLL > p[CTRL_DEAD_BAND] || RADIO_ROLL < -p[CTRL_DEAD_BAND]))
	    reqFlightMode = NAV_STATUS_DVH;

        // allow alt hold regardless of GPS or flow quality
        if (navData.mode < NAV_STATUS_ALTHOLD || (navData.mode != NAV_STATUS_ALTHOLD && reqFlightMode == NAV_STATUS_ALTHOLD)) {
            // record this altitude as the hold altitude
            navSetHoldAlt(ALTITUDE, 0);

            // set integral to current RC throttle setting
            pidZeroIntegral(navData.altSpeedPID, -VELOCITYD, motorsData.throttle * RADIO_MID_THROTTLE / MOTORS_SCALE);
            pidZeroIntegral(navData.altPosPID, ALTITUDE, 0.0f);

            navData.holdSpeedAlt = navData.targetHoldSpeedAlt = -VELOCITYD;
            navData.holdMaxVertSpeed = NAV_DFLT_VRT_SPEED;
            navData.mode = NAV_STATUS_ALTHOLD;

            // notify ground
            AQ_NOTICE("Altitude Hold engaged\n");
        }

        // are we not in position hold mode now?
        if ((navData.navCapable || navUkfData.flowQuality > 0.0f) && reqFlightMode > NAV_STATUS_ALTHOLD && navData.mode != NAV_STATUS_POSHOLD && navData.mode != NAV_STATUS_DVH) {
            // only zero bias if coming from lower mode
            if (navData.mode < NAV_STATUS_POSHOLD) {
                navData.holdTiltN = 0.0f;
                navData.holdTiltE = 0.0f;

                // speed
                pidZeroIntegral(navData.speedNPID, UKF_VELN, 0.0f);
                pidZeroIntegral(navData.speedEPID, UKF_VELE, 0.0f);

                // pos
                pidZeroIntegral(navData.distanceNPID, UKF_POSN, 0.0f);
                pidZeroIntegral(navData.distanceEPID, UKF_POSE, 0.0f);
            }

            // store this position as hold position
            navUkfSetHereAsPositionTarget();

            if (navData.navCapable) {
                // set this position as home if we have none
                if (navData.homeLeg.targetLat == (double)0.0 || navData.homeLeg.targetLon == (double)0.0)
                    navSetHomeCurrent();
            }

            // activate pos hold
            navData.mode = NAV_STATUS_POSHOLD;

            navData.holdSpeedN = 0.0f;
            navData.holdSpeedE = 0.0f;

            navData.holdMaxHorizSpeed = NAV_DFLT_HOR_SPEED;
            navData.holdMaxVertSpeed = NAV_DFLT_VRT_SPEED;

            // notify ground
            AQ_NOTICE("Position Hold engaged\n");
        }
        // DVH
        else if ((navData.navCapable || navUkfData.flowQuality > 0.0f) && reqFlightMode == NAV_STATUS_DVH) {
            navData.mode = NAV_STATUS_DVH;
        }
        // coming out of DVH mode?
        else if (navData.mode == NAV_STATUS_DVH) {
            // allow speed to drop before holding position (or if RTH engaged)
            // FIXME: RTH switch may no longer be engaged but craft is still returning to home?
            if ((UKF_VELN < +0.1f && UKF_VELN > -0.1f && UKF_VELE < +0.1f && UKF_VELE > -0.1f) || rcIsSwitchActive(NAV_CTRL_HOM_GO)) {
                navUkfSetHereAsPositionTarget();

                navData.mode = NAV_STATUS_POSHOLD;
            }
        }
    }
    // default to manual mode
    else {
	if (navData.mode != NAV_STATUS_MANUAL)
            AQ_NOTICE("Manual mode active.\n");

        navData.mode = NAV_STATUS_MANUAL;
        // reset mission legs
        navData.missionLeg = leg = 0;
        // keep up with changing altitude
        navSetHoldAlt(ALTITUDE, 0);
    }

    // ceiling set ?, 0 is disable
    if (navData.ceilingAlt) {
        // ceiling reached 1st time
        if (ALTITUDE > navData.ceilingAlt && !navData.setCeilingFlag) {
            navData.setCeilingFlag = 1;
            navData.ceilingTimer = timerMicros();
        }
        // ceiling still reached for 5 seconds
        else if (navData.setCeilingFlag && ALTITUDE > navData.ceilingAlt && (timerMicros() - navData.ceilingTimer) > (1e6 * 5) ) {
            navData.ceilingTimer = timerMicros();
            if (!navData.setCeilingReached)
                AQ_NOTICE("Warning: Altitude ceiling reached.");
            navData.setCeilingReached = 1;
        }
        else if ((navData.setCeilingFlag || navData.setCeilingReached) && ALTITUDE <= navData.ceilingAlt) {
            if (navData.setCeilingReached)
                AQ_NOTICE("Notice: Altitude returned to within ceiling.");
            navData.setCeilingFlag = 0;
            navData.setCeilingReached = 0;
        }
    }

    if (!(supervisorData.state & STATE_RADIO_LOSS1))
	navDoUserCommands(&leg, curLeg);

    if (UKF_POSN != 0.0f || UKF_POSE != 0.0f) {
        navData.holdCourse = compassNormalize(atan2f(-UKF_POSE, -UKF_POSN) * RAD_TO_DEG);
        navData.holdDistance = __sqrtf(UKF_POSN*UKF_POSN + UKF_POSE*UKF_POSE);
    }
    else {
        navData.holdCourse = 0.0f;
        navData.holdDistance = 0.0f;
    }

    if (navData.mode == NAV_STATUS_MISSION) {
        // have we arrived yet?
        if (navData.loiterCompleteTime == 0) {
            // are we close enough (distance and altitude)?
            // goto/home test
            if (((curLeg->type == NAV_LEG_GOTO || curLeg->type == NAV_LEG_HOME) &&
                navData.holdDistance < curLeg->targetRadius &&
                fabsf(navData.holdAlt - ALTITUDE) < curLeg->targetRadius) ||
            // orbit test
                (curLeg->type == NAV_LEG_ORBIT &&
                fabsf(navData.holdDistance - curLeg->targetRadius) <= 2.0f &&
                fabsf(navData.holdAlt - ALTITUDE) <= 1.0f) ||
            // takeoff test
                (curLeg->type == NAV_LEG_TAKEOFF &&
                navData.holdDistance < curLeg->targetRadius &&
                fabsf(navData.holdAlt - ALTITUDE) < curLeg->targetRadius)
                ) {
                    // freeze heading if relative, unless orbiting
                    if (curLeg->type != NAV_LEG_ORBIT && signbit(navData.targetHeading))
                	navData.targetHeading = AQ_YAW;

                    // start the loiter clock
                    navData.loiterCompleteTime = currentTime + curLeg->loiterTime;
                    AQ_PRINTF("NAV: Reached waypoint %d.\n", leg);
#ifdef USE_SIGNALING
                    signalingOnetimeEvent(SIG_EVENT_OT_WP_REACHED);
#endif
#ifdef USE_MAVLINK
                    // notify ground
                    mavlinkWpReached(leg);
#endif
            }
        }
        // have we loitered long enough?
        else if (currentTime > navData.loiterCompleteTime && curLeg->type != NAV_LEG_LAND) {
            // next leg
            leg = ++navData.missionLeg;
            curLeg = navLoadLeg(leg);
            if (!navData.hasMissionLeg)
                navData.mode = NAV_STATUS_POSHOLD;
        }
    }

    // DVH
    if (navData.mode == NAV_STATUS_DVH) {
        float factor = (1.0f / 500.0f) * navData.holdMaxHorizSpeed;
        float x = 0.0f;
        float y = 0.0f;

        if (RADIO_PITCH > p[CTRL_DEAD_BAND])
            x = -(RADIO_PITCH - p[CTRL_DEAD_BAND]) * factor;
        if (RADIO_PITCH < -p[CTRL_DEAD_BAND])
            x = -(RADIO_PITCH + p[CTRL_DEAD_BAND]) * factor;
        if (RADIO_ROLL > p[CTRL_DEAD_BAND])
            y = +(RADIO_ROLL - p[CTRL_DEAD_BAND]) * factor;
        if (RADIO_ROLL < -p[CTRL_DEAD_BAND])
            y = +(RADIO_ROLL + p[CTRL_DEAD_BAND]) * factor;

        // do we want to ignore rotation of craft (headfree/carefree mode)?
        if (navData.headFreeMode > NAV_HEADFREE_OFF) {
            if (navData.hfUseStoredReference) {
                // rotate to stored frame
                navData.holdSpeedN = x * navData.hfReferenceCos - y * navData.hfReferenceSin;
                navData.holdSpeedE = y * navData.hfReferenceCos + x * navData.hfReferenceSin;
            }
            else {
                // don't rotate to any frame (pitch/roll move to N/S/E/W)
                navData.holdSpeedN = x;
                navData.holdSpeedE = y;
            }
        }
        else {
            // rotate to earth frame
            navData.holdSpeedN = x * navUkfData.yawCos - y * navUkfData.yawSin;
            navData.holdSpeedE = y * navUkfData.yawCos + x * navUkfData.yawSin;
        }
    }
    // orbit POI
    else if (navData.mode == NAV_STATUS_MISSION && curLeg->type == NAV_LEG_ORBIT) {
        float velX, velY;

        // maintain orbital radius
        velX = -pidUpdate(navData.distanceNPID, curLeg->targetRadius, navData.holdDistance);

        // maintain orbital velocity (clock wise)
        velY = -curLeg->maxHorizSpeed;

        // rotate to earth frame
        navData.holdSpeedN = velX * navUkfData.yawCos - velY * navUkfData.yawSin;
        navData.holdSpeedE = velY * navUkfData.yawCos + velX * navUkfData.yawSin;
    }
    else {
        // distance => velocity
        navData.holdSpeedN = pidUpdate(navData.distanceNPID, 0.0f, UKF_POSN);
        navData.holdSpeedE = pidUpdate(navData.distanceEPID, 0.0f, UKF_POSE);
    }

    // normalize N/E speed requests to fit below max nav speed
    tmp = __sqrtf(navData.holdSpeedN*navData.holdSpeedN + navData.holdSpeedE*navData.holdSpeedE);
    if (tmp > navData.holdMaxHorizSpeed) {
        navData.holdSpeedN = (navData.holdSpeedN / tmp) * navData.holdMaxHorizSpeed;
        navData.holdSpeedE = (navData.holdSpeedE / tmp) * navData.holdMaxHorizSpeed;
    }

    // velocity => tilt
    navData.holdTiltN = -pidUpdate(navData.speedNPID, navData.holdSpeedN, UKF_VELN);
    navData.holdTiltE = +pidUpdate(navData.speedEPID, navData.holdSpeedE, UKF_VELE);

    if (navData.mode > NAV_STATUS_MANUAL) {
        float vertStick;

        // Throttle controls vertical speed
        vertStick = RADIO_THROT - RADIO_MID_THROTTLE;
        if ((vertStick > p[CTRL_DBAND_THRO]  && !navData.setCeilingReached)  || vertStick < -p[CTRL_DBAND_THRO]) {
            // altitude velocity proportional to throttle stick
            navData.targetHoldSpeedAlt = (vertStick - p[CTRL_DBAND_THRO] * (vertStick > 0.0f ? 1.0f : -1.0f)) * NAV_DFLT_VRT_SPEED * (1.0f / RADIO_MID_THROTTLE);

            navData.verticalOverride = 1;
        }
        // are we trying to land?
        else if (navData.mode == NAV_STATUS_MISSION && curLeg->type == NAV_LEG_LAND) {
            navData.targetHoldSpeedAlt = -navData.holdMaxVertSpeed;
        }
        // coming out of vertical override?
        else if (navData.verticalOverride) {
            navData.targetHoldSpeedAlt = 0.0f;

            // slow down before trying to hold altitude
            if (fabsf(VELOCITYD) < 0.025f)
                navData.verticalOverride = 0;

            // set new hold altitude to wherever we are while still in override
            if (navData.mode != NAV_STATUS_MISSION)
                navSetHoldAlt(ALTITUDE, 0);
        }
        // PID has the throttle
        else {
            navData.targetHoldSpeedAlt = pidUpdate(navData.altPosPID, navData.holdAlt, ALTITUDE);
        }

        // constrain vertical velocity
        tmp = configGetParamValue(NAV_MAX_DECENT);
        tmp = (navData.holdMaxVertSpeed < tmp) ? -navData.holdMaxVertSpeed : -tmp;
        navData.targetHoldSpeedAlt = constrainFloat(navData.targetHoldSpeedAlt, tmp, navData.holdMaxVertSpeed);

        // smooth vertical velocity changes
        navData.holdSpeedAlt += (navData.targetHoldSpeedAlt - navData.holdSpeedAlt) * 0.05f;
    }
    else {
        navData.verticalOverride = 0;
    }

    // calculate POI angle (used for tilt in gimbal function)
    if (navData.mode == NAV_STATUS_MISSION && curLeg->poiAltitude != 0.0f) {
        float a, b, c;

        a = navData.holdDistance;
        b = ALTITUDE - curLeg->poiAltitude;
        c = __sqrtf(a*a + b*b);

        navData.poiAngle = asinf(a/c) * RAD_TO_DEG - 90.0f;
    }
    else {
        navData.poiAngle = 0.0f;
    }

    if (navData.mode == NAV_STATUS_MISSION) {
        // recalculate autonomous heading
        navSetHoldHeading(navData.targetHeading);

        // wait for low throttle if landing
        if (curLeg->type == NAV_LEG_LAND && motorsData.throttle <= 1)
            // shut everything down (sure hope we are really on the ground :)
            supervisorDisarm();
    }

    navData.lastUpdate = currentTime;
}
示例#4
0
文件: nav.c 项目: ChrelleP/autoquad
void navNavigate(void) {
    unsigned long currentTime;
    unsigned char leg = navData.missionLeg;
    navMission_t *curLeg = &navData.missionLegs[leg];
    float tmp;

    currentTime = IMU_LASTUPD;

    navSetFixType();

    // is there sufficient position accuracy to navigate?
    if (navData.fixType == 3)
        navData.navCapable = 1;
    // do not drop out of mission due to (hopefully) temporary GPS degradation
    else if (navData.mode < NAV_STATUS_POSHOLD)
        navData.navCapable = 0;

    // Can we navigate && do we want to be in mission mode?
    if ((supervisorData.state & STATE_ARMED) && navData.navCapable && RADIO_FLAPS > 250) {
        //  are we currently in position hold mode && do we have a clear mission ahead of us?
        if ((navData.mode == NAV_STATUS_POSHOLD || navData.mode == NAV_STATUS_DVH) && leg < NAV_MAX_MISSION_LEGS && curLeg->type > 0) {
            curLeg = navLoadLeg(leg);
            navData.mode = NAV_STATUS_MISSION;
        }
    }
    // do we want to be in position hold mode?
    else if ((supervisorData.state & STATE_ARMED) && RADIO_FLAPS > -250) {
        // always allow alt hold
        if (navData.mode < NAV_STATUS_ALTHOLD) {
            // record this altitude as the hold altitude
            navSetHoldAlt(ALTITUDE, 0);

            // set integral to current RC throttle setting
            pidZeroIntegral(navData.altSpeedPID, -VELOCITYD, motorsData.throttle * RADIO_MID_THROTTLE / MOTORS_SCALE);
            pidZeroIntegral(navData.altPosPID, ALTITUDE, 0.0f);

            navData.holdSpeedAlt = navData.targetHoldSpeedAlt = -VELOCITYD;
            navData.holdMaxVertSpeed = p[NAV_ALT_POS_OM];
            navData.mode = NAV_STATUS_ALTHOLD;

            // notify ground
            AQ_NOTICE("Altitude Hold engaged\n");
        }

        // are we not in position hold mode now?
        if ((navData.navCapable || navUkfData.flowQuality > 0.0f) && navData.mode != NAV_STATUS_POSHOLD && navData.mode != NAV_STATUS_DVH) {
            // only zero bias if coming from lower mode
            if (navData.mode < NAV_STATUS_POSHOLD) {
                navData.holdTiltN = 0.0f;
                navData.holdTiltE = 0.0f;

                // speed
                pidZeroIntegral(navData.speedNPID, UKF_VELN, 0.0f);
                pidZeroIntegral(navData.speedEPID, UKF_VELE, 0.0f);

                // pos
                pidZeroIntegral(navData.distanceNPID, UKF_POSN, 0.0f);
                pidZeroIntegral(navData.distanceEPID, UKF_POSE, 0.0f);
            }

            // store this position as hold position
            navUkfSetHereAsPositionTarget();

            if (navData.navCapable) {
                // set this position as home if we have none
                if (navData.homeLeg.targetLat == (double)0.0 || navData.homeLeg.targetLon == (double)0.0)
                    navSetHomeCurrent();
            }

            // activate pos hold
            navData.mode = NAV_STATUS_POSHOLD;

            navData.holdSpeedN = 0.0f;
            navData.holdSpeedE = 0.0f;

            navData.holdMaxHorizSpeed = p[NAV_MAX_SPEED];
            navData.holdMaxVertSpeed = p[NAV_ALT_POS_OM];

            // notify ground
            AQ_NOTICE("Position Hold engaged\n");
        }
        // DVH
        else if ((navData.navCapable || navUkfData.flowQuality > 0.0f) && (
            RADIO_PITCH > p[CTRL_DEAD_BAND] ||
            RADIO_PITCH < -p[CTRL_DEAD_BAND] ||
            RADIO_ROLL > p[CTRL_DEAD_BAND] ||
            RADIO_ROLL < -p[CTRL_DEAD_BAND])) {
                    navData.mode = NAV_STATUS_DVH;
        }
        else if (navData.mode == NAV_STATUS_DVH) {
            // allow speed to drop before holding position (or if RTH engaged)
            if ((UKF_VELN < +0.1f && UKF_VELN > -0.1f && UKF_VELE < +0.1f && UKF_VELE > -0.1f) || RADIO_AUX2 < -250) {
                navUkfSetHereAsPositionTarget();

                navData.mode = NAV_STATUS_POSHOLD;
            }
        }
    }
    else {
        // switch to manual mode
        navData.mode = NAV_STATUS_MANUAL;
        // reset mission legs
        navData.missionLeg = leg = 0;
        // keep up with changing altitude
        navSetHoldAlt(ALTITUDE, 0);
    }

    // ceiling set ?, 0 is disable
    if (navData.ceilingAlt) {
        // ceiling reached 1st time
        if (ALTITUDE > navData.ceilingAlt && !navData.setCeilingFlag) {
            navData.setCeilingFlag = 1;
            navData.ceilingTimer = timerMicros();
        }
        // ceiling still reached for 5 seconds
        else if (navData.setCeilingFlag && ALTITUDE > navData.ceilingAlt && (timerMicros() - navData.ceilingTimer) > (1e6 * 5) ) {
            navData.ceilingTimer = timerMicros();
            if (!navData.setCeilingReached)
                AQ_NOTICE("Warning: Altitude ceiling reached.");
            navData.setCeilingReached = 1;
        }
        else if ((navData.setCeilingFlag || navData.setCeilingReached) && ALTITUDE <= navData.ceilingAlt) {
            if (navData.setCeilingReached)
                AQ_NOTICE("Notice: Altitude returned to within ceiling.");
            navData.setCeilingFlag = 0;
            navData.setCeilingReached = 0;
        }
    }

    // home set
    if ((supervisorData.state & STATE_ARMED) && RADIO_AUX2 > 250) {
        if (!navData.homeActionFlag) {
            navSetHomeCurrent();
            navData.homeActionFlag = 1;
        }
    }
    // recall home
    else if ((supervisorData.state & STATE_ARMED) && RADIO_AUX2 < -250) {
        if (!navData.homeActionFlag) {
            navRecallHome();
            AQ_NOTICE("Returning to home position\n");
            navData.homeActionFlag = 1;
        }
    }
    // switch to middle, clear action flag
    else {
        navData.homeActionFlag = 0;
    }

    // heading-free mode
    if ((int)p[NAV_HDFRE_CHAN] > 0 && (int)p[NAV_HDFRE_CHAN] <= RADIO_MAX_CHANNELS) {

        navSetHeadFreeMode();

        // set/maintain headfree frame reference
        if (!navData.homeActionFlag && ( navData.headFreeMode == NAV_HEADFREE_SETTING ||
                (navData.headFreeMode == NAV_HEADFREE_DYNAMIC && navData.mode == NAV_STATUS_DVH) )) {
            uint8_t dfRefTyp = 0;
            if ((supervisorData.state & STATE_FLYING) && navData.homeLeg.targetLat != (double)0.0f && navData.homeLeg.targetLon != (double)0.0f) {
                if (NAV_HF_HOME_DIST_D_MIN && NAV_HF_HOME_DIST_FREQ && (currentTime - navData.homeDistanceLastUpdate) > (AQ_US_PER_SEC / NAV_HF_HOME_DIST_FREQ)) {
                    navData.distanceToHome = navCalcDistance(gpsData.lat, gpsData.lon, navData.homeLeg.targetLat, navData.homeLeg.targetLon);
                    navData.homeDistanceLastUpdate = currentTime;
                }
                if (!NAV_HF_HOME_DIST_D_MIN || navData.distanceToHome > NAV_HF_HOME_DIST_D_MIN)
                    dfRefTyp = 1;
            }
            navSetHfReference(dfRefTyp);
        }
    }

    if (UKF_POSN != 0.0f || UKF_POSE != 0.0f) {
        navData.holdCourse = compassNormalize(atan2f(-UKF_POSE, -UKF_POSN) * RAD_TO_DEG);
        navData.holdDistance = __sqrtf(UKF_POSN*UKF_POSN + UKF_POSE*UKF_POSE);
    }
    else {
        navData.holdCourse = 0.0f;
        navData.holdDistance = 0.0f;
    }

    if (navData.mode == NAV_STATUS_MISSION) {
        // have we arrived yet?
        if (navData.loiterCompleteTime == 0) {
            // are we close enough (distance and altitude)?
            // goto/home test
            if (((curLeg->type == NAV_LEG_GOTO || curLeg->type == NAV_LEG_HOME) &&
                navData.holdDistance < curLeg->targetRadius &&
                fabsf(navData.holdAlt - ALTITUDE) < curLeg->targetRadius) ||
            // orbit test
                (curLeg->type == NAV_LEG_ORBIT &&
                fabsf(navData.holdDistance - curLeg->targetRadius) +
                fabsf(navData.holdAlt - ALTITUDE) < 2.0f)  ||
            // takeoff test
                (curLeg->type == NAV_LEG_TAKEOFF &&
                navData.holdDistance < curLeg->targetRadius &&
                fabsf(navData.holdAlt - ALTITUDE) < curLeg->targetRadius)
                ) {
                    // freeze heading unless orbiting
                    if (curLeg->type != NAV_LEG_ORBIT)
                        navSetHoldHeading(AQ_YAW);

                    // start the loiter clock
                    navData.loiterCompleteTime = currentTime + curLeg->loiterTime;
#ifdef USE_MAVLINK
                    // notify ground
                    mavlinkWpReached(leg);
#endif
            }
        }
        // have we loitered long enough?
        else if (currentTime > navData.loiterCompleteTime && curLeg->type != NAV_LEG_LAND) {
            // next leg
            if (++leg < NAV_MAX_MISSION_LEGS && navData.missionLegs[leg].type > 0) {
                curLeg = navLoadLeg(leg);
            }
            else {
                navData.mode = NAV_STATUS_POSHOLD;
            }
        }
    }

    // DVH
    if (navData.mode == NAV_STATUS_DVH) {
        float factor = (1.0f / 500.0f) * navData.holdMaxHorizSpeed;
        float x = 0.0f;
        float y = 0.0f;

        if (RADIO_PITCH > p[CTRL_DEAD_BAND])
            x = -(RADIO_PITCH - p[CTRL_DEAD_BAND]) * factor;
        if (RADIO_PITCH < -p[CTRL_DEAD_BAND])
            x = -(RADIO_PITCH + p[CTRL_DEAD_BAND]) * factor;
        if (RADIO_ROLL > p[CTRL_DEAD_BAND])
            y = +(RADIO_ROLL - p[CTRL_DEAD_BAND]) * factor;
        if (RADIO_ROLL < -p[CTRL_DEAD_BAND])
            y = +(RADIO_ROLL + p[CTRL_DEAD_BAND]) * factor;

        // do we want to ignore rotation of craft (headfree/carefree mode)?
        if (navData.headFreeMode > NAV_HEADFREE_OFF) {
            if (navData.hfUseStoredReference) {
                // rotate to stored frame
                navData.holdSpeedN = x * navData.hfReferenceCos - y * navData.hfReferenceSin;
                navData.holdSpeedE = y * navData.hfReferenceCos + x * navData.hfReferenceSin;
            }
            else {
                // don't rotate to any frame (pitch/roll move to N/S/E/W)
                navData.holdSpeedN = x;
                navData.holdSpeedE = y;
            }
        }
        else {
            // rotate to earth frame
            navData.holdSpeedN = x * navUkfData.yawCos - y * navUkfData.yawSin;
            navData.holdSpeedE = y * navUkfData.yawCos + x * navUkfData.yawSin;
        }
    }
    // orbit POI
    else if (navData.mode == NAV_STATUS_MISSION && curLeg->type == NAV_LEG_ORBIT) {
        float velX, velY;

        // maintain orbital radius
        velX = -pidUpdate(navData.distanceNPID, curLeg->targetRadius, navData.holdDistance);

        // maintain orbital velocity (clock wise)
        velY = -curLeg->maxHorizSpeed;

        // rotate to earth frame
        navData.holdSpeedN = velX * navUkfData.yawCos - velY * navUkfData.yawSin;
        navData.holdSpeedE = velY * navUkfData.yawCos + velX * navUkfData.yawSin;
    }
    else {
        // distance => velocity
        navData.holdSpeedN = pidUpdate(navData.distanceNPID, 0.0f, UKF_POSN);
        navData.holdSpeedE = pidUpdate(navData.distanceEPID, 0.0f, UKF_POSE);
    }

    // normalize N/E speed requests to fit below max nav speed
    tmp = __sqrtf(navData.holdSpeedN*navData.holdSpeedN + navData.holdSpeedE*navData.holdSpeedE);
    if (tmp > navData.holdMaxHorizSpeed) {
        navData.holdSpeedN = (navData.holdSpeedN / tmp) * navData.holdMaxHorizSpeed;
        navData.holdSpeedE = (navData.holdSpeedE / tmp) * navData.holdMaxHorizSpeed;
    }

    // velocity => tilt
    navData.holdTiltN = -pidUpdate(navData.speedNPID, navData.holdSpeedN, UKF_VELN);
    navData.holdTiltE = +pidUpdate(navData.speedEPID, navData.holdSpeedE, UKF_VELE);

    if (navData.mode > NAV_STATUS_MANUAL) {
        float vertStick;

        // Throttle controls vertical speed
        vertStick = RADIO_THROT - RADIO_MID_THROTTLE;
        if ((vertStick > p[CTRL_DBAND_THRO]  && !navData.setCeilingReached)  || vertStick < -p[CTRL_DBAND_THRO]) {
            // altitude velocity proportional to throttle stick
            navData.targetHoldSpeedAlt = (vertStick - p[CTRL_DBAND_THRO] * (vertStick > 0.0f ? 1.0f : -1.0f)) * p[NAV_ALT_POS_OM] * (1.0f / RADIO_MID_THROTTLE);

            navData.verticalOverride = 1;
        }
        // are we trying to land?
        else if (navData.mode == NAV_STATUS_MISSION && curLeg->type == NAV_LEG_LAND) {
            navData.targetHoldSpeedAlt = -navData.holdMaxVertSpeed;
        }
        // coming out of vertical override?
        else if (navData.verticalOverride) {
            navData.targetHoldSpeedAlt = 0.0f;

            // slow down before trying to hold altitude
            if (fabsf(VELOCITYD) < 0.025f)
                navData.verticalOverride = 0;

            // set new hold altitude to wherever we are while still in override
            if (navData.mode != NAV_STATUS_MISSION)
                navSetHoldAlt(ALTITUDE, 0);
        }
        // PID has the throttle
        else {
            navData.targetHoldSpeedAlt = pidUpdate(navData.altPosPID, navData.holdAlt, ALTITUDE);
        }

        // constrain vertical velocity
        navData.targetHoldSpeedAlt = constrainFloat(navData.targetHoldSpeedAlt, (navData.holdMaxVertSpeed < p[NAV_MAX_DECENT]) ? -navData.holdMaxVertSpeed : -p[NAV_MAX_DECENT], navData.holdMaxVertSpeed);

        // smooth vertical velocity changes
        navData.holdSpeedAlt += (navData.targetHoldSpeedAlt - navData.holdSpeedAlt) * 0.05f;
    }
    else {
        navData.verticalOverride = 0;
    }

    // calculate POI angle (used for tilt in gimbal function)
    if (navData.mode == NAV_STATUS_MISSION && curLeg->poiAltitude != 0.0f) {
        float a, b, c;

        a = navData.holdDistance;
        b = ALTITUDE - curLeg->poiAltitude;
        c = __sqrtf(a*a + b*b);

        navData.poiAngle = asinf(a/c) * RAD_TO_DEG - 90.0f;
    }
    else {
        navData.poiAngle = 0.0f;
    }

    if (navData.mode == NAV_STATUS_MISSION) {
        // recalculate autonomous heading
        navSetHoldHeading(navData.targetHeading);

        // wait for low throttle if landing
        if (curLeg->type == NAV_LEG_LAND && motorsData.throttle <= 1)
            // shut everything down (sure hope we are really on the ground :)
            supervisorDisarm();
    }

    navData.lastUpdate = currentTime;
}
示例#5
0
void supervisorTaskCode(void *unused) {
    uint32_t count = 0;

    AQ_NOTICE("Supervisor task started\n");

    // wait for ADC vIn data
    while (analogData.batCellCount == 0)
        yield(100);

    supervisorCreateSOCTable();

    supervisorData.vInLPF = analogData.vIn;
    supervisorData.soc = 100.0f;

    while (1) {
        yield(1000/SUPERVISOR_RATE);

    if (supervisorData.state & STATE_CALIBRATION) {
        int i;

        // try to indicate completion percentage
        i = constrainInt(20*((calibData.percentComplete)/(100.0f/3.0f)), 1, 21);
        if (i > 20)
            digitalHi(supervisorData.readyLed);
        else if (!(count % i))
            digitalTogg(supervisorData.readyLed);
#ifdef SUPERVISOR_DEBUG_PORT
        i = constrainInt(20*((calibData.percentComplete-100.0f/3.0f)/(100.0f/3.0f)), 1, 21);
        if (i > 20)
            digitalHi(supervisorData.debugLed);
        else if (!(count % i))
            digitalTogg(supervisorData.debugLed);
#endif
        i = constrainInt(20*((calibData.percentComplete-100.0f/3.0f*2.0f)/(100.0f/3.0f)), 1, 21);
        if (i > 20)
            digitalHi(supervisorData.gpsLed);
        else if (!(count % i))
            digitalTogg(supervisorData.gpsLed);

        // user looking to go back to DISARMED mode?
        if (RADIO_THROT < p[CTRL_MIN_THROT] && RADIO_RUDD < -500) {
            if (!supervisorData.armTime) {
                supervisorData.armTime = timerMicros();
            }
            else if ((timerMicros() - supervisorData.armTime) > SUPERVISOR_DISARM_TIME) {
                supervisorDisarm();
                supervisorData.armTime = 0;
            }
        }
        else {
            supervisorData.armTime = 0;
        }
    }
    else if (supervisorData.state & STATE_DISARMED) {
#ifdef SUPERVISOR_DEBUG_PORT
        // 0.5 Hz blink debug LED if config file could be found on uSD card
        if (supervisorData.configRead && (!(count % SUPERVISOR_RATE))) {
            // only for first 15s
            if (timerMicros() > 15000000) {
                supervisorData.configRead = 0;
                digitalLo(supervisorData.debugLed);
            }
            else {
                digitalTogg(supervisorData.debugLed);
            }
        }
#endif

        // 1 Hz blink if disarmed, 5 Hz if writing to uSD card
        if (!(count % ((supervisorData.diskWait) ? SUPERVISOR_RATE/10 : SUPERVISOR_RATE/2)))
            digitalTogg(supervisorData.readyLed);

        // Arm if all the switches are in default (startup positions) - flaps down, aux2 centered
        if (RADIO_THROT < p[CTRL_MIN_THROT] && RADIO_RUDD > +500 && RADIO_FLAPS < -250 && !navData.homeActionFlag && navData.headFreeMode == NAV_HEADFREE_OFF) {
            if (!supervisorData.armTime) {
                supervisorData.armTime = timerMicros();
            }
            else if ((timerMicros() - supervisorData.armTime) > SUPERVISOR_DISARM_TIME) {
                supervisorArm();
                supervisorData.armTime = 0;
            }
        }
        else {
            supervisorData.armTime = 0;
        }

        // various functions
        if (RADIO_THROT < p[CTRL_MIN_THROT] && RADIO_RUDD < -500) {
#ifdef HAS_DIGITAL_IMU
            // tare function (lower left)
            if (RADIO_ROLL < -500 && RADIO_PITCH > +500) {
                supervisorTare();
            }
#endif
            // config write (upper right)
            if (RADIO_VALID && RADIO_ROLL > +500 && RADIO_PITCH < -500) {
                supervisorLEDsOn();
                configFlashWrite();
#ifdef DIMU_HAVE_EEPROM
                dIMURequestCalibWrite();
#endif
                supervisorLEDsOff();
            }

            // calibration mode (upper left)
            if (RADIO_ROLL < -500 && RADIO_PITCH < -500) {
                supervisorCalibrate();
            }
        }
    }
    else if (supervisorData.state & STATE_ARMED) {
        // Disarm only if in manual mode
        if (RADIO_THROT < p[CTRL_MIN_THROT] && RADIO_RUDD < -500 && navData.mode == NAV_STATUS_MANUAL) {
            if (!supervisorData.armTime) {
                supervisorData.armTime = timerMicros();
            }
            else if ((timerMicros() - supervisorData.armTime) > SUPERVISOR_DISARM_TIME) {
                supervisorDisarm();
                supervisorData.armTime = 0;
            }
        }
        else {
            supervisorData.armTime = 0;
        }
    }

    // radio loss
    if ((supervisorData.state & STATE_FLYING) && (navData.mode < NAV_STATUS_MISSION || (supervisorData.state & STATE_RADIO_LOSS2))) {
        // regained?
        if (RADIO_QUALITY > 1.0f) {
            supervisorData.lastGoodRadioMicros = timerMicros();

            if (supervisorData.state & STATE_RADIO_LOSS1)
                AQ_NOTICE("Warning: radio signal regained\n");

            supervisorData.state &= ~(STATE_RADIO_LOSS1 | STATE_RADIO_LOSS2);
        }
        // loss 1
        else if (!(supervisorData.state & STATE_RADIO_LOSS1) && (timerMicros() - supervisorData.lastGoodRadioMicros) > SUPERVISOR_RADIO_LOSS1) {
            supervisorData.state |= STATE_RADIO_LOSS1;
            AQ_NOTICE("Warning: Radio loss stage 1 detected\n");

            // hold position
            RADIO_FLAPS = 0;    // position hold
            RADIO_AUX2 = 0;     // normal home mode
            RADIO_PITCH = 0;    // center sticks
            RADIO_ROLL = 0;
            RADIO_RUDD = 0;
            RADIO_THROT = RADIO_MID_THROTTLE;  // center throttle
        }
        // loss 2
        else if (!(supervisorData.state & STATE_RADIO_LOSS2) && (timerMicros() - supervisorData.lastGoodRadioMicros) > SUPERVISOR_RADIO_LOSS2) {
            supervisorData.state |= STATE_RADIO_LOSS2;
            AQ_NOTICE("Warning: Radio loss stage 2! Initiating recovery mission.\n");

            // only available with GPS
            if (navData.navCapable) {
                navMission_t *wp;
                uint8_t wpi = 0;
                uint8_t fsOption = (uint8_t)p[SPVR_FS_RAD_ST2];

                navClearWaypoints();
                wp = navGetWaypoint(wpi++);

                if (fsOption > SPVR_OPT_FS_RAD_ST2_LAND && navCalcDistance(gpsData.lat, gpsData.lon, navData.homeLeg.targetLat, navData.homeLeg.targetLon) > SUPERVISOR_HOME_POS_DETECT_RADIUS) {
                    float targetAltitude;

                    // ascend
                    if (fsOption == SPVR_OPT_FS_RAD_ST2_ASCEND && ALTITUDE < navData.homeLeg.targetAlt + p[SPVR_FS_ADD_ALT]) {
                        // the home leg's altitude is recorded without pressure offset
                        targetAltitude = navData.homeLeg.targetAlt + p[SPVR_FS_ADD_ALT] + navData.presAltOffset;

                        wp->type = NAV_LEG_GOTO;
                        wp->relativeAlt = 0;
                        wp->targetAlt = targetAltitude;
                        wp->targetLat = gpsData.lat;
                        wp->targetLon = gpsData.lon;
                        wp->targetRadius = SUPERVISOR_HOME_ALT_DETECT_MARGIN;
                        wp->maxHorizSpeed = navData.homeLeg.maxHorizSpeed;
                        wp->maxVertSpeed = navData.homeLeg.maxVertSpeed;
                        wp->poiHeading = navData.homeLeg.poiHeading;
                        wp->loiterTime = 0;
                        wp->poiAltitude = 0.0f;

                        wp = navGetWaypoint(wpi++);
                    }
                    else {
                        // the greater of our current altitude or home's altitude
                        targetAltitude = ((ALTITUDE > navData.homeLeg.targetAlt) ? ALTITUDE : navData.homeLeg.targetAlt) + navData.presAltOffset;
                    }

                    // go home with previously determined altitude
                    wp->type = NAV_LEG_GOTO;
                    wp->relativeAlt = 0;
                    wp->targetAlt = targetAltitude;
                    wp->targetLat = navData.homeLeg.targetLat;
                    wp->targetLon = navData.homeLeg.targetLon;
                    wp->targetRadius = SUPERVISOR_HOME_POS_DETECT_RADIUS;
                    wp->maxHorizSpeed = navData.homeLeg.maxHorizSpeed;
                    wp->maxVertSpeed = navData.homeLeg.maxVertSpeed;
                    wp->poiHeading = navData.homeLeg.poiHeading;
                    wp->loiterTime = 0;
                    wp->poiAltitude = 0.0f;

                    wp = navGetWaypoint(wpi++);

                    // decend to home
                    wp->type = NAV_LEG_HOME;
                    wp->targetRadius = SUPERVISOR_HOME_POS_DETECT_RADIUS;
                    wp->loiterTime = 0;
                    wp->poiAltitude = 0.0f;

                    wp = navGetWaypoint(wpi++);
                }

                // land
                wp->type = NAV_LEG_LAND;
                wp->maxVertSpeed = p[NAV_LANDING_VEL];
                wp->maxHorizSpeed = 0.0f;
                wp->poiAltitude = 0.0f;

                // go
                navData.missionLeg = 0;
                RADIO_FLAPS = 500;                  // mission mode
            }
            // no GPS, slow decent in PH mode
            else {
                RADIO_FLAPS = 0;    // position hold
                RADIO_AUX2 = 0;     // normal home mode
                RADIO_PITCH = 0;    // center sticks
                RADIO_ROLL = 0;
                RADIO_RUDD = 0;
                RADIO_THROT = RADIO_MID_THROTTLE * 3 / 4;  // 1/4 max decent
            }
        }
    }

    // smooth vIn readings
    supervisorData.vInLPF += (analogData.vIn - supervisorData.vInLPF) * (0.1f / SUPERVISOR_RATE);

    // determine battery state of charge
    supervisorData.soc = supervisorSOCTableLookup(supervisorData.vInLPF);

    // low battery
    if (!(supervisorData.state & STATE_LOW_BATTERY1) && supervisorData.vInLPF < (p[SPVR_LOW_BAT1]*analogData.batCellCount)) {
        supervisorData.state |= STATE_LOW_BATTERY1;
        AQ_NOTICE("Warning: Low battery stage 1\n");

        // TODO: something
    }
    else if (!(supervisorData.state & STATE_LOW_BATTERY2) && supervisorData.vInLPF < (p[SPVR_LOW_BAT2]*analogData.batCellCount)) {
        supervisorData.state |= STATE_LOW_BATTERY2;
        AQ_NOTICE("Warning: Low battery stage 2\n");

        // TODO: something
    }

    if (supervisorData.state & STATE_FLYING) {
        // count flight time in seconds
        supervisorData.flightTime += (1.0f / SUPERVISOR_RATE);

        // calculate remaining flight time
        if (supervisorData.soc < 99.0f) {
            supervisorData.flightSecondsAvg += (supervisorData.flightTime / (100.0f - supervisorData.soc) - supervisorData.flightSecondsAvg) * (0.01f / SUPERVISOR_RATE);
            supervisorData.flightTimeRemaining = supervisorData.flightSecondsAvg * supervisorData.soc;
        }
        else {
            supervisorData.flightSecondsAvg = supervisorData.flightTime;
            supervisorData.flightTimeRemaining = 999.9f * 60.0f;		// unknown
        }

        // rapidly flash Ready LED if we are critically low on power
        if (supervisorData.state & STATE_LOW_BATTERY2)
            digitalTogg(supervisorData.readyLed);
    }
    else if (supervisorData.state & STATE_ARMED) {
        digitalHi(supervisorData.readyLed);
    }

#ifdef SUPERVISOR_DEBUG_PORT
    // DEBUG LED to indicate radio RX state
    if (!supervisorData.configRead && RADIO_INITIALIZED && supervisorData.state != STATE_CALIBRATION) {
        // packet received in the last 50ms?
        if (RADIO_VALID) {
            digitalHi(supervisorData.debugLed);
        }
        else {
            if (RADIO_BINDING)
                digitalTogg(supervisorData.debugLed);
            else
                digitalLo(supervisorData.debugLed);
        }
    }
#endif

    count++;

#ifdef USE_SIGNALING
    signalingEvent();
#endif
    }
}