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();
}