Beispiel #1
1
        static void Main(string[] args)
        {
            try
            {
                //Initiallize the GPS object
                gps = new GPS();

                //Hook the basic event handlers
                gps.Attach += new AttachEventHandler(gps_Attach);
                gps.Detach += new DetachEventHandler(gps_Detach);
                gps.Error += new ErrorEventHandler(gps_Error);

                //Hook the phidget specific event handlers
                gps.PositionChange += new GPSPositionChangeEventHandler(gps_PositionChange);
                gps.PositionFixStatusChange += gps_PositionFixStatusChange;

                //open the object for GPS device connections
                gps.open();

                //Wait for a GPS device to be attached
                Console.WriteLine("Waiting for GPS to be attached....");
                gps.waitForAttachment();

                //Prompt for user input to continue
                Console.WriteLine("Press any button to continue...");
                Console.Read();

                //Prompt the user for input to terminate
                Console.WriteLine("Press any key to end...");
                Console.Read();

                //User input was read so we can terminate, close the GPS object
                gps.close();

                //Set the GPS object to null to get it out of memory
                gps = null;

                //If no exceptions were thrown by this point it is ok to terminate
                Console.WriteLine("ok");
            }
            catch (PhidgetException ex)
            {
                Console.WriteLine(ex.Description);
            }
        }
Beispiel #2
0
        //add track data to database (to gpsData table) and clear list of position
        public void addTrackData(GPS.Position <GpsPosition> positionList)
        {
            GpsPosition[] positionArray =new GpsPosition[positionList.Count];
            positionList.CopyTo(positionArray, 0);
            positionList.Clear();
            StringBuilder query = new StringBuilder();
            try
            {
                foreach (GpsPosition pos in positionArray)
                {
                    query.Append("INSERT INTO gpsData ('trackId', 'longitude', 'latitude', 'altitude', 'fix') VALUES (");
                    query.Append(trackId + ", '");
                    query.Append(pos.Longitude + "', '");
                    query.Append(pos.Latitude + "', '");
                    query.Append(pos.SeaLevelAltitude + "', '");
                    query.Append((int)pos.PositionDilutionOfPrecision + "');");

                }

                createSqliteConn();

                executeQuery(query, false);

                closeSqliteConn();

            }
            catch (SQLiteException sqliteEx)
            {
                MessageBox.Show("exception "+ sqliteEx.Message);
            }
        }
 // Use this for initialization
 void Start()
 {
     gps = GPs.GetComponent<GPS>();
     a = gps.dlat.ToString();
     b = gps.dlon.ToString();
     c = gps.alt.ToString();
 }
Beispiel #4
0
 /// <summary>
 /// Creates a travel object.
 /// </summary>
 /// <param name="_gps">GPS watcher</param>
 /// <param name="_car">Car object</param>
 public Travel(GPS.GPS _gps, Car _car)
 {
     this.GPS = _gps;
     this.Car = _car;
     this.TravelState = TravelStates.clear;
     this.ChangeRouteType(ChangeRouteTypeTravelEvent.RouteTypes.mixed);
     GPS.DistanceChanged += new EventHandler<DistanceEventArgs>(DistanceChanged);
 }
    // --------------------------------------------------
    public void parseBsonToSensorData(BsonDocument result)
    {
        compSensor = new compass(result["compass"].ToDouble());
        magSensor = new magnometer(result["magnometer.x"].ToDouble(), result["magnometer.y"].ToDouble(), result["magnometer.z"].ToDouble());
        barometricSensor = new barometer(result["barometer.pressure"].ToDouble(), result["barometer.temperature"].ToDouble());

        GPS.latitude lat = new GPS.latitude(result["latitude.degrees"].ToDouble(), result["latitude.minutes"].ToDouble(), result["latitude.direction"].ToString());
        GPS.longitude lon = new GPS.longitude(result["longitude.degrees"].ToDouble(), result["longitude.minutes"].ToDouble(), result["longitude.direction"].ToString());
        gpsSensor = new GPS(lat, lon);
    }
Beispiel #6
0
        /// <summary>
        /// Returns the Direction from the Current Position to the given Waypoint
        /// </summary>
        /// <param name="gps">The Current GPS</param>
        /// <param name="myWaypoint">A Waypoint Object containing the position of the Target Waypoint</param>
        /// <returns>The Bearing from the Current Position to the Waypoint in degrees (0..360)</returns>
        public double GetDirection(GPS gps, UAVCommons.Navigation.WayPoint myWaypoint)
        {
            double lat1 = Convert.ToDouble(gps["lbRMCPositionLatitude"].Value);
            double lat2 = myWaypoint.Latitude;
            double lon1 = Convert.ToDouble(gps["lbRMCPositionLongitude"].Value);
            double lon2 = myWaypoint.Longitude;

            // Calculate Bearing in Radians
            double resultRad = (Math.Atan2((Math.Cos(lat1)*Math.Sin(lat2)) - (Math.Sin(lat1)*Math.Cos(lat2)*Math.Cos(lon2 - lon1)),
                               Math.Sin(lon2 - lon1)*Math.Cos(lat2)) % (2*Math.PI));

            return resultRad*(180/Math.PI);
        }
