/// <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, }); }