From aa4910a6c520fb704aeae37b7f5348b01d0e0505 Mon Sep 17 00:00:00 2001 From: Max Paperno Date: Sun, 21 Feb 2016 15:24:53 -0500 Subject: [PATCH] Nav: Do not allow relative heading on landing type waypoint (prevents random yawing); Specifically hold current position when landing (possibly prevent diagonal descent); Centralize speed defaults into macros; Optimize RAM usage for navData structure. --- src/buildnum.h | 2 +- src/nav.c | 45 +++++++++++++++++-------------------- src/nav.h | 61 ++++++++++++++++++++++++++++---------------------- 3 files changed, 56 insertions(+), 52 deletions(-) diff --git a/src/buildnum.h b/src/buildnum.h index 61cf0b92..58b28d36 100644 --- a/src/buildnum.h +++ b/src/buildnum.h @@ -1 +1 @@ -#define BUILDNUMBER 1885 +#define BUILDNUMBER 1886 diff --git a/src/nav.c b/src/nav.c index b62e1f1e..d74fcb06 100644 --- a/src/nav.c +++ b/src/nav.c @@ -14,6 +14,7 @@ along with AutoQuad. If not, see . Copyright © 2011-2014 Bill Nesbitt + Copyright 2013-2016 Maxim Paperno */ #include "nav.h" @@ -82,8 +83,8 @@ void navSetHomeCurrent(void) { navData.homeLeg.targetLat = gpsData.lat; navData.homeLeg.targetLon = gpsData.lon; navData.homeLeg.targetRadius = 1.0f; - navData.homeLeg.maxHorizSpeed = p[NAV_MAX_SPEED]; - navData.homeLeg.maxVertSpeed = p[NAV_ALT_POS_OM]; + navData.homeLeg.maxHorizSpeed = NAV_DFLT_HOR_SPEED; + navData.homeLeg.maxVertSpeed = NAV_DFLT_VRT_SPEED; navData.homeLeg.poiHeading = AQ_YAW; if (p[NAV_CEILING]) navData.ceilingAlt = (ALTITUDE + p[NAV_CEILING]); // home altitude + x meter as ceiling @@ -105,9 +106,9 @@ void navRecallHome(void) { navSetHoldHeading(navData.homeLeg.poiHeading); if (navData.holdMaxHorizSpeed == 0.0f) - navData.holdMaxHorizSpeed = p[NAV_MAX_SPEED]; + navData.holdMaxHorizSpeed = NAV_DFLT_HOR_SPEED; if (navData.holdMaxVertSpeed == 0.0f) - navData.holdMaxVertSpeed = p[NAV_ALT_POS_OM]; + navData.holdMaxVertSpeed = NAV_DFLT_VRT_SPEED; } // set reference frame angle for heading-free mode @@ -189,7 +190,6 @@ navMission_t *navLoadLeg(uint8_t leg) { 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; } @@ -202,17 +202,13 @@ navMission_t *navLoadLeg(uint8_t leg) { if (curLeg->targetLat != (double)0.0 && curLeg->targetLon != (double)0.0) navUkfSetGlobalPositionTarget(curLeg->targetLat, curLeg->targetLon); navData.targetHeading = curLeg->poiHeading; - navData.holdMaxHorizSpeed = p[NAV_MAX_SPEED]; + navData.holdMaxHorizSpeed = NAV_DFLT_HOR_SPEED; } else if (curLeg->type == NAV_LEG_TAKEOFF) { // store this position as the takeoff position navUkfSetHereAsPositionTarget(); navData.targetHeading = AQ_YAW; - - if (curLeg->maxVertSpeed == 0.0f) - navData.holdMaxVertSpeed = p[NAV_LANDING_VEL]; - else - navData.holdMaxVertSpeed = curLeg->maxVertSpeed; + navData.holdMaxVertSpeed = curLeg->maxVertSpeed; // set the launch location as home navSetHomeCurrent(); @@ -220,16 +216,15 @@ navMission_t *navLoadLeg(uint8_t leg) { navData.homeLeg.poiHeading = -0.0f; // relative } else if (curLeg->type == NAV_LEG_LAND) { - if (curLeg->maxVertSpeed == 0.0f) - navData.holdMaxVertSpeed = p[NAV_LANDING_VEL]; - else - navData.holdMaxVertSpeed = curLeg->maxVertSpeed; + navUkfSetHereAsPositionTarget(); // hold current position + navData.targetHeading = fabsf(curLeg->poiHeading); // always fixed + navData.holdMaxVertSpeed = curLeg->maxVertSpeed; } if (navData.holdMaxHorizSpeed == 0.0f) - navData.holdMaxHorizSpeed = p[NAV_MAX_SPEED]; + navData.holdMaxHorizSpeed = NAV_DFLT_HOR_SPEED; if (navData.holdMaxVertSpeed == 0.0f) - navData.holdMaxVertSpeed = p[NAV_ALT_POS_OM]; + navData.holdMaxVertSpeed = NAV_DFLT_VRT_SPEED; navData.loiterCompleteTime = 0; navData.hasMissionLeg = 1; @@ -398,7 +393,7 @@ void navNavigate(void) { pidZeroIntegral(navData.altPosPID, ALTITUDE, 0.0f); navData.holdSpeedAlt = navData.targetHoldSpeedAlt = -VELOCITYD; - navData.holdMaxVertSpeed = p[NAV_ALT_POS_OM]; + navData.holdMaxVertSpeed = NAV_DFLT_VRT_SPEED; navData.mode = NAV_STATUS_ALTHOLD; // notify ground @@ -436,8 +431,8 @@ void navNavigate(void) { navData.holdSpeedN = 0.0f; navData.holdSpeedE = 0.0f; - navData.holdMaxHorizSpeed = p[NAV_MAX_SPEED]; - navData.holdMaxVertSpeed = p[NAV_ALT_POS_OM]; + navData.holdMaxHorizSpeed = NAV_DFLT_HOR_SPEED; + navData.holdMaxVertSpeed = NAV_DFLT_VRT_SPEED; // notify ground AQ_NOTICE("Position Hold engaged\n"); @@ -618,7 +613,7 @@ void navNavigate(void) { 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.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; } @@ -644,7 +639,9 @@ void navNavigate(void) { } // constrain vertical velocity - navData.targetHoldSpeedAlt = constrainFloat(navData.targetHoldSpeedAlt, (navData.holdMaxVertSpeed < p[NAV_MAX_DECENT]) ? -navData.holdMaxVertSpeed : -p[NAV_MAX_DECENT], navData.holdMaxVertSpeed); + tmp = p[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; @@ -777,8 +774,8 @@ uint8_t navRecordWaypoint(void) { navData.missionLegs[idx].targetAlt = gpsData.height; navData.missionLegs[idx].targetLat = gpsData.lat; navData.missionLegs[idx].targetLon = gpsData.lon; - navData.missionLegs[idx].maxHorizSpeed = p[NAV_MAX_SPEED]; - navData.missionLegs[idx].maxVertSpeed = p[NAV_ALT_POS_OM]; + 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; diff --git a/src/nav.h b/src/nav.h index aa6b72c6..d90c79e6 100644 --- a/src/nav.h +++ b/src/nav.h @@ -14,6 +14,7 @@ along with AutoQuad. If not, see . Copyright © 2011-2015 Bill Nesbitt + Copyright 2013-2016 Maxim Paperno */ #ifndef _nav_h @@ -34,7 +35,12 @@ #define NAV_HF_HOME_DIST_D_MIN 2.0f // do not compute dynamic bearing when closer than this to home position (zero to never compute) #define NAV_HF_HOME_DIST_FREQ 4 // update distance to home at this Hz, should be > 0 and <= 400 #define NAV_HF_HOME_BRG_D_MAX 1.0f * DEG_TO_RAD // re-compute headfree reference angles when bearing to home changes by this many degrees (zero to always re-compute) -#define NAV_HF_DYNAMIC_DELAY ((int)3e6f) // delay micros before entering dynamic mode after switch it toggled high +#define NAV_HF_DYNAMIC_DELAY ((int)3e6f) // delay micros before entering dynamic mode after switch is toggled high + +#define NAV_DFLT_HOR_SPEED p[NAV_MAX_SPEED] // default horizontal navigation speed +#define NAV_DFLT_VRT_SPEED p[NAV_ALT_POS_OM] // default vertical navigation speed +#define NAV_DFLT_TOF_SPEED p[NAV_LANDING_VEL] // default autonomous takeoff speed +#define NAV_DFLT_LND_SPEED p[NAV_LANDING_VEL] // default autonomous landing speed enum navStatusTypes { NAV_STATUS_MANUAL = 0, // full manual control @@ -73,8 +79,8 @@ typedef struct { float maxVertSpeed; // m/s float poiHeading; // POI heading (>= 0 is absolute, < 0 is relative to target bearing) float poiAltitude; // altitude of POI - used for camera tilting - uint8_t relativeAlt; // 0 == absolute, 1 == relative - uint8_t type; + uint8_t type:4; + uint8_t relativeAlt:1; // 0 == absolute, 1 == relative } navMission_t; typedef struct { @@ -93,6 +99,11 @@ typedef struct { float holdSpeedAlt; // required speed (Up/Down) float targetHoldSpeedAlt; float presAltOffset; + float distanceToHome; // current distance to home position in m, if set (only updated when in headfree mode) + float bearingToHome; // current bearing to home position in rad, if set (only updated when in headfree mode) + float hfReferenceCos; // stored reference heading for HF mode + float hfReferenceSin; + float ceilingAlt; pidStruct_t *speedNPID; // PID to control N/S speed - output tilt in degrees pidStruct_t *speedEPID; // PID to control E/W speed - output tilt in degrees @@ -103,36 +114,32 @@ typedef struct { navMission_t missionLegs[NAV_MAX_MISSION_LEGS]; navMission_t homeLeg; - uint32_t loiterCompleteTime; uint32_t lastUpdate; - - uint8_t mode; // navigation mode, one of navStatusTypes - uint8_t spvrModeOverride; // forced navigation mode desired, regardless of user controls (eg. failsafe mode). 0 = no override - uint8_t navCapable; - uint8_t missionLeg; - uint8_t tempMissionLoaded; // flag indicating that a temporary/default mission has been loaded (not a user-specified one) - uint8_t fixType; // GPS fix type, 0 = no fix, 2 = 2D, 3 = 3D (navCapable) - uint8_t homeActionFlag; // flag to avoid repeating set/recall home actions until switch is moved back to midpoint - float distanceToHome; // current distance to home position in m, if set (only updated when in headfree mode) - float bearingToHome; // current bearing to home position in rad, if set (only updated when in headfree mode) + uint32_t loiterCompleteTime; + uint32_t wpRecTimer; // track how long WP recording switch is active before taking action + uint32_t hfDynamicModeTimer; // track how long switch is active before entering dynamic HF mode uint32_t homeDistanceLastUpdate; // timestamp of last home position update (only updated when in headfree mode) + uint32_t ceilingTimer; - uint32_t wpRecTimer; // track how long WP recording switch is active before taking action - uint8_t wpActionFlag; // flag to avoid repeating waypoint record/skip actions until switch is turned back off + uint8_t missionLeg; - uint8_t headFreeMode; // headfree/carefree mode status - uint32_t hfDynamicModeTimer; // track how long switch is active before entering dynamic HF mode - float hfReferenceCos; // stored reference heading for HF mode - float hfReferenceSin; - uint8_t hfUseStoredReference; // set to true to use stored reference in HF mode instead of N/E + uint8_t mode:3; // navigation mode, one of navStatusTypes + uint8_t spvrModeOverride:3; // forced navigation mode desired, regardless of user controls (eg. failsafe mode). 0 = no override + uint8_t headFreeMode:3; // headfree/carefree mode status + uint8_t fixType:2; // GPS fix type, 0 = no fix, 2 = 2D, 3 = 3D (navCapable) + + uint8_t navCapable:1; + uint8_t tempMissionLoaded:1; // flag indicating that a temporary/default mission has been loaded (not a user-specified one) + uint8_t verticalOverride:1; + uint8_t homeActionFlag:1; // flag to avoid repeating set/recall home actions until switch is moved back to midpoint + uint8_t wpActionFlag:1; // flag to avoid repeating waypoint record/skip actions until switch is turned back off + uint8_t hfUseStoredReference:1; // set to true to use stored reference in HF mode instead of N/E + uint8_t setCeilingFlag:1; + uint8_t setCeilingReached:1; + uint8_t hasMissionLeg:1; + // padding bits: 10 - uint8_t setCeilingFlag; - uint8_t setCeilingReached; - uint8_t ceilingTimer; - uint8_t verticalOverride; - uint8_t hasMissionLeg; - float ceilingAlt; } navStruct_t; extern navStruct_t navData;