mirror of
https://github.com/f4exb/sdrangel.git
synced 2024-11-04 16:01:14 -05:00
780 lines
22 KiB
C++
780 lines
22 KiB
C++
///////////////////////////////////////////////////////////////////////////////////
|
|
// 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 <QVector3D>
|
|
|
|
#include "util/radiosonde.h"
|
|
#include "util/coordinates.h"
|
|
|
|
RS41Frame::RS41Frame(const QByteArray ba) :
|
|
m_statusValid(false),
|
|
m_batteryVoltage(0.0),
|
|
m_pcbTemperature(0),
|
|
m_humiditySensorHeating(0),
|
|
m_transmitPower(0),
|
|
m_maxSubframeNumber(0),
|
|
m_subframeNumber(0),
|
|
m_measValid(false),
|
|
m_gpsInfoValid(false),
|
|
m_posValid(false),
|
|
m_latitude(0.0),
|
|
m_longitude(0.0),
|
|
m_height(0.0),
|
|
m_bytes(ba),
|
|
m_pressureCalibrated(false),
|
|
m_temperatureCalibrated(false),
|
|
m_humidityTemperatureCalibrated(false),
|
|
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_EMPTY:
|
|
break;
|
|
}
|
|
i += 2 + blockLength + 2; // ID, length, data, CRC
|
|
}
|
|
}
|
|
|
|
QString RS41Frame::toHex()
|
|
{
|
|
return m_bytes.toHex();
|
|
}
|
|
|
|
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 = getUInt16(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);
|
|
}
|
|
}
|
|
|
|
// 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, float *poly, 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, float *capCal, float *matrixCal, float height, float *vectorPCal, 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, 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;
|
|
}
|