Skip to content

Commit

Permalink
Squashed commit of the following: giacomo892/nav_cruise_mode
Browse files Browse the repository at this point in the history
commit 7215e2a
Merge: 5c76229 23fe9c8
Author: giacomo892 <giacomo892@users.noreply.github.com>
Date:   Tue May 29 15:54:37 2018 +0200

    Rebase On Development

commit 23fe9c8
Author: giacomo892 <giacomo892@users.noreply.github.com>
Date:   Tue May 29 10:50:12 2018 +0200

    ULTIMATE FIX

commit 5c76229
Merge: 6ccd0cc 8f6d7a0
Author: Alberto García Hierro <fiam@rm-fr.net>
Date:   Mon May 28 22:14:36 2018 +0100

    Merge pull request iNavFlight#3289 from giacomo892/fix_fakegps

    Fix FAKE_GPS not providing good position/velocity data

commit 8f6d7a0
Author: giacomo892 <giacomo892@users.noreply.github.com>
Date:   Mon May 28 17:02:14 2018 +0200

    Fix for FAKE_GPS

commit a06ea84
Author: giacomo892 <giacomo892@users.noreply.github.com>
Date:   Mon May 28 16:42:28 2018 +0200

    added more debug

commit 299cdba
Author: giacomo892 <giacomo892@users.noreply.github.com>
Date:   Mon May 28 16:36:15 2018 +0200

    logic fixes

commit 1cc9d50
Author: giacomo892 <giacomo892@users.noreply.github.com>
Date:   Mon May 28 13:37:28 2018 +0200

    cleanup and logic improvements

commit 6ccd0cc
Merge: fe70d31 4f58602
Author: Alberto García Hierro <fiam@rm-fr.net>
Date:   Sun May 27 22:32:45 2018 +0100

    Merge pull request iNavFlight#2450 from iNavFlight/agh_wind_estimation

    Implement wind estimation

commit 4f58602
Author: Alberto García Hierro <alberto@garciahierro.com>
Date:   Sun May 27 21:10:37 2018 +0100

    Refactor calculation for horizontal wind speed to wind_estimator.c

    Add float getEstimatedHorizontalWindSpeed(uint16_t *angle)

commit fe70d31
Merge: 2c32ebb f2ffbe8
Author: Alberto García Hierro <fiam@rm-fr.net>
Date:   Sun May 27 21:14:43 2018 +0100

    Merge pull request iNavFlight#3241 from iNavFlight/agh_map_radar_fixes

    Reduce initial map scale, break early when we're too close to home

commit 2c32ebb
Merge: cde920e 4623e36
Author: Alberto García Hierro <fiam@rm-fr.net>
Date:   Sun May 27 21:14:24 2018 +0100

    Merge pull request iNavFlight#3264 from iNavFlight/agh_cms_osd_alarms

    Add entries for adjusting all OSD alarms in CMS

commit e0862c3
Author: Alberto García Hierro <alberto@garciahierro.com>
Date:   Thu May 24 10:32:42 2018 +0100

    Remove extra semicolon at end of line

commit 0b62513
Author: Alberto García Hierro <alberto@garciahierro.com>
Date:   Tue May 22 21:57:08 2018 +0100

    Re-enable OSD wind speed indicators

commit 042a10d
Author: Alberto García Hierro <alberto@garciahierro.com>
Date:   Mon May 21 10:50:33 2018 +0100

    Increase wind measurement timeout to 10s

commit 39d3076
Author: Alberto García Hierro <alberto@garciahierro.com>
Date:   Sun May 20 12:08:13 2018 +0100

    Fix threshold for maximum estimated wind length increase

    - Threshold was in m/s, while the wind is calculated in cm/s.
    - Fix debug[] values, no need to *100 since wind is already cm/s.
    - Replace a runtime sqrtf() with a compile time sq().
    - Rename a few variables to snakeCase.

