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;
}
}
