int SerialThread::writePacket(QByteArray packet)
{
if (!m_logInFile || !m_logInOutFile)
{
if (m_logsEnabled)
{
openLogs();
}
}
if (m_logsEnabled)
{
m_logWriteMutex.lock();
m_logInOutFile->write(packet);
m_logInOutFile->flush();
m_logOutFile->write(packet);
m_logOutFile->flush();
m_logWriteMutex.unlock();
}
#ifdef Q_OS_WIN32
int len=0;
for (int i=0;i<packet.size();i++)
{
char c = packet.data()[i];
if (!::WriteFile(m_portHandle, (void*)&c, (DWORD)1, (LPDWORD)&len, NULL))
{
qDebug() << "Serial Write Error";
return -1;
}
msleep(m_interByteSendDelay);
}
return 0;
#else
for (int i=0;i<packet.size();i++)
{
char c = packet.data()[i];
write(m_portHandle,&c,1);
msleep(m_interByteSendDelay);
}
return 0;
#endif //Q_OS_WIN32
}
void LogThread::newOptionValue(const QString &group,const QString &name,const QVariant &value)
{
if(group!="Write_log")
return;
if(name=="transfer_format")
transfer_format=value.toString();
else if(name=="error_format")
error_format=value.toString();
else if(name=="folder_format")
folder_format=value.toString();
else if(name=="sync")
{
sync=value.toBool();
ULTRACOPIER_DEBUGCONSOLE(DebugLevel_Notice,QString("sync flag is set on: %1").arg(sync));
if(sync)
{
if(log.isOpen())
log.flush();
}
}
else if(name=="transfer")
log_enable_transfer=options->getOptionValue("Write_log","enabled").toBool() && value.toBool();
else if(name=="error")
log_enable_error=options->getOptionValue("Write_log","enabled").toBool() && value.toBool();
else if(name=="folder")
log_enable_folder=options->getOptionValue("Write_log","enabled").toBool() && value.toBool();
if(name=="enabled")
{
enabled=value.toBool();
if(enabled)
openLogs();
else
closeLogs();
}
}
void SerialThread::setLogFileName(QString filename)
{
m_logFileName = filename;
if (m_logInFile || m_logInOutFile || m_logOutFile)
{
if (m_logInFile)
{
m_logInFile->close();
}
if (m_logInOutFile)
{
m_logInOutFile->close();
}
if (m_logOutFile)
{
m_logOutFile->close();
}
if (m_logsEnabled)
{
openLogs();
}
}
//m_logFile = new QFile(m_logFileName);
}
void writeLogs()
{
static const pm_char * error_displayed = NULL;
if (isFunctionActive(FUNCTION_LOGS) && logDelay > 0) {
tmr10ms_t tmr10ms = get_tmr10ms();
if (lastLogTime == 0 || (tmr10ms_t)(tmr10ms - lastLogTime) >= (tmr10ms_t)logDelay*10) {
lastLogTime = tmr10ms;
if (!g_oLogFile.fs) {
const pm_char * result = openLogs();
if (result != NULL) {
if (result != error_displayed) {
error_displayed = result;
POPUP_WARNING(result);
}
return;
}
}
#if defined(RTCLOCK)
{
static struct gtm utm;
static gtime_t lastRtcTime = 0;
if ( g_rtcTime != lastRtcTime )
{
lastRtcTime = g_rtcTime;
gettime(&utm);
}
f_printf(&g_oLogFile, "%4d-%02d-%02d,%02d:%02d:%02d.%02d0,", utm.tm_year+1900, utm.tm_mon+1, utm.tm_mday, utm.tm_hour, utm.tm_min, utm.tm_sec, g_ms100);
}
#else
f_printf(&g_oLogFile, "%d,", tmr10ms);
#endif
#if defined(FRSKY)
#if !defined(CPUARM)
f_printf(&g_oLogFile, "%d,%d,%d,", frskyStreaming, RAW_FRSKY_MINMAX(frskyData.rssi[0]), RAW_FRSKY_MINMAX(frskyData.rssi[1]));
for (uint8_t i=0; i<MAX_FRSKY_A_CHANNELS; i++) {
int16_t converted_value = applyChannelRatio(i, RAW_FRSKY_MINMAX(frskyData.analog[i]));
f_printf(&g_oLogFile, "%d.%02d,", converted_value/100, converted_value%100);
}
#if defined(FRSKY_HUB)
TELEMETRY_BARO_ALT_PREPARE();
if (IS_USR_PROTO_FRSKY_HUB()) {
f_printf(&g_oLogFile, "%4d-%02d-%02d,%02d:%02d:%02d,%03d.%04d%c,%03d.%04d%c,%03d.%02d," TELEMETRY_GPS_SPEED_FORMAT TELEMETRY_GPS_ALT_FORMAT TELEMETRY_BARO_ALT_FORMAT TELEMETRY_VSPEED_FORMAT TELEMETRY_ASPEED_FORMAT "%d,%d,%d,%d," TELEMETRY_CELLS_FORMAT TELEMETRY_CURRENT_FORMAT "%d," TELEMETRY_VFAS_FORMAT "%d,%d,%d,",
frskyData.hub.year+2000,
frskyData.hub.month,
frskyData.hub.day,
frskyData.hub.hour,
frskyData.hub.min,
frskyData.hub.sec,
frskyData.hub.gpsLongitude_bp,
frskyData.hub.gpsLongitude_ap,
frskyData.hub.gpsLongitudeEW ? frskyData.hub.gpsLongitudeEW : '-',
frskyData.hub.gpsLatitude_bp,
frskyData.hub.gpsLatitude_ap,
frskyData.hub.gpsLatitudeNS ? frskyData.hub.gpsLatitudeNS : '-',
frskyData.hub.gpsCourse_bp,
frskyData.hub.gpsCourse_ap,
TELEMETRY_GPS_SPEED_ARGS
TELEMETRY_GPS_ALT_ARGS
TELEMETRY_BARO_ALT_ARGS
TELEMETRY_VSPEED_ARGS
TELEMETRY_ASPEED_ARGS
frskyData.hub.temperature1,
frskyData.hub.temperature2,
frskyData.hub.rpm,
frskyData.hub.fuelLevel,
TELEMETRY_CELLS_ARGS
TELEMETRY_CURRENT_ARGS
frskyData.hub.currentConsumption,
TELEMETRY_VFAS_ARGS
frskyData.hub.accelX,
frskyData.hub.accelY,
frskyData.hub.accelZ);
}
#endif
#if defined(WS_HOW_HIGH)
if (IS_USR_PROTO_WS_HOW_HIGH()) {
f_printf(&g_oLogFile, "%d,", TELEMETRY_RELATIVE_BARO_ALT_BP);
}
#endif
#endif
#if defined(CPUARM)
for (int i=0; i<MAX_SENSORS; i++) {
TelemetrySensor & sensor = g_model.telemetrySensors[i];
TelemetryItem & telemetryItem = telemetryItems[i];
if (sensor.logs) {
if (sensor.unit == UNIT_GPS) {
if (telemetryItem.gps.longitudeEW && telemetryItem.gps.latitudeNS)
f_printf(&g_oLogFile, "%03d.%04d%c %03d.%04d%c,", telemetryItem.gps.longitude_bp, telemetryItem.gps.longitude_ap, telemetryItem.gps.longitudeEW, telemetryItem.gps.latitude_bp, telemetryItem.gps.latitude_ap, telemetryItem.gps.latitudeNS);
else
f_printf(&g_oLogFile, ",");
}
else if (sensor.unit == UNIT_DATETIME) {
if (telemetryItem.datetime.datestate)
f_printf(&g_oLogFile, "%4d-%02d-%02d %02d:%02d:%02d,", telemetryItem.datetime.year, telemetryItem.datetime.month, telemetryItem.datetime.day, telemetryItem.datetime.hour, telemetryItem.datetime.min, telemetryItem.datetime.sec);
else
f_printf(&g_oLogFile, ",");
}
else if (sensor.prec == 2) {
div_t qr = div(telemetryItem.value, 100);
if (telemetryItem.value < 0) f_printf(&g_oLogFile, "-");
f_printf(&g_oLogFile, "%d.%02d,", abs(qr.quot), abs(qr.rem));
}
else if (sensor.prec == 1) {
div_t qr = div(telemetryItem.value, 10);
if (telemetryItem.value < 0) f_printf(&g_oLogFile, "-");
f_printf(&g_oLogFile, "%d.%d,", abs(qr.quot), abs(qr.rem));
}
else {
f_printf(&g_oLogFile, "%d,", telemetryItem.value);
}
}
}
#endif
#endif
for (uint8_t i=0; i<NUM_STICKS+NUM_POTS; i++) {
f_printf(&g_oLogFile, "%d,", calibratedStick[i]);
}
#if defined(PCBTARANIS)
int result = f_printf(&g_oLogFile, "%d,%d,%d,%d,%d,%d,%d,%d\n",
get3PosState(SA),
get3PosState(SB),
get3PosState(SC),
get3PosState(SD),
get3PosState(SE),
get2PosState(SF),
get3PosState(SG),
get2PosState(SH));
#endif
if (result<0 && !error_displayed) {
error_displayed = STR_SDCARD_ERROR;
POPUP_WARNING(STR_SDCARD_ERROR);
closeLogs();
}
}
}
else {
error_displayed = NULL;
if (g_oLogFile.fs) {
closeLogs();
}
}
}
int SerialThread::readSerial(int timeout)
{
if (!m_logInFile || !m_logInOutFile)
{
if (m_logsEnabled)
{
openLogs();
}
}
qint64 currms = QDateTime::currentMSecsSinceEpoch();
int readlen = m_buffer.size();
QByteArray qbuffer;
if (m_buffer.size() > 10240)
{
//Error here somehow;
Q_ASSERT(m_buffer.size() < 10240);
}
unsigned char buffer[10240];
qbuffer.append(m_buffer);
/*for (int i=0;i<m_buffer.size();i++)
{
buffer[i] = m_buffer[i];
qbuffer.append(m_buffer[i]);
}*/
m_buffer.clear();
//bool inpacket = false;
//bool inescape = false;
QString byteoutofpacket;
while (currms + timeout > QDateTime::currentMSecsSinceEpoch())
{
#ifdef Q_OS_WIN32
if (!ReadFile(m_portHandle,(LPVOID)buffer,1024,(LPDWORD)&readlen,NULL))
{
//Serial error here
qDebug() << "Serial Read error";
}
#else
readlen = read(m_portHandle,buffer,1024);
#endif //Q_OS_WIN32
if (readlen < 0)
{
//Nothing on the port
msleep(10);
}
else
{
if (m_logsEnabled)
{
m_logInFile->write((const char*)buffer,readlen);
m_logInFile->flush();
}
}
if (readlen == 0)
{
msleep(10);
}
for (int i=0;i<readlen;i++)
{
if (buffer[i] == 0xAA)
{
if (m_inpacket)
{
//Start byte in the middle of a packet
//Clear out the buffer and start fresh
m_inescape = false;
if (m_logsEnabled)
{
m_logWriteMutex.lock();
m_logInOutFile->write(QByteArray().append(0xAA));
QByteArray nbuffer(qbuffer);
nbuffer.replace(0xBB,QByteArray().append(0xBB).append(0x44));
nbuffer.replace(0xAA,QByteArray().append(0xBB).append(0x55));
nbuffer.replace(0xCC,QByteArray().append(0xBB).append(0x33));
m_logInOutFile->write((const char*)nbuffer.data(),nbuffer.size());
m_logInOutFile->write(QByteArray().append(0xCC));
m_logInOutFile->flush();
m_logWriteMutex.unlock();
}
qbuffer.clear();
qDebug() << "Buffer error";
m_packetErrorCount++;
}
//qbuffer.append(buffer[i]);
//qDebug() << "Start of packet";
//Start of packet
m_inpacket = true;
}
else if (buffer[i] == 0xCC && m_inpacket)
{
//qDebug() << "End of packet. Size:" << qbuffer.size();
//End of packet
m_inpacket = false;
//qbuffer.append(buffer[i]);
//m_logFile->flush();
//emit parseBuffer(qbuffer);
//New Location of checksum
unsigned char sum = 0;
for (int i=0;i<qbuffer.size()-1;i++)
{
sum += qbuffer[i];
}
if (m_logsEnabled)
{
m_logWriteMutex.lock();
m_logInOutFile->write(QByteArray().append(0xAA));
QByteArray nbuffer(qbuffer);
nbuffer.replace(0xBB,QByteArray().append(0xBB).append(0x44));
nbuffer.replace(0xAA,QByteArray().append(0xBB).append(0x55));
nbuffer.replace(0xCC,QByteArray().append(0xBB).append(0x33));
m_logInOutFile->write((const char*)nbuffer.data(),nbuffer.size());
m_logInOutFile->write(QByteArray().append(0xCC));
m_logInOutFile->flush();
m_logWriteMutex.unlock();
}
//qDebug() << "Payload sum:" << QString::number(sum);
//qDebug() << "Checksum sum:" << QString::number((unsigned char)currPacket[currPacket.length()-1]);
if (sum != (unsigned char)qbuffer[qbuffer.size()-1])
{
qDebug() << "BAD CHECKSUM!";
m_packetErrorCount++;
//return QPair<QByteArray,QByteArray>();
}
else
{
m_packetErrorCount=0;
m_queuedMessages.append(qbuffer.mid(0,qbuffer.length()-1));
}
//return qbuffer;
QString output;
for (int i=0;i<qbuffer.size();i++)
{
int num = (unsigned char)qbuffer[i];
output.append(" ").append((num < 0xF) ? "0" : "").append(QString::number(num,16));
}
//qDebug() << "Full packet:";
//qDebug() << output;
qbuffer.clear();
}
else
{
if (m_inpacket && !m_inescape)
{
if (buffer[i] == 0xBB)
{
//Need to escape the next byte
//retval = logfile.read(1);
m_inescape = true;
}
else
{
qbuffer.append(buffer[i]);
}
}
else if (m_inpacket && m_inescape)
{
if (buffer[i] == 0x55)
{
qbuffer.append((char)0xAA);
}
else if (buffer[i] == 0x44)
{
qbuffer.append((char)0xBB);
}
else if (buffer[i] == 0x33)
{
qbuffer.append((char)0xCC);
}
else
{
qDebug() << "Error, escaped character is not valid!:" << QString::number(buffer[i],16);
m_packetErrorCount++;
}
m_inescape = false;
}
else
{
//qDebug() << "Byte out of a packet:" << QString::number(buffer[i],16);
byteoutofpacket += QString::number(buffer[i],16) + " ";
}
}
}
//qDebug() << "Bytes out of a packet:" << byteoutofpacket;
}
m_buffer.append(qbuffer);
if (readlen > 0)
{
//m_buffer.write(buffer,readlen);
// m_buffer.append((const char*)buffer,readlen);
}
return m_packetErrorCount;
}
// TODO test when disk full
void writeLogs()
{
static const pm_char * error_displayed = NULL;
if (isFunctionActive(FUNCTION_LOGS) && logDelay > 0) {
tmr10ms_t tmr10ms = get_tmr10ms();
if (lastLogTime == 0 || (tmr10ms_t)(tmr10ms - lastLogTime) >= (tmr10ms_t)logDelay*10) {
lastLogTime = tmr10ms;
if (!g_oLogFile.fs) {
const pm_char * result = openLogs();
if (result != NULL) {
if (result != error_displayed) {
error_displayed = result;
POPUP_WARNING(result);
}
return;
}
}
#if defined(RTCLOCK)
struct gtm utm;
gettime(&utm);
f_printf(&g_oLogFile, "%4d-%02d-%02d,%02d:%02d:%02d.%02d0,", utm.tm_year+1900, utm.tm_mon+1, utm.tm_mday, utm.tm_hour, utm.tm_min, utm.tm_sec, g_ms100);
#else
f_printf(&g_oLogFile, "%d,", tmr10ms);
#endif
#if defined(FRSKY)
#if defined(PCBTARANIS) && defined(REVPLUS)
f_printf(&g_oLogFile, "%d,", RAW_FRSKY_MINMAX(frskyData.rssi[0]));
#elif defined(CPUARM)
f_printf(&g_oLogFile, "%d,%d,", RAW_FRSKY_MINMAX(frskyData.swr), RAW_FRSKY_MINMAX(frskyData.rssi[0]));
#else
f_printf(&g_oLogFile, "%d,%d,%d,", frskyStreaming, RAW_FRSKY_MINMAX(frskyData.rssi[0]), RAW_FRSKY_MINMAX(frskyData.rssi[1]));
#endif
for (uint8_t i=0; i<MAX_FRSKY_A_CHANNELS; i++) {
int16_t converted_value = applyChannelRatio(i, RAW_FRSKY_MINMAX(frskyData.analog[i]));
f_printf(&g_oLogFile, "%d.%02d,", converted_value/100, converted_value%100);
}
#endif
#if defined(FRSKY_HUB)
TELEMETRY_BARO_ALT_PREPARE();
if (IS_USR_PROTO_FRSKY_HUB()) {
f_printf(&g_oLogFile, "%4d-%02d-%02d,%02d:%02d:%02d,%03d.%04d%c,%03d.%04d%c,%03d.%02d," TELEMETRY_GPS_SPEED_FORMAT TELEMETRY_GPS_ALT_FORMAT TELEMETRY_BARO_ALT_FORMAT TELEMETRY_VSPEED_FORMAT TELEMETRY_ASPEED_FORMAT "%d,%d,%d,%d," TELEMETRY_CELLS_FORMAT TELEMETRY_CURRENT_FORMAT "%d," TELEMETRY_VFAS_FORMAT "%d,%d,%d,",
frskyData.hub.year+2000,
frskyData.hub.month,
frskyData.hub.day,
frskyData.hub.hour,
frskyData.hub.min,
frskyData.hub.sec,
frskyData.hub.gpsLongitude_bp,
frskyData.hub.gpsLongitude_ap,
frskyData.hub.gpsLongitudeEW ? frskyData.hub.gpsLongitudeEW : '-',
frskyData.hub.gpsLatitude_bp,
frskyData.hub.gpsLatitude_ap,
frskyData.hub.gpsLatitudeNS ? frskyData.hub.gpsLatitudeNS : '-',
frskyData.hub.gpsCourse_bp,
frskyData.hub.gpsCourse_ap,
TELEMETRY_GPS_SPEED_ARGS
TELEMETRY_GPS_ALT_ARGS
TELEMETRY_BARO_ALT_ARGS
TELEMETRY_VSPEED_ARGS
TELEMETRY_ASPEED_ARGS
frskyData.hub.temperature1,
frskyData.hub.temperature2,
frskyData.hub.rpm,
frskyData.hub.fuelLevel,
TELEMETRY_CELLS_ARGS
TELEMETRY_CURRENT_ARGS
frskyData.hub.currentConsumption,
TELEMETRY_VFAS_ARGS
frskyData.hub.accelX,
frskyData.hub.accelY,
frskyData.hub.accelZ);
}
#endif
#if defined(WS_HOW_HIGH)
if (IS_USR_PROTO_WS_HOW_HIGH()) {
f_printf(&g_oLogFile, "%d,", TELEMETRY_RELATIVE_BARO_ALT_BP);
}
#endif
for (uint8_t i=0; i<NUM_STICKS+NUM_POTS; i++) {
f_printf(&g_oLogFile, "%d,", calibratedStick[i]);
}
#if defined(PCBTARANIS)
int result = f_printf(&g_oLogFile, "%d,%d,%d,%d,%d,%d,%d,%d\n",
get3PosState(SA),
get3PosState(SB),
get3PosState(SC),
get3PosState(SD),
get3PosState(SE),
get2PosState(SF),
get3PosState(SG),
get2PosState(SH));
#else
int result = f_printf(&g_oLogFile, "%d,%d,%d,%d,%d,%d,%d\n",
get2PosState(THR),
get2PosState(RUD),
get2PosState(ELE),
get3PosState(ID),
get2PosState(AIL),
get2PosState(GEA),
get2PosState(TRN));
#endif
if (result<0 && !error_displayed) {
error_displayed = STR_SDCARD_ERROR;
POPUP_WARNING(STR_SDCARD_ERROR);
closeLogs();
}
}
}
else {
error_displayed = NULL;
if (g_oLogFile.fs) {
closeLogs();
}
}
}