commit 04e5ced
Author: Alberto García Hierro <alberto@garciahierro.com>
Date:   Mon May 14 22:25:28 2018 +0100

    Fix value of DEBUG_WIND_ESTIMATOR

    After latest rebase, it should be placed after DEBUG_STAGE2

commit eb0fd00
Author: Alberto García Hierro <alberto@garciahierro.com>
Date:   Fri May 11 14:03:37 2018 +0100

    Add WIND_ESTIMATOR debug mode

    Uses debug[0-2] to log estimated wind in earth frame X, Y, Z.

commit c21718b
Author: Alberto García Hierro <alberto@garciahierro.com>
Date:   Wed Feb 14 17:23:26 2018 +0000

    Fix incorrect calculation for theta during wind estimation

    Kudos to @junwoo091400 and @alexbojko for spotting it

commit b535fec
Author: Alberto García Hierro <alberto@garciahierro.com>
Date:   Sun Nov 5 18:31:54 2017 +0000

    Add back check for bounding wind updates

    Allow either updates which lower the total wind speed or increase
    it slightly. Arduplane uses a similar algorithm, so let's fly
    test this and see what happens.

commit 462e5e0
Author: Alberto García Hierro <alberto@garciahierro.com>
Date:   Fri Nov 3 18:14:22 2017 +0000

    Add Wind Estimation paper by William Premerlani

    Used to implement the wind estimator

commit 24f1327
Author: Alberto García Hierro <alberto@garciahierro.com>
Date:   Fri Nov 3 18:10:26 2017 +0000

    Move wind estimator to flight/, call it only on GPS updates

    - Move navigation_wind_estimator.{c,h} to flight/wind_estimator.{c,h}
    - Rename gpsThread() to gpsUpdate(), make it return a bool which is
    true iff new GPS data was received.
    - When the GPS returns new data, call into the wind estimator to
    update itself.
    - Make sure wind estimator can be disabled at compile time via
    the USE_WIND_ESTIMATOR #ifdef
    - Disable OSD wind indicators when wind estimator support is not
    compiled in.
    - Don't check for both GPS and USE_WIND_ESTIMATOR in ifdefs, instead
    fail compilation with an error in USE_WIND_ESTIMATOR is defined
    without defining GPS.

commit b93ae78
Author: Alberto García Hierro <alberto@garciahierro.com>
Date:   Fri Nov 3 16:23:58 2017 +0000

    Make wind estimator require GPS support

    It doesn't depend on NAV anymore, just GPS

commit ad6bccb
Author: Alberto García Hierro <alberto@garciahierro.com>
Date:   Fri Nov 3 16:22:57 2017 +0000

    When wind estimation isn't valid, show indicators as '---'

    Allows users to tell apart unknown wind from wind detected as
    zero.

commit 6437432
Author: Alberto García Hierro <alberto@garciahierro.com>
Date:   Fri Nov 3 15:57:57 2017 +0000

    Change wind velocity presentation to the user in OSD

    Don't change speed in Z axis, always show vertical wind speed in
    earth frame.
    For the XY plane, just calculate magnitude and angle relative to
    the aircraft in osd.c. This way we don't need to perform a full
    3D rotation.

    This lets use remove the getEstimatedWindVelocityBodyFrame()
    function.

commit 68f634e
Author: Alberto García Hierro <alberto@garciahierro.com>
Date:   Fri Nov 3 15:26:30 2017 +0000

    Add comments explaning how the wind estimator works

commit a013861
Author: Alberto García Hierro <alberto@garciahierro.com>
Date:   Fri Nov 3 15:25:59 2017 +0000

    Calculate wind speed in cm/s

    Due to incorrect scaling, it was calculated in m/s

commit 6f77d3c
Author: Alberto García Hierro <alberto@garciahierro.com>
Date:   Fri Nov 3 15:25:23 2017 +0000

    Remove unused include

commit 6ff6c22
Author: Alberto García Hierro <alberto@garciahierro.com>
Date:   Fri Nov 3 15:22:55 2017 +0000

    Tweak the filtering for estimated wind speed a bit

    Remove conditional which allowed only negative or small positive
    changes to the estimated wind speed. Leave the current 0.95 and
    0.05 weights for past estimate/current estimate. Eventually we'll
    add a proper filter.

