/// <summary>
/// calculate vmc and vmg values if possible with current state
/// </summary>
/// <param name="state">current race state</param>
public void Calculate(State state)
{
if(state.Location!=null && state.TargetMark!=null && state.TargetMark.Location!=null && state.Course is CourseByMarks)
{
double meters = CoordinatePoint.HaversineDistance(state.Location, state.TargetMark.Location);
if (meters < _distanceCutoff)//if the reported distance is more than this threshold, it's probably garbage data
{
state.StateValues[StateValue.DistanceToTargetMarkInYards] = meters * MetersToYards;
}
var calculation = new MarkCalculation();
calculation.Location = state.Location;
calculation.Time = state.BestTime;
_previousCalculations.Add(calculation);
while(_previousCalculations.Count>_previousCalculationCount)
{
_previousCalculations.RemoveAt(0);
}
if (_previousCalculations.Count > 1)
{
var previous = _previousCalculations[_previousCalculations.Count - 2];
var duration = calculation.Time - previous.Time;
//calculate vmc
var previousDistanceMeters = CoordinatePoint.HaversineDistance(previous.Location,
state.TargetMark.Location);
var distanceDelta = previousDistanceMeters - meters;
var vmcMetersPerSecond = distanceDelta/duration.TotalSeconds;
var vmcKnots = MetersPerSecondToKnots * vmcMetersPerSecond;
calculation.VelocityMadeGoodOnCourse = vmcKnots;
state.StateValues[StateValue.VelocityMadeGoodOnCourse] = vmcKnots;//_previousCalculations.Average(x => x.VelocityMadeGoodOnCourse);
state.StateValues[StateValue.VelocityMadeGoodOnCoursePercent] = vmcKnots / state.StateValues[StateValue.SpeedInKnots] * 100;
//TODO: calculate vmg
if (state.PreviousMark != null && state.StateValues.ContainsKey(StateValue.SpeedInKnots))
{
calculation.VelocityMadeGood = VelocityMadeGood(state.TargetMark, state.PreviousMark,
calculation.Location, previous.Location, state.StateValues[StateValue.SpeedInKnots]);
state.StateValues[StateValue.VelocityMadeGoodPercent] = calculation.VelocityMadeGood.Value / state.StateValues[StateValue.SpeedInKnots] * 100;
var relativeAngle = RelativeAngleToCourse(state.TargetMark, state.PreviousMark, calculation.Location, previous.Location);
state.StateValues[StateValue.CourseOverGroundRelativeToCourse] = AngleUtilities.RadiansToDegrees(relativeAngle);
}
}
}
else if(state.Course is CourseByAngle && state.StateValues.ContainsKey(StateValue.CourseOverGroundDirection) && state.StateValues.ContainsKey(StateValue.SpeedInKnots))
{
state.StateValues[StateValue.VelocityMadeGood] = VelocityMadeGood((state.Course as CourseByAngle).CourseAngle, state.StateValues[StateValue.CourseOverGroundDirection], state.StateValues[StateValue.SpeedInKnots]);
state.StateValues[StateValue.VelocityMadeGoodPercent] = state.StateValues[StateValue.VelocityMadeGood] / state.StateValues[StateValue.SpeedInKnots] * 100;
var relativeAngle = AngleUtilities.AngleDifference(AngleUtilities.DegreestoRadians((state.Course as CourseByAngle).CourseAngle), AngleUtilities.DegreestoRadians(state.StateValues[StateValue.CourseOverGroundDirection]));
state.StateValues[StateValue.CourseOverGroundRelativeToCourse] = AngleUtilities.RadiansToDegrees(relativeAngle);
}
}
/// <summary>
/// intiialize the plugins
/// </summary>
public void Initialize()
{
_commands = new Queue<Action<ISystemController, IRaceController>>();
_configuration = new PluginConfiguration();
_configuration.Plugins = _allPlugins.Select(x=>x).ToList();
_state = _raceController.State;
var failed = new List<IPlugin>();
foreach (var plugin in _configuration.Plugins)
{
try
{
_logger.Info("Initializing Plugin " + plugin.GetType().Name);
InitializePlugin(plugin);
}
catch (Exception ex)
{
_logger.Fatal("Exception initializing plugin "+plugin.GetType().Name, ex);
failed.Add(plugin);
}
}
foreach (var plugin in failed)
{
EvictPlugin(_configuration,plugin,false);
}
//remove any plugins that failed to initialize
//_configuration.Plugins = _configuration.Plugins.Where(x => x.Initialized).ToList();
//foreach (var plugin in _configuration.Plugins)
//{
// _logger.Info(plugin.GetType().Name + " Initialized OK");
//}
}
/// <inheritdoc />
public void Update(State state)
{
state.StateValues [StateValue.ApparentWindAngle] = 45;
state.StateValues [StateValue.ApparentWindSpeedKnots] =5;
state.StateValues [StateValue.MastHeel] = 20;
state.StateValues [StateValue.MastPitch] =2;
}
/// <inheritdoc />
public void Calculate(State state)
{
if (state.StateValues.ContainsKey(StateValue.CourseOverGroundDirection))
{
var cogRads = AngleUtilities.DegreestoRadians(state.StateValues[StateValue.CourseOverGroundDirection]);
//make sure whe're not in an "exclusion" aka a few seconds before/after a known tack
if (!_lastTackAt.HasValue || (_lastTackAt.Value + _dataExclusionTime < state.BestTime)) {
if (!_currentTackStartCourseOverGroundRadians.HasValue) {
_currentTackStartCourseOverGroundRadians = cogRads;
}
_history.Add (new CourseHistory () { Time = state.BestTime, CourseOverGroundRadians = cogRads });
//make sure we have enough data to do the calculation accurately
if (_history.Count > 1) {
if (_history.Max (x => x.Time) - _history.Min (x => x.Time) > _tackThresholdTime) {
CheckForTack (state);
}
}
}
//calculate the delta on the current tack
if (state.StateValues.ContainsKey(StateValue.CourseOverGroundDirection) && _currentTackStartCourseOverGroundRadians.HasValue)
{
var delta = AngleUtilities.AngleDifference(cogRads, _currentTackStartCourseOverGroundRadians.Value);
state.StateValues[StateValue.CurrentTackCourseOverGroundDelta] = AngleUtilities.RadiansToDegrees(delta);
}
}
PurgeOldHistory();
}
public void Calculate (State state)
{
//inputs
// true wind or absolute wind
// course over ground
// speed
// current mark
// lat/lon
//outputs
// favored tack
// distance to mark
// distance to tack
// eta to mark
// eta to tack
//if there's wind data
//find the angle to the wind
//calculate vmc/vmg
//calculate vmc/vmg for the opposite tack based on reversing the angle to the wind
//else
//find the vmc/vmg
//find the previous tack, vmc/vmg for that tack
//add/subtract the current tack delta
}
/// <inheritdoc />
public void Update(State state)
{
if (_lastTime != null) {
var difference = state.BestTime - _lastTime.Value;
float dtime = (float)difference.TotalMilliseconds / 1000000.0f;
_imu.Update (dtime);
var accel = _imu.GetAccel ();
var gyro = _imu.GetGyro ();
//these probably need to be normalized to some known scale
//state.Accel = new Vector3(accel.x, accel.y, accel.z);
//state.Gyro = new Vector3(gyro.x,gyro.y,gyro.z);
//var rpy = _imu.GetRollYawPitch ();
_logger.Debug ("MPU-6050: Acceleration(" + string.Format ("{0:0.00}", accel.x) + "," + string.Format ("{0:0.00}", accel.y) + "," + string.Format ("{0:0.00}", accel.z) + ") Gyro(" + string.Format ("{0:0.00}", gyro.x) + "," + string.Format ("{0:0.00}", gyro.y) + "," + string.Format ("{0:0.00}", gyro.z) + ")");
//_logger.Debug ("MPU-6050: Roll/Pitch/Yaw(" + string.Format ("{0:0.00}", rpy.x*360.0) + "," + string.Format ("{0:0.00}", gyro.y*360.0) + "," + string.Format ("{0:0.00}", gyro.z*360.0) + ")");
//_logger.Info ("Heel:" + (accel.x * 360.0));
state.StateValues[StateValue.Heel] = accel.x * (360.0/4.0);//((double)accel.y).ToDegrees();
state.StateValues[StateValue.Pitch] = accel.y * (360.0 / 4.0);//((double)accel.x).ToDegrees();
//if (framecounter++ == 100 && imu != null)
//_imu.Calibrate ();
}
_lastTime = state.BestTime;
}
/// <inheritdoc />
public void Calculate(State state)
{
if (state.StateValues.ContainsKey(StateValue.Heel)
&& state.StateValues.ContainsKey(StateValue.MastHeel))
{
state.StateValues [StateValue.MastBendBeam] = state.StateValues [StateValue.MastHeel] - state.StateValues [StateValue.Heel];
}
if (state.StateValues.ContainsKey(StateValue.Pitch)
&& state.StateValues.ContainsKey(StateValue.MastPitch))
{
state.StateValues [StateValue.MastBendCenterline] = state.StateValues [StateValue.MastPitch] - state.StateValues [StateValue.Pitch];
}
}
public void Calculate(State state)
{
if (state.StateValues.ContainsKey (StateValue.ApparentWindAngle)
&& state.StateValues.ContainsKey (StateValue.ApparentWindSpeedKnots)
&& state.StateValues.ContainsKey (StateValue.SpeedInKnots)
&& (state.StateValues.ContainsKey (StateValue.CourseOverGroundDirection)
|| state.StateValues.ContainsKey (StateValue.MagneticHeading)
|| state.StateValues.ContainsKey (StateValue.MagneticHeadingWithVariation))) {
double boatHeading = 0;
if (state.StateValues.ContainsKey (StateValue.CourseOverGroundDirection)) {
boatHeading = state.StateValues [StateValue.CourseOverGroundDirection];
} else if (state.StateValues.ContainsKey (StateValue.MagneticHeadingWithVariation)) {
boatHeading = state.StateValues [StateValue.MagneticHeadingWithVariation];
} else if (state.StateValues.ContainsKey (StateValue.MagneticHeading)) {
boatHeading = state.StateValues [StateValue.MagneticHeading];
}
double [] trueWindSpeed = new double[1];
double [] trueWindDirection = new double[1];
TrueWind (1
,new double[]{ boatHeading}
,new double[]{ state.StateValues [StateValue.SpeedInKnots]}
,new double[]{ state.StateValues [StateValue.ApparentWindAngle]}
,0.0
//TODO: change this to mag heading once it's reliable
,new double[]{ boatHeading}
,new double[1]
,new double[]{ state.StateValues [StateValue.ApparentWindSpeedKnots]}
,new double[]{ double.MaxValue,double.MaxValue,double.MaxValue,double.MaxValue,double.MaxValue}
,trueWindDirection
,trueWindSpeed);
if (trueWindDirection[0] != double.MaxValue)
{
state.StateValues [StateValue.TrueWindAngle] = AngleUtilities.NormalizeAngleDegrees (trueWindDirection [0] - boatHeading);
state.StateValues [StateValue.TrueWindDirection] = trueWindDirection [0];
}
if (trueWindSpeed[0] != double.MaxValue)
{
state.StateValues [StateValue.TrueWindSpeedKnots] = trueWindSpeed [0];
}
}
}
/// <inheritdoc />
public void Calculate(State state)
{
if (state.Location!=null
&& state.Location.Latitude!=null
&& state.Location.Longitude!=null
&& state.StateValues.ContainsKey(StateValue.AltitudeInMeters))
{
double now = TSAGeoMag.decimalYear(state.BestTime);
state.StateValues[StateValue.MagneticDeviation] = _tsaGeoMag.getDeclination(state.Location.Latitude.Value, state.Location.Longitude.Value, now,state.StateValues[StateValue.AltitudeInMeters]/1000.0);
if(state.StateValues.ContainsKey(StateValue.MagneticHeading))
{
state.StateValues[StateValue.MagneticHeadingWithVariation] = state.StateValues[StateValue.MagneticHeading] + state.StateValues[StateValue.MagneticDeviation];
}
_logger.Debug("Calculated Magnetic Deviation as " + state.StateValues[StateValue.MagneticDeviation] + " for " + state.Location.Latitude.Value + "," + state.Location.Longitude.Value + " altitude " + state.StateValues[StateValue.AltitudeInMeters]);
}
}
/// <inheritdoc />
public void Update(State state)
{
short x=0, y=0, z=0;
_hmc5883.GetHeading(ref x,ref y,ref z);
//state.Magneto = new Vector3(x, y, z);
double heading = Math.Atan2(y, x);
if (heading < 0)
{
heading += 2.0 * Math.PI;
}
//convert to degrees
heading = heading * (180.0 / Math.PI);
heading = 360-heading;
_logger.Debug("HMC5883L Heading(" + x + "," + y + "," + z + ") (" + heading + ")");
state.StateValues[StateValue.MagneticHeading] = heading;
}
/// <summary>
/// compare the new state to the last values and determine if a tack has occured
/// if so, update the state with the new tack
/// </summary>
/// <param name="state"></param>
private void CheckForTack(State state)
{
var latest = _history.Last();
var deltas = _history.Where(x=>x.Time > latest.Time - _tackThresholdTime).Select(x => Math.Abs(AngleUtilities.AngleDifference(latest.CourseOverGroundRadians, x.CourseOverGroundRadians))).Max();
if(deltas>_tackThreshold)
{
//tack detected
_lastTackAt = latest.Time;
var priorToTack = _history.Where(x => x.Time < latest.Time - _dataExclusionTime).OrderByDescending(x => x.Time).FirstOrDefault();
if (priorToTack != null)
{
_previousTackCourseOverGroundRadians = priorToTack.CourseOverGroundRadians;
}
else
{
_previousTackCourseOverGroundRadians = null;
}
_history.Clear();
_currentTackStartCourseOverGroundRadians = null;
var difference = AngleUtilities.AngleDifference(_previousTackCourseOverGroundRadians.Value, latest.CourseOverGroundRadians);
var differenceDegrees = AngleUtilities.RadiansToDegrees(difference);
string message = string.Format("Tack: {0:0.0}°", differenceDegrees);
_logger.Info(message);
state.AddMessage(MessageCategory.Tactical, MessagePriority.Normal, 5, message);
//record the tack in the state
if (state.RaceStarted && _currentTack != null) {
_currentTack.CourseOverGround = AngleUtilities.RadiansToDegrees (_previousTackCourseOverGroundRadians.Value);
state.Tacks.Add (_currentTack);
}
_currentTack = new Tack ();
_currentTack.At = latest.Time;
}
}
/// <inheritdoc />
public override void Update(State state)
{
string sentenceType = string.Empty;
//read from the file and add to the buffer until we find a repeated sentence
do
{
if (string.IsNullOrEmpty(_currentSentence))
{
_currentSentence = _reader.ReadLine();
sentenceType = _currentSentence.Substring(0, _currentSentence.IndexOf(","));
}
_buffer.Enqueue(_currentSentence);
_currentSentence = _reader.ReadLine();
sentenceType = _currentSentence.Substring(0, _currentSentence.IndexOf(","));
}
while (sentenceType!= "$GPGGA" && !_reader.EndOfStream);
//let the base class parse it as if it came from the port
base.Update(state);
}
public void Record (State state)
{
//make sure we have the sensor data we need, otherwise there's no point
if (StateHasRequiredValues(state))
{
//using (var transaction = new TransactionScope())
{
var existing = FindValue (state);
if (existing != null)
{
if (existing.SpeedInKnots < state.StateValues [StateValue.SpeedInKnots])
{
existing.SpeedInKnots = state.StateValues [StateValue.SpeedInKnots];
existing.Time = state.BestTime;
_connection.Execute ("update Polar set SpeedInKnots=@SpeedInKnots,Time=@Time where Id=@Id", existing);
}
}
else
{
double speed = state.StateValues [StateValue.TrueWindSpeedKnots];
double angle = state.StateValues [StateValue.TrueWindAngle];
NormalizeWind (ref angle, ref speed);
var newValue = new PolarValue ()
{
Time = state.BestTime,
TrueWindAngle = angle,
TrueWindSpeedKnots = speed,
SpeedInKnots = state.StateValues [StateValue.SpeedInKnots]
};
_connection.Execute ("insert into Polar(TrueWindAngle,TrueWindSpeedKnots,SpeedInKnots,Time) values (@TrueWindAngle,@TrueWindSpeedKnots,@SpeedInKnots,@Time)", newValue);
}
//transaction.Complete();
}
}
}
public void Record (State state)
{
//using (var transaction = new TransactionScope())
{
RecordRace (state);
RecordState (state);
RecordStateValues (state);
//make sure all tacks are recorded by walking the list backwards and looking for zero ids
//(once they are recorded they get real ids)
if (state.Tacks != null) {
for (int i = state.Tacks.Count - 1; i >= 0 && state.Tacks[i].Id==0; i--) {
RecordTack (state.Tacks [i]);
}
}
//transaction.Complete();
}
}
public RaceController(ILogger logger, double autoRoundMarkDistanceMeters)
{
_state = new State();
_logger = logger;
_autoRoundMarkDistanceMeters = autoRoundMarkDistanceMeters;
}
/// <inheritdoc />
public void NewRace()
{
_state = new State ();
}
private bool StateHasRequiredValues (State state)
{
return state.StateValues.ContainsKey (StateValue.TrueWindAngle)
&& state.StateValues.ContainsKey (StateValue.TrueWindSpeedKnots)
&& state.StateValues.ContainsKey (StateValue.SpeedInKnots);
}
public void Calculate (State state)
{
if (StateHasRequiredValues (state)) {
var polarValue = FindValue (state);
if (polarValue != null) {
state.StateValues [StateValue.PeakSpeedInKnotsForWind] = polarValue.SpeedInKnots;
state.StateValues [StateValue.PeakSpeedPercentForWind] = state.StateValues [StateValue.SpeedInKnots] / polarValue.SpeedInKnots * 100.0;
}
}
}
private PolarValue FindValue (State state)
{
//get the exact values from the state
double windSpeed = state.StateValues [StateValue.TrueWindSpeedKnots];
double windAngle = state.StateValues [StateValue.TrueWindAngle];
//round/normalize them to fit in our polar
NormalizeWind (ref windAngle, ref windSpeed);
//find the existing segment in the graph (if it exists)
var newPolarValue = new PolarValue () {
TrueWindAngle = windAngle,
TrueWindSpeedKnots = windSpeed,
SpeedInKnots = state.StateValues[StateValue.SpeedInKnots],
Time = state.BestTime
};
var existing = _connection.Query<PolarValue> ("select * from Polar where TrueWindAngle=@TrueWindAngle and TrueWindSpeedKnots<=@TrueWindSpeedKnots order by SpeedInKnots desc", newPolarValue).FirstOrDefault();
return existing;
}
public void Save (State state)
{
_db
}
public void Setup ()
{
_state = new State ();
//it's actually OK with nulls, which raises some questions....
_calculator = new TrueWindCalculator (null, null);
}
/// <summary>
/// update the the pebble with data from the current race/boat state
/// </summary>
/// <param name="state"></param>
public void Update(State state)
{
//don't send anything until the last send has completed or errored
if (_lastSend == null || _lastSend.IsCanceled || _lastSend.IsCompleted || _lastSend.IsFaulted
//or if it has exceeded the send timeout
|| !_lastSendAt.HasValue || state.SystemTime - _lastSendAt.Value > _sendTimeout)
{
if (!_pebble.IsAlive)
{
//begin a reconnect thread out of process only if there isn't already one in progress
if ((!_lastReconnectAttempt.HasValue
|| state.SystemTime -_lastReconnectAttempt > new TimeSpan(0,0,10)) && (_reconnect == null || _reconnect.IsFaulted || _reconnect.IsCanceled || _reconnect.IsCompleted)) {
_reconnect = BeginReconnect (state.SystemTime);
}
} else {
_transactionId--;
AppMessagePacket message = new AppMessagePacket ();
message.ApplicationId = _uuid;
message.TransactionId = _transactionId;
message.Command = (byte)Command.Push;
string captions = "";
for (int i = 0; i < _lineCount; i++) {
LineStateMap map = null;
lock (_lineValueIndexes) {
map = _lineStateMaps [_lineValueIndexes [i]];
}
message.Values.Add (new AppMessageString () { Key = (uint)message.Values.Count, Value = map.Caption });
captions = captions + map.Caption + ",";
message.Values.Add (new AppMessageString () {
Key = (uint)message.Values.Count,
Value = map.Get (state)
});
}
if (state.Message != null) {
message.Values.Add (new AppMessageString () {
Key = (uint)message.Values.Count,
Value = state.Message.Text
});
}
_lastSend = _pebble.SendApplicationMessage (message);
_lastSendAt = state.SystemTime;
_logger.Debug ("Sent state to pebble " + _pebble.PebbleID + " (" + captions + ")");
}
}
}
public void RecordRace (State state)
{
//TODO: need a better way to communicate race events from the supervisor to the recorder
if (state.StartTime != _lastRaceStart)
{
if (!state.StartTime.HasValue || state.StartTime < state.BestTime)
{
if (_lastRaceId.HasValue)
{
//if there is a previous race, set the end time
_connection.Execute ("update Race set end=@end where id=@id", new { end = state.BestTime, id = _lastRaceId.Value });
_lastRaceId = null;
}
if (state.StartTime.HasValue)
{
_connection.Execute ("insert into Race (start) values (@start)", new { start = state.StartTime.Value });
//TODO: there has to be a better way to do this
_lastRaceId = (long)_connection.ExecuteScalar ("select max(id) from Race");
}
}
else if(state.StartTime.HasValue)
{
if (_lastRaceId.HasValue)
{
//if there is an existing but unstarted race just change the start time
_connection.Execute ("update Race set start=@start where id=@id", new { start = state.StartTime.HasValue, id = _lastRaceId.Value });
}
else
{
_connection.Execute ("insert into Race (start) values (@start)", new { start = state.StartTime.Value });
//TODO: there has to be a better way to do this
_lastRaceId = (long)_connection.ExecuteScalar ("select max(id) from Race");
}
}
}
_lastRaceStart = state.StartTime;
}
private void RecordStateValues (State state)
{
foreach (var key in state.StateValues.Keys)
{
_connection.Execute ("insert into StateValue(" +
"time," +
"key," +
"value" +
") values (" +
"@BestTime," +
"@Key," +
"@Value" +
")",
new {
BestTime = state.BestTime,
Key = (int)key,
Value = state.StateValues[key],
});
}
}
private void RecordState (State state)
{
_connection.Execute ("insert into StateLog(" +
"time," +
"latitude," +
"longitude" +
") values (" +
"@BestTime," +
"@LocationLatitudeValue," +
"@LocationLongitudeValue" +
")",
new {
BestTime = state.BestTime,
LocationLatitudeValue = state.Location!=null && state.Location.Latitude!=null ? state.Location.Latitude.Value : 0,
LocationLongitudeValue = state.Location!=null && state.Location.Longitude!=null ? state.Location.Longitude.Value : 0,
});
}