Beispiel #7
0
        /// <summary>
        /// Constructor for the Application object.
        /// </summary>
        public App()
        {
            // Global handler for uncaught exceptions.
            UnhandledException += Application_UnhandledException;

            // Standard Silverlight initialization
            InitializeComponent();

            // Phone-specific initialization
            InitializePhoneApplication();

            //GPS Init
            GPSWatcher = new GPS();
               // GPSWatcher.startTracking();

            //TravelInit
            Travel = new Travel(GPSWatcher,new Car());

            //Set global app width&height
            double height = ((FrameworkElement)App.Current.RootVisual).ActualHeight;
            double width = ((FrameworkElement)App.Current.RootVisual).ActualWidth;

            // Show graphics profiling information while debugging.
            if (System.Diagnostics.Debugger.IsAttached)
            {
                // Display the current frame rate counters.
                Application.Current.Host.Settings.EnableFrameRateCounter = true;

                // Show the areas of the app that are being redrawn in each frame.
                //Application.Current.Host.Settings.EnableRedrawRegions = true;

                // Enable non-production analysis visualization mode,
                // which shows areas of a page that are handed off to GPU with a colored overlay.
                //Application.Current.Host.Settings.EnableCacheVisualization = true;

                // Disable the application idle detection by setting the UserIdleDetectionMode property of the
                // application's PhoneApplicationService object to Disabled.
                // Caution:- Use this under debug mode only. Application that disables user idle detection will continue to run
                // and consume battery power when the user is not using the phone.
                PhoneApplicationService.Current.UserIdleDetectionMode = IdleDetectionMode.Disabled;
            }
        }
    public void MoveCamera()
    {
        if (target == null)
        {
            target = GameObject.FindWithTag("Player");
            if (target == null)
            {
                return;
            }
        }
        if (Gps == null)
        {
            try { Gps = target.GetComponent <CarPlayerController>().Gps; }
            catch
            {
                try { Gps = target.GetComponent <HotrodPlayerController>().Gps; }
                catch
                {
                    try { Gps = target.GetComponent <ModelTPlayerController>().Gps; } catch { }
                }
            }
            if (Gps == null)
            {
                Debug.Log("GPS is null");
            }
        }

        _targetVelocity = target.GetComponent <Rigidbody>().velocity.magnitude;
        //FollowCam Version 2
        distance = 2f;
        if (_targetVelocity > 15f)
        //THe Cam tries to stay 2m behind the car but the car pulls away becuase of the Lerp
        {
            LerpSpeed = _targetVelocity / 15f;
        }
        else
        {
            LerpSpeed = 1;
        }

        if (_targetVelocity > 0.5f)
        {
            _statCamPosX = 0;
            //This works OK. Looks ahead a few segments and extrapolates back
            //so the camera swings out as you go round a bend
            Vector3 AP = Gps.LookAheadOffset(_targetVelocity * 4); // Rd.XSecs[LookaheadIdx].MidPt;
            Vector3 PP = Gps.LookAheadOffset(_targetVelocity * 3); //Rd.XSecs[ProjectedIdx].MidPt;
            Vector3 CurrentDirection = (transform.position - target.transform.position).normalized;
            Vector3 WantedDirection  = (PP - AP).normalized + Vector3.up * 0.6f;
            Vector3 NewDirection     = Vector3.Lerp(CurrentDirection, WantedDirection, Time.deltaTime);
            //Vector3 wantedPosition = target.transform.position + (direction * distance) + (Vector3.up * height);
            //Vector3 newPosition = Vector3.Slerp(transform.position, wantedPosition, LerpSpeed * Time.deltaTime);
            transform.position = target.transform.position + NewDirection * 10;
            trFollowCamInner.LookAt(target.transform.position);    //I tried looking in front of the car but it needs a lerp
        }
        else
        {
            //Swing the camera round to look at the side of the car
            //Which side of the car to look at:
            if (_statCamPosX == 0)
            {
                float Side = Random.value; if (Side > 0.5)
                {
                    _statCamPosX = -5f;
                }
                else
                {
                    _statCamPosX = 5f;
                }
            }
            LerpSpeed = 1;
            Vector3 p           = target.transform.position + target.transform.right * _statCamPosX + transform.up * 2;
            Vector3 newPosition = Vector3.Lerp(transform.position, p, LerpSpeed * Time.deltaTime);
            transform.position = new Vector3(newPosition.x, newPosition.y, newPosition.z);
            //transform.position = target.transform.position + target.transform.up*15f; // * -5f + transform.up;       //remove this line
            trFollowCamInner.LookAt(target.transform.position);
        }

#if UNITY_EDITOR
#elif UNITY_ANDROID || UNITY_IOS || UNITY_IPHONE
        if (!_horizonTilt)
        {
            return;
        }

        //this works but there's a delay
        float TiltAngle = Mathf.Atan2(-Input.acceleration.x, -Input.acceleration.y) * Mathf.Rad2Deg;
        //float DesiredTiltAngle = -Mathf.Asin(accelx) * Mathf.Rad2Deg;  //this was really jittery at big angles
        //if (accel.y > 0) { if (accel.x < 0) DesiredTiltAngle = -180 - DesiredTiltAngle; else DesiredTiltAngle = -DesiredTiltAngle + 180; }
        TiltAngles.Push(TiltAngle);
        float SmoothTiltAngle = TiltAngles.Avg; // OldTiltAngle * 0.7f + DesiredTiltAngle * 0.3f;
        trFollowCamInner.Rotate(Vector3.forward, SmoothTiltAngle);
#endif
    }
Beispiel #9
0
 public BoidsContact(MyIGCMessage msg)
 {
     this.source = msg.Source;
     this.pos    = new GPS(msg.Data.ToString());
 }
    public void MoveCamera()
    {
        if (_rbTarget == null)
        {
            {
                target = GameObject.FindWithTag("Player");
            }
            if (target == null)
            {
                return;
            }
            _rbTarget = target.GetComponent <Rigidbody>();
        }
        if (Gps == null)
        {
            try { Gps = target.GetComponent <CarPlayerController>().Gps; }
            catch
            {
                try { Gps = target.GetComponent <HotrodPlayerController>().Gps; }
                catch
                {
                    try { Gps = target.GetComponent <ModelTPlayerController>().Gps; } catch { }
                }
            }
            if (Gps == null)
            {
                Debug.Log("GPS is null");
            }
        }
        _targetVelocity = _rbTarget.velocity.magnitude;
        //FollowCam Version 1. This one is fine
        if (_targetVelocity > 15f)
        //Need to catch up with the car or it disappears in the distance
        {
            LerpSpeed = _targetVelocity / 12;
        }
        else
        {
            LerpSpeed = 1;
        }
        //This helps you see round bends because the cam moves up ans over the car so you can see the curve better
        Bend _nextBend = Gps.NextBend;

        if (Mathf.Abs(_nextBend.Angle) > 90 && Gps.CurrSegIdx > _nextBend.TurninXSec.Idx)
        {
            height = 40;
        }
        else
        {
            height = 12;
        }

        if (_targetVelocity > 0.5f)
        {
            _statCamPosX = 0;
            //This works OK. It simply stays behind the cars velocity vector so you can see skids
            //THe Cam tries to stay 2m behind the car but the car pulls away becuase of the Lerp
            //The outerCam deals with the horizontal movement. The InnerCam deals with the height and the rotation
            Vector3 wantedPosition = target.transform.position - new Vector3(_rbTarget.velocity.x, 0, _rbTarget.velocity.z).normalized *4;
            transform.position = Vector3.Slerp(transform.position, wantedPosition, LerpSpeed * Time.deltaTime);

            //Now lerp the height a bit slower than the distance
            wantedPosition = Vector3.up * height;
            trFollowCamInner.localPosition = Vector3.Slerp(trFollowCamInner.localPosition, wantedPosition, LerpSpeed / 4 * Time.deltaTime);
        }
        else
        {
            LerpSpeed = 1; height = 1;
            //Swing the camera round to look at the side of the car
            //Which side of the car to look at:
            if (_statCamPosX == 0)
            {
                float Side = Random.value; if (Side > 0.5)
                {
                    _statCamPosX = -5f;
                }
                else
                {
                    _statCamPosX = 5f;
                }
            }
            Vector3 p           = target.transform.position + Vector3.right * _statCamPosX;
            Vector3 newPosition = Vector3.Lerp(transform.position, p, LerpSpeed * Time.deltaTime);
            transform.position = newPosition;

            //Now lerp the height a bit slower
            Vector3 wantedPosition = Vector3.up * height;
            trFollowCamInner.localPosition = Vector3.Slerp(trFollowCamInner.localPosition, wantedPosition, LerpSpeed / 2 * Time.deltaTime);
        }

        trFollowCamInner.LookAt(target.transform.position);
        trFollowCamInner.Rotate(new Vector3(-_frustrumAngle / 2, 0, 0), Space.Self);


#if UNITY_EDITOR
#elif UNITY_ANDROID || UNITY_IOS || UNITY_IPHONE
        if (!_horizonTilt)
        {
            return;
        }

        //this works but there's a delay
        float TiltAngle = Mathf.Atan2(-Input.acceleration.x, -Input.acceleration.y) * Mathf.Rad2Deg;
        //float DesiredTiltAngle = -Mathf.Asin(accelx) * Mathf.Rad2Deg;  //this was really jittery at big angles
        //if (accel.y > 0) { if (accel.x < 0) DesiredTiltAngle = -180 - DesiredTiltAngle; else DesiredTiltAngle = -DesiredTiltAngle + 180; }
        TiltAngles.Push(TiltAngle);
        float SmoothTiltAngle = TiltAngles.Avg; // OldTiltAngle * 0.7f + DesiredTiltAngle * 0.3f;
        trFollowCamInner.Rotate(Vector3.forward, SmoothTiltAngle);
#endif
    }