commit a416827
Author: Alberto García Hierro <alberto@garciahierro.com>
Date:   Fri Nov 3 15:20:35 2017 +0000

    Check for valid GPS heading, not valid nav system heading

    We're using just information from the GPS, so having a valid heading
    due to the presence of a compass won't work. This also makes this
    code not depend on the NAV system anymore, so we'll move it
    somewhere else.

commit 78e62d3
Author: Alberto García Hierro <alberto@garciahierro.com>
Date:   Fri Nov 3 15:18:02 2017 +0000

    Move check for valid GPS heading to a function

    Allows calling it from both the IMU and the wind estimator

commit c52f687
Author: Alberto García Hierro <alberto@garciahierro.com>
Date:   Fri Nov 3 10:38:36 2017 +0000

    Remove double ;

    Thanks to @giacomo892 for pointing it

commit 9719562
Author: Alberto García Hierro <alberto@garciahierro.com>
Date:   Thu Nov 2 20:46:11 2017 +0000

    Check gpsSol flags for valid VelNE and VelD before using them

commit 03c06f9
Author: Alberto García Hierro <alberto@garciahierro.com>
Date:   Thu Nov 2 20:43:27 2017 +0000

    Only estimate wind speed in fixed wing

commit 91c919f
Author: Alberto García Hierro <alberto@garciahierro.com>
Date:   Thu Nov 2 19:47:25 2017 +0000

    Respect OSD units when formatting wind speed

    Move formatting to a new osdFormatWindSpeedStr() function
    which is used for both wind speed indicators.

commit 13c3b6f
Author: Alberto García Hierro <alberto@garciahierro.com>
Date:   Thu Nov 2 19:04:51 2017 +0000

    Rename OSD field constants for wind indicators

    Cosmetic change

    OSD_HORIZONTAL_WIND -> OSD_WIND_SPEED_HORIZONTAL
    OSD_VERTICAL_WIND -> OSD_WIND_SPEED_VERTICAL

commit d9a6b4f
Author: Alberto García Hierro <alberto@garciahierro.com>
Date:   Thu Nov 2 19:02:53 2017 +0000

    Use dedicated symbols for OSD wind indicators

commit e3c5882
Author: Alberto García Hierro <alberto@garciahierro.com>
Date:   Wed Nov 1 22:02:28 2017 +0000

    Initial, very basic implementation of OSD wind indicators

    An indicator shows the wind in the XY plane while another shows
    the Z axis.

commit 9a45a0c
Author: Alberto García Hierro <alberto@garciahierro.com>
Date:   Wed Nov 1 19:24:40 2017 +0000

    Move wind estimator to navigation

    - Move est_wind_airspeed to navigation_wind_estimator
    - Make a few stylistical changes in all the file to use
    INAV conventions and constants for better readability.
    - Add function for retrieving wind estimates in body frame.
    - Mark with missing stuff with TODO

commit 41013ad
Author: LinJieqiang <517503838@qq.com>
Date:   Sun Oct 8 10:10:52 2017 +0800

    Use GPS and DCM to estimate the airspeed for fixwing.

commit 691a779
Author: giacomo892 <giacomo@MacBook-Pro-di-Giacomo.local>
Date:   Sun May 27 08:12:28 2018 +0200

    fixed typo

commit 4bea058
Author: giacomo892 <giacomo892@users.noreply.github.com>
Date:   Fri May 25 10:52:15 2018 +0200

    cleanup

commit cde920e
Merge: 46f0ca8 69080ba
Author: Paweł Spychalski <pspychalski@gmail.com>
Date:   Fri May 25 14:10:26 2018 +0200

    Merge pull request iNavFlight#3272 from iNavFlight/dzikuvx-docs-update

    Various docs updated and simplifications

commit 6e5e559
Author: giacomo892 <giacomo892@users.noreply.github.com>
Date:   Thu May 24 12:02:33 2018 +0200

    fixed mode switching

