Skip to content

Commit

Permalink
support SET_HEAD and SET_POI (#5851)
Browse files Browse the repository at this point in the history 
* support SET_HEAD and SET_POI

* [DOC] update Navigation.md for SET_POI and SET_HEAD
  • Loading branch information
@stronnag
stronnag authored Jun 23, 2020
1 parent 060a6b1 commit 9cc0368
Show file tree
Hide file tree
Showing 4 changed files with 78 additions and 20 deletions.
6 changes: 4 additions & 2 deletions docs/Navigation.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -58,12 +58,14 @@ When deciding what altitude to maintain, RTH has 4 different modes of operation

Parameters:

* `<action>` - The action to be taken at the WP. The following are enumerations are available in inav 2.5 and later:
* `<action>` - The action to be taken at the WP. The following are enumerations are available in inav 2.6 and later:
* 0 - Unused / Unassigned
* 1 - WAYPOINT
* 3 - POSHOLD_TIME
* 4 - RTH
* 5 - SET_POI
* 6 - JUMP
* 7 - SET_HEAD
* 8 - LAND

* `<lat>` - Latitude (WGS84), in degrees * 1E7 (for example 123456789 means 12.3456789).
Expand All @@ -72,7 +74,7 @@ Parameters:

* `<alt>` - Altitude in cm.

* `<p1>` - For a RTH waypoint, p1 > 0 enables landing. For a normal waypoint it is the speed to this waypoint (cm/s), it is taken into account only for multicopters and when > 50 and < nav_auto_speed. For POSHOLD TIME waypoint it is time to loiter in seconds. For JUMP it is the target WP **index** (not number).
* `<p1>` - For a RTH waypoint, p1 > 0 enables landing. For a normal waypoint it is the speed to this waypoint (cm/s), it is taken into account only for multicopters and when > 50 and < nav_auto_speed. For POSHOLD TIME waypoint it is time to loiter in seconds. For JUMP it is the target WP **index** (not number). For SET_HEAD, it is the desired heading (0-359) or -1 to cancel a previous SET_HEAD or SET_POI.

* `<p2>` - For a POSHOLD TIME it is the speed to this waypoint (cm/s), it is taken into account only for multicopters and when > 50 and < nav_auto_speed. For JUMP it is the number of iterations of the JUMP.

Expand Down
8 changes: 4 additions & 4 deletions src/main/fc/cli.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1338,7 +1338,7 @@ static void cliWaypoints(char *cmdline)
} else if (sl_strcasecmp(cmdline, "save") == 0) {
posControl.waypointListValid = false;
for (int i = 0; i < NAV_MAX_WAYPOINTS; i++) {
if (!(posControl.waypointList[i].action == NAV_WP_ACTION_WAYPOINT || posControl.waypointList[i].action == NAV_WP_ACTION_JUMP || posControl.waypointList[i].action == NAV_WP_ACTION_RTH || posControl.waypointList[i].action == NAV_WP_ACTION_HOLD_TIME || posControl.waypointList[i].action == NAV_WP_ACTION_LAND)) break;
if (!(posControl.waypointList[i].action == NAV_WP_ACTION_WAYPOINT || posControl.waypointList[i].action == NAV_WP_ACTION_JUMP || posControl.waypointList[i].action == NAV_WP_ACTION_RTH || posControl.waypointList[i].action == NAV_WP_ACTION_HOLD_TIME || posControl.waypointList[i].action == NAV_WP_ACTION_LAND || posControl.waypointList[i].action == NAV_WP_ACTION_SET_POI || posControl.waypointList[i].action == NAV_WP_ACTION_SET_HEAD)) break;
if (posControl.waypointList[i].flag == NAV_WP_FLAG_LAST) {
posControl.waypointCount = i + 1;
posControl.waypointListValid = true;
Expand Down Expand Up @@ -1933,9 +1933,9 @@ static void cliGvar(char *cmdline) {
int32_t i = args[INDEX];
if (
i >= 0 && i < MAX_GLOBAL_VARIABLES &&
args[DEFAULT] >= INT32_MIN && args[DEFAULT] <= INT32_MAX &&
args[MIN] >= INT32_MIN && args[MIN] <= INT32_MAX &&
args[MAX] >= INT32_MIN && args[MAX] <= INT32_MAX
args[DEFAULT] >= INT32_MIN && args[DEFAULT] <= INT32_MAX &&
args[MIN] >= INT32_MIN && args[MIN] <= INT32_MAX &&
args[MAX] >= INT32_MIN && args[MAX] <= INT32_MAX
) {
globalVariableConfigsMutable(i)->defaultValue = args[DEFAULT];
globalVariableConfigsMutable(i)->min = args[MIN];
Expand Down
70 changes: 56 additions & 14 deletions src/main/navigation/navigation.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -172,6 +172,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
}
);

static navWapointHeading_t wpHeadingControl;
navigationPosControl_t posControl;
navSystemStatus_t NAV_Status;

Expand Down Expand Up @@ -208,6 +209,8 @@ void calculateInitialHoldPosition(fpVector3_t * pos);
void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t yaw, int32_t distance);
void calculateNewCruiseTarget(fpVector3_t * origin, int32_t yaw, int32_t distance);
static bool isWaypointPositionReached(const fpVector3_t * pos, const bool isWaypointHome);
static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint);
static navigationFSMEvent_t nextForNonGeoStates(void);

void initializeRTHSanityChecker(const fpVector3_t * pos);
bool validateRTHSanityChecker(void);
Expand Down Expand Up @@ -1415,6 +1418,21 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_INITIALIZE(nav
}
}

