示例#1
0
void control_quadrotor_attitude_reset()
{
	pidZeroIntegral(controlData.pitchRate,0.0f,0.0f);
	pidZeroIntegral(controlData.rollRate,0.0f,0.0f);
	pidZeroIntegral(controlData.yawRate,0.0f,0.0f);
	pidZeroIntegral(controlData.pitchAngle,0.0f,0.0f);
	pidZeroIntegral(controlData.rollAngle,0.0f,0.0f);
	pidZeroIntegral(controlData.yawAngle,0.0f,0.0f);
}
示例#2
0
文件: nav.c 项目: ChrelleP/autoquad
void navNavigate(void) {
    unsigned long currentTime;
    unsigned char leg = navData.missionLeg;
    navMission_t *curLeg = &navData.missionLegs[leg];
    float tmp;

    currentTime = IMU_LASTUPD;

    navSetFixType();

    // is there sufficient position accuracy to navigate?
    if (navData.fixType == 3)
        navData.navCapable = 1;
    // do not drop out of mission due to (hopefully) temporary GPS degradation
    else if (navData.mode < NAV_STATUS_POSHOLD)
        navData.navCapable = 0;

    // Can we navigate && do we want to be in mission mode?
    if ((supervisorData.state & STATE_ARMED) && navData.navCapable && RADIO_FLAPS > 250) {
        //  are we currently in position hold mode && do we have a clear mission ahead of us?
        if ((navData.mode == NAV_STATUS_POSHOLD || navData.mode == NAV_STATUS_DVH) && leg < NAV_MAX_MISSION_LEGS && curLeg->type > 0) {
            curLeg = navLoadLeg(leg);
            navData.mode = NAV_STATUS_MISSION;
        }
    }
    // do we want to be in position hold mode?
    else if ((supervisorData.state & STATE_ARMED) && RADIO_FLAPS > -250) {
        // always allow alt hold
        if (navData.mode < NAV_STATUS_ALTHOLD) {
            // record this altitude as the hold altitude
            navSetHoldAlt(ALTITUDE, 0);

            // set integral to current RC throttle setting
            pidZeroIntegral(navData.altSpeedPID, -VELOCITYD, motorsData.throttle * RADIO_MID_THROTTLE / MOTORS_SCALE);
            pidZeroIntegral(navData.altPosPID, ALTITUDE, 0.0f);

            navData.holdSpeedAlt = navData.targetHoldSpeedAlt = -VELOCITYD;
            navData.holdMaxVertSpeed = p[NAV_ALT_POS_OM];
            navData.mode = NAV_STATUS_ALTHOLD;

            // notify ground
            AQ_NOTICE("Altitude Hold engaged\n");
        }

        // are we not in position hold mode now?
        if ((navData.navCapable || navUkfData.flowQuality > 0.0f) && navData.mode != NAV_STATUS_POSHOLD && navData.mode != NAV_STATUS_DVH) {
            // only zero bias if coming from lower mode
            if (navData.mode < NAV_STATUS_POSHOLD) {
                navData.holdTiltN = 0.0f;
                navData.holdTiltE = 0.0f;

                // speed
                pidZeroIntegral(navData.speedNPID, UKF_VELN, 0.0f);
                pidZeroIntegral(navData.speedEPID, UKF_VELE, 0.0f);

                // pos
                pidZeroIntegral(navData.distanceNPID, UKF_POSN, 0.0f);
                pidZeroIntegral(navData.distanceEPID, UKF_POSE, 0.0f);
            }

            // store this position as hold position
            navUkfSetHereAsPositionTarget();

            if (navData.navCapable) {
                // set this position as home if we have none
                if (navData.homeLeg.targetLat == (double)0.0 || navData.homeLeg.targetLon == (double)0.0)
                    navSetHomeCurrent();
            }

            // activate pos hold
            navData.mode = NAV_STATUS_POSHOLD;

            navData.holdSpeedN = 0.0f;
            navData.holdSpeedE = 0.0f;

            navData.holdMaxHorizSpeed = p[NAV_MAX_SPEED];
            navData.holdMaxVertSpeed = p[NAV_ALT_POS_OM];

            // notify ground
            AQ_NOTICE("Position Hold engaged\n");
        }
        // DVH
        else if ((navData.navCapable || navUkfData.flowQuality > 0.0f) && (
            RADIO_PITCH > p[CTRL_DEAD_BAND] ||
            RADIO_PITCH < -p[CTRL_DEAD_BAND] ||
            RADIO_ROLL > p[CTRL_DEAD_BAND] ||
            RADIO_ROLL < -p[CTRL_DEAD_BAND])) {
                    navData.mode = NAV_STATUS_DVH;
        }
        else if (navData.mode == NAV_STATUS_DVH) {
            // allow speed to drop before holding position (or if RTH engaged)
            if ((UKF_VELN < +0.1f && UKF_VELN > -0.1f && UKF_VELE < +0.1f && UKF_VELE > -0.1f) || RADIO_AUX2 < -250) {
                navUkfSetHereAsPositionTarget();

                navData.mode = NAV_STATUS_POSHOLD;
            }
        }
    }
    else {
        // switch to manual mode
        navData.mode = NAV_STATUS_MANUAL;
        // reset mission legs
        navData.missionLeg = leg = 0;
        // keep up with changing altitude
        navSetHoldAlt(ALTITUDE, 0);
    }

    // ceiling set ?, 0 is disable
    if (navData.ceilingAlt) {
        // ceiling reached 1st time
        if (ALTITUDE > navData.ceilingAlt && !navData.setCeilingFlag) {
            navData.setCeilingFlag = 1;
            navData.ceilingTimer = timerMicros();
        }
        // ceiling still reached for 5 seconds
        else if (navData.setCeilingFlag && ALTITUDE > navData.ceilingAlt && (timerMicros() - navData.ceilingTimer) > (1e6 * 5) ) {
            navData.ceilingTimer = timerMicros();
            if (!navData.setCeilingReached)
                AQ_NOTICE("Warning: Altitude ceiling reached.");
            navData.setCeilingReached = 1;
        }
        else if ((navData.setCeilingFlag || navData.setCeilingReached) && ALTITUDE <= navData.ceilingAlt) {
            if (navData.setCeilingReached)
                AQ_NOTICE("Notice: Altitude returned to within ceiling.");
            navData.setCeilingFlag = 0;
            navData.setCeilingReached = 0;
        }
    }

    // home set
    if ((supervisorData.state & STATE_ARMED) && RADIO_AUX2 > 250) {
        if (!navData.homeActionFlag) {
            navSetHomeCurrent();
            navData.homeActionFlag = 1;
        }
    }
    // recall home
    else if ((supervisorData.state & STATE_ARMED) && RADIO_AUX2 < -250) {
        if (!navData.homeActionFlag) {
            navRecallHome();
            AQ_NOTICE("Returning to home position\n");
            navData.homeActionFlag = 1;
        }
    }
    // switch to middle, clear action flag
    else {
        navData.homeActionFlag = 0;
    }

    // heading-free mode
    if ((int)p[NAV_HDFRE_CHAN] > 0 && (int)p[NAV_HDFRE_CHAN] <= RADIO_MAX_CHANNELS) {

        navSetHeadFreeMode();

        // set/maintain headfree frame reference
        if (!navData.homeActionFlag && ( navData.headFreeMode == NAV_HEADFREE_SETTING ||
                (navData.headFreeMode == NAV_HEADFREE_DYNAMIC && navData.mode == NAV_STATUS_DVH) )) {
            uint8_t dfRefTyp = 0;
            if ((supervisorData.state & STATE_FLYING) && navData.homeLeg.targetLat != (double)0.0f && navData.homeLeg.targetLon != (double)0.0f) {
                if (NAV_HF_HOME_DIST_D_MIN && NAV_HF_HOME_DIST_FREQ && (currentTime - navData.homeDistanceLastUpdate) > (AQ_US_PER_SEC / NAV_HF_HOME_DIST_FREQ)) {
                    navData.distanceToHome = navCalcDistance(gpsData.lat, gpsData.lon, navData.homeLeg.targetLat, navData.homeLeg.targetLon);
                    navData.homeDistanceLastUpdate = currentTime;
                }
                if (!NAV_HF_HOME_DIST_D_MIN || navData.distanceToHome > NAV_HF_HOME_DIST_D_MIN)
                    dfRefTyp = 1;
            }
            navSetHfReference(dfRefTyp);
        }
    }

    if (UKF_POSN != 0.0f || UKF_POSE != 0.0f) {
        navData.holdCourse = compassNormalize(atan2f(-UKF_POSE, -UKF_POSN) * RAD_TO_DEG);
        navData.holdDistance = __sqrtf(UKF_POSN*UKF_POSN + UKF_POSE*UKF_POSE);
    }
    else {
        navData.holdCourse = 0.0f;
        navData.holdDistance = 0.0f;
    }

    if (navData.mode == NAV_STATUS_MISSION) {
        // have we arrived yet?
        if (navData.loiterCompleteTime == 0) {
            // are we close enough (distance and altitude)?
            // goto/home test
            if (((curLeg->type == NAV_LEG_GOTO || curLeg->type == NAV_LEG_HOME) &&
                navData.holdDistance < curLeg->targetRadius &&
                fabsf(navData.holdAlt - ALTITUDE) < curLeg->targetRadius) ||
            // orbit test
                (curLeg->type == NAV_LEG_ORBIT &&
                fabsf(navData.holdDistance - curLeg->targetRadius) +
                fabsf(navData.holdAlt - ALTITUDE) < 2.0f)  ||
            // takeoff test
                (curLeg->type == NAV_LEG_TAKEOFF &&
                navData.holdDistance < curLeg->targetRadius &&
                fabsf(navData.holdAlt - ALTITUDE) < curLeg->targetRadius)
                ) {
                    // freeze heading unless orbiting
                    if (curLeg->type != NAV_LEG_ORBIT)
                        navSetHoldHeading(AQ_YAW);

                    // start the loiter clock
                    navData.loiterCompleteTime = currentTime + curLeg->loiterTime;
#ifdef USE_MAVLINK
                    // notify ground
                    mavlinkWpReached(leg);
#endif
            }
        }
        // have we loitered long enough?
        else if (currentTime > navData.loiterCompleteTime && curLeg->type != NAV_LEG_LAND) {
            // next leg
            if (++leg < NAV_MAX_MISSION_LEGS && navData.missionLegs[leg].type > 0) {
                curLeg = navLoadLeg(leg);
            }
            else {
                navData.mode = NAV_STATUS_POSHOLD;
            }
        }
    }

    // DVH
    if (navData.mode == NAV_STATUS_DVH) {
        float factor = (1.0f / 500.0f) * navData.holdMaxHorizSpeed;
        float x = 0.0f;
        float y = 0.0f;

        if (RADIO_PITCH > p[CTRL_DEAD_BAND])
            x = -(RADIO_PITCH - p[CTRL_DEAD_BAND]) * factor;
        if (RADIO_PITCH < -p[CTRL_DEAD_BAND])
            x = -(RADIO_PITCH + p[CTRL_DEAD_BAND]) * factor;
        if (RADIO_ROLL > p[CTRL_DEAD_BAND])
            y = +(RADIO_ROLL - p[CTRL_DEAD_BAND]) * factor;
        if (RADIO_ROLL < -p[CTRL_DEAD_BAND])
            y = +(RADIO_ROLL + p[CTRL_DEAD_BAND]) * factor;

        // do we want to ignore rotation of craft (headfree/carefree mode)?
        if (navData.headFreeMode > NAV_HEADFREE_OFF) {
            if (navData.hfUseStoredReference) {
                // rotate to stored frame
                navData.holdSpeedN = x * navData.hfReferenceCos - y * navData.hfReferenceSin;
                navData.holdSpeedE = y * navData.hfReferenceCos + x * navData.hfReferenceSin;
            }
            else {
                // don't rotate to any frame (pitch/roll move to N/S/E/W)
                navData.holdSpeedN = x;
                navData.holdSpeedE = y;
            }
        }
        else {
            // rotate to earth frame
            navData.holdSpeedN = x * navUkfData.yawCos - y * navUkfData.yawSin;
            navData.holdSpeedE = y * navUkfData.yawCos + x * navUkfData.yawSin;
        }
    }
    // orbit POI
    else if (navData.mode == NAV_STATUS_MISSION && curLeg->type == NAV_LEG_ORBIT) {
        float velX, velY;

        // maintain orbital radius
        velX = -pidUpdate(navData.distanceNPID, curLeg->targetRadius, navData.holdDistance);

        // maintain orbital velocity (clock wise)
        velY = -curLeg->maxHorizSpeed;

        // rotate to earth frame
        navData.holdSpeedN = velX * navUkfData.yawCos - velY * navUkfData.yawSin;
        navData.holdSpeedE = velY * navUkfData.yawCos + velX * navUkfData.yawSin;
    }
    else {
        // distance => velocity
        navData.holdSpeedN = pidUpdate(navData.distanceNPID, 0.0f, UKF_POSN);
        navData.holdSpeedE = pidUpdate(navData.distanceEPID, 0.0f, UKF_POSE);
    }

    // normalize N/E speed requests to fit below max nav speed
    tmp = __sqrtf(navData.holdSpeedN*navData.holdSpeedN + navData.holdSpeedE*navData.holdSpeedE);
    if (tmp > navData.holdMaxHorizSpeed) {
        navData.holdSpeedN = (navData.holdSpeedN / tmp) * navData.holdMaxHorizSpeed;
        navData.holdSpeedE = (navData.holdSpeedE / tmp) * navData.holdMaxHorizSpeed;
    }

    // velocity => tilt
    navData.holdTiltN = -pidUpdate(navData.speedNPID, navData.holdSpeedN, UKF_VELN);
    navData.holdTiltE = +pidUpdate(navData.speedEPID, navData.holdSpeedE, UKF_VELE);

    if (navData.mode > NAV_STATUS_MANUAL) {
        float vertStick;

        // Throttle controls vertical speed
        vertStick = RADIO_THROT - RADIO_MID_THROTTLE;
        if ((vertStick > p[CTRL_DBAND_THRO]  && !navData.setCeilingReached)  || vertStick < -p[CTRL_DBAND_THRO]) {
            // altitude velocity proportional to throttle stick
            navData.targetHoldSpeedAlt = (vertStick - p[CTRL_DBAND_THRO] * (vertStick > 0.0f ? 1.0f : -1.0f)) * p[NAV_ALT_POS_OM] * (1.0f / RADIO_MID_THROTTLE);

            navData.verticalOverride = 1;
        }
        // are we trying to land?
        else if (navData.mode == NAV_STATUS_MISSION && curLeg->type == NAV_LEG_LAND) {
            navData.targetHoldSpeedAlt = -navData.holdMaxVertSpeed;
        }
        // coming out of vertical override?
        else if (navData.verticalOverride) {
            navData.targetHoldSpeedAlt = 0.0f;

            // slow down before trying to hold altitude
            if (fabsf(VELOCITYD) < 0.025f)
                navData.verticalOverride = 0;

            // set new hold altitude to wherever we are while still in override
            if (navData.mode != NAV_STATUS_MISSION)
                navSetHoldAlt(ALTITUDE, 0);
        }
        // PID has the throttle
        else {
            navData.targetHoldSpeedAlt = pidUpdate(navData.altPosPID, navData.holdAlt, ALTITUDE);
        }

        // constrain vertical velocity
        navData.targetHoldSpeedAlt = constrainFloat(navData.targetHoldSpeedAlt, (navData.holdMaxVertSpeed < p[NAV_MAX_DECENT]) ? -navData.holdMaxVertSpeed : -p[NAV_MAX_DECENT], navData.holdMaxVertSpeed);

        // smooth vertical velocity changes
        navData.holdSpeedAlt += (navData.targetHoldSpeedAlt - navData.holdSpeedAlt) * 0.05f;
    }
    else {
        navData.verticalOverride = 0;
    }

    // calculate POI angle (used for tilt in gimbal function)
    if (navData.mode == NAV_STATUS_MISSION && curLeg->poiAltitude != 0.0f) {
        float a, b, c;

        a = navData.holdDistance;
        b = ALTITUDE - curLeg->poiAltitude;
        c = __sqrtf(a*a + b*b);

        navData.poiAngle = asinf(a/c) * RAD_TO_DEG - 90.0f;
    }
    else {
        navData.poiAngle = 0.0f;
    }

    if (navData.mode == NAV_STATUS_MISSION) {
        // recalculate autonomous heading
        navSetHoldHeading(navData.targetHeading);

        // wait for low throttle if landing
        if (curLeg->type == NAV_LEG_LAND && motorsData.throttle <= 1)
            // shut everything down (sure hope we are really on the ground :)
            supervisorDisarm();
    }

    navData.lastUpdate = currentTime;
}
示例#3
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;
}
示例#4
0
文件: control.c 项目: tenyan/quadfork
void controlTaskCode(void *unused) {
	float yaw;
	float throttle;
	float ratesDesired[3];
	uint16_t overrides[3];
#ifdef USE_QUATOS
	float quatDesired[4];
	float ratesActual[3];
#else
	float pitch, roll;
	float pitchCommand, rollCommand, ruddCommand;
#endif	// USE_QUATOS

	AQ_NOTICE("Control task started\n");

	// disable all axes' rate overrides
	overrides[0] = 0;
	overrides[1] = 0;
	overrides[2] = 0;

	while (1) {
		// wait for work
		CoWaitForSingleFlag(imuData.dRateFlag, 0);

		// this needs to be done ASAP with the freshest of data
		if (supervisorData.state & STATE_ARMED) {
			if (RADIO_THROT > p[CTRL_MIN_THROT] || navData.mode > NAV_STATUS_MANUAL) {
				supervisorThrottleUp(1);

				// are we in altitude hold mode?
				if (navData.mode > NAV_STATUS_MANUAL) {
					// override throttle with nav's request
					throttle = pidUpdate(navData.altSpeedPID, navData.holdSpeedAlt, -VELOCITYD) * MOTORS_SCALE / RADIO_MID_THROTTLE;

					// don't allow negative throttle to be built up
					if (navData.altSpeedPID->iState < 0.0f) {
						navData.altSpeedPID->iState = 0.0f;
					}
				} else {
					throttle = ((uint32_t)RADIO_THROT - p[CTRL_MIN_THROT]) * MOTORS_SCALE / RADIO_MID_THROTTLE * p[CTRL_FACT_THRO];
				}
				// limit
				throttle = constrainInt(throttle, 1, MOTORS_SCALE);

				// if motors are not yet running, use this heading as hold heading
				if (motorsData.throttle == 0) {
					navData.holdHeading = AQ_YAW;
					controlData.yaw = navData.holdHeading;

					// Reset all PIDs
					pidZeroIntegral(controlData.pitchRatePID, 0.0f, 0.0f);
					pidZeroIntegral(controlData.rollRatePID, 0.0f, 0.0f);
					pidZeroIntegral(controlData.yawRatePID, 0.0f, 0.0f);

					pidZeroIntegral(controlData.pitchAnglePID, 0.0f, 0.0f);
					pidZeroIntegral(controlData.rollAnglePID, 0.0f, 0.0f);
					pidZeroIntegral(controlData.yawAnglePID, 0.0f, 0.0f);

					// also set this position as hold position
					if (navData.mode == NAV_STATUS_POSHOLD) {
						navUkfSetHereAsPositionTarget();
					}
				}

				// constrict nav (only) yaw rates
				yaw = compassDifference(controlData.yaw, navData.holdHeading);
				yaw = constrainFloat(yaw, -p[CTRL_NAV_YAW_RT]/400.0f, +p[CTRL_NAV_YAW_RT]/400.0f);
				controlData.yaw = compassNormalize(controlData.yaw + yaw);

				// DVH overrides direct user pitch / roll requests
				if (navData.mode != NAV_STATUS_DVH) {
					controlData.userPitchTarget = RADIO_PITCH * p[CTRL_FACT_PITC];
					controlData.userRollTarget = RADIO_ROLL * p[CTRL_FACT_ROLL];
				} else {
					controlData.userPitchTarget = 0.0f;
					controlData.userRollTarget = 0.0f;
				}

				// navigation requests
				if (navData.mode > NAV_STATUS_ALTHOLD) {
					controlData.navPitchTarget = navData.holdTiltN;
					controlData.navRollTarget = navData.holdTiltE;
				} else {
					controlData.navPitchTarget = 0.0f;
					controlData.navRollTarget = 0.0f;
				}

				// manual rate cut through for yaw
				if (RADIO_RUDD > p[CTRL_DEAD_BAND] || RADIO_RUDD < -p[CTRL_DEAD_BAND]) {
					// fisrt remove dead band
					if (RADIO_RUDD > p[CTRL_DEAD_BAND]) {
						ratesDesired[2] = (RADIO_RUDD - p[CTRL_DEAD_BAND]);
					} else {
						ratesDesired[2] = (RADIO_RUDD + p[CTRL_DEAD_BAND]);
					}

					// calculate desired rate based on full stick scale
					ratesDesired[2] = ratesDesired[2] * p[CTRL_MAN_YAW_RT] * DEG_TO_RAD * (1.0f / 700.0f);

					// keep up with actual craft heading
					controlData.yaw = AQ_YAW;
					navData.holdHeading = AQ_YAW;

					// request override
					overrides[2] = CONTROL_MIN_YAW_OVERRIDE;
				} else {
					// currently overriding?
					if (overrides[2] > 0) {
						// request zero rate
						ratesDesired[2] = 0.0f;

						// follow actual craft heading
						controlData.yaw = AQ_YAW;
						navData.holdHeading = AQ_YAW;

						// decrease override timer
						overrides[2]--;
					}
				}

#ifdef USE_QUATOS
				// determine which frame of reference to control from
				if (navData.mode <= NAV_STATUS_ALTHOLD)
					// craft frame - manual
				{
					eulerToQuatYPR(quatDesired, controlData.yaw, controlData.userPitchTarget, controlData.userRollTarget);
				} else
					// world frame - autonomous
				{
					eulerToQuatRPY(quatDesired, controlData.navRollTarget, controlData.navPitchTarget, controlData.yaw);
				}

				// reset controller on startup
				if (motorsData.throttle == 0) {
					quatDesired[0] = UKF_Q1;
					quatDesired[1] = UKF_Q2;
					quatDesired[2] = UKF_Q3;
					quatDesired[3] = UKF_Q4;
					quatosReset(quatDesired);
				}

				ratesActual[0] = IMU_DRATEX + UKF_GYO_BIAS_X;
				ratesActual[1] = IMU_DRATEY + UKF_GYO_BIAS_Y;
				ratesActual[2] = IMU_DRATEZ + UKF_GYO_BIAS_Z;
				quatos(&UKF_Q1, quatDesired, ratesActual, ratesDesired, overrides);

				quatosPowerDistribution(throttle);
				motorsSendThrust();
				motorsData.throttle = throttle;
#else

				// smooth
				controlData.userPitchTarget = utilFilter3(controlData.userPitchFilter, controlData.userPitchTarget);
				controlData.userRollTarget = utilFilter3(controlData.userRollFilter, controlData.userRollTarget);

				// smooth
				controlData.navPitchTarget = utilFilter3(controlData.navPitchFilter, controlData.navPitchTarget);
				controlData.navRollTarget = utilFilter3(controlData.navRollFilter, controlData.navRollTarget);

				// rotate nav's NE frame of reference to our craft's local frame of reference
				pitch = controlData.navPitchTarget * navUkfData.yawCos - controlData.navRollTarget * navUkfData.yawSin;
				roll  = controlData.navRollTarget * navUkfData.yawCos + controlData.navPitchTarget * navUkfData.yawSin;

				// combine nav & user requests (both are already smoothed)
				controlData.pitch = pitch + controlData.userPitchTarget;
				controlData.roll = roll + controlData.userRollTarget;

				if (p[CTRL_PID_TYPE] == 0) {
					// pitch angle
					pitchCommand = pidUpdate(controlData.pitchAnglePID, controlData.pitch, AQ_PITCH);
					// rate
					pitchCommand += pidUpdate(controlData.pitchRatePID, 0.0f, IMU_DRATEY);

					// roll angle
					rollCommand = pidUpdate(controlData.rollAnglePID, controlData.roll, AQ_ROLL);
					// rate
					rollCommand += pidUpdate(controlData.rollRatePID, 0.0f, IMU_DRATEX);
				} else if (p[CTRL_PID_TYPE] == 1) {
					// pitch rate from angle
					pitchCommand = pidUpdate(controlData.pitchRatePID, pidUpdate(controlData.pitchAnglePID, controlData.pitch, AQ_PITCH), IMU_DRATEY);

					// roll rate from angle
					rollCommand = pidUpdate(controlData.rollRatePID, pidUpdate(controlData.rollAnglePID, controlData.roll, AQ_ROLL), IMU_DRATEX);
				}


				else if (p[CTRL_PID_TYPE] == 2) {

					// pitch angle
					pitchCommand = pidUpdate(controlData.pitchAnglePID, controlData.pitch, AQ_PITCH);
					// rate
					pitchCommand += pidUpdate(controlData.pitchRatePID, 0.0f, IMU_DRATEY);

					int axis = 0; // ROLL
					float PID_P = 3.7;
					float PID_I = 0.031;
					float PID_D = 23.0;


					float           error, errorAngle, AngleRateTmp, RateError, delta, deltaSum;
					float           PTerm, ITerm, PTermACC = 0, ITermACC = 0, PTermGYRO = 0, ITermGYRO = 0, DTerm;
					static int16_t  lastGyro[3] = { 0, 0, 0 };
					static float    delta1[3], delta2[3];
					static float    errorGyroI[3] = { 0, 0, 0 }, errorAngleI[2] = { 0, 0 };
					static float    lastError[3]  = { 0, 0, 0 }, lastDTerm[3]   = { 0, 0, 0 }; // pt1 element http://www.multiwii.com/forum/viewtopic.php?f=23&t=2624;
					static int16_t  axisPID[3];
					static float rollPitchRate = 0.0;
					static float newpidimax = 0.0;
					float dT;
					uint8_t ANGLE_MODE = 0;
					uint8_t HORIZON_MODE = 0;

					uint16_t cycleTime = IMU_LASTUPD -
										 controlData.lastUpdate;                                          // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop

					if ((ANGLE_MODE || HORIZON_MODE)) {        // MODE relying on ACC
						errorAngle = constrainFloat(2.0f * (float)controlData.roll, -500.0f, +500.0f) - AQ_ROLL;
					}

					if (!ANGLE_MODE) {                                   //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID
						AngleRateTmp = (float)(rollPitchRate + 27) * (float)controlData.roll *
									   0.0625f; // AngleRateTmp = ((int32_t) (cfg.rollPitchRate + 27) * rcCommand[axis]) >> 4;
						if (HORIZON_MODE) {
							AngleRateTmp += PID_I * errorAngle *
											0.0390625f; //increased by x10 //0.00390625f AngleRateTmp += (errorAngle * (float)cfg.I8[PIDLEVEL]) >> 8;
						}
					} else {                                                // it's the ANGLE mode - control is angle based, so control loop is needed
						AngleRateTmp = PID_P * errorAngle * 0.0223214286f; // AngleRateTmp = (errorAngle * (float)cfg.P8[PIDLEVEL]) >> 4; * LevelPprescale;
					}

					RateError         = AngleRateTmp - IMU_DRATEX;
					PTerm             = PID_P * RateError * 0.0078125f;
					errorGyroI[axis] += PID_I * RateError * (float)cycleTime / 2048.0f;
					errorGyroI[axis]  = constrainFloat(errorGyroI[axis], -newpidimax, newpidimax);
					ITerm             = errorGyroI[axis] / 8192.0f;
					delta             = RateError - lastError[axis];
					lastError[axis]   = RateError;
					delta             = delta * 16383.75f / (float)cycleTime;
					deltaSum          = delta1[axis] + delta2[axis] + delta;
					delta2[axis]      = delta1[axis];
					delta1[axis]      = delta;
					DTerm             = PID_D * deltaSum * 0.00390625f;
					axisPID[axis]     = PTerm + ITerm + DTerm;


					rollCommand = AngleRateTmp;

				}

				else {
					pitchCommand = 0.0f;
					rollCommand = 0.0f;
					ruddCommand = 0.0f;
				}

				// yaw rate override?
				if (overrides[2] > 0)
					// manual yaw rate
				{
					ruddCommand = pidUpdate(controlData.yawRatePID, ratesDesired[2], IMU_DRATEZ);
				} else
					// seek a 0 deg difference between hold heading and actual yaw
				{
					ruddCommand = pidUpdate(controlData.yawRatePID, pidUpdate(controlData.yawAnglePID, 0.0f, compassDifference(controlData.yaw, AQ_YAW)),
											IMU_DRATEZ);
				}

				rollCommand = constrainFloat(rollCommand, -p[CTRL_MAX], p[CTRL_MAX]);
				pitchCommand = constrainFloat(pitchCommand, -p[CTRL_MAX], p[CTRL_MAX]);
				ruddCommand = constrainFloat(ruddCommand, -p[CTRL_MAX], p[CTRL_MAX]);

				motorsCommands(throttle, pitchCommand, rollCommand, ruddCommand);
#endif

			}
			// no throttle input
			else {
				supervisorThrottleUp(0);

				motorsOff();
			}
		}
		// not armed
		else {
			motorsOff();
		}

		controlData.lastUpdate = IMU_LASTUPD;
		controlData.loops++;
	}
}