Beispiel #11
0
    public VTOLUAV()                //  Constructor -> Is Called when the Object is created
    {                               // When the Object is loaded from the sd card this method will not be called
        uavData.Add(totalpidgain);
        joystick.values.Add(phi_rolerate);
        joystick.values.Add(theta_rolerate);
        joystick.values.Add(psi_rolerate);
        joystick.values.Add(throttle);
        uavData.Add(joystick);

        lagesensor = new AHRS("lagesensor", UsbHelper.GetDevicPathebyClass("AHRS"));
        lagesensor["cursettings"].Value = "371,376,379,17,-127,-87";

        uavData.Add(lagesensor);
        gpsempfänger = new GPS("gpsempfänger", UsbHelper.GetDevicPathebyClass("GPS"), 38400);
        uavData.Add(gpsempfänger);
        //	empfängerusb = new UAVJoystick ("empfängerusb", "", new IntPtr ());
        //uavData.Add (empfängerusb);
        watch.Start();
        // Servo Initialisierung
        servo1 = new PWM("servo1", 0, null, 1);                  //("Name", startwert, "null"=ausgabegerät_maestro, Kanal am Maestro)
        uavData.Add(servo1);
        servo1 ["LowLimit"].DoubleValue  = -100;
        servo1 ["Neutral"].DoubleValue   = 0;
        servo1 ["HighLimit"].DoubleValue = 100;
        servo1 ["Invert"].IntValue       = 0;
        servo1.SetHomePosition(0);

        servo2 = new PWM("servo2", 0, null, 2);
        uavData.Add(servo2);
        servo2 ["LowLimit"].DoubleValue  = -100;
        servo2 ["Neutral"].DoubleValue   = 0;
        servo2 ["HighLimit"].DoubleValue = 100;
        servo2 ["Invert"].IntValue       = 0;
        servo2.SetHomePosition(0);

        servo3 = new PWM("servo3", 100, null, 3);
        uavData.Add(servo3);
        servo3 ["LowLimit"].DoubleValue  = -80;
        servo3 ["Neutral"].DoubleValue   = 0;
        servo3 ["HighLimit"].DoubleValue = 80;
        servo3 ["Invert"].IntValue       = 1;
        servo3.SetHomePosition(100);

        servo4 = new PWM("servo4", 100, null, 4);
        uavData.Add(servo4);
        servo4 ["LowLimit"].DoubleValue  = -80;
        servo4 ["Neutral"].DoubleValue   = 0;
        servo4 ["HighLimit"].DoubleValue = 80;
        servo4 ["Invert"].IntValue       = 1;
        servo4.SetHomePosition(100);

        servo5 = new PWM("servo5", -80, null, 5);
        uavData.Add(servo5);

        servo5 ["LowLimit"].DoubleValue  = -100;
        servo5 ["Neutral"].DoubleValue   = 0;
        servo5 ["HighLimit"].DoubleValue = 100;
        servo5 ["Invert"].IntValue       = 0;
        servo5.SetHomePosition(-80);

        servo6 = new PWM("servo6", -80, null, 6);
        uavData.Add(servo6);

        servo6 ["LowLimit"].DoubleValue  = -100;
        servo6 ["Neutral"].DoubleValue   = 0;
        servo6 ["HighLimit"].DoubleValue = 100;
        servo6 ["Invert"].IntValue       = 0;
        servo6.SetHomePosition(-80);

        // PID CONFIG --------------------------------------
        kp_Höhe = new UAVParameter("kp_Höhe", 7, 0, 10, 0);                   //Sinnvoll=1Höhe = new UAVParameter ("kp_Höhe", 1, max, min, updaterate)
        kd_Höhe = new UAVParameter("kd_Höhe", 10, -10, 10, 0);                //Sinnvoll=-3  Achtung aus irgend einem Grund negativ !!!
        ki_Höhe = new UAVParameter("ki_Höhe", 0, 0, 10, 0);                   //Sinnvoll=1
        ks_Höhe = new UAVParameter("ks_Höhe", 0.0, 0, 10, 0);                 //Sinnvoll=0.2

        lp_Höhe = new UAVParameter("lp_Höhe", 1, 0, 10, 0);
        ld_Höhe = new UAVParameter("ld_Höhe", 1, 0, 10, 0);
        li_Höhe = new UAVParameter("li_Höhe", 1, 0, 10, 0);
        ls_Höhe = new UAVParameter("ls_Höhe", 0.2, 0, 10, 0);

        uavData.Add(kp_Höhe);
        uavData.Add(kd_Höhe);
        uavData.Add(ki_Höhe);
        uavData.Add(ks_Höhe);

        uavData.Add(lp_Höhe);
        uavData.Add(ld_Höhe);
        uavData.Add(li_Höhe);
        uavData.Add(ls_Höhe);
        //----------------------------------------------------
        kp_Quer = new UAVParameter("kp_Quer", 5, 0, 10, 0);                   //Sinnvoll=1
        kd_Quer = new UAVParameter("kd_Quer", 5, 0, 10, 0);                   //Sinnvoll=3
        ki_Quer = new UAVParameter("ki_Quer", 0, 0, 10, 0);                   //Sinnvoll=1
        ks_Quer = new UAVParameter("ks_Quer", 0.0, 0, 10, 0);                 //Sinnvoll=0.2

        lp_Quer = new UAVParameter("lp_Quer", 1, 0, 10, 0);
        ld_Quer = new UAVParameter("ld_Quer", 1, 0, 10, 0);
        li_Quer = new UAVParameter("li_Quer", 1, 0, 10, 0);
        ls_Quer = new UAVParameter("ls_Quer", 0.2, 0, 10, 0);

        uavData.Add(kp_Quer);
        uavData.Add(kd_Quer);
        uavData.Add(ki_Quer);
        uavData.Add(ks_Quer);

        uavData.Add(lp_Quer);
        uavData.Add(ld_Quer);
        uavData.Add(li_Quer);
        uavData.Add(ls_Quer);
        //----------------------------------------------------
        kp_Seite = new UAVParameter("kp_Seite", -3, -10, 10, 0);
        kd_Seite = new UAVParameter("kd_Seite", 6, -10, 10, 0);
        ki_Seite = new UAVParameter("ki_Seite", 0, -10, 10, 0);
        ks_Seite = new UAVParameter("ks_Seite", 0, -10, 10, 0);

        lp_Seite = new UAVParameter("lp_Seite", 0.5, 0, 10, 0);
        ld_Seite = new UAVParameter("ld_Seite", 0.5, 0, 10, 0);
        li_Seite = new UAVParameter("li_Seite", 1, 0, 10, 0);
        ls_Seite = new UAVParameter("ls_Seite", 0, 0, 10, 0);

        uavData.Add(kp_Seite);
        uavData.Add(kd_Seite);
        uavData.Add(ki_Seite);
        uavData.Add(ks_Seite);

        uavData.Add(lp_Seite);
        uavData.Add(ld_Seite);
        uavData.Add(li_Seite);
        uavData.Add(ls_Seite);
        //----------------------------------------------------
        sp_Höhe  = new UAVParameter("Höhe_SP", 0, -90, 90, 10);
        sp_Quer  = new UAVParameter("Quer_SP", 0, -180, 180, 10);
        sp_Seite = new UAVParameter("Seite_SP", 0, -180, 180, 10);
        uavData.Add(sp_Höhe);
        uavData.Add(sp_Quer);
        uavData.Add(sp_Seite);

        output_Höhe  = new UAVParameter("PID_Out_Höhe", 0, -100, 100, 0);
        output_Quer  = new UAVParameter("PID_Out_Quer", 0, -100, 100, 0);
        output_Seite = new UAVParameter("PID_Out_Seite", 0, -100, 100, 0);

        uavData.Add(output_Höhe);
        uavData.Add(output_Quer);
        uavData.Add(output_Seite);
        SteuerMotorLeistung = new UAVParameter("SteuerMotorLeistung", 0, 0, 100, 0);
        HauptMotorLeistung  = new UAVParameter("HauptMotorLeistung", -80, -100, 100, 0);
        HauptMotorDiff      = new UAVParameter("HauptMotorDiff", 0, -20, 20, 0);
        uavData.Add(SteuerMotorLeistung);
        uavData.Add(HauptMotorLeistung);
        uavData.Add(HauptMotorDiff);


        NewSeitePV = new UAVParameter("NewSeitePV", 0, -180, 180, 0);
        uavData.Add(NewSeitePV);
        uavData.Add(output_Höhe);
    }
