Ejemplo n.º 1
0
        private void MoveAI(AIPlane AIAircraft, Waypoint newWp)
        {
            simconnect.TransmitClientEvent(AIAircraft.SimConnectObjectId,
                                SIMCONNECT_EVENTS.EVENTID_AXIS_SLEW_AHEAD_SET,
                                0,
                                GROUP_PRIORITIES.SIMCONNECT_GROUP_PRIORITY_HIGHEST,
                                SIMCONNECT_EVENT_FLAG.GROUPID_IS_PRIORITY);

            var aimove = new AIMoveStruct()
            {
                latitude = newWp.Latitude,
                longitude = newWp.Longitude,
                truealtitude = newWp.Altitude,
                pitch = newWp.Pitch,
                bank = newWp.Bank,
                heading = newWp.Heading,

            };

            Logger.Trace(AIAircraft.Callsign +  " doing WARP SPEED");
            simconnect.SetDataOnSimObject(DEFINITIONS.AIMoveStruct, AIAircraft.SimConnectObjectId, SIMCONNECT_DATA_SET_FLAG.DEFAULT, aimove);
        }
Ejemplo n.º 2
0
 // Set new target and reset "timer"
 public void SetTargetWaypoint(Waypoint wp)
 {
     TargetWaypoint = wp;
     RemainingSecondsUntilTarget = SimConnectInterface.SECONDS_BETWEEN_POSITIONREPORTS_FROM_NETWORK;
 }
Ejemplo n.º 3
0
 private static Waypoint CreateWaypointFromAIPositionReportStruct(ref AIPositionReportStruct aiposreport, DateTime timestamp)
 {
     var currentWp = new Waypoint()
     {
         Altitude = aiposreport.truealtitude,
         Longitude = aiposreport.longitude,
         Latitude = aiposreport.latitude,
         GroundSpeed = SimMath.knotsToMetersPerSecond(aiposreport.groundspeed),
         Pitch = aiposreport.pitch,
         Bank = aiposreport.bank,
         Heading = aiposreport.heading,
         Timestamp = timestamp,
         OnGround = aiposreport.simOnGround != 0
     };
     return currentWp;
 }
