/// <summary>
/// Сохранить запись в файле
/// </summary>
private void SaveRecord(FileFormats.F01Record rec)
{
try
{
if (telemetryStream == null)
{
string dir = AppDirs.LogDir + "AeroQuad" + Path.DirectorySeparatorChar;
Directory.CreateDirectory(dir);
string path = dir + "telemetry_" + DateTime.Now.ToString("yyyy'-'MM'-'dd'_'HH'-'mm'-'ss") + ".f01";
telemetryStream = new FileStream(path, FileMode.Create, FileAccess.Write, FileShare.Read);
}
byte[] bytes = rec.GetBytes();
telemetryStream.Write(bytes, 0, bytes.Length);
telemetryStream.Flush(true);
}
catch (Exception ex)
{
WriteToLog("Error saving record to file: " + ex.Message);
}
}
/// <summary>
/// Расшифровать и обработать полученные данные
/// </summary>
private bool DecodeResponse()
{
// принимаемые данные имеют вид:
// -0.00,-0.00,-0.00,0.05,0.13,-9.85,0,0,0,<0D><0A>
// [Roll Gyro Rate],[Pitch Gyro Rate],[Yaw Gyro Rate],[Accel X Axis],[Accel Y Axis],[Accel Z Axis],[Magnetometer Raw Value X Axis],[Magnetometer Raw Value Y Axis],[Magnetometer Raw Value Z Axis]\r\n
// http://aeroquad.com/showwiki.php?title=Serial-Commands:-AeroQuad-Ground-Station-Interface-Definitions
bool decodeOK = true;
try
{
int paramIndex = TELEMETRY_START_SIGNAL - 1; // индекс устанавливаемого тега
if (inBufLen > 0)
{
// распознавание ответа
string line = Encoding.Default.GetString(inBuf, 0, inBufLen).TrimEnd();
string[] parts = line.Split(Separator, StringSplitOptions.RemoveEmptyEntries);
if (parts.Length == 9)
{
FileFormats.F01Record rec = new FileFormats.F01Record();
rec.Time = DateTime.Now.ToBinary();
for (int i = 0; i < 9; i++)
{
try
{
double val = double.Parse(parts[i], NumFormat);
SetCurData(paramIndex, val, 1);
rec.SetFieldByIndex(i, val);
paramIndex++;
}
catch (FormatException ex)
{
throw new FormatException(
"Unable to convert string \"" + parts[i] + "\" to number.", ex);
}
}
if (recordOn)
{
// определение успешности обработки данных
decodeOK = paramIndex == KPTags.Length;
if (decodeOK)
{
// увеличение счётчика принятых сообщений
IncCounter(RECEIVED_MSG_SIGNAL);
// увеличение счётчика сообщений в секунду
msgPerSec++;
// запись телеметрии в файл
SaveRecord(rec);
}
else
{
// увеличение счётчика повреждённых сообщений
IncCounter(FAILED_MSG_SIGNAL);
}
}
}
}
// установка тега наличия связи
SetCurData(CONNECT_SIGNAL - 1, decodeOK ? 1.0 : 0.0, 1);
// установка тега количества сообщений в секунду
if (recordOn)
{
int curSec = DateTime.Now.Second;
if (startSec != curSec)
{
SetCurData(MSG_PER_SEC_SIGNAL - 1, msgPerSec, 1);
startSec = curSec;
msgPerSec = 0;
}
}
// установка недостоверности для непринятых тегов
for (int i = paramIndex; i < KPTags.Length; i++)
{
SetCurData(i, 0.0, 0);
}
}
catch (Exception ex)
{
WriteToLog("Error decoding response: " + ex.Message);
}
return(decodeOK);
}