Beispiel #12
0
        public void GetLocalGMTOffset_AtOffset()
        {
            var dateTime = DateTime.UtcNow;

            GPS.GetLocalGMTOffset(dateTime).Should().Be(TimeZoneInfo.Local.GetUtcOffset(dateTime));
        }
Beispiel #13
0
        public void GPSOriginTimeToDateTime_Invalid_MilliSecondsInWeek()
        {
            Action act = () => GPS.GPSOriginTimeToDateTime(1000, 1_000_000_000);

            act.Should().Throw <TRexException>().WithMessage("GPS millisecondsInWeek: * not in range 0*");
        }
 public GPSAdapter(GPS target)
 {
     _target = target;
 }
Beispiel #15
0
 public GPSDriveState(GPS gate, double proxy)
 {
     this.gate           = gate;
     this.GATE_PROXIMITY = proxy;
 }
Beispiel #16
0
 public GPSDriveState(GPS gate)
 {
     this.gate = gate;
 }
Beispiel #17
0
        /// <summary>
        /// Given a Plot representing the obstacles, find the Line representing the best gap.
        /// Do GPS driving according to our rendition of the "Follow the Gap" algorithm:
        /// Reference here: https://pdfs.semanticscholar.org/c432/3017af7bce46fc7574ada008b8af1011e614.pdf
        /// This algorithm avoids obstacles by finding the gap between them. It has a threshold gap (i.e. robot width),
        /// and if the measured gap is greater than the threshold gap, the robot follows the calculated gap angle.
        /// In our case, the best gap will also be the one with the smallest displacement from the goal (the gate).
        /// </summary>
        public Line FindBestGap(Plot obstacles)
        {
            List <Region> regions   = obstacles.Regions;
            Line          bestGap   = null;
            double        bestAngle = Double.MaxValue;

            // Get GPS, heading data
            GPS   currGPS     = AscentPacketHandler.GPSData;
            short currCompass = AscentPacketHandler.Compass;

            // Add distance to averaging queue
            double distance = currGPS.GetDistanceTo(this.gate);

            while (this.averagingQueue.Count >= AVERAGING_QUEUE_CAPACITY)
            {
                double value;
                this.averagingQueue.TryDequeue(out value);
            }
            this.averagingQueue.Enqueue(distance);

            // Heading from current GPS to gate GPS
            double idealDirection = currGPS.GetHeadingTo(gate);

            // Check first and last Region gaps (may be same Region if only one Region)
            Region firstRegion = regions.ElementAt(0);
            Region lastRegion  = regions.ElementAt(regions.Count - 1);

            // Left and right fields of view (FOV)
            Line lrfLeftFOV  = DriveContext.LRF_LEFT_FOV_EDGE;
            Line lrfRightFOV = DriveContext.LRF_RIGHT_FOV_EDGE;

            // Check if leftmost Coordinate in the leftmost Region is on the right half of the entire FOV. If it is, make leftEdgeCoordinate where the
            // max-acceptable-range meets the left FOV line, since the FindClosestPointOnLine function return will cause errors for 180 degree FOV.
            Coordinate leftEdgeCoordinate = Line.FindClosestPointOnLine(lrfLeftFOV, firstRegion.StartCoordinate);

            if (firstRegion.StartCoordinate.X > 0)
            {
                leftEdgeCoordinate = new Coordinate(-AutonomousService.OBSTACLE_DETECTION_DISTANCE, 0, CoordSystem.Cartesian);
            }

            // Checking the left edge gap
            Line leftEdgeGap = new Line(leftEdgeCoordinate, firstRegion.StartCoordinate);

            if (leftEdgeGap.Length >= DriveContext.ASCENT_WIDTH)
            {
                double     angle;
                Coordinate leftMidPoint = leftEdgeGap.FindMidpoint();

                if (leftMidPoint.X > 0)
                {
                    angle = currCompass + (90 - (Math.Atan2(leftMidPoint.Y, leftMidPoint.X) * (180 / Math.PI)));
                    angle = angle % 360;
                }
                else if (leftMidPoint.X < 0)
                {
                    angle = currCompass - (90 - (Math.Atan2(-1 * leftMidPoint.Y, leftMidPoint.X) * (180 / Math.PI)));
                    angle = angle % 360;
                }
                else
                {
                    // midpoint.X is 0
                    angle = currCompass;
                }

                if (Math.Abs(idealDirection - angle) < bestAngle)
                {
                    bestAngle = idealDirection - angle;
                    bestGap   = leftEdgeGap;
                }
            }

            // Check if rightmost Coordinate in the rightmost Region is on the left half of the entire FOV. If it is, make rightEdgeCoordinate where the
            // max-acceptable-range meets the right FOV line, since the FindClosestPointOnLine function return will cause errors for 180 degree FOV.
            Coordinate rightEdgeCoordinate = Line.FindClosestPointOnLine(lrfRightFOV, lastRegion.EndCoordinate);

            if (lastRegion.EndCoordinate.X < 0)
            {
                rightEdgeCoordinate = new Coordinate(AutonomousService.OBSTACLE_DETECTION_DISTANCE, 0, CoordSystem.Cartesian);
            }

            // Checking the right edge gap
            Line rightEdgeGap = new Line(rightEdgeCoordinate, lastRegion.EndCoordinate);

            if (rightEdgeGap.Length >= DriveContext.ASCENT_WIDTH)
            {
                double     angle;
                Coordinate rightMidPoint = rightEdgeGap.FindMidpoint();

                if (rightMidPoint.X > 0)
                {
                    angle = currCompass + (90 - (Math.Atan2(rightMidPoint.Y, rightMidPoint.X) * (180 / Math.PI)));
                    angle = angle % 360;
                }
                else if (rightMidPoint.X < 0)
                {
                    angle = currCompass - (90 - (Math.Atan2(-1 * rightMidPoint.Y, rightMidPoint.X) * (180 / Math.PI)));
                    angle = angle % 360;
                }
                else
                {
                    // midpoint.X is 0
                    angle = currCompass;
                }

                if (Math.Abs(idealDirection - angle) < bestAngle)
                {
                    bestAngle = angle;
                    bestGap   = rightEdgeGap;
                }
            }

            // Iterate through the rest of the gaps to find the bestGap by calculating valid (large enough) gaps as Line objects
            for (int i = 0; i < regions.Count - 2; i++) // don't iterate through entire list, will result in index out of bounds error on rightRegion assignment
            {
                Region leftRegion  = regions[i];
                Region rightRegion = regions[i + 1];

                // Gap is distance, just needs to be big enough (the width of the robot)
                double gap = Plot.GapDistanceBetweenRegions(leftRegion, rightRegion);
                if (gap >= DriveContext.ASCENT_WIDTH)
                {
                    Line gapLine = new Line(new Coordinate(leftRegion.EndCoordinate.X, leftRegion.EndCoordinate.Y, CoordSystem.Cartesian),
                                            new Coordinate(rightRegion.StartCoordinate.X, rightRegion.StartCoordinate.Y, CoordSystem.Cartesian));

                    Coordinate midpoint = gapLine.FindMidpoint();
                    if (bestGap == null)
                    {
                        bestGap = gapLine;
                    }
                    else
                    {
                        double angle;

                        if (midpoint.X > 0)
                        {
                            angle = currCompass + (90 - (Math.Atan2(midpoint.Y, midpoint.X) * (180 / Math.PI)));
                            angle = angle % 360;
                        }
                        else if (midpoint.X < 0)
                        {
                            angle = currCompass - (90 - (Math.Atan2(-1 * midpoint.Y, midpoint.X) * (180 / Math.PI)));
                            angle = angle % 360;
                        }
                        else
                        {
                            // midpoint.X is 0
                            angle = currCompass;
                        }

                        // Check if the gap found gets us closer to our destination
                        if (Math.Abs(idealDirection - angle) < bestAngle)
                        {
                            bestAngle = angle;
                            bestGap   = gapLine;
                        }
                    }
                }
            }

            return(bestGap);
        }
