// ###########################################################################
//
// P R I V A T E
//
// ###########################################################################
#region PRIVATE
// ===========================================================================
/// \brief Thread for UART reception
///
/// The thread stops in case of any reception error
// ===========================================================================
private void ThreadRx()
{
Telemetries telemetries = new Telemetries( );
// Reception loop
while (stopThread == false)
{
string msg = null;
// Get incoming message
try
{
msg = port.ReadLine( );
}
catch (TimeoutException) { continue; }
catch (Exception) { break; }
if (msg == null || msg == "")
{
break;
}
// Log the data
Log(msg + "\n");
// Process the incoming telemetry
if (msg [0] == '#')
{
bool lastTm;
try
{
lastTm = ProcessTelemetry(msg, ref telemetries);
}
// Stop in case of transmission error
catch (Exception e) { break; }
// Signal the end of a telemetry cycle
if (lastTm && TelemetriesEvent != null)
{
TelemetriesEvent(this, telemetries);
}
}
// Process the response to a query
else if (msg [0] == '=')
{
queryResponse = String.Copy(msg);
responseEvent.Set( );
}
// Error
else
{
break;
}
}
}
public bool InsertTelemetryFileOnly(Telemetries telemetry)
{
string relativePath = $"{folderPathToStoreInfo}\\{telemetriesPath}{format}";
List <Telemetries> telemetries = this.GetAllTelemetriesForced();
telemetries.Add(telemetry);
File.WriteAllText(relativePath, JsonConvert.SerializeObject(telemetries));
return(true);
}
Response <Telemetries> ISubscriptionService.InsertTelemetry(Telemetries telemetry)
{
Task insert = Task.Run(() => { this.InsertTelemetryAsync(telemetry); });
insert.Wait();
return(new Response <Telemetries>()
{
Success = true, ResponseObject = telemetry
});
}
public Response <Telemetries> InsertTelemetry(Telemetries telemetry)
{
return(new Response <Telemetries>()
{
Success = true,
ResponseObject = new Telemetries()
{
telemeteryId = telemetry.telemeteryId
}
});
}
// ===========================================================================
/// \brief Process an incoming telemetry
///
/// \param tm Telemetry string
/// \param[in,out] telemetries All the telemetries, among which one is updated
/// according to the incoming string.
///
/// \return True if the last telemetry of a cycle is received
// ===========================================================================
private bool ProcessTelemetry(string tm, ref Telemetries telemetries)
{
bool last = false;
// Extract all the parameters into an array.
int idxParam = tm.IndexOf('{');
string paramString = tm.Substring(idxParam + 1, tm.Length - idxParam - 2);
string [] parameters = paramString.Split(',');
// Keep the telemetry name
tm = tm.Substring(1, idxParam - 2);
// Processing
switch (tm)
{
case "SENSORS":
telemetries.sensors.hall = Convert.ToInt32(parameters [0]) * (2.0f * (float)Math.PI / 60.0f);
telemetries.sensors.gyro = Convert.ToInt32(parameters [1]) * ((float)Math.PI / (1000.0f * 180.0f));
telemetries.sensors.accX = Convert.ToInt32(parameters [2]) * 9.81f / 1000.0f;
telemetries.sensors.accY = Convert.ToInt32(parameters [3]) * 9.81f / 1000.0f;
telemetries.sensors.accZ = Convert.ToInt32(parameters [4]) * 9.81f / 1000.0f;
break;
case "STATE":
telemetries.state.bodyAngle = Convert.ToInt32(parameters [0]) * ((float)Math.PI / (10.0f * 180.0f));
telemetries.state.bodySpeed = Convert.ToInt32(parameters [1]) * ((float)Math.PI / (10.0f * 180.0f));
telemetries.state.wheelSpeed = Convert.ToInt32(parameters [2]) * (2.0f * (float)Math.PI / 60.0f);
telemetries.targetState.bodyAngle = Convert.ToInt32(parameters [3]) * ((float)Math.PI / (10.0f * 180.0f));
telemetries.targetState.bodySpeed = Convert.ToInt32(parameters [4]) * ((float)Math.PI / (10.0f * 180.0f));
telemetries.targetState.wheelSpeed = Convert.ToInt32(parameters [5]) * (2.0f * (float)Math.PI / 60.0f);
break;
case "CTRL":
telemetries.command.brake = (Convert.ToInt32(parameters [0]) > 0);
telemetries.command.torque = Convert.ToInt32(parameters [1]) * 1e-6f;
break;
case "TM.Start":
telemetries.timestamp = Convert.ToUInt32(parameters [1]);
break;
case "TM.End":
last = true;
break;
}
return(last);
}
public bool RemoveTelemetry(Telemetries telemetry)
{
string relativePath = $"{folderPathToStoreInfo}\\{telemetriesPath}{format}";
List <Telemetries> localTelemetries = this.GetAllTelemetriesForced();
if (subscription.RemoveTelemetery(telemetry).Success)
{
telemetry = localTelemetries.Find(x => x.telemeteryId == telemetry.telemeteryId);
if (telemetry != null)
{
localTelemetries.Remove(telemetry);
File.WriteAllText(relativePath, JsonConvert.SerializeObject(localTelemetries));
}
}
return(false);
}
public bool InsertTelemetry(Telemetries telemetry)
{
string relativePath = $"{folderPathToStoreInfo}\\{telemetriesPath}{format}";
List <Telemetries> telemetries = this.GetAllTelemetriesForced();
Response <Telemetries> insert = subscription.InsertTelemetry(telemetry);
if (insert.Success)
{
telemetry.telemeteryId = insert.ResponseObject.telemeteryId;
telemetries.Add(telemetry);
File.WriteAllText(relativePath, JsonConvert.SerializeObject(telemetries));
return(true);
}
return(false);
}
public async Task <Response <Telemetries> > InsertTelemetryAsync(Telemetries telemetry)
{
SetupBlob(telemetryKey);
if (blob.Exists())
{
var telemetryData = blob.DownloadText();
List <Telemetries> myList = JsonConvert.DeserializeObject <List <Telemetries> >(telemetryData);
myList.Add(new Telemetries(telemetry.telemeteryId, telemetry.telemeteryName, telemetry.telemeteryUnit));
string data = JsonConvert.SerializeObject(myList);
await blob.UploadTextAsync(data);
}
return(new Response <Telemetries>()
{
Success = true,
ResponseObject = telemetry
});
}
public Response <bool> RemoveTelemetery(Telemetries telemetry)
{
SetupBlob(telemetryKey);
if (blob.Exists())
{
var telemetryData = blob.DownloadText();
List <Telemetries> myList = JsonConvert.DeserializeObject <List <Telemetries> >(telemetryData);
telemetry = myList.Find(x => x.telemeteryId == telemetry.telemeteryId);
if (telemetry != null)
{
myList.Remove(telemetry);
}
string data = JsonConvert.SerializeObject(myList);
Task t = Task.Run(() => { blob.UploadTextAsync(data); });
t.Wait();
}
return(new Response <bool>()
{
Success = true,
ResponseObject = true
});
}
public void Send(ITelemetry item)
{
Telemetries.Add(item);
}
public void Flush()
{
Telemetries.Clear();
}
public Response <Telemetries> InsertTelemetry(Telemetries telemetry)
{
throw new NotImplementedException();
}
public bool AddTelemetry(Telemetries telemetry)
{
return(dataService.InsertTelemetry(telemetry));
}
