From 1f7838ec41a3cc7566e71c9657ba10395468b871 Mon Sep 17 00:00:00 2001 From: Jonathan Hudson Date: Thu, 15 Sep 2022 05:59:17 +0000 Subject: [PATCH] Treat wp.p3 as bitfield (#8393) * treat wp p3 as bit mask * update Navigation.md * remove SET_POI from list of 'actionable' WP types --- docs/Navigation.md | 20 ++++++++++++++++---- src/main/navigation/navigation.c | 4 ++-- src/main/navigation/navigation.h | 9 +++++++++ 3 files changed, 27 insertions(+), 6 deletions(-) diff --git a/docs/Navigation.md b/docs/Navigation.md index a4ce90aa85f..dd290df552e 100755 --- a/docs/Navigation.md +++ b/docs/Navigation.md @@ -77,13 +77,24 @@ Parameters: * `` - Longitude. - * `` - Altitude in cm. + * `` - Altitude in cm. See `p3` bit 0 for datum definition. * `` - 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. * `` - 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. - * `` - Reserved for future use. If `p2` is provided, then `p3` is also required. + * `` - 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. * `` - Last waypoint must have `flag` set to 165 (0xA5). @@ -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). @@ -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 -``` \ No newline at end of file +``` diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 9d47d9749b6..a99f19aa306 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -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) @@ -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; diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index d67d679992c..1f2ed8ee788 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -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;