Beispiel #18
0
 /// <summary>
 /// 发送行到串口
 /// </summary>
 /// <param name="Line"></param>
 private void SendSerialLineToUIThread(string Line)
 {
     GPS.ProcessNmeaData(Line);
 }
Beispiel #19
0
 private void Awake()
 {
     Instance = this;
     DontDestroyOnLoad(gameObject);
 }
Beispiel #20
0
        /// <summary>
        /// Find the next DriveCommand to be issued to ROCKS.
        /// </summary>
        /// <returns>DriveCommand - the next drive command for ROCKS to execute.</returns>
        public DriveCommand FindNextDriveCommand()
        {
            GPS    currGPS        = AscentPacketHandler.GPSData;
            short  currCompass    = AscentPacketHandler.Compass;
            double idealDirection = currGPS.GetHeadingTo(gate);
            double distance       = AscentPacketHandler.GPSData.GetDistanceTo(gate);

            // Add distance to averaging queue
            while (this.averagingQueue.Count >= AVERAGING_QUEUE_CAPACITY)
            {
                double value;
                this.averagingQueue.TryDequeue(out value);
            }
            this.averagingQueue.Enqueue(distance);

            // Debugging - delete
            Console.Write("currCompass: "******" | headingToGoal: " + idealDirection + " | distance: " + distance + " | ");

            // Stop when within proximity to see if average distance of last 5 distances is within proximity.
            // If so, wait to switch to Vision, otherwise this acts as a buffer.
            if (distance <= GATE_PROXIMITY)
            {
                return(DriveCommand.Straight(Speed.HALT));
            }

            // If current heading within threshold, go straight
            if (IMU.IsHeadingWithinThreshold(currCompass, idealDirection, THRESHOLD_HEADING_ANGLE))
            {
                return(DriveCommand.Straight(Speed.SLOW_OPERATION));
            }

            // not aligned with endGPS point, need to turn
            // the math here is strange due to compass vs unit circle stuff
            // the first case takes care of all time when the ideal direction is in some way east of us,
            // and the second case takes care of all time when the ideal direction is in some way west of us
            double opposite = (idealDirection + 180) % 360;

            if (idealDirection < opposite)
            {
                // Modulo not necessary - ideal direction < 180
                if (currCompass > idealDirection && currCompass < opposite)
                {
                    // Turn left
                    return(DriveCommand.LeftTurn(Speed.SLOW_TURN));
                }
                else
                {
                    // Turn right
                    return(DriveCommand.RightTurn(Speed.SLOW_TURN));
                }
            }
            else
            {
                // Modulo necessary
                if ((currCompass > idealDirection && currCompass < 360) || (currCompass >= 0 && currCompass < opposite))
                {
                    // Turn left
                    return(DriveCommand.LeftTurn(Speed.SLOW_TURN));
                }
                else
                {
                    // Turn right
                    return(DriveCommand.RightTurn(Speed.SLOW_TURN));
                }
            }
        }
 public string GetLocation(WiFi wifi, _3G thirdGeneration, GPS gps)
 {
     return(_geolocation.GetLocation(wifi, thirdGeneration, gps));
 }
Beispiel #22
0
        public string ToCSVString()
        {
            string csvString = "";

            csvString = csvString + Registration + "," + Grade + "," + Make + "," + Model + "," + Year.ToString() + "," + NumSeats.ToString() + "," + Transmission + "," + Fuel + "," + GPS.ToString() + "," + SunRoof.ToString() + "," + DailyRate.ToString() + "," + Colour + "";
            return(csvString);
        }
Beispiel #23
0
 private void Start()
 {
     gps = FindObjectOfType <GPS>();
 }
        private void GetDirections(object sender, System.Windows.Input.GestureEventArgs e)
        {
            Friend selected = (sender as MenuItem).DataContext as Friend;

            GPS.GetDirections(new GeoCoordinate(selected.LastPosition.Latitude, selected.LastPosition.Longitude));
        }
Beispiel #25
0
 public void GetLocalGMTOffset()
 {
     GPS.GetLocalGMTOffset().Should().Be(TimeZoneInfo.Local.GetUtcOffset(DateTime.UtcNow));
 }
Beispiel #26
0
        public static dynamic InformationNameConveter(DEVICECATEGORY DeviceName, String FieldNameStr)
        {
            switch (DeviceName)
            {
            case DEVICECATEGORY.TS700MM:
                TS700MM TS700MMFieldName = Enum.GetValues(typeof(TS700MM)).Cast <TS700MM>().ToList().FirstOrDefault(Item => Item.ToString() == FieldNameStr.ToUpper());
                if (TS700MMFieldName == TS700MM.NULL)
                {
                    return(null);
                }
                else
                {
                    return(TS700MMFieldName);
                }

            case DEVICECATEGORY.ASTROHEVENDOME:
                ASTROHEVENDOME ASTROHEVENDOMEFieldName = Enum.GetValues(typeof(ASTROHEVENDOME)).Cast <ASTROHEVENDOME>().ToList().FirstOrDefault(Item => Item.ToString() == FieldNameStr.ToUpper());
                if (ASTROHEVENDOMEFieldName == ASTROHEVENDOME.NULL)
                {
                    return(null);
                }
                else
                {
                    return(ASTROHEVENDOMEFieldName);
                }

            case DEVICECATEGORY.IMAGING:
                IMAGING IMAGINGFieldName = Enum.GetValues(typeof(IMAGING)).Cast <IMAGING>().ToList().FirstOrDefault(Item => Item.ToString() == FieldNameStr.ToUpper());
                if (IMAGINGFieldName == IMAGING.NULL)
                {
                    return(null);
                }
                else
                {
                    return(IMAGINGFieldName);
                }

            case DEVICECATEGORY.SQM:
                SQM SQMFieldName = Enum.GetValues(typeof(SQM)).Cast <SQM>().ToList().FirstOrDefault(Item => Item.ToString() == FieldNameStr.ToUpper());
                if (SQMFieldName == SQM.NULL)
                {
                    return(null);
                }
                else
                {
                    return(SQMFieldName);
                }

            case DEVICECATEGORY.SEEING:
                SEEING SEEINGFieldName = Enum.GetValues(typeof(SEEING)).Cast <SEEING>().ToList().FirstOrDefault(Item => Item.ToString() == FieldNameStr.ToUpper());
                if (SEEINGFieldName == SEEING.NULL)
                {
                    return(null);
                }
                else
                {
                    return(SEEINGFieldName);
                }

            case DEVICECATEGORY.ALLSKY:
                ALLSKY ALLSKYFieldName = Enum.GetValues(typeof(ALLSKY)).Cast <ALLSKY>().ToList().FirstOrDefault(Item => Item.ToString() == FieldNameStr.ToUpper());
                if (ALLSKYFieldName == ALLSKY.NULL)
                {
                    return(null);
                }
                else
                {
                    return(ALLSKYFieldName);
                }

            case DEVICECATEGORY.WEATHERSTATION:
                WEATHERSTATION WEATHERSTATIONFieldName = Enum.GetValues(typeof(WEATHERSTATION)).Cast <WEATHERSTATION>().ToList().FirstOrDefault(Item => Item.ToString() == FieldNameStr.ToUpper());
                if (WEATHERSTATIONFieldName == WEATHERSTATION.NULL)
                {
                    return(null);
                }
                else
                {
                    return(WEATHERSTATIONFieldName);
                }

            case DEVICECATEGORY.LANOUTLET:
                LANOUTLET LANOUTLETFieldName = Enum.GetValues(typeof(LANOUTLET)).Cast <LANOUTLET>().ToList().FirstOrDefault(Item => Item.ToString() == FieldNameStr.ToUpper());
                if (LANOUTLETFieldName == LANOUTLET.NULL)
                {
                    return(null);
                }
                else
                {
                    return(LANOUTLETFieldName);
                }

            case DEVICECATEGORY.CCTV:
                CCTV CCTVFieldName = Enum.GetValues(typeof(CCTV)).Cast <CCTV>().ToList().FirstOrDefault(Item => Item.ToString() == FieldNameStr.ToUpper());
                if (CCTVFieldName == CCTV.NULL)
                {
                    return(null);
                }
                else
                {
                    return(CCTVFieldName);
                }

            case DEVICECATEGORY.GPS:
                GPS GPSFieldName = Enum.GetValues(typeof(GPS)).Cast <GPS>().ToList().FirstOrDefault(Item => Item.ToString() == FieldNameStr.ToUpper());
                if (GPSFieldName == GPS.NULL)
                {
                    return(null);
                }
                else
                {
                    return(GPSFieldName);
                }

            case DEVICECATEGORY.ASTROCLIENT:
                ASTROCLIENT ASTROCLIENTFieldName = Enum.GetValues(typeof(ASTROCLIENT)).Cast <ASTROCLIENT>().ToList().FirstOrDefault(Item => Item.ToString() == FieldNameStr.ToUpper());
                if (ASTROCLIENTFieldName == ASTROCLIENT.NULL)
                {
                    return(null);
                }
                else
                {
                    return(ASTROCLIENTFieldName);
                }

            case DEVICECATEGORY.ASTROSERVER:
                ASTROSERVER ASTROSERVERFieldName = Enum.GetValues(typeof(ASTROSERVER)).Cast <ASTROSERVER>().ToList().FirstOrDefault(Item => Item.ToString() == FieldNameStr.ToUpper());
                if (ASTROSERVERFieldName == ASTROSERVER.NULL)
                {
                    return(null);
                }
                else
                {
                    return(ASTROSERVERFieldName);
                }

            default: return(null);
            }
        }
