// set headfree mode based on radio command
void navSetHeadFreeMode(void) {
// HF switch to set/dynamic position
// when disarmed one can also set the orientation heading in this position (for 2-pos control)
if (rcIsSwitchActive(NAV_CTRL_HF_SET) || (!(supervisorData.state & STATE_ARMED) && navData.headFreeMode != NAV_HEADFREE_LOCKED && rcIsSwitchActive(NAV_CTRL_HF_LOCK))) {
if (navData.headFreeMode != NAV_HEADFREE_DYNAMIC) {
if (navData.headFreeMode != NAV_HEADFREE_SETTING && navData.headFreeMode != NAV_HEADFREE_DYN_DELAY) {
navData.headFreeMode = NAV_HEADFREE_SETTING;
navData.hfDynamicModeTimer = timerMicros();
AQ_NOTICE("Set new reference heading for Heading-Free mode\n");
#ifdef USE_SIGNALING
signalingOnetimeEvent(SIG_EVENT_OT_HF_SET);
#endif
}
else if ((supervisorData.state & STATE_ARMED) && timerMicros() - navData.hfDynamicModeTimer > NAV_HF_DYNAMIC_DELAY) {
navData.headFreeMode = NAV_HEADFREE_DYNAMIC;
AQ_NOTICE("Now in dynamic Heading-Free DVH mode\n");
}
else if (navData.headFreeMode == NAV_HEADFREE_SETTING)
navData.headFreeMode = NAV_HEADFREE_DYN_DELAY;
}
}
// HF switch to locked/on position
else if (rcIsSwitchActive(NAV_CTRL_HF_LOCK)) {
if (navData.headFreeMode != NAV_HEADFREE_LOCKED) {
AQ_NOTICE("Now in locked Heading-Free DVH mode\n");
navData.headFreeMode = NAV_HEADFREE_LOCKED;
}
}
// HF channel at off position
else {
if (navData.headFreeMode > NAV_HEADFREE_DYN_DELAY)
AQ_NOTICE("Now in normal DVH mode\n");
navData.headFreeMode = NAV_HEADFREE_OFF;
}
}
static void spiStartTxn(spiStruct_t *interface) {
spiSlot_t *slot = &interface->slots[interface->tail];
spiClient_t *client = slot->client;
uint32_t tmp;
// is the current txn taking too long?
if (interface->txRunning != 0 && (timerMicros() - interface->txnStart) > SPI_MAX_TXN_TIME) {
spiDisableDMA(interface);
spiDisableSPI(interface);
spiDeselect(client);
interface->tail = (interface->tail + 1) % SPI_SLOTS;
slot = &interface->slots[interface->tail];
client = slot->client;
interface->txRunning = 0;
interface->txnTimeouts++;
}
if (interface->tail != interface->head && interface->txRunning == 0) {
interface->txRunning = 1;
interface->txnStart = timerMicros();
// set baud rate
tmp = interface->spi->CR1 & SPI_BAUD_MASK;
tmp |= client->baud;
interface->spi->CR1 = tmp;
// clear DR
SPI_I2S_ReceiveData(interface->spi);
spiSelect(client);
// specify "in transaction"
if (client->flag)
*client->flag = 0;
interface->rxDMAStream->M0AR = slot->rxBuf;
interface->rxDMAStream->NDTR = slot->size;
DMA_Cmd(interface->rxDMAStream, ENABLE);
interface->txDMAStream->M0AR = slot->txBuf;
interface->txDMAStream->NDTR = slot->size;
DMA_Cmd(interface->txDMAStream, ENABLE);
SPI_Cmd(interface->spi, ENABLE);
}
}
void hmc5983Decode(void) {
volatile uint8_t *d = hmc5983Data.rxBuf;
int32_t mag[3];
float divisor;
int i;
if (hmc5983Data.enabled) {
mag[0] = 0;
mag[1] = 0;
mag[2] = 0;
divisor = (float)HMC5983_SLOTS;
for (i = 0; i < HMC5983_SLOTS; i++) {
int j = i*HMC5983_SLOT_SIZE;
// check if we are in the middle of a transaction for this slot
if (i == hmc5983Data.slot && hmc5983Data.spiFlag == 0) {
divisor -= 1.0f;
}
else {
mag[1] += (int16_t)__rev16(*(uint16_t *)&d[j+1]);
mag[2] += (int16_t)__rev16(*(uint16_t *)&d[j+3]);
mag[0] += (int16_t)__rev16(*(uint16_t *)&d[j+5]);
}
}
divisor = 1.0f / divisor;
hmc5983ScaleMag(mag, hmc5983Data.rawMag, divisor);
hmc5983CalibMag(hmc5983Data.rawMag, hmc5983Data.mag);
hmc5983Data.lastUpdate = timerMicros();
}
}
void navUkfGpsVelUpdate(uint32_t gpsMicros, float velN, float velE, float velD, float sAcc) {
float y[3];
float noise[3];
float velDelta[3];
int histIndex;
y[0] = velN;
y[1] = velE;
y[2] = velD;
// determine how far back this GPS velocity update came from
histIndex = (timerMicros() - (gpsMicros + UKF_VEL_DELAY)) / (int)(1e6f * AQ_OUTER_TIMESTEP);
histIndex = navUkfData.navHistIndex - histIndex;
if (histIndex < 0)
histIndex += UKF_HIST;
if (histIndex < 0 || histIndex >= UKF_HIST)
histIndex = 0;
// calculate delta from current position
velDelta[0] = UKF_VELN - navUkfData.velN[histIndex];
velDelta[1] = UKF_VELE - navUkfData.velE[histIndex];
velDelta[2] = UKF_VELD - navUkfData.velD[histIndex];
// set current position state to historic data
UKF_VELN = navUkfData.velN[histIndex];
UKF_VELE = navUkfData.velE[histIndex];
UKF_VELD = navUkfData.velD[histIndex];
noise[0] = UKF_GPS_VEL_N + sAcc * __sqrtf(gpsData.tDOP*gpsData.tDOP + gpsData.nDOP*gpsData.nDOP) * UKF_GPS_VEL_M_N;
noise[1] = UKF_GPS_VEL_N + sAcc * __sqrtf(gpsData.tDOP*gpsData.tDOP + gpsData.eDOP*gpsData.eDOP) * UKF_GPS_VEL_M_N;
noise[2] = UKF_GPS_VD_N + sAcc * __sqrtf(gpsData.tDOP*gpsData.tDOP + gpsData.vDOP*gpsData.vDOP) * UKF_GPS_VD_M_N;
srcdkfMeasurementUpdate(navUkfData.kf, 0, y, 3, 3, noise, navUkfVelUpdate);
// add the historic position delta back to the current state
UKF_VELN += velDelta[0];
UKF_VELE += velDelta[1];
UKF_VELD += velDelta[2];
#ifdef UKF_LOG_FNAME
{
float *log = (float *)&ukfLog[navUkfData.logPointer];
int i = 0;
*(uint32_t *)&log[i++] = 0xffffffff;
log[i++] = y[0];
log[i++] = y[1];
log[i++] = y[2];
log[i++] = noise[0];
log[i++] = noise[1];
log[i++] = noise[2];
log[i++] = velDelta[0];
log[i++] = velDelta[1];
log[i++] = velDelta[2];
navUkfData.logPointer = (navUkfData.logPointer + UKF_LOG_SIZE) % UKF_LOG_BUF_SIZE;
filerSetHead(navUkfData.logHandle, navUkfData.logPointer);
}
#endif
}
void motorsReceiveTelem(uint8_t canId, uint8_t doc, void *p) {
uint32_t *data = (uint32_t *)p;
uint32_t *storage = (uint32_t *)&motorsData.canStatus[canId-1];
uint32_t micros = timerMicros();
#ifdef MOTORS_CAN_LOGGING
motorsLog_t *buf = (motorsLog_t *)(motorsLogBuf + motorsData.head);
buf->sync = 0xff;
buf->escId = 0xc0 | canId; // 2 MSB are high as part of sync bits
buf->micros = micros;
buf->data[0] = data[0];
buf->data[1] = data[1];
motorsData.head = (motorsData.head + sizeof(motorsLog_t)) % sizeof(motorsLogBuf);
filerSetHead(motorsData.logHandle, motorsData.head);
#endif
// copy status data to our storage (8 bytes)
storage[0] = data[0];
storage[1] = data[1];
// record reception time
motorsData.canStatusTime[canId-1] = micros;
}
static void spiNotify(spiClient_t *client) {
if (client->flag)
*client->flag = timerMicros();
if (client->callback)
client->callback(0);
}
static void motorsCanRequestTelem(int motorId) {
#if MOTORS_CAN_TELEM_RATE > 0
// request telemetry
canSetTelemetryValueNoWait(CAN_TT_NODE, motorsData.can[motorId]->networkId, 0, CAN_TELEM_STATUS);
canSetTelemetryRateNoWait(CAN_TT_NODE, motorsData.can[motorId]->networkId, MOTORS_CAN_TELEM_RATE);
motorsData.canTelemReqTime[motorId] = timerMicros();
#endif
}
// every ~15.25us
void ADC_DMA_HANDLER(void) {
register uint32_t flag = ADC_DMA_ISR;
register unsigned long *s, *a;
register uint16_t *w;
int i;
// clear intr flags
ADC_DMA_CR = (uint32_t)ADC_DMA_FLAGS;
// second half?
w = ((flag & ADC_DMA_TC_FLAG) == RESET) ? adcDMAData.adc123Raw1 : adcDMAData.adc123Raw2;
// accumulate totals
s = adcData.interrupt123Sums;
*s++ += (w[36] + w[39] + w[42] + w[45]); // rateX
*s++ += (w[1] + w[4] + w[7] + w[10]); // rateY
*s++ += (w[2] + w[5] + w[8] + w[11]); // rateZ
*s++ += (w[0] + w[3] + w[6] + w[9]); // magX
*s++ += (w[12] + w[15] + w[18] + w[21]); // magY
*s++ += (w[24] + w[27] + w[30] + w[33]); // magZ
*s++ += (w[14]); // temp1
*s++ += (w[32]); // Vin
*s++ += (w[13] + w[16] + w[19] + w[22]); // accX
*s++ += (w[25] + w[28] + w[31] + w[34]); // accY
*s++ += (w[37] + w[40] + w[43] + w[46]); // accZ
*s++ += (w[20] + w[23] + w[26] + w[29]); // press1
*s++ += (w[38] + w[41] + w[44] + w[47]); // press2
*s++ += (w[17]); // temp2
*s += (w[35]); // temp3
if (++adcData.sample == ADC_SAMPLES) {
register unsigned long micros = timerMicros();
adcData.sample = 0;
if (++adcData.loops & 0x01) {
if (ADC_MAG_SIGN) {
digitalLo(adcData.magSetReset);
} else {
digitalHi(adcData.magSetReset);
}
}
s = adcData.interrupt123Sums;
a = (unsigned long *)adcData.adcSums;
for (i = 0; i < ADC_SENSORS; i++) {
*a++ = *s;
*s++ = 0;
}
adcData.sampleTime = micros - adcData.lastSample;
adcData.lastSample = micros;
CoEnterISR();
isr_SetFlag(adcData.adcFlag);
CoExitISR();
}
}
static void motorsCheckCanStatus(int motorId) {
#if MOTORS_CAN_TELEM_RATE > 0
// no status report within the last second?
if ((timerMicros() - motorsData.canStatusTime[motorId]) > 1e6f) {
// has it been more than 1 second since our last request?
if ((timerMicros() - motorsData.canTelemReqTime[motorId]) > 1e6f) {
// clear status information
uint32_t *storage = (uint32_t *)&motorsData.canStatus[motorId];
storage[0] = 0;
storage[1] = 0;
motorsCanRequestTelem(motorId);
}
}
// if ESC is reporting as being disarmed (and should not be)
else if (motorsData.canStatus[motorId].state == ESC32_STATE_DISARMED && (supervisorData.state & STATE_ARMED)) {
// send an arm command
canCommandArm(CAN_TT_NODE, motorsData.can[motorId]->networkId);
}
#endif
}
int futabaCharIn(radioInstance_t *r, uint8_t c) {
// force top of frame if its been more than 3ms since last input
// this is a safeguard in case we started receiving bytes in the middle of a frame
// shortest Futaba frame = 7ms - 3ms for data = 4ms minimum gap
if (timerMicros() - futabaData.lastCharReceived > 3000)
futabaData.state = FUTABA_WAIT_SYNC;
futabaData.lastCharReceived = timerMicros();
switch (futabaData.state) {
case FUTABA_WAIT_SYNC:
if (c == FUTABA_START_CHAR) {
futabaData.state = FUTABA_WAIT_DATA;
futabaData.dataCount = 0;
futabaData.validFrame = 0;
}
break;
case FUTABA_WAIT_DATA:
futabaData.u.rawBuf[futabaData.dataCount++] = c;
if (futabaData.dataCount == 23)
futabaData.state = FUTABA_WAIT_END;
// make sure at least one channel value byte is > 0
// all zeros means something is wrong with Rx (observed in the wild)
if (c)
futabaData.validFrame = 1;
break;
case FUTABA_WAIT_END:
futabaData.state = FUTABA_WAIT_SYNC;
if ((c & FUTABA_END_CHAR) == 0 && futabaData.validFrame)
return futabaDecode(r);
break;
}
return 0;
}
void max21100Decode(void) {
volatile uint8_t *d = max21100Data.rxBuf;
int32_t acc[3], temp, gyo[3];
float divisor;
int i;
for (i = 0; i < 3; i++) {
acc[i] = 0;
gyo[i] = 0;
}
temp = 0;
divisor = (float)MAX21100_SLOTS;
for (i = 0; i < MAX21100_SLOTS; i++) {
int j = i*MAX21100_SLOT_SIZE;
// check if we are in the middle of a transaction for this slot
if (i == max21100Data.slot && max21100Data.spiFlag == 0) {
divisor -= 1.0f;
}
else {
gyo[0] += (int16_t)__rev16(*(uint16_t *)&d[j+1]);
gyo[1] += (int16_t)__rev16(*(uint16_t *)&d[j+3]);
gyo[2] += (int16_t)__rev16(*(uint16_t *)&d[j+5]);
acc[0] += (int16_t)__rev16(*(uint16_t *)&d[j+7]);
acc[1] += (int16_t)__rev16(*(uint16_t *)&d[j+9]);
acc[2] += (int16_t)__rev16(*(uint16_t *)&d[j+11]);
temp += (int16_t)__rev16(*(uint16_t *)&d[j+19]);
}
}
divisor = 1.0f / divisor;
max21100Data.rawTemp = temp * divisor * (1.0f / 255.0f);
max21100Data.temp = utilFilter(&max21100Data.tempFilter, max21100Data.rawTemp);
max21100ScaleAcc(acc, max21100Data.rawAcc, divisor);
max21100CalibAcc(max21100Data.rawAcc, max21100Data.acc);
max21100ScaleGyo(gyo, max21100Data.rawGyo, divisor);
max21100CalibGyo(max21100Data.rawGyo, max21100Data.gyo);
max21100Data.lastUpdate = timerMicros();
}
static void radioProcessInstance(radioInstance_t *r) {
serialPort_t *s = r->serialPort;
int8_t q;
switch (r->radioType) {
case RADIO_TYPE_SPEKTRUM11:
case RADIO_TYPE_SPEKTRUM10:
case RADIO_TYPE_DELTANG:
while (serialAvailable(s))
if (spektrumCharIn(r, serialRead(s))) {
r->lastUpdate = timerMicros();
radioReceptionQuality(r, 1);
}
break;
case RADIO_TYPE_SBUS:
while (serialAvailable(s))
if ((q = futabaCharIn(r, serialRead(s)))) {
r->lastUpdate = timerMicros();
radioReceptionQuality(r, q);
}
break;
case RADIO_TYPE_PPM:
if (ppmDataAvailable(r))
r->lastUpdate = timerMicros();
break;
case RADIO_TYPE_SUMD:
while (serialAvailable(s))
if ((q = grhottCharIn(r, serialRead(s)))) {
r->lastUpdate = timerMicros();
radioReceptionQuality(r, q);
}
break;
case RADIO_TYPE_MLINK:
while (serialAvailable(s))
if ((q = mlinkrxCharIn(r, serialRead(s)))) {
r->lastUpdate = timerMicros();
radioReceptionQuality(r, q);
}
break;
}
// no radio signal?
if (timerMicros() - r->lastUpdate > RADIO_UPDATE_TIMEOUT)
radioReceptionQuality(r, -1); // minimum signal quality (0%) if no updates within timeout value
else if (r->radioType == RADIO_TYPE_PPM)
radioReceptionQuality(r, ppmGetSignalQuality(r)); // signal quality based on PPM status
}
static void dIMUTaskCode(void *unused) {
uint32_t loops = 0;
while (1) {
// wait for work
CoWaitForSingleFlag(dImuData.flag, 0);
if (dImuData.calibReadWriteFlag)
dIMUReadWriteCalib();
// double rate gyo loop
#ifdef DIMU_HAVE_MPU6000
mpu6000DrateDecode();
#endif
#ifdef DIMU_HAVE_MAX21100
max21100DrateDecode();
#endif
imuDImuDRateReady();
// full sensor loop
if (!(loops % (DIMU_OUTER_PERIOD/DIMU_INNER_PERIOD))) {
#ifdef DIMU_HAVE_MPU6000
mpu6000Decode();
#endif
#ifdef DIMU_HAVE_MAX21100
max21100Decode();
#endif
#ifdef DIMU_HAVE_HMC5983
hmc5983Decode();
#endif
#ifdef DIMU_HAVE_MS5611
ms5611Decode();
#endif
dImuData.lastUpdate = timerMicros();
imuDImuSensorReady();
dIMUCalcTempDiff();
}
loops++;
}
}
static void spiEndTxn(spiStruct_t *interface) {
uint8_t tail = interface->tail;
spiClient_t *client = interface->slots[tail].client;
uint32_t tmp;
spiDisableSPI(interface);
spiDisableDMA(interface);
spiDeselect(client);
// record longest txn
tmp = timerMicros() - interface->txnStart;
if (tmp > interface->txnMaxTime)
interface->txnMaxTime = tmp;
spiNotify(client);
tail = (tail + 1) % SPI_SLOTS;
interface->tail = tail;
interface->txRunning = 0;
spiTriggerSchedule(client->interface);
}
void canSensorsReceiveTelem(uint8_t canId, uint8_t doc, void *data) {
canSensorsData.values[canId] = *(float *)data;
// record reception time
canSensorsData.rcvTimes[canId] = timerMicros();
}
void gimbalUpdate(void) {
uint16_t pwm;
float tilt, pitch;
if (timerMicros() < 5e6f)
return;
// calculate manual/PoI tilt angle override
if (p[GMBL_TILT_PORT]) {
tilt = RADIO_AUX3 + navData.poiAngle * GMBL_DEGREES_TO_PW * p[GMBL_SCAL_PITCH];
// smooth
if (tilt != gimbalData.tilt)
gimbalData.tilt -= (gimbalData.tilt - tilt) * p[GMBL_SLEW_RATE];
}
// stabilized pitch axis output
if (gimbalData.pitchPort) {
pitch = -AQ_PITCH * GMBL_DEGREES_TO_PW * p[GMBL_SCAL_PITCH];
// stabilization AND manual/PoI override output to pitchPort
if (p[GMBL_TILT_PORT] == p[GMBL_PITCH_PORT])
pitch += gimbalData.tilt;
pwm = constrainInt(pitch + p[GMBL_NTRL_PITCH], p[GMBL_PWM_MIN_PT], p[GMBL_PWM_MAX_PT]);
*gimbalData.pitchPort->ccr = pwm;
}
// unstabilized cameara tilt (pitch) output with manual and PoI override
if (gimbalData.tiltPort) {
pwm = constrainInt(gimbalData.tilt + p[GMBL_NTRL_PITCH], p[GMBL_PWM_MIN_PT], p[GMBL_PWM_MAX_PT]);
*gimbalData.tiltPort->ccr = pwm;
}
// stabilized roll axis output
if (gimbalData.rollPort) {
if (p[GMBL_ROLL_EXPO])
pwm = constrainInt((-AQ_ROLL * (fabsf(AQ_ROLL) / (100 / p[GMBL_ROLL_EXPO]))) * GMBL_DEGREES_TO_PW * p[GMBL_SCAL_ROLL] + p[GMBL_NTRL_ROLL], p[GMBL_PWM_MIN_RL], p[GMBL_PWM_MAX_RL]);
else
pwm = constrainInt(-AQ_ROLL * GMBL_DEGREES_TO_PW * p[GMBL_SCAL_ROLL] + p[GMBL_NTRL_ROLL], p[GMBL_PWM_MIN_RL], p[GMBL_PWM_MAX_RL]);
*gimbalData.rollPort->ccr = pwm;
}
// trigger output, manual/passthrough or automatic modes
if (gimbalData.triggerPort) {
pwm = RADIO_GEAR + GMBL_TRIG_NTRL_PWM;
// manual trigger active
if (RADIO_GEAR > p[GMBL_TRIG_CH_NEU] + p[CTRL_DEAD_BAND] || RADIO_GEAR < p[GMBL_TRIG_CH_NEU] - p[CTRL_DEAD_BAND]) {
gimbalData.trigger = 0; // cancel automatic trigger, if any
gimbalData.triggerTimer = 0;
if (!gimbalData.triggerLogVal) // first activation after channel had returned to neutral
gimbalData.triggerLogVal = ++gimbalData.triggerCount;
}
// automated trigger detection if trigger channel at neutral position
else {
// if trigger is not already active
if (!gimbalData.triggerTimer) {
gimbalData.triggerLogVal = 0; // trigger should be off at this point (auto or manual)
if (!gimbalData.trigger && (supervisorData.state & STATE_FLYING)) {
if (p[GMBL_TRIG_TIME] && timerMicros() - gimbalData.triggerLastTime >= p[GMBL_TRIG_TIME] * 1e6) {
gimbalData.trigger = 1;
gimbalData.triggerLastTime = timerMicros();
AQ_NOTICE("Time trigger activated.\n");
}
else if (p[GMBL_TRIG_DIST] && (gimbalData.triggerLastLat != gpsData.lat || gimbalData.triggerLastLon != gpsData.lon) &&
navCalcDistance(gimbalData.triggerLastLat, gimbalData.triggerLastLon, gpsData.lat, gpsData.lon) >= p[GMBL_TRIG_DIST]) {
gimbalData.trigger = 1;
gimbalData.triggerLastLat = gpsData.lat;
gimbalData.triggerLastLon = gpsData.lon;
AQ_NOTICE("Distance trigger activated.\n");
}
}
if (gimbalData.trigger) {
gimbalData.triggerTimer = timerMicros();
gimbalData.triggerLogVal = ++gimbalData.triggerCount;
gimbalData.trigger = 0;
}
}
// if trigger is active, keep correct pwm output until timeout
if (gimbalData.triggerTimer) {
if (timerMicros() - gimbalData.triggerTimer > p[GMBL_TRIG_ON_TIM] * 1e3)
gimbalData.triggerTimer = 0;
else
pwm = p[GMBL_TRIG_ON_PWM];
}
} // end auto trigger
pwm = constrainInt(pwm, GMBL_TRIG_MIN_PWM, GMBL_TRIG_MAX_PWM);
*gimbalData.triggerPort->ccr = pwm;
}
// simple passthrough output of any channel
if (gimbalData.passthroughPort) {
pwm = constrainInt(radioData.channels[(int)(p[GMBL_PSTHR_CHAN]-1)] + GMBL_TRIG_NTRL_PWM, GMBL_TRIG_MIN_PWM, GMBL_TRIG_MAX_PWM);
*gimbalData.passthroughPort->ccr = pwm;
}
}
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 delayMicros(unsigned long t) {
t = t + timerMicros();
while (timerMicros() < t)
;
}
void navUkfGpsPosUpdate(uint32_t gpsMicros, double lat, double lon, float alt, float hAcc, float vAcc) {
float y[3];
float noise[3];
float posDelta[3];
int histIndex;
if (navUkfData.holdLat == (double)0.0) {
navUkfData.holdLat = lat;
navUkfData.holdLon = lon;
navUkfCalcEarthRadius(lat);
navUkfSetGlobalPositionTarget(lat, lon);
navUkfResetPosition(-UKF_POSN, -UKF_POSE, alt - UKF_POSD);
}
else {
navUkfCalcGlobalDistance(lat, lon, &y[0], &y[1]);
y[2] = alt;
// determine how far back this GPS position update came from
histIndex = (timerMicros() - (gpsMicros + UKF_POS_DELAY)) / (int)(1e6f * AQ_OUTER_TIMESTEP);
histIndex = navUkfData.navHistIndex - histIndex;
if (histIndex < 0)
histIndex += UKF_HIST;
if (histIndex < 0 || histIndex >= UKF_HIST)
histIndex = 0;
// calculate delta from current position
posDelta[0] = UKF_POSN - navUkfData.posN[histIndex];
posDelta[1] = UKF_POSE - navUkfData.posE[histIndex];
posDelta[2] = UKF_POSD - navUkfData.posD[histIndex];
// set current position state to historic data
UKF_POSN = navUkfData.posN[histIndex];
UKF_POSE = navUkfData.posE[histIndex];
UKF_POSD = navUkfData.posD[histIndex];
noise[0] = UKF_GPS_POS_N + hAcc * __sqrtf(gpsData.tDOP*gpsData.tDOP + gpsData.nDOP*gpsData.nDOP) * UKF_GPS_POS_M_N;
noise[1] = UKF_GPS_POS_N + hAcc * __sqrtf(gpsData.tDOP*gpsData.tDOP + gpsData.eDOP*gpsData.eDOP) * UKF_GPS_POS_M_N;
noise[2] = UKF_GPS_ALT_N + vAcc * __sqrtf(gpsData.tDOP*gpsData.tDOP + gpsData.vDOP*gpsData.vDOP) * UKF_GPS_ALT_M_N;
srcdkfMeasurementUpdate(navUkfData.kf, 0, y, 3, 3, noise, navUkfPosUpdate);
// add the historic position delta back to the current state
UKF_POSN += posDelta[0];
UKF_POSE += posDelta[1];
UKF_POSD += posDelta[2];
#ifdef UKF_LOG_FNAME
{
float *log = (float *)&ukfLog[navUkfData.logPointer];
int i = 0;
*(uint32_t *)&log[i++] = 0xffffffff;
log[i++] = y[0];
log[i++] = y[1];
log[i++] = y[2];
log[i++] = noise[0];
log[i++] = noise[1];
log[i++] = noise[2];
log[i++] = posDelta[0];
log[i++] = posDelta[1];
log[i++] = posDelta[2];
navUkfData.logPointer = (navUkfData.logPointer + UKF_LOG_SIZE) % UKF_LOG_BUF_SIZE;
filerSetHead(navUkfData.logHandle, navUkfData.logPointer);
}
#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
}
}
void navDoUserCommands(unsigned char *leg, navMission_t *curLeg) {
// home set
if (rcIsSwitchActive(NAV_CTRL_HOM_SET)) {
if (!navData.homeActionFlag && (supervisorData.state & STATE_ARMED) && navData.navCapable)
navSetHomeCurrent();
navData.homeActionFlag = 1;
}
// recall home
else if (rcIsSwitchActive(NAV_CTRL_HOM_GO)) {
if (!navData.homeActionFlag && (supervisorData.state & STATE_ARMED) && navData.navCapable) {
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 (rcIsControlConfigured(NAV_CTRL_HF_SET) || rcIsControlConfigured(NAV_CTRL_HF_LOCK)) {
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;
unsigned long currentTime = IMU_LASTUPD;
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 && navData.navCapable && (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);
}
}
// waypoint recording/skip: switch active for 1s = record waypoint or skip to next wpt if already in mission mode
if (rcIsControlConfigured(NAV_CTRL_WP_REC)) {
if (rcIsSwitchActive(NAV_CTRL_WP_REC)) {
if (!navData.wpActionFlag) {
if (!navData.wpRecTimer) {
navData.wpRecTimer = timerMicros();
}
else if (timerMicros() - navData.wpRecTimer > 1e6) {
if (rcIsSwitchActive(NAV_CTRL_MISN)) {
if (*leg + 1 < navGetWaypointCount())
++*leg;
else
*leg = 0;
curLeg = navLoadLeg(*leg);
} else if (RADIO_ROLL < 500 && RADIO_PITCH < 500)
navRecordWaypoint();
navData.wpRecTimer = 0;
navData.wpActionFlag = 1;
}
}
} else {
navData.wpRecTimer = 0;
navData.wpActionFlag = 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;
}
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;
}
