Skip to content

Commit

Permalink
Nav: Do not allow relative heading on landing type waypoint (prevents…
Browse files Browse the repository at this point in the history 
… random yawing);

Specifically hold current position when landing (possibly prevent diagonal descent);
Centralize speed defaults into macros;
Optimize RAM usage for navData structure.
  • Loading branch information
@mpaperno
mpaperno committed Feb 21, 2016
1 parent db96202 commit aa4910a
Show file tree
Hide file tree
Showing 3 changed files with 56 additions and 52 deletions.
2 changes: 1 addition & 1 deletion src/buildnum.h
View file
Original file line number Diff line number Diff line change
@@ -1 +1 @@
#define BUILDNUMBER 1885
#define BUILDNUMBER 1886
45 changes: 21 additions & 24 deletions src/nav.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,7 @@
along with AutoQuad. If not, see <http://www.gnu.org/licenses/>.
Copyright © 2011-2014 Bill Nesbitt
Copyright 2013-2016 Maxim Paperno
*/

#include "nav.h"
Expand Down Expand Up @@ -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
Expand All @@ -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
Expand Down Expand Up @@ -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;
}
Expand All @@ -202,34 +202,29 @@ 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();
navData.homeLeg.targetAlt = navData.holdAlt;
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;
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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");
Expand Down Expand Up @@ -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;
}
Expand All @@ -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;
Expand Down Expand Up @@ -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;
Expand Down
61 changes: 34 additions & 27 deletions src/nav.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,7 @@
along with AutoQuad. If not, see <http://www.gnu.org/licenses/>.
Copyright © 2011-2015 Bill Nesbitt
Copyright 2013-2016 Maxim Paperno
*/

#ifndef _nav_h
Expand All @@ -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
Expand Down Expand Up @@ -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 {
Expand All @@ -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
Expand All @@ -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;
Expand Down

0 comments on commit aa4910a

Please sign in to comment.