Ejemplo n.º 1
0
// 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;
    }
}
Ejemplo n.º 2
0
// write config to uSD
int8_t configWriteFile(char *fname) {
    char buf[128];
    int8_t fh;
    int8_t ret;
    int n;
    int i;

    if (fname == 0)
	fname = CONFIG_FILE_NAME;

    if ((fh = filerGetHandle(fname)) < 0) {
	AQ_NOTICE("config: cannot get write file handle\n");
	return -1;
    }

    for (i = 0; i < CONFIG_NUM_PARAMS; i++) {
	n = configFormatParam(buf, i);
	ret = filerWrite(fh, buf, -1, n);

	if (ret < n) {
	    AQ_NOTICE("config: file write error\n");
	    ret = -1;
	    break;
	}
    }

    filerClose(fh);

    return ret;
}
Ejemplo n.º 3
0
void signalingInit(void) {

    memset((void *)&sigData, 0, sizeof(sigData));

    sigData.patPos = 0;
    sigData.patLen = 10;  // 10 = 1Hz. Changing it will! affect the led pattern event

    if (p[SIG_LED_1_PRT]) {
	sigData.ledPort1 = pwmInitDigitalOut(p[SIG_LED_1_PRT]-1);

	if (sigData.ledPort1)
	    sigData.enabled = 1;
	else
	    AQ_NOTICE("Warning: Could not open LED 1 signaling port!\n");
    }
    if (p[SIG_LED_2_PRT]) {
	sigData.ledPort2 = pwmInitDigitalOut(p[SIG_LED_2_PRT]-1);

	if (sigData.ledPort2)
	    sigData.enabled = 1;
	else
	    AQ_NOTICE("Warning: Could not open LED 2 signaling port!\n");
    }
    if (p[SIG_BEEP_PRT]) {
	if (p[SIG_BEEP_PRT] > 0) {  // piezo buzzer
	    sigData.beeperPort = pwmInitDigitalOut(abs(p[SIG_BEEP_PRT])-1);
	    sigData.beeperType = 0;
	}
	else {	// piezo speaker
	    sigData.beeperPort = pwmInitOut(abs(p[SIG_BEEP_PRT])-1, 200.0f / SIG_SPEAKER_FREQ * 5000, 0, 0);
	    sigData.beeperType = 1;
	}

	if (sigData.beeperPort)
	    sigData.enabled = 1;
	else
	    AQ_NOTICE("Warning: Could not open Beeper signaling port!\n");
    }
    if (p[SIG_PWM_PRT]) {
	sigData.pwmPort = pwmInitOut(p[SIG_PWM_PRT]-1, 2500, 950, 0);

	if (sigData.pwmPort)
	    sigData.enabled = 1;
	else
	    AQ_NOTICE("Warning: Could not open PWM signaling port!\n");
    }

    signalingWriteOutput(SIG_EVENT_NONE);

    // beep the cell count
    if (sigData.beeperPort) {
	int i;
	for (i = 0; i < analogData.batCellCount; i++) {
	    signalingBeep(2000, 50);
	    delay(150);
	}
	delay(300);
	signalingBeep(2000, 250);  // ready beep
    }
}
Ejemplo n.º 4
0
void supervisorArm(void) {
    if (motorsArm()) {
        supervisorData.state = STATE_ARMED | (supervisorData.state & (STATE_LOW_BATTERY1 | STATE_LOW_BATTERY2));
        AQ_NOTICE("Armed\n");
#ifdef USE_SIGNALING
        signalingOnetimeEvent(SIG_EVENT_OT_ARMING);
#endif
    }
    else {
        motorsDisarm();
        AQ_NOTICE("Arm motors failed - disarmed\n");
    }
}
Ejemplo n.º 5
0
void controlInit(void) {
	AQ_NOTICE("Control init\n");

	memset((void *)&controlData, 0, sizeof(controlData));

#ifdef USE_QUATOS
	quatosInit(AQ_INNER_TIMESTEP, AQ_OUTER_TIMESTEP);
#endif

	utilFilterInit3(controlData.userPitchFilter, AQ_INNER_TIMESTEP, 0.1f, 0.0f);
	utilFilterInit3(controlData.userRollFilter, AQ_INNER_TIMESTEP, 0.1f, 0.0f);
	utilFilterInit3(controlData.navPitchFilter, AQ_INNER_TIMESTEP, 0.125f, 0.0f);
	utilFilterInit3(controlData.navRollFilter, AQ_INNER_TIMESTEP, 0.125f, 0.0f);

	controlData.pitchRatePID = pidInit(&p[CTRL_TLT_RTE_P], &p[CTRL_TLT_RTE_I], &p[CTRL_TLT_RTE_D], &p[CTRL_TLT_RTE_F], &p[CTRL_TLT_RTE_PM],
									   &p[CTRL_TLT_RTE_IM], &p[CTRL_TLT_RTE_DM], &p[CTRL_TLT_RTE_OM], 0, 0, 0, 0);
	controlData.rollRatePID = pidInit(&p[CTRL_TLT_RTE_P], &p[CTRL_TLT_RTE_I], &p[CTRL_TLT_RTE_D], &p[CTRL_TLT_RTE_F], &p[CTRL_TLT_RTE_PM],
									  &p[CTRL_TLT_RTE_IM], &p[CTRL_TLT_RTE_DM], &p[CTRL_TLT_RTE_OM], 0, 0, 0, 0);
	controlData.yawRatePID = pidInit(&p[CTRL_YAW_RTE_P], &p[CTRL_YAW_RTE_I], &p[CTRL_YAW_RTE_D], &p[CTRL_YAW_RTE_F], &p[CTRL_YAW_RTE_PM],
									 &p[CTRL_YAW_RTE_IM], &p[CTRL_YAW_RTE_DM], &p[CTRL_YAW_RTE_OM], 0, 0, 0, 0);

	controlData.pitchAnglePID = pidInit(&p[CTRL_TLT_ANG_P], &p[CTRL_TLT_ANG_I], &p[CTRL_TLT_ANG_D], &p[CTRL_TLT_ANG_F], &p[CTRL_TLT_ANG_PM],
										&p[CTRL_TLT_ANG_IM], &p[CTRL_TLT_ANG_DM], &p[CTRL_TLT_ANG_OM], 0, 0, 0, 0);
	controlData.rollAnglePID = pidInit(&p[CTRL_TLT_ANG_P], &p[CTRL_TLT_ANG_I], &p[CTRL_TLT_ANG_D], &p[CTRL_TLT_ANG_F], &p[CTRL_TLT_ANG_PM],
									   &p[CTRL_TLT_ANG_IM], &p[CTRL_TLT_ANG_DM], &p[CTRL_TLT_ANG_OM], 0, 0, 0, 0);
	controlData.yawAnglePID = pidInit(&p[CTRL_YAW_ANG_P], &p[CTRL_YAW_ANG_I], &p[CTRL_YAW_ANG_D], &p[CTRL_YAW_ANG_F], &p[CTRL_YAW_ANG_PM],
									  &p[CTRL_YAW_ANG_IM], &p[CTRL_YAW_ANG_DM], &p[CTRL_YAW_ANG_OM], 0, 0, 0, 0);

	controlTaskStack = aqStackInit(CONTROL_STACK_SIZE, "CONTROL");

	controlData.controlTask = CoCreateTask(controlTaskCode, (void *)0, CONTROL_PRIORITY, &controlTaskStack[CONTROL_STACK_SIZE-1],
										   CONTROL_STACK_SIZE);
}
Ejemplo n.º 6
0
static int motorsPwmInit(int i) {
    uint32_t res = PWM_RESOLUTION;
    uint32_t freq = MOTORS_PWM_FREQ;
    int8_t  esc32Mode = -1;
    int ret = 1;

    if (i >= PWM_NUM_PORTS) {
	AQ_PRINTF("Motors: PWM port number %d does not exist!\n", i);
	ret = 0;
    }
    else if (MOTORS_ESC_TYPE == ESC_TYPE_ONBOARD_PWM) {
#if defined(HAS_ONBOARD_ESC)
	res = ONBOARD_ESC_PWM_RESOLUTION;
	freq = MOTORS_ONBOARD_PWM_FREQ;
#else
	AQ_NOTICE("Motors: Onboard ESC not supported on this hardware!\n");
	ret = 0;
#endif
    }
    else if (MOTORS_ESC_TYPE == ESC_TYPE_ESC32) {
	esc32Mode = USE_QUATOS;
    }

    if (ret)
	motorsData.pwm[i] = pwmInitOut(i, res, freq, 0, esc32Mode);

    return ret;
}
Ejemplo n.º 7
0
// read config from uSD
int8_t configReadFile(char *fname) {
    char *fileBuf;
    int8_t fh;
    int ret;
    int p1;

    if (fname == 0)
	fname = CONFIG_FILE_NAME;

    if ((fh = filerGetHandle(fname)) < 0) {
	AQ_NOTICE("config: cannot get read file handle\n");
	return -1;
    }

    fileBuf = (char *)aqCalloc(CONFIG_FILE_BUF_SIZE, sizeof(char));

    p1 = 0;
    while ((ret = filerRead(fh, fileBuf, -1, CONFIG_FILE_BUF_SIZE)) > 0)
	p1 = configParseParams((char *)fileBuf, ret, p1);

    filerClose(fh);

    if (fileBuf)
	aqFree(fileBuf, CONFIG_FILE_BUF_SIZE, sizeof(char));

    return ret;
}
Ejemplo n.º 8
0
void supervisorTare(void) {
    supervisorLEDsOn();
#ifndef USE_SIMU
    dIMUTare();
#endif
    AQ_NOTICE("Level calibration complete.\n");
    supervisorLEDsOff();
}
Ejemplo n.º 9
0
float esc32SetupCan(canNodes_t *canNode, uint8_t mode) {
    char *s;
    int16_t paramId;
    int i = 0;
    float ret = 0.0f;

    if ((s = canGetVersion(canNode->networkId)) != 0) {
	AQ_PRINTF("ESC32: CAN ID: %2d, ver: %s\n", canNode->canId, s);

        paramId = canGetParamIdByName(canNode->networkId, (uint8_t *)"STARTUP_MODE");

        if (paramId < 0) {
            AQ_NOTICE("ESC32: CAN cannot read parameters\n");
        }
        else if (canGetParamById(canNode->networkId, paramId) != (float)mode) {
	    if (*canSetRunMode(CAN_TT_NODE, canNode->networkId, mode) == 0)
		AQ_NOTICE("ESC32: CAN failed to set run mode\n");

	    if (*canSetParamById(canNode->networkId, paramId, (float)mode) == 0)
                AQ_NOTICE("ESC32: CAN failed to set parameter\n");
            else
                i++;
	}

	i += esc32ReadFile(0, canNode);
	if (i > 0) {
	    if (canCommandConfigWrite(CAN_TT_NODE, canNode->networkId)) {
		AQ_PRINTF("ESC32: CAN updated %d param(s) in flash\n", i);
	    }
	    else {
		AQ_NOTICE("ESC32: CAN failed to flash params\n");
	    }
	}

	ret = strtof(s, NULL);
    }
    else {
	AQ_PRINTF("ESC32: cannot detect ESC CAN ID %d\n", canNode->canId);
    }

    if (!isfinite(ret))
        ret = 0.0f;

    return ret;
}
Ejemplo n.º 10
0
void commandInit(void) {
    AQ_NOTICE("Command interface init\n");

    memset((void *)&commandData, 0, sizeof(commandData));

    commRegisterRcvrFunc(COMM_STREAM_TYPE_TELEMETRY, commandTaskCode);

    commandData.state = COMMAND_WAIT_SYNC1;
}
Ejemplo n.º 11
0
// write config to uSD
int8_t configWriteFile(char *fname) {
    char *buf;
    int8_t fh;
    int8_t ret;
    int n;
    int i;

    if (fname == 0)
	fname = CONFIG_FILE_NAME;

    if ((fh = filerGetHandle(fname)) < 0) {
	AQ_NOTICE("config: cannot get write file handle\n");
	return -1;
    }

    if (!(buf = (char *)aqCalloc(128, sizeof(char)))) {
	AQ_NOTICE("config: Error writing to file, cannot allocate memory.\n");
	filerClose(fh);
	return -1;
    }

    for (i = 0; i < CONFIG_NUM_PARAMS; i++) {
	n = configFormatParam(buf, i);
	if (n)
	    ret = filerWrite(fh, buf, -1, n);

	if (!n || ret < n) {
	    ret = -1;
	    break;
	}
    }

    filerClose(fh);

    if (buf)
	aqFree(buf, 128, sizeof(char));

    if (ret > -1)
	AQ_NOTICE("config: Parameters saved to local storage file.\n");
    else
	AQ_NOTICE("config: Error writing parameters to file.\n");

    return ret;
}
Ejemplo n.º 12
0
void supervisorDisarm(void) {
    motorsDisarm();
    calibDeinit();
    supervisorLEDsOff();
    supervisorData.state = STATE_DISARMED | (supervisorData.state & (STATE_LOW_BATTERY1 | STATE_LOW_BATTERY2));
    AQ_NOTICE("Disarmed\n");
#ifdef USE_SIGNALING
    signalingOnetimeEvent(SIG_EVENT_OT_DISARMING);
#endif
}
Ejemplo n.º 13
0
static void dIMUReadCalib(void) {
#ifdef DIMU_HAVE_EEPROM
    uint8_t *buf;
    int size;
    int p1 = 0;

    buf = eepromOpenRead();

    if (buf == 0) {
        AQ_NOTICE("DIMU: cannot read EEPROM parameters!\n");
    }
    else {
        while ((size = eepromRead(DIMU_EEPROM_BLOCK_SIZE)) != 0)
            p1 = configParseParams((char *)buf, size, p1);

        AQ_NOTICE("DIMU: read calibration parameters from EEPROM\n");
    }
#endif
}
Ejemplo n.º 14
0
// read config from uSD
int8_t configReadFile(char *fname) {
    char *fileBuf;
    int8_t fh;
    int ret;
    int p1;

    if (fname == 0)
	fname = CONFIG_FILE_NAME;

    if ((fh = filerGetHandle(fname)) < 0) {
	AQ_NOTICE("config: cannot get read file handle\n");
	return -1;
    }

    if (!(fileBuf = (char *)aqCalloc(CONFIG_FILE_BUF_SIZE, sizeof(char)))) {
	AQ_NOTICE("config: Error reading from file, cannot allocate memory.\n");
	filerClose(fh);
	return -1;
    }

    p1 = 0;
    while ((ret = filerRead(fh, fileBuf, -1, CONFIG_FILE_BUF_SIZE)) > 0) {
	p1 = configParseParams((char *)fileBuf, ret, p1);
	if (p1 < 0) {
	    ret = -1;
	    break;
	}
    }

    filerClose(fh);

    if (fileBuf)
	aqFree(fileBuf, CONFIG_FILE_BUF_SIZE, sizeof(char));

    if (ret > -1)
	AQ_NOTICE("config: Parameters loaded from local storage file.\n");
    else
	AQ_NOTICE("config: Failed to read parameters from local file.");

    return ret;
}
Ejemplo n.º 15
0
void esc32SetupOw(const GPIO_TypeDef *port, const uint16_t pin, uint8_t mode) {
    int16_t paramId;
    int i = 0;

    owInit((GPIO_TypeDef *)port, pin);

    // get ESC32 version
    owData.buf[0] = OW_VERSION;
    owTransaction(1, 16);

    if (owData.status != OW_STATUS_NO_PRESENSE) {
	AQ_PRINTF("ESC32: OW ver: %s\n", owData.buf);

        paramId = esc32OwGetParamId("STARTUP_MODE");

        if (paramId < 0) {
            AQ_NOTICE("ESC32: OW cannot read parameters\n");
        }
	else if (esc32OwReadParamById(paramId) != (float)mode) {
	    if (esc32OwSetMode(mode) != mode)
		AQ_NOTICE("ESC32: OW failed to set run mode\n");

	    if (esc32OwWriteParam(paramId, (float)mode) != (float)mode)
                AQ_NOTICE("ESC32: OW failed to write parameter\n");
            else
                i++;
	}

	i += esc32ReadFile(0, 0);
	if (i > 0) {
	    esc32OwConfigWrite();
            AQ_PRINTF("ESC32: OW updated %d param(s) in flash\n", i);
	}
    }
    else {
	AQ_NOTICE("ESC32: cannot detect ESC via OW\n");
    }
}
Ejemplo n.º 16
0
void configFlashRead(void) {
    configRec_t *recs;
    int i, j;

    recs = (void *)flashStartAddr();

    for (i = 0; i < CONFIG_NUM_PARAMS; i++) {
        for (j = 0; j < CONFIG_NUM_PARAMS; j++)
            if (!strncasecmp(recs[i].name, configParameterStrings[j], 16))
                p[j] = recs[i].val;
    }

    AQ_NOTICE("config: Parameters restored from flash memory.\n");
}
Ejemplo n.º 17
0
void motorsEscPwmCalibration(void) {
    // check esc type and calibration request bit
    if (MOTORS_ESC_TYPE != ESC_TYPE_STD_PWM || !((uint32_t)p[MOT_ESC_TYPE] & MOT_ESC_TYPE_CALIB_ENABLE))
	return;

    // reset calibration request bit
    p[MOT_ESC_TYPE] = (uint32_t)p[MOT_ESC_TYPE] & ~(MOT_ESC_TYPE_CALIB_ENABLE);
    // store new param, or fail
    if (!configSaveParamsToFlash())
	return;

    for (int i = 5; i > 0; --i) {
	AQ_PRINTF("Warning: ESC calibration, full throttle in %d sec!\n", i);
	yield(1000);
    }
    AQ_NOTICE("ESC Calibration: Setting maximum.\n");
    motorsPwmToAll(p[MOT_MAX]);
    yield(4000);
    AQ_NOTICE("ESC Calibration: Setting minimum.\n");
    motorsPwmToAll(p[MOT_MIN]);
    yield(3000);
    AQ_NOTICE("ESC Calibration: Finished. Restart system now.\n");
}
Ejemplo n.º 18
0
// allocates memory from 64KB CCM
void *aqDataCalloc(uint16_t count, uint16_t size) {
    uint32_t words;

    // round up to word size
    words = (count*size + sizeof(int)-1) / sizeof(int);

    if ((dataSramUsed + words) > UTIL_CCM_HEAP_SIZE) {
        AQ_NOTICE("Out of data SRAM!\n");
    }
    else {
        dataSramUsed += words;
    }

    return (void *)(ccmHeap + dataSramUsed - words);
}
Ejemplo n.º 19
0
static void dIMUWriteCalib(void) {
#ifdef DIMU_HAVE_EEPROM
    char *lineBuf;
    uint8_t *buf;
    int n;
    int i, j, k;

    if (!(lineBuf = (char *)aqCalloc(128, sizeof(char)))) {
        AQ_NOTICE("DIMU: Error writing to EEPROM, cannot allocate memory.\n");
        return;
    }

    buf = eepromOpenWrite();

    k = 0;
    for (i = 0; i < sizeof(dImuCalibParameters) / sizeof(uint16_t); i++) {
        n = configFormatParam(lineBuf, dImuCalibParameters[i]);

        for (j = 0; j < n; j++) {
            buf[k++] = lineBuf[j];
            if (k == DIMU_EEPROM_BLOCK_SIZE) {
                eepromWrite();
                k = 0;
            }
        }
    }
    if (k != 0)
        eepromWrite();
    if (lineBuf)
        aqFree(lineBuf, 128, sizeof(char));

    AQ_NOTICE("DIMU: wrote calibration parameters to EEPROM\n");

    eepromClose();
#endif
}
Ejemplo n.º 20
0
void *aqCalloc(size_t count, size_t size) {
    char *addr = 0;

    if (count * size) {
        addr = calloc(count, size);

        heapUsed += count * size;
        if (heapUsed > heapHighWater)
            heapHighWater = heapUsed;

        if (addr == 0)
            AQ_NOTICE("Out of heap memory!\n");
    }

    return addr;
}
Ejemplo n.º 21
0
void navSetHomeCurrent(void) {
    navData.homeLeg.type = NAV_LEG_GOTO;
    navData.homeLeg.relativeAlt = 0;
    navData.homeLeg.targetAlt = ALTITUDE;
    navData.homeLeg.targetLat = gpsData.lat;
    navData.homeLeg.targetLon = gpsData.lon;
    navData.homeLeg.maxHorizSpeed = p[NAV_MAX_SPEED];
    navData.homeLeg.poiHeading = AQ_YAW;
    if (p[NAV_CEILING])
        navData.ceilingAlt = (ALTITUDE + p[NAV_CEILING]);    // home altitude + x meter as ceiling

    // notify ground
    AQ_NOTICE("Home position set\n");
#ifdef USE_MAVLINK
    mavlinkAnnounceHome();
#endif
}
Ejemplo n.º 22
0
unsigned char navClearWaypoints(void) {
    if (navData.mode >= NAV_STATUS_MISSION)
	return 0;

    for (int i = 0; i < NAV_MAX_MISSION_LEGS; i++)
	navData.missionLegs[i].type = 0;

    navData.tempMissionLoaded = 0;

    AQ_NOTICE("NAV: Waypoints cleared.\n");
#ifdef USE_SIGNALING
    signalingOnetimeEvent(SIG_EVENT_OT_WP_CLEARED);
#endif
#ifdef USE_MAVLINK
    mavlinkWpSendCount();
#endif

    return 1;
}
Ejemplo n.º 23
0
void navInit(void) {
	int i = 0;

	AQ_NOTICE("Nav init\n");

	memset((void *)&navData, 0, sizeof(navData));

	navData.speedNPID = pidInit(&p[NAV_SPEED_P], &p[NAV_SPEED_I], 0, 0, &p[NAV_SPEED_PM], &p[NAV_SPEED_IM], 0, &p[NAV_SPEED_OM], 0, 0, 0, 0);
	navData.speedEPID = pidInit(&p[NAV_SPEED_P], &p[NAV_SPEED_I], 0, 0, &p[NAV_SPEED_PM], &p[NAV_SPEED_IM], 0, &p[NAV_SPEED_OM], 0, 0, 0, 0);
	navData.distanceNPID = pidInit(&p[NAV_DIST_P], &p[NAV_DIST_I], 0, 0, &p[NAV_DIST_PM], &p[NAV_DIST_IM], 0, &p[NAV_DIST_OM], 0, 0, 0, 0);
	navData.distanceEPID = pidInit(&p[NAV_DIST_P], &p[NAV_DIST_I], 0, 0, &p[NAV_DIST_PM], &p[NAV_DIST_IM], 0, &p[NAV_DIST_OM], 0, 0, 0, 0);
	navData.altSpeedPID = pidInit(&p[NAV_ALT_SPED_P], &p[NAV_ALT_SPED_I], 0, 0, &p[NAV_ALT_SPED_PM], &p[NAV_ALT_SPED_IM], 0,
								  &p[NAV_ALT_SPED_OM], 0, 0, 0, 0);
	navData.altPosPID = pidInit(&p[NAV_ALT_POS_P], &p[NAV_ALT_POS_I], 0, 0, &p[NAV_ALT_POS_PM], &p[NAV_ALT_POS_IM], 0, &p[NAV_ALT_POS_OM], 0, 0,
								0, 0);

	navData.mode = NAV_STATUS_MANUAL;
	navData.ceilingAlt = 0.0f;
	navData.setCeilingFlag = 0;
	navData.setCeilingReached = 0;
	navData.homeActionFlag = 0;
	navData.distanceToHome = 0.0f;
	navData.headFreeMode = NAV_HEADFREE_OFF;
	navData.hfUseStoredReference = 0;

	navSetHoldHeading(AQ_YAW);
	navSetHoldAlt(ALTITUDE, 0);

	// HOME
	navData.missionLegs[i].type = NAV_LEG_HOME;
	navData.missionLegs[i].targetRadius = 0.10f;
	navData.missionLegs[i].loiterTime = 0;
	navData.missionLegs[i].poiAltitude = ALTITUDE;
	i++;

	// land
	navData.missionLegs[i].type = NAV_LEG_LAND;
	navData.missionLegs[i].maxHorizSpeed = 1.0f;
	navData.missionLegs[i].poiAltitude = 0.0f;
	i++;
}
Ejemplo n.º 24
0
void navInit(void) {

    AQ_NOTICE("Nav init\n");

    memset((void *)&navData, 0, sizeof(navData));

    navData.speedNPID    = pidInit(NAV_SPEED_P,    NAV_SPEED_I,    0, 0, NAV_SPEED_PM,    NAV_SPEED_IM,    0, NAV_SPEED_OM);
    navData.speedEPID    = pidInit(NAV_SPEED_P,    NAV_SPEED_I,    0, 0, NAV_SPEED_PM,    NAV_SPEED_IM,    0, NAV_SPEED_OM);
    navData.distanceNPID = pidInit(NAV_DIST_P,     NAV_DIST_I,     0, 0, NAV_DIST_PM,     NAV_DIST_IM,     0, NAV_DIST_OM);
    navData.distanceEPID = pidInit(NAV_DIST_P,     NAV_DIST_I,     0, 0, NAV_DIST_PM,     NAV_DIST_IM,     0, NAV_DIST_OM);
    navData.altSpeedPID  = pidInit(NAV_ALT_SPED_P, NAV_ALT_SPED_I, 0, 0, NAV_ALT_SPED_PM, NAV_ALT_SPED_IM, 0, NAV_ALT_SPED_OM);
    navData.altPosPID    = pidInit(NAV_ALT_POS_P,  NAV_ALT_POS_I,  0, 0, NAV_ALT_POS_PM,  NAV_ALT_POS_IM,  0, NAV_ALT_POS_OM);

    navData.mode = NAV_STATUS_MANUAL;
    navData.headFreeMode = NAV_HEADFREE_OFF;

    navSetHoldHeading(AQ_YAW);
    navSetHoldAlt(ALTITUDE, 0);

    navLoadDefaultMission();
}
Ejemplo n.º 25
0
void radioTaskCode(void *unused) {
    int i;

    AQ_NOTICE("Radio task started\n");

    while (1) {
	// wait for data
	yield(2); // 2ms

        for (i = 0; i < RADIO_NUM; i++) {
            radioInstance_t *r = &radioData.radioInstances[i];

            if (r->radioType > RADIO_TYPE_NONE) {
                radioProcessInstance(r);

                // find best signal
                if (radioData.mode == RADIO_MODE_DIVERSITY && r->quality > *radioData.quality)
                    radioMakeCurrent(r);
            }
        }
    }
}
Ejemplo n.º 26
0
void supervisorArm(void) {
    uint8_t rcStat = rcCheckValidController();
    if (rcStat) {
	AQ_NOTICE("Error: Can't arm due to RC error(s):\n");
	rcReportAllErrors(rcStat);
    }
    else if (rcIsSwitchActive(NAV_CTRL_AH) || rcIsSwitchActive(NAV_CTRL_PH) || rcIsSwitchActive(NAV_CTRL_MISN))
	AQ_NOTICE("Error: Can't arm, not in manual flight mode.\n");
    else if (rcIsSwitchActive(NAV_CTRL_HOM_SET) || rcIsSwitchActive(NAV_CTRL_HOM_GO))
	AQ_NOTICE("Error: Can't arm, home command active.\n");
    else if (rcIsSwitchActive(NAV_CTRL_HF_SET) || rcIsSwitchActive(NAV_CTRL_HF_LOCK))
	AQ_NOTICE("Error: Can't arm, heading-free mode active.\n");
    else if (motorsArm()) {
	supervisorData.state = STATE_ARMED | (supervisorData.state & (STATE_LOW_BATTERY1 | STATE_LOW_BATTERY2));
	AQ_NOTICE("Armed\n");
#ifdef USE_SIGNALING
	signalingOnetimeEvent(SIG_EVENT_OT_ARMING);
#endif
    }
    else {
	motorsDisarm();
	AQ_NOTICE("Error: Arm motors failed - disarmed.\n");
    }
}
Ejemplo n.º 27
0
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
    }
}
Ejemplo n.º 28
0
void supervisorTare(void) {
    supervisorLEDsOn();
    dIMUTare();
    AQ_NOTICE("Level calibration complete.\n");
    supervisorLEDsOff();
}
Ejemplo n.º 29
0
void supervisorCalibrate(void) {
    supervisorData.state = STATE_CALIBRATION;
    AQ_NOTICE("Starting MAG calibration mode.\n");
    calibInit();
}
Ejemplo n.º 30
0
void runTaskCode(void *unused) {
    uint32_t axis = 0;
    uint32_t loops = 0;

    AQ_NOTICE("Run task started\n");

    while (1) {
	// wait for data
	CoWaitForSingleFlag(imuData.sensorFlag, 0);

	// soft start GPS accuracy
	runData.accMask *= 0.999f;

	navUkfInertialUpdate();

	// record history for acc & mag & pressure readings for smoothing purposes
	// acc
	runData.sumAcc[0] -= runData.accHist[0][runData.sensorHistIndex];
	runData.sumAcc[1] -= runData.accHist[1][runData.sensorHistIndex];
	runData.sumAcc[2] -= runData.accHist[2][runData.sensorHistIndex];

	runData.accHist[0][runData.sensorHistIndex] = IMU_ACCX;
	runData.accHist[1][runData.sensorHistIndex] = IMU_ACCY;
	runData.accHist[2][runData.sensorHistIndex] = IMU_ACCZ;

	runData.sumAcc[0] += runData.accHist[0][runData.sensorHistIndex];
	runData.sumAcc[1] += runData.accHist[1][runData.sensorHistIndex];
	runData.sumAcc[2] += runData.accHist[2][runData.sensorHistIndex];

	// mag
	runData.sumMag[0] -= runData.magHist[0][runData.sensorHistIndex];
	runData.sumMag[1] -= runData.magHist[1][runData.sensorHistIndex];
	runData.sumMag[2] -= runData.magHist[2][runData.sensorHistIndex];

	runData.magHist[0][runData.sensorHistIndex] = IMU_MAGX;
	runData.magHist[1][runData.sensorHistIndex] = IMU_MAGY;
	runData.magHist[2][runData.sensorHistIndex] = IMU_MAGZ;

	runData.sumMag[0] += runData.magHist[0][runData.sensorHistIndex];
	runData.sumMag[1] += runData.magHist[1][runData.sensorHistIndex];
	runData.sumMag[2] += runData.magHist[2][runData.sensorHistIndex];

	// pressure
	runData.sumPres -= runData.presHist[runData.sensorHistIndex];
	runData.presHist[runData.sensorHistIndex] = AQ_PRESSURE;
	runData.sumPres += runData.presHist[runData.sensorHistIndex];

	runData.sensorHistIndex = (runData.sensorHistIndex + 1) % RUN_SENSOR_HIST;

	if (!((loops+1) % 20)) {
	   simDoAccUpdate(runData.sumAcc[0]*(1.0f / (float)RUN_SENSOR_HIST), runData.sumAcc[1]*(1.0f / (float)RUN_SENSOR_HIST), runData.sumAcc[2]*(1.0f / (float)RUN_SENSOR_HIST));
	}
	else if (!((loops+7) % 20)) {
	   simDoPresUpdate(runData.sumPres*(1.0f / (float)RUN_SENSOR_HIST));
	}
#ifndef USE_DIGITAL_IMU
	else if (!((loops+13) % 20) && AQ_MAG_ENABLED) {
	   simDoMagUpdate(runData.sumMag[0]*(1.0f / (float)RUN_SENSOR_HIST), runData.sumMag[1]*(1.0f / (float)RUN_SENSOR_HIST), runData.sumMag[2]*(1.0f / (float)RUN_SENSOR_HIST));
	}
#endif
	// optical flow update
	else if (navUkfData.flowCount >= 10 && !navUkfData.flowLock) {
	    navUkfFlowUpdate();
	}
	// only accept GPS updates if there is no optical flow
	else if (CoAcceptSingleFlag(gpsData.gpsPosFlag) == E_OK && navUkfData.flowQuality == 0.0f && gpsData.hAcc < NAV_MIN_GPS_ACC && gpsData.tDOP != 0.0f) {
	    navUkfGpsPosUpdate(gpsData.lastPosUpdate, gpsData.lat, gpsData.lon, gpsData.height, gpsData.hAcc + runData.accMask, gpsData.vAcc + runData.accMask);
	    CoClearFlag(gpsData.gpsPosFlag);
	    // refine static sea level pressure based on better GPS altitude fixes
	    if (gpsData.hAcc < runData.bestHacc && gpsData.hAcc < NAV_MIN_GPS_ACC) {
                navPressureAdjust(gpsData.height);
		runData.bestHacc = gpsData.hAcc;
	    }
	}
	else if (CoAcceptSingleFlag(gpsData.gpsVelFlag) == E_OK && navUkfData.flowQuality == 0.0f && gpsData.sAcc < NAV_MIN_GPS_ACC/2 && gpsData.tDOP != 0.0f) {
	    navUkfGpsVelUpdate(gpsData.lastVelUpdate, gpsData.velN, gpsData.velE, gpsData.velD, gpsData.sAcc + runData.accMask);
	    CoClearFlag(gpsData.gpsVelFlag);
	}
	// observe zero position
	else if (!((loops+4) % 20) && (gpsData.hAcc >= NAV_MIN_GPS_ACC || gpsData.tDOP == 0.0f) && navUkfData.flowQuality == 0.0f) {
	    navUkfZeroPos();
	}
	// observer zero velocity
	else if (!((loops+10) % 20) && (gpsData.sAcc >= NAV_MIN_GPS_ACC/2 || gpsData.tDOP == 0.0f) && navUkfData.flowQuality == 0.0f) {
	    navUkfZeroVel();
	}
	// observe that the rates are exactly 0 if not flying or moving
	else if (!(supervisorData.state & STATE_FLYING)) {
	    float stdX, stdY, stdZ;

	    arm_std_f32(runData.accHist[0], RUN_SENSOR_HIST, &stdX);
	    arm_std_f32(runData.accHist[1], RUN_SENSOR_HIST, &stdY);
	    arm_std_f32(runData.accHist[2], RUN_SENSOR_HIST, &stdZ);

	    if ((stdX + stdY + stdZ) < (IMU_STATIC_STD*2)) {
		if (!((axis + 0) % 3))
		    navUkfZeroRate(IMU_RATEX, 0);
		else if (!((axis + 1) % 3))
		    navUkfZeroRate(IMU_RATEY, 1);
		else
		    navUkfZeroRate(IMU_RATEZ, 2);
		axis++;
	    }
	}

        navUkfFinish();
        altUkfProcess(AQ_PRESSURE);

        // determine which altitude estimate to use
        if (gpsData.hAcc > p[NAV_ALT_GPS_ACC]) {
            runData.altPos = &ALT_POS;
            runData.altVel = &ALT_VEL;
        }
        else {
            runData.altPos = &UKF_ALTITUDE;
            runData.altVel = &UKF_VELD;
        }

	CoSetFlag(runData.runFlag);	// new state data

	navNavigate();
#ifndef HAS_AIMU
	analogDecode();
#endif
	if (!(loops % (int)(1.0f / AQ_OUTER_TIMESTEP)))
	    loggerDoHeader();
	loggerDo();
	gimbalUpdate();

#ifdef CAN_CALIB
	canTxIMUData(loops);
#endif
        calibrate();

	loops++;
    }
}