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);
}
// Set new target and reset "timer"
public void SetTargetWaypoint(Waypoint wp)
{
TargetWaypoint = wp;
RemainingSecondsUntilTarget = SimConnectInterface.SECONDS_BETWEEN_POSITIONREPORTS_FROM_NETWORK;
}
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;
}
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);
}
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
));
}
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;
}
/*
// 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;
}
// 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);
}
// 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;
}
//*********************************************************************************************
// 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;
}
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);
}