mirror of
https://github.com/opentx/opentx.git
synced 2025-07-24 16:55:20 +03:00
52435703a0
* Rename EXT option to TELEMETRY * Change test accordingly * Rename FRSKY to TELEMETRY_FRSKY * Rename ARDUPILOT to TELEMETRY_ARDUPILOT * More renames (JETI, MAVLINK, NMEA) * Fixes to make commit-test happy * Further tests added to commit-test * Cleanup * Rename EXTSTD to TELEMETRY_NONE
670 lines
20 KiB
C++
670 lines
20 KiB
C++
/*
|
||
* Copyright (C) OpenTX
|
||
*
|
||
* Based on code named
|
||
* th9x - http://code.google.com/p/th9x
|
||
* er9x - http://code.google.com/p/er9x
|
||
* gruvin9x - http://code.google.com/p/gruvin9x
|
||
*
|
||
* License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
|
||
*
|
||
* This program is free software; you can redistribute it and/or modify
|
||
* it under the terms of the GNU General Public License version 2 as
|
||
* published by the Free Software Foundation.
|
||
*
|
||
* 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 for more details.
|
||
*/
|
||
|
||
#include "opentx.h"
|
||
|
||
TelemetryItem telemetryItems[MAX_SENSORS];
|
||
uint8_t allowNewSensors;
|
||
|
||
// TODO in maths
|
||
uint32_t getDistFromEarthAxis(int32_t latitude)
|
||
{
|
||
uint32_t lat = abs(latitude) / 10000;
|
||
uint32_t angle2 = (lat*lat) / 10000;
|
||
uint32_t angle4 = angle2 * angle2;
|
||
return 139*(((uint32_t)10000000-((angle2*(uint32_t)123370)/81)+(angle4/25))/12500);
|
||
}
|
||
|
||
void TelemetryItem::setValue(const TelemetrySensor & sensor, int32_t val, uint32_t unit, uint32_t prec)
|
||
{
|
||
int32_t newVal = val;
|
||
|
||
if (unit == UNIT_CELLS) {
|
||
uint32_t data = uint32_t(newVal);
|
||
uint8_t cellsCount = (data >> 24);
|
||
uint8_t cellIndex = ((data >> 16) & 0x0F);
|
||
uint16_t cellValue = (data & 0xFFFF);
|
||
if (cellsCount == 0) {
|
||
cellsCount = (cellIndex >= cells.count ? cellIndex + 1 : cells.count);
|
||
if (cellsCount != cells.count) {
|
||
clear();
|
||
cells.count = cellsCount;
|
||
// we skip this round as we are not sure we received all cells values
|
||
return;
|
||
}
|
||
}
|
||
else if (cellsCount != cells.count) {
|
||
clear();
|
||
cells.count = cellsCount;
|
||
}
|
||
cells.values[cellIndex].set(cellValue);
|
||
if (cellIndex+1 == cells.count) {
|
||
newVal = 0;
|
||
for (int i=0; i<cellsCount; i++) {
|
||
if (cells.values[i].state) {
|
||
newVal += cells.values[i].value;
|
||
}
|
||
else {
|
||
// we didn't receive all cells values
|
||
return;
|
||
}
|
||
}
|
||
newVal = sensor.getValue(newVal, UNIT_VOLTS, 2);
|
||
}
|
||
else {
|
||
// we didn't receive all cells values
|
||
return;
|
||
}
|
||
}
|
||
else if (unit == UNIT_DATETIME) {
|
||
uint32_t data = uint32_t(newVal);
|
||
if (data & 0x000000ff) {
|
||
datetime.year = (uint16_t) ((data & 0xff000000) >> 24);
|
||
datetime.month = (uint8_t) ((data & 0x00ff0000) >> 16);
|
||
datetime.day = (uint8_t) ((data & 0x0000ff00) >> 8);
|
||
if (datetime.year != 0) {
|
||
datetime.datestate = 1;
|
||
}
|
||
#if defined(RTCLOCK)
|
||
if (g_eeGeneral.adjustRTC && (datetime.datestate == 1)) {
|
||
struct gtm t;
|
||
gettime(&t);
|
||
t.tm_year = datetime.year+4;
|
||
t.tm_mon = datetime.month-1;
|
||
t.tm_mday = datetime.day;
|
||
rtcSetTime(&t);
|
||
}
|
||
#endif
|
||
}
|
||
else {
|
||
datetime.hour = ((uint8_t) ((data & 0xff000000) >> 24) + g_eeGeneral.timezone + 24) % 24;
|
||
datetime.min = (uint8_t) ((data & 0x00ff0000) >> 16);
|
||
datetime.sec = (uint16_t) ((data & 0x0000ff00) >> 8);
|
||
if (datetime.datestate == 1) {
|
||
datetime.timestate = 1;
|
||
}
|
||
#if defined(RTCLOCK)
|
||
if (g_eeGeneral.adjustRTC && datetime.datestate == 1) {
|
||
struct gtm t;
|
||
gettime(&t);
|
||
if (abs((t.tm_hour-datetime.hour)*3600 + (t.tm_min-datetime.min)*60 + (t.tm_sec-datetime.sec)) > 20) {
|
||
// we adjust RTC only if difference is > 20 seconds
|
||
t.tm_hour = datetime.hour;
|
||
t.tm_min = datetime.min;
|
||
t.tm_sec = datetime.sec;
|
||
g_rtcTime = gmktime(&t); // update local timestamp and get wday calculated
|
||
rtcSetTime(&t);
|
||
}
|
||
}
|
||
#endif
|
||
}
|
||
if (datetime.year == 0) {
|
||
return;
|
||
}
|
||
newVal = 0;
|
||
}
|
||
else if (unit == UNIT_GPS_LATITUDE) {
|
||
if (!pilotLatitude) {
|
||
pilotLatitude = newVal;
|
||
distFromEarthAxis = getDistFromEarthAxis(newVal);
|
||
}
|
||
gps.latitude = newVal;
|
||
lastReceived = now();
|
||
return;
|
||
}
|
||
else if (unit == UNIT_GPS_LONGITUDE) {
|
||
if (!pilotLongitude) {
|
||
pilotLongitude = newVal;
|
||
}
|
||
gps.longitude = newVal;
|
||
lastReceived = now();
|
||
return;
|
||
}
|
||
else if (unit == UNIT_DATETIME_YEAR) {
|
||
datetime.year = newVal;
|
||
return;
|
||
}
|
||
else if (unit == UNIT_DATETIME_DAY_MONTH) {
|
||
uint32_t data = uint32_t(newVal);
|
||
datetime.month = data >> 8;
|
||
datetime.day = data & 0xFF;
|
||
datetime.datestate = 1;
|
||
return;
|
||
}
|
||
else if (unit == UNIT_DATETIME_HOUR_MIN) {
|
||
uint32_t data = uint32_t(newVal);
|
||
datetime.hour = ((data & 0xFF) + g_eeGeneral.timezone + 24) % 24;
|
||
datetime.min = data >> 8;
|
||
}
|
||
else if (unit == UNIT_DATETIME_SEC) {
|
||
datetime.sec = newVal & 0xFF;
|
||
datetime.timestate = 1;
|
||
newVal = 0;
|
||
}
|
||
else if (unit == UNIT_RPMS) {
|
||
if (sensor.custom.ratio != 0) {
|
||
newVal = (newVal * sensor.custom.offset) / sensor.custom.ratio;
|
||
}
|
||
}
|
||
else if (unit == UNIT_TEXT) {
|
||
*((uint32_t*)&text[prec]) = newVal;
|
||
lastReceived = now();
|
||
return;
|
||
}
|
||
else {
|
||
newVal = sensor.getValue(newVal, unit, prec);
|
||
if (sensor.autoOffset) {
|
||
if (!isAvailable()) {
|
||
std.offsetAuto = -newVal;
|
||
}
|
||
newVal += std.offsetAuto;
|
||
}
|
||
if (sensor.filter) {
|
||
if (!isAvailable()) {
|
||
for (int i=0; i<TELEMETRY_AVERAGE_COUNT; i++) {
|
||
std.filterValues[i] = newVal;
|
||
}
|
||
}
|
||
else {
|
||
// Calculate the average from values[] and value,
|
||
// also shift readings in values [] array.
|
||
// There is a possibility of value overflow in `sum` but
|
||
// in reality no sensor value should be so big to cause it.
|
||
int32_t sum = std.filterValues[0];
|
||
for (int i=0; i<TELEMETRY_AVERAGE_COUNT-1; i++) {
|
||
int32_t tmp = std.filterValues[i+1];
|
||
std.filterValues[i] = tmp;
|
||
sum += tmp;
|
||
}
|
||
std.filterValues[TELEMETRY_AVERAGE_COUNT-1] = newVal;
|
||
sum += newVal;
|
||
newVal = sum/(TELEMETRY_AVERAGE_COUNT+1);
|
||
}
|
||
}
|
||
}
|
||
|
||
if (!isAvailable()) {
|
||
valueMin = newVal;
|
||
valueMax = newVal;
|
||
}
|
||
else if (newVal < valueMin) {
|
||
valueMin = newVal;
|
||
}
|
||
else if (newVal > valueMax) {
|
||
valueMax = newVal;
|
||
if (sensor.unit == UNIT_VOLTS) {
|
||
valueMin = newVal; // the batt was changed
|
||
}
|
||
}
|
||
|
||
for (int i=0; i<MAX_SENSORS; i++) {
|
||
TelemetrySensor & it = g_model.telemetrySensors[i];
|
||
if (it.type == TELEM_TYPE_CALCULATED && it.formula == TELEM_FORMULA_TOTALIZE && &g_model.telemetrySensors[it.consumption.source-1] == &sensor) {
|
||
TelemetryItem & item = telemetryItems[i];
|
||
int32_t increment = it.getValue(val, unit, prec);
|
||
item.setValue(it, item.value+increment, it.unit, it.prec);
|
||
}
|
||
}
|
||
|
||
value = newVal;
|
||
lastReceived = now();
|
||
}
|
||
|
||
bool TelemetryItem::isAvailable()
|
||
{
|
||
return (lastReceived != TELEMETRY_VALUE_UNAVAILABLE);
|
||
}
|
||
|
||
bool TelemetryItem::isFresh()
|
||
{
|
||
return (lastReceived < TELEMETRY_VALUE_TIMER_CYCLE) && (uint8_t(now() - lastReceived) < 2);
|
||
}
|
||
|
||
bool TelemetryItem::isOld()
|
||
{
|
||
return (lastReceived == TELEMETRY_VALUE_OLD);
|
||
}
|
||
|
||
void TelemetryItem::per10ms(const TelemetrySensor & sensor)
|
||
{
|
||
switch (sensor.formula) {
|
||
case TELEM_FORMULA_CONSUMPTION:
|
||
if (sensor.consumption.source) {
|
||
TelemetrySensor & currentSensor = g_model.telemetrySensors[sensor.consumption.source-1];
|
||
TelemetryItem & currentItem = telemetryItems[sensor.consumption.source-1];
|
||
if (!currentItem.isAvailable()) {
|
||
return;
|
||
}
|
||
else if (currentItem.isOld()) {
|
||
lastReceived = TELEMETRY_VALUE_OLD;
|
||
return;
|
||
}
|
||
int32_t current = convertTelemetryValue(currentItem.value, currentSensor.unit, currentSensor.prec, UNIT_AMPS, 1);
|
||
currentItem.consumption.prescale += current;
|
||
if (currentItem.consumption.prescale >= 3600) {
|
||
currentItem.consumption.prescale -= 3600;
|
||
setValue(sensor, value+1, sensor.unit, sensor.prec);
|
||
}
|
||
lastReceived = now();
|
||
}
|
||
break;
|
||
|
||
default:
|
||
break;
|
||
}
|
||
}
|
||
|
||
void TelemetryItem::eval(const TelemetrySensor & sensor)
|
||
{
|
||
switch (sensor.formula) {
|
||
case TELEM_FORMULA_CELL:
|
||
if (sensor.cell.source) {
|
||
TelemetryItem & cellsItem = telemetryItems[sensor.cell.source-1];
|
||
if (cellsItem.isOld()) {
|
||
lastReceived = TELEMETRY_VALUE_OLD;
|
||
}
|
||
else {
|
||
unsigned int index = sensor.cell.index;
|
||
if (index == TELEM_CELL_INDEX_LOWEST || index == TELEM_CELL_INDEX_HIGHEST || index == TELEM_CELL_INDEX_DELTA) {
|
||
unsigned int lowest=0, highest=0;
|
||
for (int i=0; i<cellsItem.cells.count; i++) {
|
||
if (cellsItem.cells.values[i].state) {
|
||
if (!lowest || cellsItem.cells.values[i].value < cellsItem.cells.values[lowest-1].value)
|
||
lowest = i+1;
|
||
if (!highest || cellsItem.cells.values[i].value > cellsItem.cells.values[highest-1].value)
|
||
highest = i+1;
|
||
}
|
||
else {
|
||
lowest = highest = 0;
|
||
}
|
||
}
|
||
if (lowest) {
|
||
switch (index) {
|
||
case TELEM_CELL_INDEX_LOWEST:
|
||
setValue(sensor, cellsItem.cells.values[lowest-1].value, UNIT_VOLTS, 2);
|
||
break;
|
||
case TELEM_CELL_INDEX_HIGHEST:
|
||
setValue(sensor, cellsItem.cells.values[highest-1].value, UNIT_VOLTS, 2);
|
||
break;
|
||
case TELEM_CELL_INDEX_DELTA:
|
||
setValue(sensor, cellsItem.cells.values[highest-1].value - cellsItem.cells.values[lowest-1].value, UNIT_VOLTS, 2);
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
else {
|
||
index -= 1;
|
||
if (index < cellsItem.cells.count && cellsItem.cells.values[index].state) {
|
||
setValue(sensor, cellsItem.cells.values[index].value, UNIT_VOLTS, 2);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
break;
|
||
|
||
case TELEM_FORMULA_DIST:
|
||
if (sensor.dist.gps) {
|
||
TelemetryItem gpsItem = telemetryItems[sensor.dist.gps-1];
|
||
TelemetryItem * altItem = NULL;
|
||
if (!gpsItem.isAvailable()) {
|
||
return;
|
||
}
|
||
else if (gpsItem.isOld()) {
|
||
lastReceived = TELEMETRY_VALUE_OLD;
|
||
return;
|
||
}
|
||
if (sensor.dist.alt) {
|
||
altItem = &telemetryItems[sensor.dist.alt-1];
|
||
if (!altItem->isAvailable()) {
|
||
return;
|
||
}
|
||
else if (altItem->isOld()) {
|
||
lastReceived = TELEMETRY_VALUE_OLD;
|
||
return;
|
||
}
|
||
}
|
||
uint32_t angle = abs(gpsItem.gps.latitude - gpsItem.pilotLatitude);
|
||
uint32_t dist = EARTH_RADIUS * angle / 1000000;
|
||
uint32_t result = dist*dist;
|
||
|
||
angle = abs(gpsItem.gps.longitude - gpsItem.pilotLongitude);
|
||
dist = gpsItem.distFromEarthAxis * angle / 1000000;
|
||
result += dist*dist;
|
||
|
||
if (altItem) {
|
||
dist = abs(altItem->value) / g_model.telemetrySensors[sensor.dist.alt-1].getPrecDivisor();
|
||
result += dist*dist;
|
||
}
|
||
|
||
setValue(sensor, isqrt32(result), UNIT_METERS);
|
||
}
|
||
break;
|
||
|
||
case TELEM_FORMULA_ADD:
|
||
case TELEM_FORMULA_AVERAGE:
|
||
case TELEM_FORMULA_MIN:
|
||
case TELEM_FORMULA_MAX:
|
||
case TELEM_FORMULA_MULTIPLY:
|
||
{
|
||
int32_t value=0, count=0, available=0, maxitems=4, mulprec=0;
|
||
if (sensor.formula == TELEM_FORMULA_MULTIPLY) {
|
||
maxitems = 2;
|
||
value = 1;
|
||
}
|
||
for (int i=0; i<maxitems; i++) {
|
||
int8_t source = sensor.calc.sources[i];
|
||
if (source) {
|
||
unsigned int index = abs(source)-1;
|
||
TelemetrySensor & telemetrySensor = g_model.telemetrySensors[index];
|
||
TelemetryItem & telemetryItem = telemetryItems[index];
|
||
if (sensor.formula == TELEM_FORMULA_AVERAGE) {
|
||
if (telemetryItem.isAvailable())
|
||
available = 1;
|
||
else
|
||
continue;
|
||
if (telemetryItem.isOld())
|
||
continue;
|
||
}
|
||
else {
|
||
if (!telemetryItem.isAvailable()) {
|
||
return;
|
||
}
|
||
else if (telemetryItem.isOld()) {
|
||
lastReceived = TELEMETRY_VALUE_OLD;
|
||
return;
|
||
}
|
||
}
|
||
int32_t sensorValue = telemetryItem.value;
|
||
if (source < 0)
|
||
sensorValue = -sensorValue;
|
||
count += 1;
|
||
if (sensor.formula == TELEM_FORMULA_MULTIPLY) {
|
||
mulprec += telemetrySensor.prec;
|
||
value *= convertTelemetryValue(sensorValue, telemetrySensor.unit, 0, sensor.unit, 0);
|
||
}
|
||
else {
|
||
sensorValue = convertTelemetryValue(sensorValue, telemetrySensor.unit, telemetrySensor.prec, sensor.unit, sensor.prec);
|
||
if (sensor.formula == TELEM_FORMULA_MIN)
|
||
value = (count==1 ? sensorValue : min<int32_t>(value, sensorValue));
|
||
else if (sensor.formula == TELEM_FORMULA_MAX)
|
||
value = (count==1 ? sensorValue : max<int32_t>(value, sensorValue));
|
||
else
|
||
value += sensorValue;
|
||
}
|
||
}
|
||
}
|
||
if (sensor.formula == TELEM_FORMULA_AVERAGE) {
|
||
if (count == 0) {
|
||
if (available)
|
||
lastReceived = TELEMETRY_VALUE_OLD;
|
||
return;
|
||
}
|
||
else {
|
||
value = (value + count/2) / count;
|
||
}
|
||
}
|
||
else if (sensor.formula == TELEM_FORMULA_MULTIPLY) {
|
||
if (count == 0)
|
||
return;
|
||
value = convertTelemetryValue(value, sensor.unit, mulprec, sensor.unit, sensor.prec);
|
||
}
|
||
setValue(sensor, value, sensor.unit, sensor.prec);
|
||
break;
|
||
}
|
||
|
||
default:
|
||
break;
|
||
}
|
||
}
|
||
|
||
void delTelemetryIndex(uint8_t index)
|
||
{
|
||
memclear(&g_model.telemetrySensors[index], sizeof(TelemetrySensor));
|
||
telemetryItems[index].clear();
|
||
storageDirty(EE_MODEL);
|
||
}
|
||
|
||
int availableTelemetryIndex()
|
||
{
|
||
for (int index=0; index<MAX_SENSORS; index++) {
|
||
TelemetrySensor & telemetrySensor = g_model.telemetrySensors[index];
|
||
if (!telemetrySensor.isAvailable()) {
|
||
return index;
|
||
}
|
||
}
|
||
return -1;
|
||
}
|
||
|
||
int lastUsedTelemetryIndex()
|
||
{
|
||
for (int index=MAX_SENSORS-1; index>=0; index--) {
|
||
TelemetrySensor & telemetrySensor = g_model.telemetrySensors[index];
|
||
if (telemetrySensor.isAvailable()) {
|
||
return index;
|
||
}
|
||
}
|
||
return -1;
|
||
}
|
||
|
||
void setTelemetryValue(TelemetryProtocol protocol, uint16_t id, uint8_t subId, uint8_t instance, int32_t value, uint32_t unit, uint32_t prec)
|
||
{
|
||
bool available = false;
|
||
|
||
for (int index=0; index<MAX_SENSORS; index++) {
|
||
TelemetrySensor & telemetrySensor = g_model.telemetrySensors[index];
|
||
if (telemetrySensor.type == TELEM_TYPE_CUSTOM && telemetrySensor.id == id && telemetrySensor.subId == subId && (telemetrySensor.instance == instance || g_model.ignoreSensorIds)) {
|
||
telemetryItems[index].setValue(telemetrySensor, value, unit, prec);
|
||
available = true;
|
||
// we continue search here, because sensors can share the same id and instance
|
||
}
|
||
}
|
||
|
||
if (available || !allowNewSensors) {
|
||
return;
|
||
}
|
||
|
||
int index = availableTelemetryIndex();
|
||
if (index >= 0) {
|
||
switch (protocol) {
|
||
#if defined(TELEMETRY_FRSKY_SPORT)
|
||
case TELEM_PROTO_FRSKY_SPORT:
|
||
frskySportSetDefault(index, id, subId, instance);
|
||
break;
|
||
#endif
|
||
#if defined(TELEMETRY_FRSKY)
|
||
case TELEM_PROTO_FRSKY_D:
|
||
frskyDSetDefault(index, id);
|
||
break;
|
||
#endif
|
||
#if defined(CROSSFIRE)
|
||
case TELEM_PROTO_CROSSFIRE:
|
||
crossfireSetDefault(index, id, instance);
|
||
break;
|
||
#endif
|
||
#if defined(MULTIMODULE)
|
||
case TELEM_PROTO_SPEKTRUM:
|
||
spektrumSetDefault(index, id, subId, instance);
|
||
break;
|
||
#endif
|
||
default:
|
||
return;
|
||
}
|
||
telemetryItems[index].setValue(g_model.telemetrySensors[index], value, unit, prec);
|
||
}
|
||
else {
|
||
POPUP_WARNING(STR_TELEMETRYFULL);
|
||
}
|
||
}
|
||
|
||
void TelemetrySensor::init(const char * label, uint8_t unit, uint8_t prec)
|
||
{
|
||
memclear(this->label, TELEM_LABEL_LEN);
|
||
strncpy(this->label, label, TELEM_LABEL_LEN);
|
||
this->unit = unit;
|
||
if (prec > 1 && (IS_DISTANCE_UNIT(unit) || IS_SPEED_UNIT(unit))) {
|
||
// 2 digits precision is not needed here
|
||
prec = 1;
|
||
}
|
||
this->prec = prec;
|
||
}
|
||
|
||
void TelemetrySensor::init(uint16_t id)
|
||
{
|
||
char label[4];
|
||
label[0] = hex2zchar((id & 0xf000) >> 12);
|
||
label[1] = hex2zchar((id & 0x0f00) >> 8);
|
||
label[2] = hex2zchar((id & 0x00f0) >> 4);
|
||
label[3] = hex2zchar((id & 0x000f) >> 0);
|
||
init(label);
|
||
}
|
||
|
||
bool TelemetrySensor::isAvailable() const
|
||
{
|
||
return ZLEN(label) > 0;
|
||
}
|
||
|
||
PACK(typedef struct {
|
||
uint8_t unitFrom;
|
||
uint8_t unitTo;
|
||
int16_t multiplier;
|
||
int16_t divisor;
|
||
}) UnitConversionRule;
|
||
|
||
const UnitConversionRule unitConversionTable[] = {
|
||
/* unitFrom unitTo multiplier divisor */
|
||
{ UNIT_METERS, UNIT_FEET, 105, 32},
|
||
{ UNIT_METERS_PER_SECOND, UNIT_FEET_PER_SECOND, 105, 32},
|
||
|
||
{ UNIT_KTS, UNIT_KMH, 1852, 1000}, // 1 knot = 1.85200 kilometers per hour
|
||
{ UNIT_KTS, UNIT_MPH, 1151, 1000}, // 1 knot = 1.15077945 miles per hour
|
||
{ UNIT_KTS, UNIT_METERS_PER_SECOND, 1000, 1944}, // 1 knot = 0.514444444 meters / second (divide with 1.94384449)
|
||
{ UNIT_KTS, UNIT_FEET_PER_SECOND, 1688, 1000}, // 1 knot = 1.68780986 feet per second
|
||
|
||
{ UNIT_KMH, UNIT_KTS, 1000, 1852}, // 1 km/h = 0.539956803 knots (divide with 1.85200)
|
||
{ UNIT_KMH, UNIT_MPH, 1000, 1609}, // 1 km/h = 0.621371192 miles per hour (divide with 1.60934400)
|
||
{ UNIT_KMH, UNIT_METERS_PER_SECOND, 10, 36}, // 1 km/h = 0.277777778 meters / second (divide with 3.6)
|
||
{ UNIT_KMH, UNIT_FEET_PER_SECOND, 911, 1000}, // 1 km/h = 0.911344415 feet per second
|
||
|
||
{ UNIT_MILLILITERS, UNIT_FLOZ, 100, 2957},
|
||
{ 0, 0, 0, 0} // termination
|
||
};
|
||
|
||
int32_t convertTelemetryValue(int32_t value, uint8_t unit, uint8_t prec, uint8_t destUnit, uint8_t destPrec)
|
||
{
|
||
for (int i=prec; i<destPrec; i++)
|
||
value *= 10;
|
||
|
||
if (unit == UNIT_CELSIUS) {
|
||
if (destUnit == UNIT_FAHRENHEIT) {
|
||
// T(°F) = T(°C)×1,8 + 32
|
||
value = 32 + (value*18) / 10;
|
||
}
|
||
} else if (unit == UNIT_FAHRENHEIT) {
|
||
if (destUnit == UNIT_CELSIUS) {
|
||
value = (value - 32) * 10/18;
|
||
}
|
||
}
|
||
else {
|
||
const UnitConversionRule * p = unitConversionTable;
|
||
while (p->divisor) {
|
||
if (p->unitFrom == unit && p->unitTo == destUnit) {
|
||
value = (value * (int32_t)p->multiplier) / (int32_t)p->divisor;
|
||
break;
|
||
}
|
||
++p;
|
||
}
|
||
}
|
||
|
||
for (int i=destPrec; i<prec; i++)
|
||
value /= 10;
|
||
|
||
return value;
|
||
}
|
||
|
||
int32_t TelemetrySensor::getValue(int32_t value, uint8_t unit, uint8_t prec) const
|
||
{
|
||
if (type == TELEM_TYPE_CUSTOM && custom.ratio) {
|
||
if (this->prec == 2) {
|
||
value *= 10;
|
||
prec = 2;
|
||
}
|
||
else {
|
||
prec = 1;
|
||
}
|
||
value = (custom.ratio * value + 122) / 255;
|
||
}
|
||
|
||
value = convertTelemetryValue(value, unit, prec, this->unit, this->prec);
|
||
|
||
if (type == TELEM_TYPE_CUSTOM) {
|
||
value += custom.offset;
|
||
if (value < 0 && onlyPositive) {
|
||
value = 0;
|
||
}
|
||
}
|
||
|
||
return value;
|
||
}
|
||
|
||
bool TelemetrySensor::isConfigurable() const
|
||
{
|
||
if (type == TELEM_TYPE_CALCULATED) {
|
||
if (formula >= TELEM_FORMULA_CELL) {
|
||
return false;
|
||
}
|
||
}
|
||
else {
|
||
if (unit >= UNIT_FIRST_VIRTUAL) {
|
||
return false;
|
||
}
|
||
}
|
||
return true;
|
||
}
|
||
|
||
bool TelemetrySensor::isPrecConfigurable() const
|
||
{
|
||
if (isConfigurable()) {
|
||
return true;
|
||
}
|
||
else if (unit == UNIT_CELLS) {
|
||
return true;
|
||
}
|
||
else {
|
||
return false;
|
||
}
|
||
}
|
||
|
||
int32_t TelemetrySensor::getPrecMultiplier() const
|
||
{
|
||
/*
|
||
Important: the return type must be signed, otherwise
|
||
mathematic operations with a negative telemetry value won't work
|
||
*/
|
||
if (prec == 2) return 1;
|
||
if (prec == 1) return 10;
|
||
return 100;
|
||
}
|
||
|
||
int32_t TelemetrySensor::getPrecDivisor() const
|
||
{
|
||
if (prec == 2) return 100;
|
||
if (prec == 1) return 10;
|
||
return 1;
|
||
}
|