void configInit(void) { float ver; #if 1 configLoadDefault(); #else // start with what's in flash configFlashRead(); // try to load any config params from uSD card if (configReadFile(0) < 0) // clear config if read error memset(p, 0, sizeof(p)); else supervisorConfigRead(); // get flash version ver = *(float *)flashStartAddr(); if (isnan(ver)) ver = 0.0f; // if compiled defaults are greater than flash version and loaded version if (DEFAULT_CONFIG_VERSION > ver && DEFAULT_CONFIG_VERSION > p[CONFIG_VERSION]) configLoadDefault(); // if flash version greater than or equal to currently loaded version else if (ver >= p[CONFIG_VERSION]) configFlashRead(); // if loaded version greater than flash version if (p[CONFIG_VERSION] > ver) configFlashWrite(); #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 } }
static void commandExecute(commandBufStruct_t *b) { switch (b->commandId) { case COMMAND_GPS_PACKET: // first character is length gpsSendPacket(*b->data, b->data+1); commandAck(NULL, 0); break; // case COMMAND_ID_GPS_PASSTHROUGH: // if (!(supervisorData.state & STATE_FLYING)) { // telemetryDisable(); // commandAck(NULL, 0); // gpsPassthrough(gpsData.gpsPort, downlinkData.serialPort); // } // else { // commandNack(NULL, 0); // } // break; case COMMAND_CONFIG_PARAM_READ: { unsigned int replyLength = configParameterRead(b->data); if (replyLength) commandAck(b->data, replyLength+8); else commandNack(NULL, 0); } break; case COMMAND_CONFIG_PARAM_WRITE: if (!(supervisorData.state & STATE_FLYING)) { unsigned int replyLength = configParameterWrite(b->data); if (replyLength) commandAck(b->data, replyLength); else commandNack(NULL, 0); } else { commandNack(NULL, 0); } break; case COMMAND_CONFIG_FLASH_READ: if (!(supervisorData.state & STATE_FLYING)) { configFlashRead(); commandAck(NULL, 0); } else { commandNack(NULL, 0); } break; case COMMAND_CONFIG_FLASH_WRITE: if (!(supervisorData.state & STATE_FLYING)) { if (configFlashWrite()) commandAck(NULL, 0); else commandNack(NULL, 0); } else { commandNack(NULL, 0); } break; case COMMAND_CONFIG_FACTORY_RESET: if (!(supervisorData.state & STATE_FLYING)) { configLoadDefault(); commandAck(NULL, 0); } else { commandNack(NULL, 0); } break; case COMMAND_TELEMETRY_DISABLE: telemetryDisable(); commandAck(NULL, 0); break; case COMMAND_TELEMETRY_ENABLE: telemetryEnable(); commandAck(NULL, 0); break; case COMMAND_ACC_IN: commandAccIn(b); break; default: commandNack(NULL, 0); break; } }