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);
}
}
//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();
}
/// <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);
}
/// <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);
}
/// <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
}
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
}
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);
}
public void GetLocalGMTOffset_AtOffset()
{
var dateTime = DateTime.UtcNow;
GPS.GetLocalGMTOffset(dateTime).Should().Be(TimeZoneInfo.Local.GetUtcOffset(dateTime));
}
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;
}
public GPSDriveState(GPS gate, double proxy)
{
this.gate = gate;
this.GATE_PROXIMITY = proxy;
}
public GPSDriveState(GPS gate)
{
this.gate = gate;
}
/// <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);
}
/// <summary>
/// 发送行到串口
/// </summary>
/// <param name="Line"></param>
private void SendSerialLineToUIThread(string Line)
{
GPS.ProcessNmeaData(Line);
}
private void Awake()
{
Instance = this;
DontDestroyOnLoad(gameObject);
}
/// <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));
}
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);
}
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));
}
public void GetLocalGMTOffset()
{
GPS.GetLocalGMTOffset().Should().Be(TimeZoneInfo.Local.GetUtcOffset(DateTime.UtcNow));
}
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);
}
}
// 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);
}
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);
//
}
public void Finish()
{
Network.Finish();
GPS.Finish();
}
void OnDestroy()
{
Gps = null;
}
// 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();
}
// 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);
}
public virtual int sceUsbGpsOpen()
{
GPS.initialize();
return(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*");
}
void Start()
{
gps = FindObjectOfType <GPS>();
Debug.Log("BuildMapLocation");
LocationProvider.OnLocationUpdated += LocationProvider_OnLocationUpdated;
}
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*");
}
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);
}
}
void Start()
{
_gps = gameObject.AddComponent <GPS>();
}