Beispiel #27
0
 // Start is called before the first frame update
 void Start()
 {
     gpsCode = GameObject.Find("SceneManager").GetComponent <GPS>();
 }
 void Start()
 {
     sphereRot = new SphereRotate(Sphere);
     gps = new GPS ();
     inputString = gps.dlat.ToString();
     inputString1 = gps.dlon.ToString();
     inputString2 = gps.alt.ToString();
 }
 void OnDestroy()
 {
     Gps       = null;
     LookPoses = null;
 }
        private void Form1_Load(object sender, EventArgs e)
        {
            gps = new GPS();

            gps.Attach += new AttachEventHandler(gps_Attach);
            gps.Detach += new DetachEventHandler(gps_Detach);
            gps.Error += new ErrorEventHandler(gps_Error);

            gps.PositionChange += new GPSPositionChangeEventHandler(gps_PositionChange);
            gps.PositionFixStatusChange += new GPSPositionFixStatusChangeEventHandler(gps_PositionFixStatusChange);

            openCmdLine(gps);

            gMapControl1.CurrentPosition = new GMap.NET.PointLatLng(0, 0);
        }
Beispiel #31
0
    public static void readCardData(this BinaryReader r, gameCard cardTemp = null)
    {
        gameCard cardToRefresh = cardTemp;
        int      flag          = r.ReadInt32();
        int      code          = 0;
        GPS      gps           = new GPS();

        if ((flag & (int)Query.Code) != 0)
        {
            code = r.ReadInt32();
        }
        if ((flag & (int)Query.Position) != 0)
        {
            gps           = r.ReadGPS();
            cardToRefresh = null;
            cardToRefresh = Program.I().ocgcore.GCS_cardGet(gps, false);
        }

        if (cardToRefresh == null)
        {
            return;
        }

        YGOSharp.Card data = cardToRefresh.get_data();

        if ((flag & (int)Query.Code) != 0)
        {
            if (data.Id != code)
            {
                data    = YGOSharp.CardsManager.Get(code);
                data.Id = code;
            }
        }
        if ((flag & (int)Query.Position) != 0)
        {
            cardToRefresh.p = gps;
        }


        if (data.Id > 0)
        {
            if ((cardToRefresh.p.location & (UInt32)CardLocation.Hand) > 0)
            {
                if (cardToRefresh.p.controller == 1)
                {
                    cardToRefresh.p.position = (Int32)CardPosition.FaceUpAttack;
                }
            }
        }

        if ((flag & (int)Query.Alias) != 0)
        {
            data.Alias = r.ReadInt32();
        }
        if ((flag & (int)Query.Type) != 0)
        {
            data.Type = r.ReadInt32();
        }

        int l1 = 0;

        if ((flag & (int)Query.Level) != 0)
        {
            l1 = r.ReadInt32();
        }
        int l2 = 0;

        if ((flag & (int)Query.Rank) != 0)
        {
            l2 = r.ReadInt32();
        }

        if ((flag & (int)Query.Attribute) != 0)
        {
            data.Attribute = r.ReadInt32();
        }
        if ((flag & (int)Query.Race) != 0)
        {
            data.Race = r.ReadInt32();
        }
        if ((flag & (int)Query.Attack) != 0)
        {
            data.Attack = r.ReadInt32();
        }
        if ((flag & (int)Query.Defence) != 0)
        {
            data.Defense = r.ReadInt32();
        }
        if ((flag & (int)Query.BaseAttack) != 0)
        {
            r.ReadInt32();
        }
        if ((flag & (int)Query.BaseDefence) != 0)
        {
            r.ReadInt32();
        }
        if ((flag & (int)Query.Reason) != 0)
        {
            r.ReadInt32();
        }
        if ((flag & (int)Query.ReasonCard) != 0)
        {
            r.ReadInt32();
        }
        if ((flag & (int)Query.EquipCard) != 0)
        {
            cardToRefresh.addTarget(Program.I().ocgcore.GCS_cardGet(r.ReadGPS(), false));
        }
        if ((flag & (int)Query.TargetCard) != 0)
        {
            int count = r.ReadInt32();
            for (int i = 0; i < count; ++i)
            {
                cardToRefresh.addTarget(Program.I().ocgcore.GCS_cardGet(r.ReadGPS(), false));
            }
        }
        if ((flag & (int)Query.OverlayCard) != 0)
        {
            var overs = Program.I().ocgcore.GCS_cardGetOverlayElements(cardToRefresh);
            int count = r.ReadInt32();
            for (int i = 0; i < count; ++i)
            {
                if (i < overs.Count)
                {
                    overs[i].set_code(r.ReadInt32());
                }
                else
                {
                    r.ReadInt32();
                }
            }
        }
        if ((flag & (int)Query.Counters) != 0)
        {
            int count = r.ReadInt32();
            for (int i = 0; i < count; ++i)
            {
                r.ReadInt32();
            }
        }
        if ((flag & (int)Query.Owner) != 0)
        {
            r.ReadInt32();
        }
        if ((flag & (int)Query.Status) != 0)
        {
            int status = r.ReadInt32();
            cardToRefresh.disabled = (status & 0x0001) == 0x0001;
        }
        if ((flag & (int)Query.LScale) != 0)
        {
            data.LScale = r.ReadInt32();
        }
        if ((flag & (int)Query.RScale) != 0)
        {
            data.RScale = r.ReadInt32();
        }
        int l3 = 0;

        if ((flag & (int)Query.Link) != 0)
        {
            l3 = r.ReadInt32(); //link value
            data.LinkMarker = r.ReadInt32();
        }
        if (((flag & (int)Query.Level) != 0) || ((flag & (int)Query.Rank) != 0) || ((flag & (int)Query.Link) != 0))
        {
            if (l1 > l2)
            {
                data.Level = l1;
            }
            else
            {
                data.Level = l2;
            }
            if (l3 > data.Level)
            {
                data.Level = l3;
            }
        }

        cardToRefresh.set_data(data);
        //
    }