static navigationFSMEvent_t nextForNonGeoStates(void)
{
/* simple helper for non-geographical states that just set other data */
const bool isLastWaypoint = (posControl.waypointList[posControl.activeWaypointIndex].flag == NAV_WP_FLAG_LAST) || (posControl.activeWaypointIndex >= (posControl.waypointCount - 1));

if (isLastWaypoint) {
// non-geo state is the last waypoint, switch to finish.
return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED;
} else {
// Finished non-geo, move to next WP
posControl.activeWaypointIndex++;
return NAV_FSM_EVENT_NONE; // re-process the state passing to the next WP
}
}

static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(navigationFSMState_t previousState)
{
/* A helper function to do waypoint-specific action */
Expand All @@ -1434,28 +1452,36 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav
if(posControl.waypointList[posControl.activeWaypointIndex].p3 != -1){
if(posControl.waypointList[posControl.activeWaypointIndex].p3 == 0){
resetJumpCounter();
const bool isLastWaypoint = (posControl.waypointList[posControl.activeWaypointIndex].flag == NAV_WP_FLAG_LAST) ||
(posControl.activeWaypointIndex >= (posControl.waypointCount - 1));

if (isLastWaypoint) {
// JUMP is the last waypoint and we reached the last jump, switch to finish.
return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED;
} else {
// Finished JUMP, move to next WP
posControl.activeWaypointIndex++;
return NAV_FSM_EVENT_NONE; // re-process the state passing to the next WP
}
return nextForNonGeoStates();
}
else
{
posControl.waypointList[posControl.activeWaypointIndex].p3--;
}
}

posControl.activeWaypointIndex = posControl.waypointList[posControl.activeWaypointIndex].p1;

return NAV_FSM_EVENT_NONE; // re-process the state passing to the next WP

case NAV_WP_ACTION_SET_POI:
if (STATE(MULTIROTOR)) {
wpHeadingControl.mode = NAV_WP_HEAD_MODE_POI;
mapWaypointToLocalPosition(&wpHeadingControl.poi_pos,
&posControl.waypointList[posControl.activeWaypointIndex]);
}
return nextForNonGeoStates();

case NAV_WP_ACTION_SET_HEAD:
if (STATE(MULTIROTOR)) {
if (posControl.waypointList[posControl.activeWaypointIndex].p1 < 0 ||
posControl.waypointList[posControl.activeWaypointIndex].p1 > 359) {
wpHeadingControl.mode = NAV_WP_HEAD_MODE_NONE;
} else {
wpHeadingControl.mode = NAV_WP_HEAD_MODE_FIXED;
wpHeadingControl.heading = DEGREES_TO_CENTIDEGREES(posControl.waypointList[posControl.activeWaypointIndex].p1);
}
}
return nextForNonGeoStates();

case NAV_WP_ACTION_RTH:
posControl.rthState.rthInitialDistance = posControl.homeDistance;
initializeRTHSanityChecker(&navGetCurrentActualPositionAndVelocity()->pos);
Expand Down Expand Up @@ -1487,11 +1513,25 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
posControl.wpInitialDistance, posControl.wpInitialDistance / 10.0f,
posControl.wpInitialAltitude, posControl.activeWaypoint.pos.z);
setDesiredPosition(&tmpWaypoint, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
if(STATE(MULTIROTOR)) {
switch (wpHeadingControl.mode) {
case NAV_WP_HEAD_MODE_NONE:
break;
case NAV_WP_HEAD_MODE_FIXED:
setDesiredPosition(NULL, wpHeadingControl.heading, NAV_POS_UPDATE_HEADING);
break;
case NAV_WP_HEAD_MODE_POI:
setDesiredPosition(&wpHeadingControl.poi_pos, 0, NAV_POS_UPDATE_BEARING);
break;
}
}
return NAV_FSM_EVENT_NONE; // will re-process state in >10ms
}
break;

