Skip to content

Commit

Permalink
Treat wp.p3 as bitfield (#8393)
Browse files Browse the repository at this point in the history
* treat wp p3 as bit mask

* update Navigation.md

* remove SET_POI from list of 'actionable' WP types
  • Loading branch information
stronnag committed Sep 15, 2022
1 parent 5423929 commit 1f7838e
Show file tree
Hide file tree
Showing 3 changed files with 27 additions and 6 deletions.
20 changes: 16 additions & 4 deletions docs/Navigation.md
Expand Up @@ -77,13 +77,24 @@ Parameters:

* `<lon>` - Longitude.

* `<alt>` - Altitude in cm.
* `<alt>` - Altitude in cm. See `p3` bit 0 for datum definition.

* `<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.

* `<p3>` - Reserved for future use. If `p2` is provided, then `p3` is also required.
* `<p3>` - A bitfield with four bits reserved for user specified actions. It is anticipated that these actions will be exposed through the logic conditions.
* Bit 0 - Altitude (`alt`) : Relative (to home altitude) (0) or Absolute (AMSL) (1).
* Bit 1 - WP Action 1
* Bit 2 - WP Action 2
* Bit 3 - WP Action 3
* Bit 4 - WP Action 4
* Bits 5 - 15 : undefined / reserved.

Note:

* If `p2` is specified, then `p3` is also required.
* `p3` is only defined for navigable WP types (WAYPOINT, POSHOLD_TIME, LAND). The affect of specifying a non-zero `p3` for other WP types is undefined.

* `<flag>` - Last waypoint must have `flag` set to 165 (0xA5).

Expand Down Expand Up @@ -116,7 +127,8 @@ wp 59 0 0 0 0 0 0 0 0

Note that the `wp` CLI command shows waypoint list indices, while the MW-XML definition used by mwp, ezgui and the configurator use WP numbers.

**Multi-missions**\
## Multi-missions

Multi-missions allows up to 9 missions to be stored in the FC at the same time. It is possible to load them into the FC using the CLI. This is acheived by entering single missions into the CLI followed by `wp save` **after** the final mission has been entered (the single missions can be entered one after the other or as a single block entry, it doesn't matter). All missions will then be saved as a Multi Mission in the FC. Saved multi missions display consecutive WP indices from 0 to the last WP in the last mission when displayed using the `wp` command.

E.g. to enter 3 missions in the CLI enter each mission as a single mission (start WP index for each mission must be 0).
Expand Down Expand Up @@ -156,4 +168,4 @@ wp 11 0 0 0 0 0 0 0 0
wp 12 0 0 0 0 0 0 0 0
...
wp 59 0 0 0 0 0 0 0 0
```
```
4 changes: 2 additions & 2 deletions src/main/navigation/navigation.c
Expand Up @@ -3349,7 +3349,7 @@ static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos

geoAltitudeConversionMode_e waypointMissionAltConvMode(geoAltitudeDatumFlag_e datumFlag)
{
return datumFlag == NAV_WP_MSL_DATUM ? GEO_ALT_ABSOLUTE : GEO_ALT_RELATIVE;
return ((datumFlag & NAV_WP_MSL_DATUM) == NAV_WP_MSL_DATUM) ? GEO_ALT_ABSOLUTE : GEO_ALT_RELATIVE;
}

static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint)
Expand Down Expand Up @@ -3933,7 +3933,7 @@ void missionPlannerSetWaypoint(void)
posControl.waypointList[posControl.wpPlannerActiveWPIndex].lon = wpLLH.lon;
posControl.waypointList[posControl.wpPlannerActiveWPIndex].alt = wpLLH.alt;
posControl.waypointList[posControl.wpPlannerActiveWPIndex].p1 = posControl.waypointList[posControl.wpPlannerActiveWPIndex].p2 = 0;
posControl.waypointList[posControl.wpPlannerActiveWPIndex].p3 = 1; // use absolute altitude datum
posControl.waypointList[posControl.wpPlannerActiveWPIndex].p3 |= NAV_WP_ALTMODE; // use absolute altitude datum
posControl.waypointList[posControl.wpPlannerActiveWPIndex].flag = NAV_WP_FLAG_LAST;
posControl.waypointListValid = true;

Expand Down
9 changes: 9 additions & 0 deletions src/main/navigation/navigation.h
Expand Up @@ -352,6 +352,15 @@ typedef enum {
NAV_WP_FLAG_LAST = 0xA5
} navWaypointFlags_e;

/* A reminder that P3 is a bitfield */
typedef enum {
NAV_WP_ALTMODE = (1<<0),
NAV_WP_USER1 = (1<<1),
NAV_WP_USER2 = (1<<2),
NAV_WP_USER3 = (1<<3),
NAV_WP_USER4 = (1<<4)
} navWaypointP3Flags_e;

typedef struct {
int32_t lat;
int32_t lon;
Expand Down

0 comments on commit 1f7838e

Please sign in to comment.