Beispiel #32
0
 public void Finish()
 {
     Network.Finish();
     GPS.Finish();
 }
 void OnDestroy()
 {
     Gps = null;
 }
Beispiel #34
0
 // Start is called before the first frame update
 void Start()
 {
     Instance = this;
     DontDestroyOnLoad(gameObject);
     StartCoroutine(StartLocationService());
 }
 void Start()
 {
     gps = GPs.GetComponent<GPS>();
     //sphereRot = new SphereRotate(Sphere);
     inputString = gps.dlat.ToString ();
     inputString1 = gps.dlon.ToString();
     inputString2 = gps.alt.ToString();
 }
Beispiel #36
0
    //  Constructor -> Is Called when the Object is created
    public VTOLUAV()
    {
        // When the Object is loaded from the sd card this method will not be called

            uavData.Add(totalpidgain);
            joystick.values.Add(phi_rolerate);
            joystick.values.Add(theta_rolerate);
            joystick.values.Add(psi_rolerate);
            joystick.values.Add(throttle);
            uavData.Add(joystick);

            lagesensor = new AHRS ("lagesensor", UsbHelper.GetDevicPathebyClass ("AHRS"));
            lagesensor["cursettings"].Value = "371,376,379,17,-127,-87";

            uavData.Add (lagesensor);
            gpsempfänger = new GPS ("gpsempfänger", UsbHelper.GetDevicPathebyClass ("GPS"), 38400);
            uavData.Add (gpsempfänger);
        //	empfängerusb = new UAVJoystick ("empfängerusb", "", new IntPtr ());
            //uavData.Add (empfängerusb);
        watch.Start();
            // Servo Initialisierung
            servo1 = new PWM ("servo1", 0, null, 1); //("Name", startwert, "null"=ausgabegerät_maestro, Kanal am Maestro)
            uavData.Add (servo1);
            servo1 ["LowLimit"].DoubleValue = -100;
            servo1 ["Neutral"].DoubleValue = 0;
            servo1 ["HighLimit"].DoubleValue = 100;
            servo1 ["Invert"].IntValue = 0;
            servo1.SetHomePosition(0);

            servo2 = new PWM ("servo2", 0, null, 2);
            uavData.Add (servo2);
            servo2 ["LowLimit"].DoubleValue = -100;
            servo2 ["Neutral"].DoubleValue = 0;
            servo2 ["HighLimit"].DoubleValue = 100;
            servo2 ["Invert"].IntValue = 0;
            servo2.SetHomePosition(0);

            servo3 = new PWM ("servo3", 100, null, 3);
            uavData.Add (servo3);
            servo3 ["LowLimit"].DoubleValue = -80;
            servo3 ["Neutral"].DoubleValue = 0;
            servo3 ["HighLimit"].DoubleValue = 80;
            servo3 ["Invert"].IntValue = 1;
            servo3.SetHomePosition(100);

            servo4 = new PWM ("servo4", 100, null, 4);
            uavData.Add (servo4);
            servo4 ["LowLimit"].DoubleValue = -80;
            servo4 ["Neutral"].DoubleValue = 0;
            servo4 ["HighLimit"].DoubleValue = 80;
            servo4 ["Invert"].IntValue = 1;
            servo4.SetHomePosition(100);

            servo5 = new PWM ("servo5", -80, null, 5);
            uavData.Add (servo5);

            servo5 ["LowLimit"].DoubleValue = -100;
            servo5 ["Neutral"].DoubleValue = 0;
            servo5 ["HighLimit"].DoubleValue = 100;
            servo5 ["Invert"].IntValue = 0;
            servo5.SetHomePosition(-80);

            servo6 = new PWM ("servo6", -80, null, 6);
            uavData.Add (servo6);

            servo6 ["LowLimit"].DoubleValue = -100;
            servo6 ["Neutral"].DoubleValue = 0;
            servo6 ["HighLimit"].DoubleValue = 100;
            servo6 ["Invert"].IntValue = 0;
            servo6.SetHomePosition(-80);

            // PID CONFIG --------------------------------------
            kp_Höhe = new UAVParameter ("kp_Höhe", 7  , 0, 10, 0);//Sinnvoll=1Höhe = new UAVParameter ("kp_Höhe", 1, max, min, updaterate)
            kd_Höhe = new UAVParameter ("kd_Höhe", 10 ,  -10, 10, 0);//Sinnvoll=-3  Achtung aus irgend einem Grund negativ !!!
            ki_Höhe = new UAVParameter ("ki_Höhe", 0  , 0, 10, 0);//Sinnvoll=1
            ks_Höhe = new UAVParameter ("ks_Höhe", 0.0, 0, 10, 0);//Sinnvoll=0.2

            lp_Höhe = new UAVParameter ("lp_Höhe", 1  , 0, 10, 0);
            ld_Höhe = new UAVParameter ("ld_Höhe", 1  , 0, 10, 0);
            li_Höhe = new UAVParameter ("li_Höhe", 1  , 0, 10, 0);
            ls_Höhe = new UAVParameter ("ls_Höhe", 0.2  , 0, 10, 0);

            uavData.Add (kp_Höhe);
            uavData.Add (kd_Höhe);
            uavData.Add (ki_Höhe);
            uavData.Add (ks_Höhe);

            uavData.Add (lp_Höhe);
            uavData.Add (ld_Höhe);
            uavData.Add (li_Höhe);
            uavData.Add (ls_Höhe);
            //----------------------------------------------------
            kp_Quer = new UAVParameter ("kp_Quer", 5  , 0, 10, 0);//Sinnvoll=1
            kd_Quer = new UAVParameter ("kd_Quer", 5  , 0, 10, 0);//Sinnvoll=3
            ki_Quer = new UAVParameter ("ki_Quer", 0  , 0, 10, 0);//Sinnvoll=1
            ks_Quer = new UAVParameter ("ks_Quer", 0.0, 0, 10, 0);//Sinnvoll=0.2

            lp_Quer = new UAVParameter ("lp_Quer", 1  , 0, 10, 0);
            ld_Quer = new UAVParameter ("ld_Quer", 1  , 0, 10, 0);
            li_Quer = new UAVParameter ("li_Quer", 1  , 0, 10, 0);
            ls_Quer = new UAVParameter ("ls_Quer", 0.2 , 0, 10, 0);

            uavData.Add (kp_Quer);
            uavData.Add (kd_Quer);
            uavData.Add (ki_Quer);
            uavData.Add (ks_Quer);

            uavData.Add (lp_Quer);
            uavData.Add (ld_Quer);
            uavData.Add (li_Quer);
            uavData.Add (ls_Quer);
            //----------------------------------------------------
            kp_Seite = new UAVParameter ("kp_Seite", -3  , -10, 10, 0);
            kd_Seite = new UAVParameter ("kd_Seite", 6  , -10, 10, 0);
            ki_Seite = new UAVParameter ("ki_Seite", 0  , -10, 10, 0);
            ks_Seite = new UAVParameter ("ks_Seite", 0  , -10, 10, 0);

            lp_Seite = new UAVParameter ("lp_Seite", 0.5  , 0, 10, 0);
            ld_Seite = new UAVParameter ("ld_Seite", 0.5  , 0, 10, 0);
            li_Seite = new UAVParameter ("li_Seite", 1  , 0, 10, 0);
            ls_Seite = new UAVParameter ("ls_Seite", 0  , 0, 10, 0);

            uavData.Add (kp_Seite);
            uavData.Add (kd_Seite);
            uavData.Add (ki_Seite);
            uavData.Add (ks_Seite);

            uavData.Add (lp_Seite);
            uavData.Add (ld_Seite);
            uavData.Add (li_Seite);
            uavData.Add (ls_Seite);
            //----------------------------------------------------
            sp_Höhe  = new UAVParameter ("Höhe_SP" , 0,  -90,  90, 10);
            sp_Quer  = new UAVParameter ("Quer_SP" , 0, -180, 180, 10);
            sp_Seite = new UAVParameter ("Seite_SP", 0, -180, 180, 10);
            uavData.Add (sp_Höhe);
            uavData.Add (sp_Quer);
            uavData.Add (sp_Seite);

            output_Höhe  = new UAVParameter("PID_Out_Höhe" ,0,-100,100,0);
            output_Quer  = new UAVParameter("PID_Out_Quer" ,0,-100,100,0);
            output_Seite = new UAVParameter("PID_Out_Seite",0,-100,100,0);

            uavData.Add(output_Höhe);
            uavData.Add(output_Quer);
            uavData.Add(output_Seite);
            SteuerMotorLeistung = new UAVParameter("SteuerMotorLeistung",0,0,100,0);
                HauptMotorLeistung = new UAVParameter("HauptMotorLeistung",-80,-100,100,0);
                HauptMotorDiff = new UAVParameter("HauptMotorDiff",0,-20,20,0);
            uavData.Add(SteuerMotorLeistung);
            uavData.Add(HauptMotorLeistung);
            uavData.Add(HauptMotorDiff);

            NewSeitePV = new UAVParameter("NewSeitePV",0,-180,180,0);
            uavData.Add(NewSeitePV);
            uavData.Add(output_Höhe);
    }
