You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
844 lines
24 KiB
844 lines
24 KiB
/////////////////////////////////////////////////////////////////////////////////// |
|
// Copyright (C) 2022 Jon Beniston, M7RCE <jon@beniston.com> // |
|
// // |
|
// Based on code and docs by einergehtnochrein, rs1729 and bazjo // |
|
// // |
|
// This program is free software; you can redistribute it and/or modify // |
|
// it under the terms of the GNU General Public License as published by // |
|
// the Free Software Foundation as version 3 of the License, or // |
|
// (at your option) any later version. // |
|
// // |
|
// This program is distributed in the hope that it will be useful, // |
|
// but WITHOUT ANY WARRANTY; without even the implied warranty of // |
|
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // |
|
// GNU General Public License V3 for more details. // |
|
// // |
|
// You should have received a copy of the GNU General Public License // |
|
// along with this program. If not, see <http://www.gnu.org/licenses/>. // |
|
/////////////////////////////////////////////////////////////////////////////////// |
|
|
|
#include <QDateTime> |
|
#include <QTimeZone> |
|
#include <QVector3D> |
|
|
|
#include "util/radiosonde.h" |
|
#include "util/coordinates.h" |
|
|
|
RS41Frame::RS41Frame(const QByteArray ba) : |
|
m_statusValid(false), |
|
m_frameNumber(0), |
|
m_batteryVoltage(0.0), |
|
m_pcbTemperature(0), |
|
m_humiditySensorHeating(0), |
|
m_transmitPower(0), |
|
m_maxSubframeNumber(0), |
|
m_subframeNumber(0), |
|
m_measValid(false), |
|
m_tempMain(0), |
|
m_tempRef1(0), |
|
m_tempRef2(0), |
|
m_humidityMain(0), |
|
m_humidityRef1(0), |
|
m_humidityRef2(0), |
|
m_humidityTempMain(0), |
|
m_humidityTempRef1(0), |
|
m_humidityTempRef2(0), |
|
m_pressureMain(0), |
|
m_pressureRef1(0), |
|
m_pressureRef2(0), |
|
m_pressureTemp(0.0), |
|
m_gpsInfoValid(false), |
|
m_posValid(false), |
|
m_latitude(0.0), |
|
m_longitude(0.0), |
|
m_height(0.0), |
|
m_speed(0.0), |
|
m_heading(0.0), |
|
m_verticalRate(0.0), |
|
m_satellitesUsed(0), |
|
m_bytes(ba), |
|
m_pressure(0.0), |
|
m_pressureCalibrated(false), |
|
m_temperature(0.0), |
|
m_temperatureCalibrated(false), |
|
m_humidityTemperature(0.0), |
|
m_humidityTemperatureCalibrated(false), |
|
m_humidity(0.0), |
|
m_humidityCalibrated(false) |
|
{ |
|
int length = getFrameLength(ba[RS41_OFFSET_FRAME_TYPE]); |
|
for (int i = RS41_OFFSET_BLOCK_0; i < length; ) |
|
{ |
|
uint8_t blockID = ba[i+0]; |
|
uint8_t blockLength = ba[i+1]; |
|
switch (blockID) |
|
{ |
|
case RS41_ID_STATUS: |
|
decodeStatus(ba.mid(i+2, blockLength)); |
|
break; |
|
case RS41_ID_MEAS: |
|
decodeMeas(ba.mid(i+2, blockLength)); |
|
break; |
|
case RS41_ID_GPSINFO: |
|
decodeGPSInfo(ba.mid(i+2, blockLength)); |
|
break; |
|
case RS41_ID_GPSRAW: |
|
break; |
|
case RS41_ID_GPSPOS: |
|
decodeGPSPos(ba.mid(i+2, blockLength)); |
|
break; |
|
case RS41_ID_POSDATETIME: |
|
decodePosDateTime(ba.mid(i+2, blockLength)); |
|
break; |
|
case RS41_ID_EMPTY: |
|
break; |
|
} |
|
i += 2 + blockLength + 2; // ID, length, data, CRC |
|
} |
|
} |
|
|
|
QString RS41Frame::toHex() |
|
{ |
|
return m_bytes.toHex(); |
|
} |
|
|
|
int16_t RS41Frame::getInt16(const QByteArray ba, int offset) const |
|
{ |
|
return (ba[offset] & 0xff) |
|
| ((ba[offset+1] & 0xff) << 8); |
|
} |
|
|
|
uint16_t RS41Frame::getUInt16(const QByteArray ba, int offset) const |
|
{ |
|
return (ba[offset] & 0xff) |
|
| ((ba[offset+1] & 0xff) << 8); |
|
} |
|
|
|
uint32_t RS41Frame::getUInt24(const QByteArray ba, int offset) const |
|
{ |
|
return (ba[offset] & 0xff) |
|
| ((ba[offset+1] & 0xff) << 8) |
|
| ((ba[offset+2] & 0xff) << 16); |
|
} |
|
|
|
uint32_t RS41Frame::getUInt32(const QByteArray ba, int offset) const |
|
{ |
|
return (ba[offset] & 0xff) |
|
| ((ba[offset+1] & 0xff) << 8) |
|
| ((ba[offset+2] & 0xff) << 16) |
|
| ((ba[offset+3] & 0xff) << 24); |
|
} |
|
|
|
void RS41Frame::decodeStatus(const QByteArray ba) |
|
{ |
|
m_statusValid = true; |
|
m_frameNumber = getUInt16(ba, 0); |
|
m_serial = QString(ba.mid(0x2, 8)); |
|
m_batteryVoltage = (ba[0xa] & 0xff) / 10.0; |
|
QStringList phases = {"Ground", "Ascent", "0x2", "Descent"}; |
|
int phase = ba[0xd] & 0x3; |
|
m_flightPhase = phases[phase]; |
|
m_batteryStatus = (ba[0xe] & 0x10) == 0 ? "OK" : "Low"; |
|
m_pcbTemperature = (ba[0x10] & 0xff); |
|
m_humiditySensorHeating = getUInt16(ba, 0x13); |
|
m_transmitPower = ba[0x15] & 0xff; |
|
m_maxSubframeNumber = ba[0x16] & 0xff; |
|
m_subframeNumber = ba[0x17] & 0xff; |
|
m_subframe = ba.mid(0x18, 16); |
|
} |
|
|
|
void RS41Frame::decodeMeas(const QByteArray ba) |
|
{ |
|
m_measValid = true; |
|
m_tempMain = getUInt24(ba, 0x0); |
|
m_tempRef1 = getUInt24(ba, 0x3); |
|
m_tempRef2 = getUInt24(ba, 0x6); |
|
m_humidityMain = getUInt24(ba, 0x9); |
|
m_humidityRef1 = getUInt24(ba, 0xc); |
|
m_humidityRef2 = getUInt24(ba, 0xf); |
|
m_humidityTempMain = getUInt24(ba, 0x12); |
|
m_humidityTempRef1 = getUInt24(ba, 0x15); |
|
m_humidityTempRef2 = getUInt24(ba, 0x18); |
|
m_pressureMain = getUInt24(ba, 0x1b); |
|
m_pressureRef1 = getUInt24(ba, 0x1e); |
|
m_pressureRef2 = getUInt24(ba, 0x21); |
|
m_pressureTemp = getInt16(ba, 0x26) / 100.0f; |
|
} |
|
|
|
void RS41Frame::decodeGPSInfo(const QByteArray ba) |
|
{ |
|
m_gpsInfoValid = true; |
|
uint16_t gpsWeek = getUInt16(ba, 0x0); |
|
uint32_t gpsTimeOfWeek = getUInt32(ba, 0x2); // Milliseconds |
|
QDateTime epoch(QDate(1980, 1, 6), QTime(0, 0, 0), Qt::OffsetFromUTC, 18); // GPS doesn't count leap seconds |
|
m_gpsDateTime = epoch.addDays(gpsWeek*7).addMSecs(gpsTimeOfWeek); |
|
} |
|
|
|
void RS41Frame::decodeGPSPos(const QByteArray ba) |
|
{ |
|
m_satellitesUsed = ba[0x12] & 0xff; |
|
if (m_satellitesUsed > 0) |
|
{ |
|
m_posValid = true; |
|
int32_t ecefX = (int32_t)getUInt32(ba, 0x0); |
|
int32_t ecefY = (int32_t)getUInt32(ba, 0x4); |
|
int32_t ecefZ = (int32_t)getUInt32(ba, 0x8); |
|
// Convert cm to m |
|
// Convert to latitude, longitude and altitude |
|
Coordinates::ecefToGeodetic(ecefX / 100.0, ecefY / 100.0, ecefZ / 100.0, m_latitude, m_longitude, m_height); |
|
int32_t velX = (int16_t)getUInt16(ba, 0xc); |
|
int32_t velY = (int16_t)getUInt16(ba, 0xe); |
|
int32_t velZ = (int16_t)getUInt16(ba, 0x10); |
|
// Convert cm/s to m/s |
|
// Calculate speed / heading |
|
Coordinates::ecefVelToSpeedHeading(m_latitude, m_longitude, velX / 100.0, velY / 100.0, velZ / 100.0, m_speed, m_verticalRate, m_heading); |
|
} |
|
} |
|
|
|
void RS41Frame::decodePosDateTime(const QByteArray ba) |
|
{ |
|
m_posValid = true; |
|
int32_t ecefX = (int32_t)getUInt32(ba, 0x0); |
|
int32_t ecefY = (int32_t)getUInt32(ba, 0x4); |
|
int32_t ecefZ = (int32_t)getUInt32(ba, 0x8); |
|
Coordinates::ecefToGeodetic(ecefX / 100.0, ecefY / 100.0, ecefZ / 100.0, m_latitude, m_longitude, m_height); |
|
if ((m_height < -1000) || (m_height > 80000)) { |
|
m_posValid = false; |
|
return; |
|
} |
|
int32_t velX = (int16_t)getUInt16(ba, 0xc); |
|
int32_t velY = (int16_t)getUInt16(ba, 0xe); |
|
int32_t velZ = (int16_t)getUInt16(ba, 0x10); |
|
Coordinates::ecefVelToSpeedHeading(m_latitude, m_longitude, velX / 100.0, velY / 100.0, velZ / 100.0, m_speed, m_verticalRate, m_heading); |
|
|
|
if (ba.size() >= 26) |
|
{ |
|
m_gpsInfoValid = true; |
|
uint16_t year = getUInt16(ba, 0x12); |
|
uint8_t month = ba[0x14] & 0xff; |
|
uint8_t day = ba[0x15] & 0xff; |
|
uint8_t hour = ba[0x16] & 0xff; |
|
uint8_t minute = ba[0x17] & 0xff; |
|
uint8_t second = ba[0x18] & 0xff; |
|
int msec = 0; |
|
if ((ba[0x19] & 0xff) < 100) { |
|
msec = (ba[0x19] & 0xff) * 10; |
|
} |
|
m_gpsDateTime = QDateTime(QDate(year, month, day), QTime(hour, minute, second, msec), QTimeZone::utc()); |
|
} |
|
} |
|
|
|
// Find the water vapor saturation pressure for a given temperature (for tCelsius < 0C). |
|
static float waterVapourSaturationPressure(float tCelsius) |
|
{ |
|
// Convert to Kelvin |
|
float T = tCelsius + 273.15f; |
|
|
|
// Correction |
|
T = - 0.4931358f |
|
+ (1.0f + 4.6094296e-3f) * T |
|
- 1.3746454e-5f * T * T |
|
+ 1.2743214e-8f * T * T * T; |
|
|
|
// Hyland and Wexler equation |
|
float p = expf(-5800.2206f / T |
|
+ 1.3914993f |
|
+ 6.5459673f * logf(T) |
|
- 4.8640239e-2f * T |
|
+ 4.1764768e-5f * T * T |
|
- 1.4452093e-8f * T * T * T); |
|
|
|
// Scale result to hPa |
|
return p / 100.0f; |
|
} |
|
|
|
static float calcT(int f, int f1, int f2, float r1, float r2, const float *poly, const float *cal) |
|
{ |
|
/*float g = (float)(f2-f1) / (r2-r1); // gain |
|
float Rb = (f1*r2-f2*r1) / (float)(f2-f1); // offset |
|
float Rc = f/g - Rb; |
|
float R = Rc * cal[0]; |
|
float T = (poly[0] + poly[1]*R + poly[2]*R*R + cal[1])*(1.0 + cal[2]); |
|
return T; |
|
*/ |
|
|
|
// Convert integer measurement to scale factor |
|
float s = (f-f1)/(float)(f2-f1); |
|
|
|
// Calculate resistance (scale between two reference resistors) |
|
float rUncal = r1 + (r2 - r1) * s; |
|
float r = rUncal * cal[0]; |
|
|
|
// Convert resistance to temperature |
|
float tUncal = poly[0] + poly[1]*r + poly[2]*r*r; |
|
|
|
// Correct temperature (5th order polynomial) |
|
float tCal = 0.0f; |
|
for (int i = 6; i > 0; i--) |
|
{ |
|
tCal *= tUncal; |
|
tCal += cal[i]; |
|
} |
|
tCal += tUncal; |
|
|
|
return tCal; |
|
} |
|
|
|
static float calcU(int cInt, int cMin, int cMax, float c1, float c2, float T, float HT, const float *capCal, const float *matrixCal, float height, const float *vectorPCal, const float *matrixPCal) |
|
{ |
|
//qDebug() << "cInt " << cInt << " cMin " << cMin << " cMax " << cMax << " c1 " << c1 << " c2 " << c2 << " T " << T << " HT " << HT << " capCal[0] " << capCal[0] << " capCal[1] " << capCal[1] << "height" << height; |
|
|
|
/*float a0 = 7.5f; |
|
float a1 = 350.0f / capCal[0]; |
|
float fh = (cInt-cMin) / (float)(cMax-cMin); |
|
float rh = 100.0f * (a1*fh - a0); |
|
float T0 = 0.0f; |
|
float T1 = -25.0f; |
|
rh += T0 - T/5.5; |
|
if (T < T1) { |
|
rh *= 1.0 + (T1-T)/90.0; |
|
} |
|
if (rh < 0.0) { |
|
rh = 0.0; |
|
} |
|
if (rh > 100.0) { |
|
rh = 100.0; |
|
} |
|
if (T < -273.0) { |
|
rh = -1.0; |
|
} |
|
|
|
qDebug() << "RH old method: " << rh;*/ |
|
|
|
|
|
// Convert integer measurement to scale factor |
|
float s = (cInt - cMin) / (float)(cMax-cMin); |
|
|
|
// Calculate capacitance (scale between two reference caps) |
|
float cUncal = c1 + (c2 - c1) * s; |
|
float cCal = (cUncal / capCal[0] - 1.0f) * capCal[1]; |
|
|
|
float t = (HT - 20.0f) / 180.0f; |
|
|
|
// Calculate standard pressure at given height in hPa |
|
float pressure = 1013.25f * expf(-1.18575919e-4f * height); |
|
|
|
// Compensation for pressure |
|
float p = pressure / 1000.0f; |
|
float powc = 1.0f; |
|
float sum = 0.0f; |
|
for (int i = 0; i < 3; i++) |
|
{ |
|
float l = 0.0f; |
|
float powt = 1.0f; |
|
for (int j = 0; j < 4; j++) |
|
{ |
|
l += matrixPCal[4*i+j] * powt; |
|
powt *= t; |
|
} |
|
float x = vectorPCal[i]; |
|
sum += l * (x * p / (1.0f + x * p) - x * powc / (1.0f + x)); |
|
powc *= cCal; |
|
} |
|
cCal -= sum; |
|
|
|
float uUncal = 0.0f; |
|
float f1 = 1.0f; |
|
for (int i = 0; i < 7; i++) |
|
{ |
|
float f2 = 1.0; |
|
for (int j = 0; j < 6; j++) |
|
{ |
|
uUncal += f1 * f2 * matrixCal[i*6+j]; |
|
f2 *= t; |
|
} |
|
f1 *= cCal; |
|
} |
|
|
|
// Adjust for difference in outside air temperature and the humidty sensor temperature |
|
float uCal = uUncal * waterVapourSaturationPressure(HT) / waterVapourSaturationPressure(T); |
|
|
|
// Ensure within range of 0..100% |
|
uCal = std::min(100.0f, uCal); |
|
uCal = std::max(0.0f, uCal); |
|
|
|
//qDebug() << "RH new method" << uCal; |
|
|
|
return uCal; |
|
} |
|
|
|
static float calcP(int f, int f1, int f2, float pressureTemp, const float *cal) |
|
{ |
|
// Convert integer measurement to scale factor |
|
float s = (f-f1) / (float)(f2-f1); |
|
|
|
float t = pressureTemp; |
|
float t2 = t * t; |
|
float t3 = t2 * t; |
|
|
|
float poly[6]; |
|
poly[0] = cal[0] + cal[7] * t + cal[11] * t2 + cal[15] * t3; |
|
poly[1] = cal[1] + cal[8] * t + cal[12] * t2 + cal[16] * t3; |
|
poly[2] = cal[2] + cal[9] * t + cal[13] * t2 + cal[17] * t3; |
|
poly[3] = cal[3] + cal[10] * t + cal[14] * t2; |
|
poly[4] = cal[4]; |
|
poly[5] = cal[5]; |
|
|
|
float p = cal[6] / s; |
|
float p2 = p * p; |
|
float p3 = p2 * p; |
|
float p4 = p3 * p; |
|
float p5 = p4 * p; |
|
|
|
float pCal = poly[0] + poly[1] * p + poly[2] * p2 + poly[3] * p3 + poly[4] * p4 + poly[5] * p5; |
|
|
|
return pCal; |
|
} |
|
|
|
float RS41Frame::getPressureFloat(const RS41Subframe *subframe) |
|
{ |
|
if (!m_pressureCalibrated) { |
|
calcPressure(subframe); |
|
} |
|
return m_pressure; |
|
} |
|
|
|
QString RS41Frame::getPressureString(const RS41Subframe *subframe) |
|
{ |
|
if (!m_pressureCalibrated) { |
|
calcPressure(subframe); |
|
} |
|
return m_pressureString; |
|
} |
|
|
|
float RS41Frame::getTemperatureFloat(const RS41Subframe *subframe) |
|
{ |
|
if (!m_temperatureCalibrated) { |
|
calcTemperature(subframe); |
|
} |
|
return m_temperature; |
|
} |
|
|
|
QString RS41Frame::getTemperatureString(const RS41Subframe *subframe) |
|
{ |
|
if (!m_temperatureCalibrated) { |
|
calcTemperature(subframe); |
|
} |
|
return m_temperatureString; |
|
} |
|
|
|
void RS41Frame::calcPressure(const RS41Subframe *subframe) |
|
{ |
|
float cal[18]; |
|
|
|
if (m_pressureMain == 0) |
|
{ |
|
m_pressure = 0.0f; |
|
m_pressureString = ""; |
|
return; |
|
} |
|
|
|
m_pressureCalibrated = subframe->getPressureCal(cal); |
|
|
|
m_pressure = calcP(m_pressureMain, m_pressureRef1, m_pressureRef2, m_pressureTemp, cal); |
|
|
|
// RS41 pressure resolution of 0.01hPa |
|
m_pressureString = QString::number(m_pressure, 'f', 2); |
|
|
|
if (!m_pressureCalibrated) { |
|
m_pressureString = m_pressureString + "U"; // U for uncalibrated |
|
} |
|
} |
|
|
|
void RS41Frame::calcTemperature(const RS41Subframe *subframe) |
|
{ |
|
float r1, r2; |
|
float poly[3]; |
|
float cal[7]; |
|
|
|
if (m_tempMain == 0) |
|
{ |
|
m_temperature = 0.0f; |
|
m_temperatureString = ""; |
|
return; |
|
} |
|
|
|
m_temperatureCalibrated = subframe->getTempCal(r1, r2, poly, cal); |
|
|
|
m_temperature = calcT(m_tempMain, m_tempRef1, m_tempRef2, |
|
r1, r2, |
|
poly, cal); |
|
|
|
// RS41 temperature resolution of 0.01C |
|
m_temperatureString = QString::number(m_temperature, 'f', 2); |
|
|
|
if (!m_temperatureCalibrated) { |
|
m_temperatureString = m_temperatureString + "U"; // U for uncalibrated |
|
} |
|
} |
|
|
|
float RS41Frame::getHumidityTemperatureFloat(const RS41Subframe *subframe) |
|
{ |
|
if (!m_humidityTemperatureCalibrated) { |
|
calcHumidityTemperature(subframe); |
|
} |
|
return m_humidityTemperature; |
|
} |
|
|
|
void RS41Frame::calcHumidityTemperature(const RS41Subframe *subframe) |
|
{ |
|
float r1, r2; |
|
float poly[3]; |
|
float cal[7]; |
|
|
|
if (m_humidityTempMain == 0) |
|
{ |
|
m_humidityTemperature = 0.0f; |
|
return; |
|
} |
|
|
|
m_humidityTemperatureCalibrated = subframe->getHumidityTempCal(r1, r2, poly, cal); |
|
|
|
m_humidityTemperature = calcT(m_humidityTempMain, m_humidityTempRef1, m_humidityTempRef2, |
|
r1, r2, |
|
poly, cal); |
|
} |
|
|
|
float RS41Frame::getHumidityFloat(const RS41Subframe *subframe) |
|
{ |
|
if (!m_humidityCalibrated) { |
|
calcHumidity(subframe); |
|
} |
|
return m_humidity; |
|
} |
|
|
|
QString RS41Frame::getHumidityString(const RS41Subframe *subframe) |
|
{ |
|
if (!m_humidityCalibrated) { |
|
calcHumidity(subframe); |
|
} |
|
return m_humidityString; |
|
} |
|
|
|
void RS41Frame::calcHumidity(const RS41Subframe *subframe) |
|
{ |
|
float c1, c2; |
|
float capCal[2]; |
|
float calMatrix[7*6]; |
|
float pCalMatrix[12]; |
|
float pCalVector[3]; |
|
|
|
if (m_humidityMain == 0) |
|
{ |
|
m_humidity = 0.0f; |
|
m_humidityString = ""; |
|
return; |
|
} |
|
|
|
float temperature = getTemperatureFloat(subframe); |
|
float humidityTemperature = getHumidityTemperatureFloat(subframe); |
|
|
|
bool humidityCalibrated = subframe->getHumidityCal(c1, c2, capCal, calMatrix); |
|
|
|
m_humidityCalibrated = m_temperatureCalibrated && m_humidityTemperatureCalibrated && humidityCalibrated; |
|
|
|
subframe->getHumidityPressureCal(pCalVector, pCalMatrix); |
|
|
|
m_humidity = calcU(m_humidityMain, m_humidityRef1, m_humidityRef2, |
|
c1, c2, |
|
temperature, humidityTemperature, |
|
capCal, calMatrix, |
|
m_height, pCalVector, pCalMatrix); |
|
|
|
// RS41 humidity resolution of 0.1% |
|
m_humidityString = QString::number(m_humidity, 'f', 1); |
|
|
|
if (!m_humidityCalibrated) { |
|
m_humidityString = m_humidityString + "U"; // U for uncalibrated |
|
} |
|
} |
|
|
|
RS41Frame* RS41Frame::decode(const QByteArray ba) |
|
{ |
|
return new RS41Frame(ba); |
|
} |
|
|
|
int RS41Frame::getFrameLength(int frameType) |
|
{ |
|
return frameType == RS41_FRAME_STD ? RS41_LENGTH_STD : RS41_LENGTH_EXT; |
|
} |
|
|
|
RS41Subframe::RS41Subframe() : |
|
m_subframe(51*16, (char)0) |
|
{ |
|
for (int i = 0; i < 51; i++) { |
|
m_subframeValid[i] = false; |
|
} |
|
} |
|
|
|
// Update subframe with subframe data from received message |
|
void RS41Subframe::update(RS41Frame *message) |
|
{ |
|
m_subframeValid[message->m_subframeNumber] = true; |
|
int offset = message->m_subframeNumber * 16; |
|
for (int i = 0; i < 16; i++) { |
|
m_subframe[offset+i] = message->m_subframe[i]; |
|
} |
|
} |
|
|
|
// Indicate if we have all the required temperature calibration data |
|
bool RS41Subframe::hasTempCal() const |
|
{ |
|
return m_subframeValid[3] && m_subframeValid[4] && m_subframeValid[5] && m_subframeValid[6] && m_subframeValid[7]; |
|
} |
|
|
|
// Get temperature calibration data |
|
// r1, r2 - Temperature reference resistances (Ohms) |
|
// poly - Resistance to temperature 2nd order polynomial |
|
bool RS41Subframe::getTempCal(float &r1, float &r2, float *poly, float *cal) const |
|
{ |
|
if (hasTempCal()) |
|
{ |
|
r1 = getFloat(0x3d); |
|
r2 = getFloat(0x41); |
|
for (int i = 0; i < 3; i++) { |
|
poly[i] = getFloat(0x4d + i * 4); |
|
} |
|
for (int i = 0; i < 7; i++) { |
|
cal[i] = getFloat(0x59 + i * 4); |
|
} |
|
return true; |
|
} |
|
else |
|
{ |
|
// Use default values |
|
r1 = 750.0f; |
|
r2 = 1100.0f; |
|
poly[0] = -243.9108f; |
|
poly[1] = 0.187654f; |
|
poly[2] = 8.2e-06f; |
|
cal[0] = 1.279928f; |
|
for (int i = 1; i < 7; i++) { |
|
cal[i] = 0.0f; |
|
} |
|
return false; |
|
} |
|
} |
|
|
|
// Indicate if we have all the required humidty calibration data |
|
bool RS41Subframe::hasHumidityCal() const |
|
{ |
|
return m_subframeValid[4] && m_subframeValid[7] |
|
&& m_subframeValid[8] && m_subframeValid[9] && m_subframeValid[0xa] && m_subframeValid[0xb] |
|
&& m_subframeValid[0xc] && m_subframeValid[0xd] && m_subframeValid[0xe] && m_subframeValid[0xf] |
|
&& m_subframeValid[0x10] && m_subframeValid[0x11] && m_subframeValid[0x12]; |
|
} |
|
|
|
// Get humidty calibration data |
|
bool RS41Subframe::getHumidityCal(float &c1, float &c2, float *capCal, float *calMatrix) const |
|
{ |
|
if (hasHumidityCal()) |
|
{ |
|
c1 = getFloat(0x45); |
|
c2 = getFloat(0x49); |
|
for (int i = 0; i < 2; i++) { |
|
capCal[i] = getFloat(0x75 + i * 4); |
|
} |
|
for (int i = 0; i < 7*6; i++) { |
|
calMatrix[i] = getFloat(0x7d + i * 4); |
|
} |
|
return true; |
|
} |
|
else |
|
{ |
|
// Use default values |
|
c1 = 0.0f; |
|
c2 = 47.0f; |
|
capCal[0] = 45.9068f; |
|
capCal[1] = 4.92924f; |
|
static const float calMatrixDefault[7*6] = { |
|
-0.002586f, -2.24367f, 9.92294f, -3.61913f, 54.3554f, -93.3012f, |
|
51.7056f, 38.8709f, 209.437f, -378.437f, 9.17326f, 19.5301f, |
|
150.257f, -150.907f, -280.315f, 182.293f, 3247.39f, 4083.65f, |
|
-233.568f, 345.375f, 200.217f, -388.246f, -3617.66f, 0.0f, |
|
225.841f, -233.051f, 0.0f, 0.0f, 0.0f, 0.0f, |
|
-93.0635f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, |
|
0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f |
|
}; |
|
std::copy(calMatrixDefault, calMatrixDefault + 7*6, calMatrix); |
|
return false; |
|
} |
|
} |
|
|
|
// Indicate if we have all the required humidty temperature sensor calibration data |
|
bool RS41Subframe::hasHumidityTempCal() const |
|
{ |
|
return m_subframeValid[3] && m_subframeValid[4] && m_subframeValid[0x12] && m_subframeValid[0x13] && m_subframeValid[0x14]; |
|
} |
|
|
|
// Get humidty temperature sensor calibration data |
|
bool RS41Subframe::getHumidityTempCal(float &r1, float &r2, float *poly, float *cal) const |
|
{ |
|
if (hasHumidityTempCal()) |
|
{ |
|
r1 = getFloat(0x3d); |
|
r2 = getFloat(0x41); |
|
for (int i = 0; i < 3; i++) { |
|
poly[i] = getFloat(0x125 + i * 4); |
|
} |
|
for (int i = 0; i < 7; i++) { |
|
cal[i] = getFloat(0x131 + i * 4); |
|
} |
|
return true; |
|
} |
|
else |
|
{ |
|
// Use default values |
|
r1 = 750.0f; |
|
r2 = 1100.0f; |
|
poly[0] = -243.9108f; |
|
poly[1] = 0.187654f; |
|
poly[2] = 8.2e-06f; |
|
cal[0] = 1.279928f; |
|
for (int i = 1; i < 7; i++) { |
|
cal[i] = 0.0f; |
|
} |
|
return false; |
|
} |
|
} |
|
|
|
// Indicate if we have all the required pressure calibration data |
|
bool RS41Subframe::hasPressureCal() const |
|
{ |
|
return m_subframeValid[0x25] && m_subframeValid[0x26] && m_subframeValid[0x27] |
|
&& m_subframeValid[0x28] && m_subframeValid[0x29] && m_subframeValid[0x2a]; |
|
} |
|
|
|
// Get pressure calibration data |
|
bool RS41Subframe::getPressureCal(float *cal) const |
|
{ |
|
if (hasPressureCal()) |
|
{ |
|
for (int i = 0; i < 18; i++) { |
|
cal[i] = getFloat(0x25e + i * 4); |
|
} |
|
return true; |
|
} |
|
else |
|
{ |
|
// Use default values - TODO: Need to obtain from inflight device |
|
for (int i = 0; i < 18; i++) { |
|
cal[i] = 0.0f; |
|
} |
|
return false; |
|
} |
|
} |
|
|
|
// Indicate if we have all the required humidity pressure calibration data |
|
bool RS41Subframe::hasHumidityPressureCal() const |
|
{ |
|
return m_subframeValid[0x2a] && m_subframeValid[0x2b] && m_subframeValid[0x2c] |
|
&& m_subframeValid[0x2d] && m_subframeValid[0x2e] && m_subframeValid[0x2f]; |
|
} |
|
|
|
bool RS41Subframe::getHumidityPressureCal(float *vec, float *mat) const |
|
{ |
|
if (hasHumidityPressureCal()) |
|
{ |
|
for (int i = 0; i < 3; i++) { |
|
vec[i] = getFloat(0x2a6 + i * 4); |
|
} |
|
for (int i = 0; i < 12; i++) { |
|
mat[i] = getFloat(0x2ba + i * 4); |
|
} |
|
return true; |
|
} |
|
else |
|
{ |
|
// Use default values - TODO: Need to obtain from inflight device |
|
for (int i = 0; i < 3; i++) { |
|
vec[i] = 0.0f; |
|
} |
|
for (int i = 0; i < 12; i++) { |
|
mat[i] = 0.0f; |
|
} |
|
return false; |
|
} |
|
} |
|
|
|
// Get type of RS41. E.g. "RS41-SGP" |
|
QString RS41Subframe::getType() const |
|
{ |
|
if (m_subframeValid[0x21] && m_subframeValid[0x22]) |
|
{ |
|
QByteArray bytes = m_subframe.mid(0x218, 10); |
|
|
|
while ((bytes.size() > 0) && (bytes.back() == '\0')) { |
|
bytes.remove(bytes.size() - 1, 1); |
|
} |
|
|
|
return QString(bytes).trimmed(); |
|
} |
|
else |
|
{ |
|
return "RS41"; |
|
} |
|
} |
|
|
|
// Get transmission frequency in MHz |
|
QString RS41Subframe::getFrequencyMHz() const |
|
{ |
|
if (m_subframeValid[0]) |
|
{ |
|
uint8_t lower = m_subframe[2] & 0xff; |
|
uint8_t upper = m_subframe[3] & 0xff; |
|
float freq = 400.0 + (upper + (lower / 255.0)) * 0.04; |
|
return QString::number(freq, 'f', 3); |
|
} |
|
else |
|
{ |
|
return ""; |
|
} |
|
} |
|
|
|
QString RS41Subframe::getBurstKillStatus() const |
|
{ |
|
if (m_subframeValid[2]) |
|
{ |
|
uint8_t status = m_subframe[0x2b]; |
|
return status == 0 ? "Inactive" : "Active"; |
|
} |
|
else |
|
{ |
|
return ""; |
|
} |
|
} |
|
|
|
// Seconds until power-off once active |
|
QString RS41Subframe::getBurstKillTimer() const |
|
{ |
|
if (m_subframeValid[0x31]) |
|
{ |
|
uint16_t secs = getUInt16(0x316); |
|
QTime t(0, 0, 0); |
|
t = t.addSecs(secs); |
|
return t.toString("hh:mm:ss"); |
|
} |
|
else |
|
{ |
|
return ""; |
|
} |
|
} |
|
|
|
uint16_t RS41Subframe::getUInt16(int offset) const |
|
{ |
|
return (m_subframe[offset] & 0xff) | ((m_subframe[offset+1] & 0xff) << 8); |
|
} |
|
|
|
float RS41Subframe::getFloat(int offset) const |
|
{ |
|
float f; |
|
// Assumes host is little endian with 32-bit float |
|
memcpy(&f, m_subframe.data() + offset, 4); |
|
return f; |
|
}
|
|
|