commit 69080ba
Author: Pawel Spychalski (DzikuVx) <pspychalski@gmail.com>
Date:   Thu May 24 13:53:18 2018 +0200

    various doc updates

commit 64bdaeb
Author: Pawel Spychalski (DzikuVx) <pspychalski@gmail.com>
Date:   Thu May 24 13:16:21 2018 +0200

    We do not have it, so just drop it

commit cb70cb9
Author: giacomo892 <giacomo892@users.noreply.github.com>
Date:   Thu May 24 11:42:42 2018 +0200

    fixes

commit 3b7baaa
Author: giacomo892 <giacomo892@users.noreply.github.com>
Date:   Wed May 23 12:18:40 2018 +0200

    hacks to have control of roll and thr in CRUISE

commit 4623e36
Author: Alberto García Hierro <alberto@garciahierro.com>
Date:   Tue May 22 22:33:23 2018 +0100

    Add entries for adjusting all OSD alarms

commit c632237
Author: giacomo892 <giacomo892@users.noreply.github.com>
Date:   Tue May 22 18:05:15 2018 +0200

    changed debug

commit 7928c11
Author: giacomo892 <giacomo892@users.noreply.github.com>
Date:   Tue May 22 17:24:44 2018 +0200

    extra fix for oscillations

commit 6bc2b5e
Author: giacomo892 <giacomo892@users.noreply.github.com>
Date:   Tue May 22 12:04:13 2018 +0200

    added settings

commit b2a26c0
Author: giacomo892 <giacomo892@users.noreply.github.com>
Date:   Mon May 21 16:21:29 2018 +0200

    nav cruise mode

commit f2ffbe8
Author: Alberto García Hierro <alberto@garciahierro.com>
Date:   Fri May 18 15:53:15 2018 +0100

    Reduce initial map scale, break early when we're too close to home

    Reduce minum map scale to 10m in SI/0.01mi in IMP. This way the radar
    can draw symbols even when the craft is very close to home.

    Also, when the craft is too close to the map center to be drawn,
    break out early to save some time.
  • Loading branch information
shellixyz committed May 29, 2018
1 parent f9a1463 commit b267bd5
Show file tree
Hide file tree
Showing 10 changed files with 298 additions and 32 deletions.
1 change: 1 addition & 0 deletions src/main/build/debug.h
Expand Up @@ -60,6 +60,7 @@ typedef enum {
DEBUG_STAGE2,
DEBUG_SAG_COMP_VOLTAGE,
DEBUG_WIND_ESTIMATOR,
DEBUG_CRUISE,
DEBUG_COUNT
} debugType_e;

Expand Down
5 changes: 4 additions & 1 deletion src/main/fc/fc_msp_box.c
Expand Up @@ -79,6 +79,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
{ BOXOSDALT2, "OSD ALT 2", 43 },
{ BOXOSDALT3, "OSD ALT 3", 44 },
{ BOXVIDEOPWR, "VIDEO PWR", 45 },
{ BOXNAVCRUISE, "NAV CRUISE", 46 },
{ CHECKBOX_ITEM_COUNT, NULL, 0xFF }
};

Expand Down Expand Up @@ -189,6 +190,7 @@ void initActiveBoxIds(void)
activeBoxIds[activeBoxIdCount++] = BOXNAVWP;
activeBoxIds[activeBoxIdCount++] = BOXHOMERESET;
activeBoxIds[activeBoxIdCount++] = BOXGCSNAV;
if(STATE(FIXED_WING)) {activeBoxIds[activeBoxIdCount++] = BOXNAVCRUISE;} //Temp fix before CRUISE will be active for MC
}
#endif

