uint8_t navRecordWaypoint(void) {
if (!navData.navCapable || navData.mode >= NAV_STATUS_MISSION)
return 0;
uint8_t idx = 0;
if (navData.tempMissionLoaded)
navClearWaypoints();
else
idx = navGetWaypointCount();
if (idx >= NAV_MAX_MISSION_LEGS)
return 0;
navData.missionLegs[idx].type = NAV_LEG_GOTO;
navData.missionLegs[idx].targetAlt = gpsData.height;
navData.missionLegs[idx].targetLat = gpsData.lat;
navData.missionLegs[idx].targetLon = gpsData.lon;
navData.missionLegs[idx].maxHorizSpeed = NAV_DFLT_HOR_SPEED;
navData.missionLegs[idx].maxVertSpeed = NAV_DFLT_VRT_SPEED;
navData.missionLegs[idx].targetRadius = 1.0f;
navData.missionLegs[idx].loiterTime = 1e6;
navData.missionLegs[idx].poiHeading = AQ_YAW;
navData.missionLegs[idx].relativeAlt = 0;
navData.missionLegs[idx].poiAltitude = 0;
AQ_PRINTF("NAV: Waypoint %d recorded\n", idx);
#ifdef USE_SIGNALING
signalingOnetimeEvent(SIG_EVENT_OT_WP_RECORDED);
#endif
#ifdef USE_MAVLINK
mavlinkWpSendCount();
#endif
return 1;
}
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
}
}
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
}
}