Ejemplo n.º 4
0
        private void CalculateSlewAI(AIPlane AIAircraft, Waypoint currentWp, Waypoint newWp)
        {
            var period = AIAircraft.RemainingSecondsUntilTarget;
            AIAircraft.RemainingSecondsUntilTarget --;
            bool onGround = currentWp.OnGround && Math.Abs(currentWp.Altitude - newWp.Altitude) < 10;

            AIAircraft.SlowingDownForParkingMode = currentWp.GroundSpeed > 0 && newWp.GroundSpeed == 0 && onGround;
            double periodCorrectionForParking = period > 1 && AIAircraft.SlowingDownForParkingMode ? -1.0 : 0;

            Logger.Debug(string.Format("{0} SlowingDownForParkingMode: {1}, periodCorrectionForParking: {2}", AIAircraft.Callsign, periodCorrectionForParking, AIAircraft.SlowingDownForParkingMode));

            // now calculate steering deltas based on predict point

            double bearing_to_wp = SimMath.bearing(currentWp.Latitude, currentWp.Longitude,
                                           newWp.Latitude, newWp.Longitude);

            uint heading_rate = 0;

            uint ahead_rate = SimMath.slew_ahead_rate(currentWp.Latitude, currentWp.Longitude,
                                          newWp.Latitude, newWp.Longitude,
                                          period + periodCorrectionForParking);
            //uint ahead_rate = SimMath.slew_ahead_rate_experimental(AIAircraft.Callsign, currentWp, newWp, period);

            bool doFinalHardTurnBeforeParking = AIAircraft.SlowingDownForParkingMode && AIAircraft.RemainingSecondsUntilTarget == 1;
            heading_rate = SimMath.slew_turn_rate(bearing_to_wp, currentWp.Heading, newWp.Heading, currentWp.GroundSpeed, doFinalHardTurnBeforeParking);

            if (SimMath.AIAircraftIsPushingBack(currentWp, newWp, heading_rate))
            {
                Logger.Debug(string.Format("{0} assuming pushback, old bearing: {1}", AIAircraft.Callsign, bearing_to_wp));

                // Hard turn with low real GS, probably a pushback
                bearing_to_wp = SimMath.bearing(newWp.Latitude, newWp.Longitude,
                                           currentWp.Latitude, currentWp.Longitude);
                heading_rate = SimMath.slew_turn_rate(bearing_to_wp, currentWp.Heading, newWp.Heading, currentWp.GroundSpeed, doFinalHardTurnBeforeParking);
                ahead_rate = ((uint)-(int)ahead_rate);

            }

            double speed = SimMath.distance(currentWp.Latitude, currentWp.Longitude, newWp.Latitude, newWp.Longitude) / (period + periodCorrectionForParking);
            var desiredHdg = SimMath.desired_heading(bearing_to_wp, newWp.Heading, doFinalHardTurnBeforeParking ? 0.9 : 0.1);
            Logger.Debug(string.Format("Period: {0}  speed: {1}   ahead rt: {2}  bearing: {3} ({5})  desired heading: {4} ({6})", period, speed, SimMath.slew_ahead_to_rate(speed), bearing_to_wp, desiredHdg, RadianToDegree(bearing_to_wp), RadianToDegree(desiredHdg)));

             /*
            uint ahead_rate = SimMath.slew_ahead_rate_experimental(AIAircraft.Callsign, currentWp, newWp, period);
            if (Math.Abs((int)ahead_rate) > 16384)
            {
            }*/

            // On ground, and not on the way up or down? ignore pitch + bank

            uint bank_rate = onGround ? 0 : SimMath.slew_rotation_to_rate((newWp.Bank - currentWp.Bank) / period, currentWp.GroundSpeed);

            uint pitch_rate = onGround ? 0 : SimMath.slew_rotation_to_rate((newWp.Pitch - currentWp.Pitch) / period, currentWp.GroundSpeed);

            uint alt_rate = SimMath.slew_alt_to_rate(SimMath.ft2m((currentWp.Altitude - newWp.Altitude)) / period);

            Logger.Debug(string.Format("CURRENT WP: La {0}, Lo {1}, Alt {2}, Pi {3}, Ba {4}, Hdg {5} ({7}), GS: {6}",
                               currentWp.Latitude.ToString("0000.00000000"),
                               currentWp.Longitude.ToString("0000.00000000"),
                               currentWp.Altitude.ToString("00000"),
                               currentWp.Pitch.ToString("0000.00000000"),
                               currentWp.Bank.ToString("0000.00000000"),
                               currentWp.Heading.ToString("0000.00000000"),
                               currentWp.GroundSpeed.ToString("000.0000"),
                               RadianToDegree(currentWp.Heading)
                               ));

            Logger.Debug(string.Format("NEW WP    : La {0}, Lo {1}, Alt {2}, Pi {3}, Ba {4}, Hdg {5} ({11}), GS {11}, Ahead rt {6}, Alt rt {7}, Pi rt {8}, Ba rt {9}, Hdg rt {10}",
                                newWp.Latitude.ToString("0000.00000000"),
                                newWp.Longitude.ToString("0000.00000000"),
                                newWp.Altitude.ToString("00000"),
                                newWp.Pitch.ToString("0000.00000000"),
                                newWp.Bank.ToString("0000.00000000"),
                                newWp.Heading.ToString("0000.00000000"),
                                (int)ahead_rate,
                                (int)alt_rate,
                                (int)pitch_rate,
                                (int)bank_rate,
                                (int)heading_rate,
                                newWp.GroundSpeed.ToString("000.0000"),
                                RadianToDegree(newWp.Heading)
                                ));

            // send the actual slew adjustments

            if (speed <= MAX_SPEED_BEFORE_WARP)
            {
                TransmitSlewEvents(AIAircraft, heading_rate, ahead_rate, bank_rate, pitch_rate, alt_rate);
                if(onGround)
                {
                    AIAltAboveGroundStruct s = new AIAltAboveGroundStruct() { altAboveGround = 0 };
                    // simconnect.SetDataOnSimObject(DEFINITIONS.AISetAltAboveGroundStruct, AIAircraft.SimConnectObjectId, SIMCONNECT_DATA_SET_FLAG.DEFAULT, s);
                }
            }
            else
                MoveAI(AIAircraft, newWp);

            // send gear up/down as necessary
            // TODO: ai_gear(ai_index, i, pos);
        }