Expand Down Expand Up @@ -294,7 +296,8 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags)
CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)), BOXFAILSAFE);
CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_ALTHOLD_MODE)), BOXNAVALTHOLD);
CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_POSHOLD_MODE)), BOXNAVPOSHOLD);
CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_RTH_MODE)), BOXNAVRTH);
CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_RTH_MODE)), BOXNAVRTH);
CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_CRUISE_MODE)), BOXNAVCRUISE);
CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_WP_MODE)), BOXNAVWP);
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)), BOXAIRMODE);
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGCSNAV)), BOXGCSNAV);
Expand Down
2 changes: 1 addition & 1 deletion src/main/fc/rc_modes.c
Expand Up @@ -161,7 +161,7 @@ void updateUsedModeActivationConditionFlags(void)
#ifdef USE_NAV
isUsingNAVModes = isModeActivationConditionPresent(BOXNAVPOSHOLD) ||
isModeActivationConditionPresent(BOXNAVRTH) ||
isModeActivationConditionPresent(BOXNAVWP);
isModeActivationConditionPresent(BOXNAVCRUISE) || isModeActivationConditionPresent(BOXNAVWP) ;
#endif
}

Expand Down
1 change: 1 addition & 0 deletions src/main/fc/rc_modes.h
Expand Up @@ -60,6 +60,7 @@ typedef enum {
BOXOSDALT2 = 33,
BOXOSDALT3 = 34,
BOXVIDEOPWR = 35,
BOXNAVCRUISE = 36,
CHECKBOX_ITEM_COUNT
} boxId_e;

Expand Down
2 changes: 1 addition & 1 deletion src/main/fc/runtime_config.h
Expand Up @@ -76,7 +76,7 @@ typedef enum {
FAILSAFE_MODE = (1 << 9),
AUTO_TUNE = (1 << 10), // old G-Tune
NAV_WP_MODE = (1 << 11),
UNUSED_MODE2 = (1 << 12),
NAV_CRUISE_MODE = (1 << 12),
FLAPERON = (1 << 13),
TURN_ASSISTANT = (1 << 14),
} flightModeFlags_e;
Expand Down
2 changes: 1 addition & 1 deletion src/main/fc/settings.yaml
Expand Up @@ -67,7 +67,7 @@ tables:
- name: i2c_speed
values: ["400KHZ", "800KHZ", "100KHZ", "200KHZ"]
- name: debug_modes
values: ["NONE", "GYRO", "NOTCH", "NAV_LANDING", "FW_ALTITUDE", "AGL", "FLOW_RAW", "SBUS", "FPORT", "ALWAYS", "STAGE2", "SAG_COMP_VOLTAGE", "WIND_ESTIMATOR"]
values: ["NONE", "GYRO", "NOTCH", "NAV_LANDING", "FW_ALTITUDE", "AGL", "FLOW_RAW", "SBUS", "FPORT", "ALWAYS", "STAGE2", "SAG_COMP_VOLTAGE", "WIND_ESTIMATOR", "CRUISE"]
- name: async_mode
values: ["NONE", "GYRO", "ALL"]
- name: aux_operator
Expand Down
10 changes: 8 additions & 2 deletions src/main/io/osd.c
Expand Up @@ -1302,12 +1302,18 @@ static bool osdDrawSingleElement(uint8_t item)
} else {
p = " PH ";
}
} else if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && navigationRequiresAngleMode()) {
}
else if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && navigationRequiresAngleMode()) {
// If navigationRequiresAngleMode() returns false when ALTHOLD is active,
// it means it can be combined with ANGLE, HORIZON, ACRO, etc...
// and its display is handled by OSD_MESSAGES rather than OSD_FLYMODE.
p = " AH ";
} else if (FLIGHT_MODE(NAV_WP_MODE))
}
else if (FLIGHT_MODE(NAV_CRUISE_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE))
p = "3CRS";
else if (FLIGHT_MODE(NAV_CRUISE_MODE))
p = "CRS ";
else if (FLIGHT_MODE(NAV_WP_MODE))
p = " WP ";
else if (FLIGHT_MODE(ANGLE_MODE))
p = "ANGL";
Expand Down
274 changes: 253 additions & 21 deletions src/main/navigation/navigation.c

Large diffs are not rendered by default.

