void supervisorLEDsOff(void) {
#ifdef SUPERVISOR_DEBUG_PORT
digitalLo(supervisorData.debugLed);
#endif
digitalLo(supervisorData.readyLed);
digitalLo(supervisorData.gpsLed);
}
void PVD_IRQHandler(void) {
// voltage dropping too low
if (EXTI_GetITStatus(EXTI_Line16) != RESET) {
// shut everything down
runDisarm(REASON_LOW_VOLTAGE);
// turn on both LEDs
digitalLo(statusLed);
digitalLo(errorLed);
EXTI_ClearITPendingBit(EXTI_Line16);
}
}
void selectRFM(uint8_t which)
{
digitalHi(GPIOB,GPIO_Pin_12);
digitalHi(GPIOC,GPIO_Pin_15);
switch (which) {
case 1: digitalLo(GPIOB,GPIO_Pin_12);
break;
case 2: digitalLo(GPIOC,GPIO_Pin_15);
break;
default:
break;
}
}
static void i2cUnstick(void)
{
GPIO_TypeDef *gpio;
gpio_config_t cfg;
uint16_t scl, sda;
int i;
// prepare pins
gpio = i2cHardwareMap[I2Cx_index].gpio;
scl = i2cHardwareMap[I2Cx_index].scl;
sda = i2cHardwareMap[I2Cx_index].sda;
digitalHi(gpio, scl | sda);
cfg.pin = scl | sda;
cfg.speed = Speed_2MHz;
cfg.mode = Mode_Out_OD;
gpioInit(gpio, &cfg);
for (i = 0; i < 8; i++) {
// Wait for any clock stretching to finish
while (!digitalIn(gpio, scl))
delayMicroseconds(10);
// Pull low
digitalLo(gpio, scl); // Set bus low
delayMicroseconds(10);
// Release high again
digitalHi(gpio, scl); // Set bus high
delayMicroseconds(10);
}
// Generate a start then stop condition
// SCL PB10
// SDA PB11
digitalLo(gpio, sda); // Set bus data low
delayMicroseconds(10);
digitalLo(gpio, scl); // Set bus scl low
delayMicroseconds(10);
digitalHi(gpio, scl); // Set bus scl high
delayMicroseconds(10);
digitalHi(gpio, sda); // Set bus sda high
// Init pins
cfg.pin = scl | sda;
cfg.speed = Speed_2MHz;
cfg.mode = Mode_AF_OD;
gpioInit(gpio, &cfg);
}
void signalLED(LEDNUM_t led_num, LEDMODE_t led_mode)
{
switch (led_mode)
{
case LEDMODE_OFF:
digitalLo(gpio_cfg[led_num].gpio, gpio_cfg[led_num].pin);
break;
case LEDMODE_ON:
digitalHi(gpio_cfg[led_num].gpio, gpio_cfg[led_num].pin);
break;
case LEDMODE_TOGGLE:
digitalToggle(gpio_cfg[led_num].gpio, gpio_cfg[led_num].pin);
break;
case LEDMODE_BLINK1:
digitalHi(gpio_cfg[led_num].gpio, gpio_cfg[led_num].pin);
xTimerChangePeriod(LedTimer[led_num], 500, 10);
break;
case LEDMODE_BLINK2:
break;
}
}
void main(void) {
rccInit();
timerInit();
configInit();
adcInit();
fetInit();
serialInit();
runInit();
cliInit();
owInit();
runDisarm(REASON_STARTUP);
inputMode = ESC_INPUT_PWM;
fetSetDutyCycle(0);
fetBeep(200, 100);
fetBeep(300, 100);
fetBeep(400, 100);
fetBeep(500, 100);
fetBeep(400, 100);
fetBeep(300, 100);
fetBeep(200, 100);
pwmInit();
statusLed = digitalInit(GPIO_STATUS_LED_PORT, GPIO_STATUS_LED_PIN);
digitalHi(statusLed);
errorLed = digitalInit(GPIO_ERROR_LED_PORT, GPIO_ERROR_LED_PIN);
digitalHi(errorLed);
#ifdef ESC_DEBUG
tp = digitalInit(GPIO_TP_PORT, GPIO_TP_PIN);
digitalLo(tp);
#endif
// self calibrating idle timer loop
{
volatile unsigned long cycles;
volatile unsigned int *DWT_CYCCNT = (int *)0xE0001004;
volatile unsigned int *DWT_CONTROL = (int *)0xE0001000;
volatile unsigned int *SCB_DEMCR = (int *)0xE000EDFC;
*SCB_DEMCR = *SCB_DEMCR | 0x01000000;
*DWT_CONTROL = *DWT_CONTROL | 1; // enable the counter
minCycles = 0xffff;
while (1) {
idleCounter++;
NOPS_4;
cycles = *DWT_CYCCNT;
*DWT_CYCCNT = 0; // reset the counter
// record shortest number of instructions for loop
totalCycles += cycles;
if (cycles < minCycles)
minCycles = cycles;
}
}
}
static void beepRev5(bool onoff)
{
if (onoff) {
digitalHi(BEEP_GPIO, BEEP_PIN);
} else {
digitalLo(BEEP_GPIO, BEEP_PIN);
}
}
void digitalTogg(digitalPin *p) {
if (digitalGet(p)) {
digitalLo(p);
}
else {
digitalHi(p);
}
}
void setTxSignal(escSerial_t *escSerial, uint8_t state)
{
if (state) {
digitalHi(escSerial->rxTimerHardware->gpio, escSerial->rxTimerHardware->pin);
} else {
digitalLo(escSerial->rxTimerHardware->gpio, escSerial->rxTimerHardware->pin);
}
}
void setTxSignal(softSerial_t *softSerial, uint8_t state)
{
if ((state == 1 && softSerial->isInverted == false) || (state == 0 && softSerial->isInverted == true)) {
digitalHi(softSerial->txTimerHardware->gpio, softSerial->txTimerHardware->pin);
} else {
digitalLo(softSerial->txTimerHardware->gpio, softSerial->txTimerHardware->pin);
}
}
void spiSelect(bool select)
{
if (select) {
digitalLo(GPIOB, Pin_12);
} else {
digitalHi(GPIOB, Pin_12);
}
}
// 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();
}
}
void runDisarm(int reason) {
fetSetDutyCycle(0);
timerCancelAlarm2();
state = ESC_STATE_DISARMED;
pwmIsrAllOn();
digitalHi(statusLed); // turn off
digitalLo(errorLed); // turn on
disarmReason = reason;
}
void spiSelect(bool select)
{
if (!isSPI)
return;
if (select) {
digitalLo(GPIOB, Pin_12);
} else {
digitalHi(GPIOB, Pin_12);
}
}
void runArm(void) {
fetSetDutyCycle(0);
timerCancelAlarm2();
digitalHi(errorLed);
state = ESC_STATE_STOPPED;
digitalLo(statusLed); // turn on
if (inputMode == ESC_INPUT_UART)
runMode = OPEN_LOOP;
fetSetBraking(0);
fetBeep(150, 800);
}
static void i2cUnstick(void)
{
gpio_config_t gpio;
uint8_t i;
gpio.pin = Pin_10 | Pin_11;
gpio.speed = Speed_2MHz;
gpio.mode = Mode_Out_OD;
gpioInit(GPIOB, &gpio);
digitalHi(GPIOB, Pin_10 | Pin_11);
for (i = 0; i < 8; i++) {
// Wait for any clock stretching to finish
while (!digitalIn(GPIOB, Pin_10))
delayMicroseconds(10);
// Pull low
digitalLo(GPIOB, Pin_10); // Set bus low
delayMicroseconds(10);
// Release high again
digitalHi(GPIOB, Pin_10); // Set bus high
delayMicroseconds(10);
}
// Generate a start then stop condition
// SCL PB10
// SDA PB11
digitalLo(GPIOB, Pin_11); // Set bus data low
delayMicroseconds(10);
digitalLo(GPIOB, Pin_10); // Set bus scl low
delayMicroseconds(10);
digitalHi(GPIOB, Pin_10); // Set bus scl high
delayMicroseconds(10);
digitalHi(GPIOB, Pin_11); // Set bus sda high
// Init pins
gpio.pin = Pin_10 | Pin_11;
gpio.speed = Speed_2MHz;
gpio.mode = Mode_AF_OD;
gpioInit(GPIOB, &gpio);
}
void setTxSignal(softSerial_t *softSerial, uint8_t state)
{
if (softSerial->port.inversion == SERIAL_INVERTED) {
state = !state;
}
if (state) {
digitalHi(softSerial->txTimerHardware->gpio, softSerial->txTimerHardware->pin);
} else {
digitalLo(softSerial->txTimerHardware->gpio, softSerial->txTimerHardware->pin);
}
}
void rccBootLoader(void) {
// check for magic cookie
if (rccReadBkpDr() == 0xDECEA5ED) {
digitalPin *statusLed, *errorLed;
rccWriteBkpDr(0); // reset flag
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB | RCC_APB2Periph_GPIOC | RCC_APB2Periph_AFIO, ENABLE);
GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable, ENABLE);
statusLed = digitalInit(GPIO_STATUS_LED_PORT, GPIO_STATUS_LED_PIN);
digitalLo(statusLed);
errorLed = digitalInit(GPIO_ERROR_LED_PORT, GPIO_ERROR_LED_PIN);
digitalLo(errorLed);
// jump to boot loader ROM
__asm volatile ("LDR R0, =0x1FFFF000\n"
"LDR SP,[R0, #0]\n"
"LDR R0,[R0, #4]\n"
"BX R0\n");
}
void spiSelect(bool select, GPIO_TypeDef* port, GPIO_Pin pin)
{
if (!isSPI)
return;
if (!port || !pin)
return;
if (select) {
digitalLo(port, pin);
} else {
digitalHi(port, pin);
}
delayMicroseconds(10);
}
void digitalWrite(uint8_t pin, uint8_t level)
{
pin = 1<<pin;
GPIO_TypeDef * gpio = gpio_type_from_pin(pin);
uint16_t gpio_pin = gpio_pin_from_pin(pin);
switch (level) {
case HIGH:
digitalLo(gpio, gpio_pin);
break;
case LOW:
digitalHi(gpio, gpio_pin);
break;
}
}
// measurement reading is done asynchronously, using interrupt
void hcsr04_start_reading(void)
{
uint32_t now = millis();
if (now < (lastMeasurementAt + 60)) {
// the repeat interval of trig signal should be greater than 60ms
// to avoid interference between connective measurements.
return;
}
lastMeasurementAt = now;
digitalHi(GPIOB, sonarHardware->trigger_pin);
// The width of trig signal must be greater than 10us
delayMicroseconds(11);
digitalLo(GPIOB, sonarHardware->trigger_pin);
}
/*
* Start a range reading
* Called periodically by the scheduler
* Measurement reading is done asynchronously, using interrupt
*/
void hcsr04_start_reading(void)
{
#if !defined(UNIT_TEST)
static uint32_t timeOfLastMeasurementMs = 0;
// the firing interval of the trigger signal should be greater than 60ms
// to avoid interference between consecutive measurements.
#define HCSR04_MinimumFiringIntervalMs 60
const uint32_t timeNowMs = millis();
if (timeNowMs > timeOfLastMeasurementMs + HCSR04_MinimumFiringIntervalMs) {
timeOfLastMeasurementMs = timeNowMs;
digitalHi(sonarHcsr04Hardware.trigger_gpio, sonarHcsr04Hardware.trigger_pin);
// The width of trigger signal must be greater than 10us, according to device spec
delayMicroseconds(11);
digitalLo(sonarHcsr04Hardware.trigger_gpio, sonarHcsr04Hardware.trigger_pin);
}
#endif
}
// distance calculation is done asynchronously, using interrupt
void hcsr04_get_distance(volatile int32_t *distance)
{
uint32_t current_time = millis();
if (current_time < (last_measurement + 60)) {
// the repeat interval of trig signal should be greater than 60ms
// to avoid interference between connective measurements.
return;
}
last_measurement = current_time;
distance_ptr = distance;
digitalHi(GPIOB, trigger_pin);
// The width of trig signal must be greater than 10us
delayMicroseconds(11);
digitalLo(GPIOB, trigger_pin);
}
digitalPin *digitalInit(GPIO_TypeDef* port, const uint16_t pin)
{
digitalPin *p;
GPIO_InitTypeDef GPIO_InitStructure;
p = (digitalPin *)calloc(1, sizeof(digitalPin));
p->port = port;
p->pin = pin;
digitalLo(p);
GPIO_InitStructure.GPIO_Pin = pin;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_Init(port, &GPIO_InitStructure);
return p;
}
// Set fixType as a rough indication of GPS data quality
void navSetFixType(void) {
unsigned long posTimeDlta = IMU_LASTUPD - gpsData.lastPosUpdate;
float hAccMsk = gpsData.hAcc + runData.accMask;
if (posTimeDlta < NAV_MAX_GPS_AGE && hAccMsk < NAV_MIN_GPS_ACC)
navData.fixType = 3;
else if (posTimeDlta < NAV_MAX_FIX_AGE && hAccMsk < NAV_MIN_FIX_ACC)
navData.fixType = 2;
else
navData.fixType = 0;
if (navData.fixType == 3) {
if (!(supervisorData.state & STATE_CALIBRATION))
digitalHi(supervisorData.gpsLed);
} else {
if (!(supervisorData.state & STATE_CALIBRATION))
digitalLo(supervisorData.gpsLed);
}
}
digitalPin *digitalInit(GPIO_TypeDef* port, const uint16_t pin) {
digitalPin *p;
GPIO_InitTypeDef GPIO_InitStructure;
p = (digitalPin *)aqDataCalloc(1, sizeof(digitalPin));
p->port = port;
p->pin = pin;
digitalLo(p);
GPIO_StructInit(&GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = pin;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_Init(port, &GPIO_InitStructure);
return p;
}
void transponderIrDisable(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
DMA_Cmd(TRANSPONDER_DMA_CHANNEL, DISABLE);
TIM_Cmd(TRANSPONDER_TIMER, DISABLE);
GPIO_StructInit(&GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = TRANSPONDER_PIN;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_DOWN;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(TRANSPONDER_GPIO, &GPIO_InitStructure);
#ifdef TRANSPONDER_INVERTED
digitalHi(TRANSPONDER_GPIO, TRANSPONDER_PIN);
#else
digitalLo(TRANSPONDER_GPIO, TRANSPONDER_PIN);
#endif
}
static void spiSelect(spiClient_t *client) {
digitalLo(client->cs);
}
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 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
}
}