Beispiel #37
0
        public virtual int sceUsbGpsOpen()
        {
            GPS.initialize();

            return(0);
        }
Beispiel #38
0
        public void DateTimeToGPSOriginTime_Invalid_NonUTCDate()
        {
            Action act = () => GPS.DateTimeToGPSOriginTime(DateTime.Now, out _, out _);

            act.Should().Throw <ArgumentException>().WithMessage("Date must be a UTC date time*");
        }
Beispiel #39
0
 void Start()
 {
     gps = FindObjectOfType <GPS>();
     Debug.Log("BuildMapLocation");
     LocationProvider.OnLocationUpdated += LocationProvider_OnLocationUpdated;
 }
Beispiel #40
0
        public void DateTimeToGPSOriginTime_Invalid_Early()
        {
            Action act = () => GPS.DateTimeToGPSOriginTime(DateTime.SpecifyKind(new DateTime(1950, 1, 1), DateTimeKind.Utc), out _, out _);

            act.Should().Throw <ArgumentException>().WithMessage("Date to be converted to GPS date is before the GPS date origin*");
        }
Beispiel #41
0
        public void Add_Leg_List(LinkedList <WayPoints> list, double lat1, double lon1, double lat2, double lon2,
                                 double alt, double heading, double curvesize, double rotdir, int gimblemode, double gimblepitch, int[,] actions,
                                 bool video, double camera_increment, double overlap)
        {
            // See if we have to reverse the points

            WayPoints wp         = new WayPoints();
            int       list_count = list.Count;

            if (list_count > 0)
            {
                double last_lat = list.Last.Value.lat;
                double last_lon = list.Last.Value.lon;
                double dist1    = GPS.GPS_Distance(last_lat, last_lon, lat1, lon1, Form1.Globals.gps_radius);
                double dist2    = GPS.GPS_Distance(last_lat, last_lon, lat2, lon2, Form1.Globals.gps_radius);
                if (dist2 < dist1)
                {
                    double temp = lat1;
                    lat1 = lat2;
                    lat2 = temp;
                    temp = lon1;
                    lon1 = lon2;
                    lon2 = temp;
                }
            }

            int[,] no_actions = new int[, ] {
                { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }, { -1, 0 }
            };

            if (video)
            {
                // If in Video mode only output start and end points

                wp.Add_Waypoint_List(list, lat1, lon1, alt, heading, curvesize, rotdir, gimblemode, gimblepitch, no_actions);
                wp.Add_Waypoint_List(list, lat2, lon2, alt, heading, curvesize, rotdir, gimblemode, gimblepitch, no_actions);
            }
            else
            {
                // Photo Mode - Break into seperate segments

                // Output first WP

                wp.Add_Waypoint_List(list, lat1, lon1, alt, heading, curvesize, rotdir, gimblemode, gimblepitch, actions);

                camera_increment = camera_increment * (1 - overlap / 100.0);
                double distance = GPS.GPS_Distance(lat1, lon1, lat2, lon2, Form1.Globals.gps_radius);
                double bearing = GPS.GPS_Bearing(lat1, lon1, lat2, lon2);
                double new_lat, new_lon, last_lat, last_lon;
                int    num_wp = (int)(distance / camera_increment);
                if (num_wp != 0)
                {
                    if (num_wp == 1)
                    {
                        new_lat = GPS.GPS_Lat_BearDist(lat1, lon1, bearing, distance / 2, Form1.Globals.gps_radius);
                        new_lon = GPS.GPS_Lon_BearDist(lat1, lon1, new_lat, bearing, distance / 2, Form1.Globals.gps_radius);
                        wp.Add_Waypoint_List(list, new_lat, new_lon, alt, heading, curvesize, rotdir, gimblemode, gimblepitch, actions);
                    }
                    else
                    {
                        double width     = (num_wp - 1) * camera_increment;
                        double dist_diff = distance - width;
                        new_lat = GPS.GPS_Lat_BearDist(lat1, lon1, bearing, dist_diff / 2, Form1.Globals.gps_radius);
                        new_lon = GPS.GPS_Lon_BearDist(lat1, lon1, new_lat, bearing, dist_diff / 2, Form1.Globals.gps_radius);
                        wp.Add_Waypoint_List(list, new_lat, new_lon, alt, heading, curvesize, rotdir, gimblemode, gimblepitch, actions);
                        last_lat = new_lat;
                        last_lon = new_lon;
                        for (int i = 0; i < num_wp - 1; i++)
                        {
                            new_lat = GPS.GPS_Lat_BearDist(last_lat, last_lon, bearing, camera_increment, Form1.Globals.gps_radius);
                            new_lon = GPS.GPS_Lon_BearDist(last_lat, last_lon, new_lat, bearing, camera_increment, Form1.Globals.gps_radius);
                            wp.Add_Waypoint_List(list, new_lat, new_lon, alt, heading, curvesize, rotdir, gimblemode, gimblepitch, actions);
                            last_lat = new_lat;
                            last_lon = new_lon;
                        }
                    }
                }

                // Output last WP

                wp.Add_Waypoint_List(list, lat2, lon2, alt, heading, curvesize, rotdir, gimblemode, gimblepitch, actions);
            }
        }
Beispiel #42
0
 void Start()
 {
     _gps = gameObject.AddComponent <GPS>();
 }