8 changes: 6 additions & 2 deletions src/main/navigation/navigation_fixedwing.c
Expand Up @@ -187,6 +187,10 @@ void applyFixedWingAltitudeAndThrottleController(timeUs_t currentTimeUs)
*-----------------------------------------------------------*/
bool adjustFixedWingHeadingFromRCInput(void)
{
if (ABS(rcCommand[YAW]) > rcControlsConfig()->pos_hold_deadband) {
return true;
}

return false;
}

Expand Down Expand Up @@ -223,7 +227,7 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
#define TAN_15DEG 0.26795f
bool needToCalculateCircularLoiter = isApproachingLastWaypoint()
&& (distanceToActualTarget <= (navConfig()->fw.loiter_radius / TAN_15DEG))
&& (distanceToActualTarget > 50.0f);
&& (distanceToActualTarget > 50.0f) && !FLIGHT_MODE(NAV_CRUISE_MODE);

// Calculate virtual position for straight movement
if (needToCalculateCircularLoiter) {
Expand Down Expand Up @@ -467,7 +471,7 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
rcCommand[ROLL] = pidAngleToRcCommand(rollCorrection, pidProfile()->max_angle_inclination[FD_ROLL]);
}

if ((navStateFlags & NAV_CTL_ALT) || (navStateFlags & NAV_CTL_POS)) {
if ((navStateFlags & NAV_CTL_ALT) || (navStateFlags & NAV_CTL_THR_FW)) {
uint16_t correctedThrottleValue = constrain(navConfig()->fw.cruise_throttle + throttleCorrection, navConfig()->fw.min_throttle, navConfig()->fw.max_throttle);

// Manual throttle increase
Expand Down
25 changes: 22 additions & 3 deletions src/main/navigation/navigation_private.h
Expand Up @@ -172,7 +172,10 @@ typedef enum {
NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING = NAV_FSM_EVENT_STATE_SPECIFIC_1,
NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND = NAV_FSM_EVENT_STATE_SPECIFIC_1,
NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED = NAV_FSM_EVENT_STATE_SPECIFIC_2,


NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D,
NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D_ADJ,
NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D,
NAV_FSM_EVENT_COUNT,
} navigationFSMEvent_t;

Expand Down Expand Up @@ -212,6 +215,13 @@ typedef enum {
NAV_STATE_LAUNCH_MOTOR_DELAY, // 27
NAV_STATE_LAUNCH_IN_PROGRESS, // 28

NAV_STATE_CRUISE_2D_INITIALIZE, // 29
NAV_STATE_CRUISE_2D_ADJUSTING, // 30
NAV_STATE_CRUISE_2D_IN_PROGRESS, // 31

NAV_STATE_CRUISE_3D_INITIALIZE, // 32
NAV_STATE_CRUISE_3D_IN_PROGRESS, // 33

NAV_STATE_COUNT,
} navigationFSMState_t;

Expand All @@ -237,9 +247,10 @@ typedef enum {
NAV_RC_ALT = (1 << 11),
NAV_RC_POS = (1 << 12),
NAV_RC_YAW = (1 << 13),

NAV_CTL_THR_FW = (1 << 14),

/* Additional flags */
NAV_CTL_LAND = (1 << 14),
NAV_CTL_LAND = (1 << 15),
} navigationFSMStateFlags_t;

typedef struct {
Expand All @@ -258,6 +269,11 @@ typedef struct {
float minimalDistanceToHome;
} rthSanityChecker_t;

typedef struct {
fpVector3_t cruiseTargetPos;
int32_t cruiseYaw;
} navCruise_t;

typedef struct {
/* Flags and navigation system state */
navigationFSMState_t navState;
Expand Down Expand Up @@ -286,6 +302,9 @@ typedef struct {

uint32_t homeDistance; // cm
int32_t homeDirection; // deg*100

/* Cruise */
navCruise_t cruise;

/* Waypoint list */
navWaypoint_t waypointList[NAV_MAX_WAYPOINTS];
Expand Down

0 comments on commit b267bd5

Please sign in to comment.