case NAV_WP_ACTION_JUMP:
case NAV_WP_ACTION_SET_HEAD:
case NAV_WP_ACTION_SET_POI:
UNREACHABLE();

case NAV_WP_ACTION_RTH:
Expand Down Expand Up @@ -1529,6 +1569,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(naviga
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_WAYPOINT_NEXT

case NAV_WP_ACTION_JUMP:
case NAV_WP_ACTION_SET_HEAD:
case NAV_WP_ACTION_SET_POI:
UNREACHABLE();

case NAV_WP_ACTION_RTH:
Expand Down Expand Up @@ -2753,7 +2795,7 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData)
}
// WP #1 - #NAV_MAX_WAYPOINTS - common waypoints - pre-programmed mission
else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS) && !ARMING_FLAG(ARMED)) {
if (wpData->action == NAV_WP_ACTION_WAYPOINT || wpData->action == NAV_WP_ACTION_JUMP || wpData->action == NAV_WP_ACTION_RTH || wpData->action == NAV_WP_ACTION_HOLD_TIME || wpData->action == NAV_WP_ACTION_LAND) {
if (wpData->action == NAV_WP_ACTION_WAYPOINT || wpData->action == NAV_WP_ACTION_JUMP || wpData->action == NAV_WP_ACTION_RTH || wpData->action == NAV_WP_ACTION_HOLD_TIME || wpData->action == NAV_WP_ACTION_LAND || wpData->action == NAV_WP_ACTION_SET_POI || wpData->action == NAV_WP_ACTION_SET_HEAD ) {
// Only allow upload next waypoint (continue upload mission) or first waypoint (new mission)
if (wpNumber == (posControl.waypointCount + 1) || wpNumber == 1) {
posControl.waypointList[wpNumber - 1] = *wpData;
Expand Down
14 changes: 14 additions & 0 deletions src/main/navigation/navigation.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -237,10 +237,18 @@ typedef enum {
NAV_WP_ACTION_WAYPOINT = 0x01,
NAV_WP_ACTION_HOLD_TIME = 0x03,
NAV_WP_ACTION_RTH = 0x04,
NAV_WP_ACTION_SET_POI = 0x05,
NAV_WP_ACTION_JUMP = 0x06,
NAV_WP_ACTION_SET_HEAD = 0x07,
NAV_WP_ACTION_LAND = 0x08
} navWaypointActions_e;

typedef enum {
NAV_WP_HEAD_MODE_NONE = 0,
NAV_WP_HEAD_MODE_POI = 1,
NAV_WP_HEAD_MODE_FIXED = 2
} navWaypointHeadings_e;

typedef enum {
NAV_WP_FLAG_LAST = 0xA5
} navWaypointFlags_e;
Expand All @@ -254,6 +262,12 @@ typedef struct {
uint8_t flag;
} navWaypoint_t;

typedef struct {
navWaypointHeadings_e mode;
uint32_t heading; // fixed heading * 100 (SET_HEAD)
fpVector3_t poi_pos; // POI location in local coordinates (SET_POI)
} navWapointHeading_t;

typedef struct radar_pois_s {
gpsLocation_t gps;
uint8_t state;
Expand Down

0 comments on commit 9cc0368

Please sign in to comment.