Exemplo n.º 1
0
void info(void) {
    AQ_PRINTF("AQ S/N: %08X-%08X-%08X\n", flashSerno(2), flashSerno(1), flashSerno(0));

#ifdef USE_MAVLINK
    AQ_PRINTF("Mavlink SYS ID: %d\n", flashSerno(0) % 250);
#endif

    AQ_PRINTF("SYS Clock: %u MHz\n", rccClocks.SYSCLK_Frequency / 1000000);
    AQ_PRINTF("%u/%u heap used/high water\n", heapUsed, heapHighWater);
    AQ_PRINTF("%u of %u CCM heap used\n", dataSramUsed * sizeof(int), UTIL_CCM_HEAP_SIZE * sizeof(int));

    yield(50);
    utilVersionString();
}
Exemplo n.º 2
0
uint8_t navRecordWaypoint(void) {
    if (!navData.navCapable || navData.mode >= NAV_STATUS_MISSION)
	return 0;

    uint8_t idx = 0;
    if (navData.tempMissionLoaded)
	navClearWaypoints();
    else
    	idx = navGetWaypointCount();

    if (idx >= NAV_MAX_MISSION_LEGS)
	return 0;

    navData.missionLegs[idx].type = NAV_LEG_GOTO;
    navData.missionLegs[idx].targetAlt = gpsData.height;
    navData.missionLegs[idx].targetLat = gpsData.lat;
    navData.missionLegs[idx].targetLon = gpsData.lon;
    navData.missionLegs[idx].maxHorizSpeed = NAV_DFLT_HOR_SPEED;
    navData.missionLegs[idx].maxVertSpeed = NAV_DFLT_VRT_SPEED;
    navData.missionLegs[idx].targetRadius = 1.0f;
    navData.missionLegs[idx].loiterTime = 1e6;
    navData.missionLegs[idx].poiHeading = AQ_YAW;
    navData.missionLegs[idx].relativeAlt = 0;
    navData.missionLegs[idx].poiAltitude = 0;

    AQ_PRINTF("NAV: Waypoint %d recorded\n", idx);
#ifdef USE_SIGNALING
    signalingOnetimeEvent(SIG_EVENT_OT_WP_RECORDED);
#endif
#ifdef USE_MAVLINK
    mavlinkWpSendCount();
#endif

    return 1;
}
Exemplo n.º 3
0
static void motorsCanInit(int i) {
    uint8_t numTry = CAN_RETRIES;
    uint8_t motorId = i;
    float escVer;

    // canId's 17-32 map to motorId's 1-16
    if (motorId >= 16)
        motorId -= 16;

    while (numTry-- && (motorsData.can[motorId] = canFindNode(CAN_TYPE_ESC, i+1)) == 0)
        yield(100);

    if (motorsData.can[motorId] == 0) {
        AQ_PRINTF("Motors: cannot find CAN id [%d]\n", i+1);
    }
    else {
#ifdef USE_QUATOS
        escVer = esc32SetupCan(motorsData.can[motorId], 1);
#else
        escVer = esc32SetupCan(motorsData.can[motorId], 0);
#endif

        motorsData.esc32Version[motorId] = (uint16_t)(escVer / 0.01f);
        motorsCanRequestTelem(motorId);
    }
}
Exemplo n.º 4
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;
}
Exemplo n.º 5
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;
}
Exemplo n.º 6
0
int configGetParamIdByName(char *name) {
    int i = -1;

    for (i = 0; i < CONFIG_NUM_PARAMS; i++)
	if (!strncmp(name, configParameterStrings[i], 16))
	    break;

    if (i == -1)
	AQ_PRINTF("config: cannot find parmeter '%s'\n", name);

    return i;
}
Exemplo n.º 7
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");
    }
}
Exemplo n.º 8
0
void utilStackCheck(void) {
    int i, j;

    for (i = 0; i < numStacks; i++) {
        for (j = 0; j < stackSizes[i]; j++)
            if (*(char *)(stackPointers[i]+j) != 0xFF)
                break;
        if (stackFrees[i] > j)
            stackFrees[i] = j;
        if (j < 16) {
            AQ_PRINTF("Possible stack overflow [%s]!\n", stackNames[i]);
        }
    }
}
Exemplo n.º 9
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");
}
Exemplo n.º 10
0
void utilVersionString(void) {
    AQ_PRINTF("AQ FW ver: %s rev%d b%d, HW ver: %d rev%d\n", FIMRWARE_VERSION, getRevisionNumber(), getBuildNumber(), BOARD_VERSION, BOARD_REVISION);
}
Exemplo n.º 11
0
void utilSerialNoString(void) {
    AQ_PRINTF("AQ S/N: %08X-%08X-%08X\n", flashSerno(2), flashSerno(1), flashSerno(0));
}
Exemplo n.º 12
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;
}
Exemplo n.º 13
0
navMission_t *navLoadLeg(uint8_t leg) {
    navMission_t *curLeg = &navData.missionLegs[leg];

    // invalid type?
    if (!curLeg->type || curLeg->type >= NAV_NUM_LEG_TYPES) {
        navData.hasMissionLeg = 0;
        return &navData.missionLegs[0];
    }

    // common
    if (curLeg->relativeAlt)
        navSetHoldAlt(curLeg->targetAlt, curLeg->relativeAlt);
    else
        navSetHoldAlt(curLeg->targetAlt - navData.presAltOffset, curLeg->relativeAlt);

    navData.holdMaxHorizSpeed = curLeg->maxHorizSpeed;
    navData.holdMaxVertSpeed = curLeg->maxVertSpeed;

    // type specific
    if (curLeg->type == NAV_LEG_HOME) {
        navSetHoldAlt(navData.homeLeg.targetAlt, navData.homeLeg.relativeAlt);
        navUkfSetGlobalPositionTarget(navData.homeLeg.targetLat, navData.homeLeg.targetLon);
        navData.targetHeading = navData.homeLeg.poiHeading;
        navData.holdMaxHorizSpeed = navData.homeLeg.maxHorizSpeed;
        navData.holdMaxVertSpeed = navData.homeLeg.maxVertSpeed;
    }
    else if (curLeg->type == NAV_LEG_GOTO) {
        if (curLeg->targetLat != (double)0.0 && curLeg->targetLon != (double)0.0)
            navUkfSetGlobalPositionTarget(curLeg->targetLat, curLeg->targetLon);
        navData.targetHeading = curLeg->poiHeading;
    }
    else if (curLeg->type == NAV_LEG_ORBIT) {
        if (curLeg->targetLat != (double)0.0 && curLeg->targetLon != (double)0.0)
            navUkfSetGlobalPositionTarget(curLeg->targetLat, curLeg->targetLon);
        navData.targetHeading = -0.0f;  // must point to target
    }
    else if (curLeg->type == NAV_LEG_TAKEOFF) {
        // store this position as the takeoff position
        navUkfSetHereAsPositionTarget();
        navData.targetHeading = AQ_YAW;
        navData.holdMaxVertSpeed = curLeg->maxVertSpeed;

        // set the launch location as home
        navSetHomeCurrent();
        navData.homeLeg.targetAlt = navData.holdAlt;
        navData.homeLeg.poiHeading = -0.0f;                // relative
    }
    else if (curLeg->type == NAV_LEG_LAND) {
        navUkfSetHereAsPositionTarget();  // hold current position
        navData.targetHeading = fabsf(curLeg->poiHeading);  // always fixed
        navData.holdMaxVertSpeed = curLeg->maxVertSpeed;
    }

    if (navData.holdMaxHorizSpeed == 0.0f)
        navData.holdMaxHorizSpeed = NAV_DFLT_HOR_SPEED;
    if (navData.holdMaxVertSpeed == 0.0f)
        navData.holdMaxVertSpeed = NAV_DFLT_VRT_SPEED;

    navData.loiterCompleteTime = 0;
    navData.hasMissionLeg = 1;
    navData.missionLeg = leg;

    AQ_PRINTF("NAV: Loaded waypoint %d.\n", leg);

#ifdef USE_MAVLINK
    // notify ground
    mavlinkWpAnnounceCurrent(leg);
#endif

    return curLeg;
}
Exemplo n.º 14
0
void radioInit(void) {
    uint16_t radioType = (uint16_t)p[RADIO_TYPE];
    int i;

    AQ_NOTICE("Radio init\n");

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

    radioData.mode = (radioType>>12) & 0x0f;

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

        // determine UART
        switch (i) {
            case 0:
                uart = RC1_UART;
                break;
#ifdef RC2_UART
            case 1:
                uart = RC2_UART;
                break;
#endif
#ifdef RC3_UART
            case 2:
                uart = RC3_UART;
                break;
#endif
            default:
                uart = 0;
                break;
        }

        r->radioType = (radioType>>(i*4)) & 0x0f;
        r->channels = &radioData.allChannels[RADIO_MAX_CHANNELS * i];

        utilFilterInit(&r->qualityFilter, (1.0f / 50.0f), 0.75f, 0.0f);

        switch (r->radioType) {
        case RADIO_TYPE_SPEKTRUM11:
        case RADIO_TYPE_SPEKTRUM10:
        case RADIO_TYPE_DELTANG:
            if (uart) {
                spektrumInit(r, uart);
                radioRCSelect(i, 0);
                AQ_PRINTF("Spektrum on RC port %d\n", i);
            }
            break;

        case RADIO_TYPE_SBUS:
            if (uart) {
                futabaInit(r, uart);
                radioRCSelect(i, 1);
                AQ_PRINTF("Futaba on RC port %d\n", i);
            }
            break;

        case RADIO_TYPE_PPM:
            ppmInit(r);
            AQ_PRINTF("PPM on RC port %d\n", i);
            break;

        case RADIO_TYPE_SUMD:
            if (uart) {
                grhottInit(r, uart);
                radioRCSelect(i, 0);
                AQ_PRINTF("GrHott on RC port %d\n", i);
            }
            break;

        case RADIO_TYPE_MLINK:
            if (uart) {
                mlinkrxInit(r, uart);
                radioRCSelect(i, 0);
                AQ_PRINTF("Mlink on RC port %d\n", i);
            }
            break;

        case RADIO_TYPE_NONE:
            break;

        default:
            AQ_NOTICE("WARNING: Invalid radio type!\n");
            break;
        }
    }

    switch (radioData.mode) {
        case RADIO_MODE_DIVERSITY:
            // select first available radio to start with
            for (i = 0; i < RADIO_NUM; i++) {
                if (radioData.radioInstances[i].radioType > RADIO_TYPE_NONE) {
                    radioMakeCurrent(&radioData.radioInstances[i]);
                    break;
                }
            }
            break;

        case RADIO_MODE_SPLIT:
            radioMakeCurrent(&radioData.radioInstances[0]);
            break;
    }

    // set mode default
    radioData.channels[(int)p[RADIO_FLAP_CH]] = -700;

    radioTaskStack = aqStackInit(RADIO_STACK_SIZE, "RADIO");

    radioData.radioTask = CoCreateTask(radioTaskCode, (void *)0, RADIO_PRIORITY, &radioTaskStack[RADIO_STACK_SIZE-1], RADIO_STACK_SIZE);
}