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; }
// 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; } }
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 }
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"); } }
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; }
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"); } }
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; }