Ejemplo n.º 5
0
        static void Main(string[] args)
        {
            var currentWp = new Waypoint()
            {
                Latitude = 47.43426034,
                Longitude = -122.30855984,
                Bank = 0,
                Altitude = 437.408073162284,
                Pitch = -0.00349065847694874,
                Heading = 0,
                GroundSpeed = 0
            };

            var newWp = new Waypoint()
            {
                Latitude = 47.43426000,
                Longitude = -122.30856000,
                Bank = 0,
                Altitude = 436,
                Pitch = 0.0174532925199433,
                Heading = 6.19591884457987,
                GroundSpeed = 8
            };

            double distance = SimMath.distance(currentWp.Latitude, currentWp.Longitude, newWp.Latitude, newWp.Longitude);

            double bearing_to_wp = SimMath.bearing(currentWp.Latitude, currentWp.Longitude,
                                               newWp.Latitude, newWp.Longitude);

            uint heading_rate = SimMath.slew_turn_rate(bearing_to_wp, currentWp.Heading, newWp.Heading, currentWp.GroundSpeed, false);

            var period = (newWp.Timestamp - currentWp.Timestamp).TotalSeconds;

            uint ahead_rate = SimMath.slew_ahead_rate(currentWp.Latitude, currentWp.Longitude,
                                           newWp.Latitude, newWp.Longitude,
                                           period);

            uint bank_rate = SimMath.slew_rotation_to_rate((newWp.Bank - currentWp.Bank) / period, currentWp.GroundSpeed);

            uint pitch_rate = SimMath.slew_rotation_to_rate((newWp.Pitch - currentWp.Pitch) / period, currentWp.GroundSpeed);

            uint alt_rate = SimMath.slew_alt_to_rate((currentWp.Altitude - newWp.Altitude) / period);

            //debug - print lat longs for excel analysis
            // time,lat,lon,alt,pitch,bank,heading,ahead rate, alt rate, pitch rate, bank rate, heading rate

            Console.WriteLine(string.Format("CURRENT WP: La {0}, Lo {1}, Alt {2}, Pi {3}, Ba {4}, Hdg {5}, GS: {6}",
                               currentWp.Latitude.ToString("####0.00000000"),
                               currentWp.Longitude.ToString("####0.00000000"),
                               currentWp.Altitude,
                               currentWp.Pitch,
                               currentWp.Bank,
                               currentWp.Heading,
                               currentWp.GroundSpeed
                               ));

            Console.WriteLine(string.Format("NEW WP: La {0}, Lo {1}, Alt {2}, Pi {3}, Ba {4}, Hdg {5}, GS {11}, Ahead rt {6}, Alt rt {7}, Pi rt {8}, Ba rt {9}, Hdg rt {10}",
                                newWp.Latitude.ToString("####0.00000000"),
                                newWp.Longitude.ToString("####0.00000000"),
                                newWp.Altitude,
                                newWp.Pitch,
                                newWp.Bank,
                                newWp.Heading,
                                ahead_rate,
                                alt_rate,
                                pitch_rate,
                                bank_rate,
                                heading_rate,
                                newWp.GroundSpeed
                                ));
        }
Ejemplo n.º 6
0
 public static Waypoint CreateWaypointFromTrafficPositionReportMsg(TrafficPositionReportMessage msg)
 {
     var currentWp = new Waypoint()
     {
         Altitude = msg.TrueAltitude,
         Bank = SimMath.deg2rad(-msg.BankAngle),
         Heading = SimMath.deg2rad(msg.Heading),
         Latitude = msg.Latitude,
         Longitude = msg.Longitude,
         Pitch = SimMath.deg2rad(-msg.Pitch),
         GroundSpeed = SimMath.knotsToMetersPerSecond(msg.Groundspeed),
         Timestamp = msg.ReceiveTime
     };
     return currentWp;
 }
