FLOW DIAGRAM (FIRST DRAFT)

START

rcCommand[PITCH]= -5;
If yes delay = 500;
lidarDistance hold_position;
=< 100 delay = 1000;
rcCommand[YAW]= 45;

no

NAV_STATE_WAYPOINT_INITIALIZE,
NAV_STATE_WAYPOINT_PRE_ACTION, If yes
NAV_STATE_WAYPOINT_IN_PROGRESS, lidarDistance rcCommand[PITCH]= 15;
NAV_STATE_WAYPOINT_REACHED, => 150 delay = 500;
NAV_STATE_WAYPOINT_NEXT,
NAV_STATE_WAYPOINT_FINISHED,
NAV_STATE_WAYPOINT_RTH_LAND,

no

rcCommand[YAW]= -45;

END

If yes
rcCommand[PITCH]= 15;
lidarDistance
delay = 500;
=> 150

rcCommand[THROTTLE];