public TelemetryReceivedEventArgs(TelemetryFeedback telemetry)
{
if(telemetry == null)
throw new ArgumentNullException("telemetry");
this.telemetry = telemetry;
}
void reporter_Elapsed(object sender, System.Timers.ElapsedEventArgs e)
{
if (System.Threading.Monitor.TryEnter(reporterSync)) //re-entrace protection
{
try
{
//compile all data
Comms.TelemetryEncoding.TelemetryFeedback feedback = new Comms.TelemetryEncoding.TelemetryFeedback()
{
// Voltages
DriveBatteryVoltage = this.DriveBatteryVoltage,
CleanBatteryVoltage = this.CleanBatteryVoltage,
PowerUsed = this.PowerUsed,
// Mining
ArmMotorAmps = this.BeltMotorAmps,
ScoopPitchAngle = this.BucketPitchAngle,
ArmSwingAngle = this.BucketPivotAngle,
ScoopLowerLimitSwitchDepressed = this.pivotLowerLimitSwitchDepressed,
ScoopUpperLimitSwitchDepressed = this.pivotUpperLimitSwitchDepressed,
// Deposition
BinLeftMotorAmps = 12,
BinRightMotorAmps = 12,
BinLowerSwitchDepressed = this.binLowerLimitSwitch,
BinUpperSwitchDepressed = this.binUpperLimitSwitch,
// Sensory
TiltX = tiltX,
TiltY = tiltY,
RearProximityLeft = rearProximityLeft,
RearProximityRight = rearProximityRight,
// Localization
X = this.X,
Y = this.Y,
Psi = this.Psi,
Confidence = this.Confidence,
State = this.State
};
//todo : store rf timing in telemetry
int timing;
if (front_rf != null)
feedback.FrontRangeFinderData = front_rf.CopyData(null, out feedback.FrontRangeFinderDataLength, out timing);
if (rear_rf != null)
feedback.RearRangeFinderData = rear_rf.CopyData(null, out feedback.RearRangeFinderDataLength, out timing);
// Raise event for telemetry processed
OnTelemetryFeedbackProcessed(new TelemetryFeedbackArgs(feedback));
//send
udp_sender.Send(feedback.Encode());
}
catch (TimeoutException ex)
{
}
catch (Exception ex)
{
Console.WriteLine(ex.Message);
}
finally
{
System.Threading.Monitor.Exit(reporterSync);
}
}
}