Ejemplo n.º 7
0
        /*
        // calculate appropriate pitch/bank/heading/speed values for replaypoint[i]
        public static void ai_update_pbhs(Waypoint[] p, int i)
        {
            // pitch & speed
            if (i == 0) { p[i].Pitch = 0; p[i].Speed = 0; }
            else
            {
                double dist = distance(p[i - 1].Latitude,
                                              p[i - 1].Longitude,
                                              p[i].Latitude,
                                              p[i].Longitude);
                p[i].Pitch = desired_pitch(p[i].Altitude - p[i - 1].Altitude,
                                                dist,
                                                p[i].zulu_time - p[i - 1].zulu_time);
                p[i].Speed = dist / (p[i].zulu_time - p[i - 1].zulu_time);
            }

            // heading
            if (i == 1) p[0].Heading = bearing(p[0].Latitude, p[0].Longitude,
                                             p[1].Latitude, p[1].Longitude);
            else if (i > 1)
            {
                p[i - 1].Heading = target_heading(p[i - 2].Latitude, p[i - 2].Longitude,
                                             p[i - 1].Latitude, p[i - 1].Longitude,
                                             p[i].Latitude, p[i].Longitude);
                //if (debug) printf("updating p[%2d-%d] (%2.5f,%2.5f) p[%2d-%d].heading=%.3f\n",
                //	i, p[i].zulu_time,p[i].latitude, p[i].longitude,i-1,p[i-1].zulu_time,p[i-1].heading);
            }

            // bank
            if (i < 2) p[i].Bank = 0;
            else
            {
                double this_bearing = bearing(p[i - 1].Latitude,
                                              p[i - 1].Longitude,
                                              p[i].Latitude,
                                              p[i].Longitude);
                double prev_bearing = bearing(p[i - 2].Latitude,
                                              p[i - 2].Longitude,
                                              p[i - 1].Latitude,
                                              p[i - 1].Longitude);
                double bearing_delta = (this_bearing + 2 * Math.PI - prev_bearing) % (2 * Math.PI);
                if (bearing_delta > Math.PI) bearing_delta = bearing_delta - 2 * Math.PI;
                double turn_radians_per_second = bearing_delta / (p[i].zulu_time - p[i - 1].zulu_time);
                p[i].Bank = -turn_radians_per_second * 4;
                p[i].Bank = Math.Min(p[i].Bank, 1.5);
                p[i].Bank = Math.Max(p[i].Bank, -1.5);
            }
        }
        */
        public static bool AIAircraftIsParked(Waypoint currentWp, Waypoint newWp)
        {
            return distance(currentWp.Latitude, currentWp.Longitude, newWp.Latitude, newWp.Longitude) < 1;
        }
Ejemplo n.º 8
0
        // what rate to set ahead slew should object move to arrive at correct time
        public static uint slew_ahead_rate_experimental(string callsign, Waypoint currentWp, Waypoint newWp, uint time_to_go)
        {
            var distance = SimMath.distance(currentWp.Latitude, currentWp.Longitude, newWp.Latitude, newWp.Longitude);
            double speed;
            if (newWp.GroundSpeed == 0)
            {

                avgSpeed = SimMath.averageSpeed(currentWp.Latitude, currentWp.Longitude, newWp.Latitude, newWp.Longitude, time_to_go);

                if ((currentWp.GroundSpeed <= avgSpeed && avgSpeed <= newWp.GroundSpeed)
                   || (newWp.GroundSpeed <= avgSpeed && avgSpeed <= currentWp.GroundSpeed))
                {
                    correctionFactor = (distance - time_to_go * currentWp.GroundSpeed) / (double)sum(time_to_go);
                }
                else
                    correctionFactor = 0;
            }

            // Linear approximation to make speed corrections more smooth

            if (time_to_go > 1)
            {
                    speed = currentWp.GroundSpeed + correctionFactor;
                    Logger.Debug(string.Format("{0} averaging speed (case 1): avgspeed: {1} corrected speed: {2}  distance: {3}", callsign, avgSpeed, speed, distance));
            }
            else
            {
                speed = SimMath.averageSpeed(currentWp.Latitude, currentWp.Longitude, newWp.Latitude, newWp.Longitude, 1);
            }
                /*else if ((currentWp.GroundSpeed <= avgSpeed && newWp.GroundSpeed <= avgSpeed)
                    || (newWp.GroundSpeed <= avgSpeed && currentWp.GroundSpeed <= avgSpeed))
                {
                    // Average speed either below or above start and end speed, we have to calculate a linear max/min speed to average the speed for the entire period
                    // Split the period in two, and make an averaged speed increase/decrease to vm first half, and then the opposite in/decrease last half.
                    double vm = (4.0 * distance / (double)time_to_go - currentWp.GroundSpeed - newWp.GroundSpeed) / 2.0;
                    double firsthalf_distance = currentWp.GroundSpeed * time_to_go / 2.0 + (vm - currentWp.GroundSpeed) * time_to_go / 2.0 / 2.0;
                    double correctionFactor = (firsthalf_distance - time_to_go / 2.0 * currentWp.GroundSpeed) / (double)sum(time_to_go / 2.0);
                    speed = currentWp.GroundSpeed + correctionFactor;
                    Logger.Debug(string.Format("{0} averaging speed (case 2): avgspeed: {1} corrected speed: {2}", callsign, avgSpeed, speed));
                }
                else
                {
                    Logger.Debug(string.Format("{0} no averaging speed: avgspeed: {1}  distance: {2}", callsign, avgSpeed, distance));
                }*/

            return slew_ahead_to_rate(speed);
        }
