Uncommeted lines for building the other nodes needed for simulation
Changed destWaypoint to tempForceWaypoint in fillTelemetryUpdate