Exemplo n.º 1
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
    }
}
Exemplo n.º 2
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
    }
}