Ejemplo n.º 9
0
        // interp returns a RelayPoint step_time after point p1
        // with time/lat/lon/alt only
        public static Waypoint interp(Waypoint p0, Waypoint p1, Waypoint p2, Waypoint p3, Int32 step_time)
        {
            double correction_coefficient = 0.17;
            Int32 time_delta = p2.zulu_time - p1.zulu_time;

            //bearings in radians
            double bearing0 = bearing(p0.Latitude, p0.Longitude, p1.Latitude, p1.Longitude);
            double bearing1 = bearing(p1.Latitude, p1.Longitude, p2.Latitude, p2.Longitude);
            double bearing2 = bearing(p2.Latitude, p2.Longitude, p3.Latitude, p3.Longitude);

            double bearing_delta1 = heading_delta(bearing0, bearing1);
            double bearing_delta2 = heading_delta(bearing1, bearing2);

            double total_turn1 = bearing_delta1 + bearing_delta2;

            double heading_correction1 = correction_coefficient * total_turn1;

            double distance1 = distance(p1.Latitude, p1.Longitude, p2.Latitude, p2.Longitude);

            double speed1 = distance1 / time_delta;

            double new_heading1 = bearing1 + heading_correction1;
            double new_heading_delta1 = heading_delta(bearing1, new_heading1);
            double speed_correction1 = (1 + (1 - 2 / Math.PI) * Math.Abs(new_heading_delta1));
            double distance_to_interp1 = speed1 * step_time * speed_correction1;

            Waypoint r = distance_and_bearing(p1, distance_to_interp1, new_heading1);
            r.zulu_time = p1.zulu_time + step_time;
            r.Altitude = p1.Altitude + ((double)step_time / (double)time_delta) * (p2.Altitude - p1.Altitude);
            return r;
        }
Ejemplo n.º 10
0
        //*********************************************************************************************
        // interp code - inject more ReplayPoints where IGC file timestep > 5  seconds
        //*********************************************************************************************
        // distance_and_bearing(...) returns a new lat/long a distance and bearing from lat1,lon1.
        // lat, longs in degrees, rbearng in radians, distance in meters
        public static Waypoint distance_and_bearing(Waypoint p, double distance, double rbearing)
        {
            double rlat1, rlong1, rdistance, rlat2, rlong2;
            Waypoint r = new Waypoint();
            rlat1 = deg2rad(p.Latitude);
            rlong1 = deg2rad(p.Longitude);
            rdistance = m2rad(distance);
            rlat2 = Math.Asin(Math.Sin(rlat1) * Math.Cos(rdistance) + Math.Cos(rlat1) * Math.Sin(rdistance) * Math.Cos(rbearing));
            if (Math.Cos(rlat2) == 0)
            {
                rlong2 = rlong1;      // endpoint a pole
            }
            else
            {
                rlong2 = ((rlong1 + Math.Asin(Math.Sin(rbearing) * Math.Sin(rdistance) / Math.Cos(rlat2)) + Math.PI) % (2 * Math.PI)) - Math.PI;
            }
            r.Latitude = rad2deg(rlat2);
            r.Longitude = rad2deg(rlong2);

            return r;
        }
Ejemplo n.º 11
0
 public static bool AIAircraftIsPushingBack(Waypoint currentWp, Waypoint newWp, uint heading_rate)
 {
     return false;
     // return Math.Abs((int)heading_rate) > 2000 && newWp.GroundSpeed < SimMath.knotsToMetersPerSecond(10) && Math.Abs(SimMath.heading_delta(newWp.Heading, currentWp.Heading)) < DegreeToRadian(30);
 }