void telemetryReceiver_TelemetryReceived(object sender, Models.TelemetryReceivedEventArgs e) { try { //track battery voltages dbVoltageSum += e.Telemetry.DriveBatteryVoltage; cbVoltageSum += e.Telemetry.CleanBatteryVoltage; ++dbVoltageCount; ++cbVoltageCount; double seconds = System.DateTime.Now.TimeOfDay.TotalSeconds; int currentSecond = (int)Math.Floor(seconds); if (currentSecond != lastSecond) //only track graph data once per second { //add to graph data App.Current.Dispatcher.Invoke((Action)(() => { driveBatteryInfo.Voltage = e.Telemetry.DriveBatteryVoltage; cleanBatteryInfo.Voltage = e.Telemetry.CleanBatteryVoltage; PowerUsedRecent = e.Telemetry.PowerUsed; if (e.Telemetry.DriveBatteryVoltage > DB_GOOD_VOLTAGE) driveBatteryInfo.Status = BatteryStatus.Good; else if (e.Telemetry.DriveBatteryVoltage > DB_OK_VOLTAGE) driveBatteryInfo.Status = BatteryStatus.Ok; else driveBatteryInfo.Status = BatteryStatus.Critical; if (driveBatteryData.Count >= MAX_VOLTAGE_GRAPH_DATA) driveBatteryData.RemoveAt(0); driveBatteryData.Add(new KeyValuePair<double, double>(seconds, e.Telemetry.DriveBatteryVoltage)); if (e.Telemetry.CleanBatteryVoltage > CB_GOOD_VOLTAGE) cleanBatteryInfo.Status = BatteryStatus.Good; else if (e.Telemetry.CleanBatteryVoltage > CB_OK_VOLTAGE) cleanBatteryInfo.Status = BatteryStatus.Ok; else cleanBatteryInfo.Status = BatteryStatus.Critical; if (cleanBatteryData.Count >= MAX_VOLTAGE_GRAPH_DATA) cleanBatteryData.RemoveAt(0); cleanBatteryData.Add(new KeyValuePair<double, double>(seconds, e.Telemetry.CleanBatteryVoltage)); if (powerUsedData.Count >= MAX_POWER_GRAPH_DATA) powerUsedData.RemoveAt(0); powerUsedData.Add(new KeyValuePair<double, double>(seconds, e.Telemetry.PowerUsed)); }), new TimeSpan(0,0,1)); lastSecond = currentSecond; //reset averaging dbVoltageCount = 0; dbVoltageSum = 0; cbVoltageCount = 0; cbVoltageSum = 0; } //*other telemetry*/ BeltMotorAmps = e.Telemetry.ArmMotorAmps; BucketPitchAngle = e.Telemetry.ScoopPitchAngle; BucketPivotAngle = e.Telemetry.ArmSwingAngle; BucketLowerLimitSwitch = e.Telemetry.ScoopLowerLimitSwitchDepressed; BucketUpperLimitSwitch = e.Telemetry.ScoopUpperLimitSwitchDepressed; // Tilt Telemetry // TiltX = e.Telemetry.TiltX; TiltY = e.Telemetry.TiltY; // Localization // X = e.Telemetry.X; Y = e.Telemetry.Y; Phi = e.Telemetry.Psi; State = e.Telemetry.State; // Rear Proximity Sensors // RearProximityLeft = e.Telemetry.RearProximityLeft; RearProximityRight = e.Telemetry.RearProximityRight; ProximityBar1 = e.Telemetry.RearProximityLeft; ProximityBar2 = e.Telemetry.RearProximityRight; // Collection Bin States // BinLeftMotorAmps = e.Telemetry.BinLeftMotorAmps; // From 'driver' perspective BinRightMotorAmps = e.Telemetry.BinRightMotorAmps; BinLowerSwitchDepressed = e.Telemetry.BinLowerSwitchDepressed; BinUpperSwitchDepressed = e.Telemetry.BinUpperSwitchDepressed; //process range finder //System.Diagnostics.Debug.WriteLine("Rear RF Data Length: " + e.Telemetry.RearRangeFinderDataLength); if (e.Telemetry.RearRangeFinderDataLength != 0) { double angle = Properties.RangeFinderSettings.Default.RearStartAngle; Models.ObservableNotifiableCollection<Models.Point> newPoints = new Models.ObservableNotifiableCollection<Models.Point>(); for (int i = 0; i < e.Telemetry.RearRangeFinderDataLength && i < Properties.RangeFinderSettings.Default.RearDataLength; ++i, angle += Properties.RangeFinderSettings.Default.RearStepAngle) { double dist = Math.Min(1000, e.Telemetry.RearRangeFinderData[i]); newPoints.Add(new Models.Point() { X = dist * Math.Cos(angle * Math.PI / 180), Y = dist * Math.Sin(angle * Math.PI / 180) }); } App.Current.Dispatcher.Invoke((Action)(() => { RearRFPoints = newPoints; }), new TimeSpan(0,0,1)); } } catch (Exception ex) { System.Diagnostics.Debug.WriteLine(ex.Message); } }
private TelemetryViewModel() { driveBatteryData = new ObservableCollection<KeyValuePair<double, double>>(); cleanBatteryData = new ObservableCollection<KeyValuePair<double, double>>(); powerUsedData = new ObservableCollection<KeyValuePair<double, double>>(); frontRFPoints = new Models.ObservableNotifiableCollection<Models.Point>(); rearRFPoints = new Models.ObservableNotifiableCollection<Models.Point>(); Activate(); }