Esempio n. 1
0
        /// <summary>
        /// Send a state to GCS
        /// </summary>
        /// <param name="state"></param>
        public void SendState(UGVState state)
        {
            double timestamp = (DateTime.Now - UnixTime).TotalMilliseconds;

            Node.SendVehicleGlobalPosition(TargetNodeID, timestamp, NodeID,
                                           (int)(state.Latitude * 10000000.0), (int)(state.Longitude * 10000000.0), 1, 0, 0, 0);
        }
        /// <summary>
        /// Send a state
        /// </summary>
        /// <param name="state"></param>
        public void SendState(UGVState state, byte id = 1)
        {
            double timestamp = (DateTime.Now - UnixTime).TotalMilliseconds;

            //id, longitude, latitude, altitude, x_speed, y_speed, z_speed
            NGCP.VehicleGlobalPosition vgp = new NGCP.VehicleGlobalPosition((byte)NodeID, (float)state.Longitude, (float)state.Latitude, 0);
            Node.Send(vgp, id);
        }
        /// <summary>
        /// Capture UGV state
        /// </summary>
        /// <param name="ugv"></param>
        /// <returns></returns>
        public static UGVState Capture(UGV ugv)
        {
            UGVState state = new UGVState();

            state.Latitude         = ugv.Latitude;
            state.Longitude        = ugv.Longitude;
            state.Pitch            = ugv.Pitch;
            state.Roll             = ugv.Roll;
            state.Heading          = ugv.Heading;
            state.FrontWheelOutput = ugv.FinalFrontWheel;
            state.RearWheelOutput  = ugv.FinalRearWheel;
            state.SteerOutput      = ugv.FinalSteering;
            state.WayPointBearing  = ugv.NextWaypointBearing;
            state.WayPointDistance = ugv.NextWaypointDistance;
            state.DriveMode        = (byte)ugv.Settings.DriveMode;
            state.TargetLocation   = ugv.TargetLockedLocation;
            return(state);
        }