piotr/edgetx-gx12
1
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects Releases Wiki Activity

M-Link telemetry (#8281)

Browse Source
This commit is contained in:
3djc
2021-03-10 19:13:36 +01:00
committed by GitHub
parent 7613f72ec5
commit b60ba23f1f
22 changed files with 276 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
radio/src/telemetry/mlink.cpp
+161
View File
@@ -0,0 +1,161 @@
/*
* Copyright (C) OpenTX
*
* 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"
struct MLinkSensor
{
const uint16_t id;
const char * name;
const TelemetryUnit unit;
const uint8_t precision;
};
const MLinkSensor mlinkSensors[] = {
{MLINK_RX_VOLTAGE, ZSTR_BATT, UNIT_VOLTS, 1},
{MLINK_VOLTAGE, ZSTR_VFAS, UNIT_VOLTS, 1},
{MLINK_CURRENT, ZSTR_CURR, UNIT_AMPS, 1},
{MLINK_VARIO, ZSTR_VSPD, UNIT_METERS_PER_SECOND, 1},
{MLINK_SPEED, ZSTR_SPEED, UNIT_KMH, 1},
{MLINK_RPM, ZSTR_RPM, UNIT_RPMS, 0},
{MLINK_TEMP, ZSTR_TEMP1, UNIT_CELSIUS, 1},
{MLINK_HEADING, ZSTR_HDG, UNIT_DEGREE, 1},
{MLINK_ALT, ZSTR_ALT , UNIT_METERS, 0},
{MLINK_FUEL, ZSTR_FUEL, UNIT_PERCENT, 0},
{MLINK_CAPACITY, ZSTR_CAPACITY, UNIT_MAH, 0},
{MLINK_FLOW, ZSTR_FLOW, UNIT_MILLILITERS, 0},
{MLINK_DISTANCE, ZSTR_DIST, UNIT_KM, 1},
{MLINK_LQI, ZSTR_RSSI, UNIT_RAW, 0},
{MLINK_LOSS, ZSTR_LOSS, UNIT_RAW, 0},
{MLINK_TX_RSSI, ZSTR_TX_RSSI, UNIT_RAW, 0},
{MLINK_TX_LQI, ZSTR_TX_QUALITY, UNIT_RAW, 0},
};
const MLinkSensor * getMLinkSensor(uint16_t id)
{
for (const MLinkSensor * sensor = mlinkSensors; sensor->id; sensor++) {
if (id == sensor->id)
return sensor;
}
return nullptr;
}
void processMLinkPacket(const uint8_t * packet)
{
const uint8_t * data = packet + 2;
// Multi telem
setTelemetryValue(PROTOCOL_TELEMETRY_MLINK, MLINK_TX_RSSI, 0, 0, (packet[0] * 100) / 31, UNIT_RAW, 0);
setTelemetryValue(PROTOCOL_TELEMETRY_MLINK, MLINK_TX_LQI, 0, 0, packet[1], UNIT_RAW, 0);
// M-Link telem
if (data[0] == 0x13) { // Telemetry type RX-9
for (uint8_t i = 1; i < 5; i += 3) { //2 sensors per packet
int32_t val = (int16_t )(data[i + 2] << 8 | data[i + 1]);
val = val >> 1; // remove alarm flag
uint8_t adress = (data[i] & 0xF0) >> 4;
switch (data[i] & 0x0F) {
case MLINK_VOLTAGE:
if ((data[i] & 0xF0) == 0x00){
setTelemetryValue(PROTOCOL_TELEMETRY_MLINK, MLINK_RX_VOLTAGE, 0, adress, val, UNIT_VOLTS, 1);
}
else {
setTelemetryValue(PROTOCOL_TELEMETRY_MLINK, MLINK_VOLTAGE, 0, adress, val, UNIT_VOLTS, 1);
}
break;
case MLINK_CURRENT:
setTelemetryValue(PROTOCOL_TELEMETRY_MLINK, MLINK_CURRENT, 0, adress, val, UNIT_AMPS, 1);
break;
case MLINK_VARIO:
setTelemetryValue(PROTOCOL_TELEMETRY_MLINK, MLINK_VARIO, 0, adress, val, UNIT_METERS_PER_SECOND, 1);
break;
case MLINK_SPEED:
setTelemetryValue(PROTOCOL_TELEMETRY_MLINK, MLINK_SPEED, 0, adress, val, UNIT_KMH, 1);
break;
case MLINK_RPM:
if (val < 0) {
val = -val * 10;
}
else {
val = val * 100;
}
setTelemetryValue(PROTOCOL_TELEMETRY_MLINK, MLINK_RPM, 0, adress, val, UNIT_RPMS, 0);
break;
case MLINK_TEMP:
setTelemetryValue(PROTOCOL_TELEMETRY_MLINK, MLINK_TEMP, 0, adress, val, UNIT_CELSIUS, 1);
break;
case MLINK_HEADING:
setTelemetryValue(PROTOCOL_TELEMETRY_MLINK, MLINK_HEADING, 0, adress, val, UNIT_DEGREE, 1);
break;
case MLINK_ALT:
setTelemetryValue(PROTOCOL_TELEMETRY_MLINK, MLINK_ALT, 0, adress, val, UNIT_METERS, 0);
break;
case MLINK_FUEL:
setTelemetryValue(PROTOCOL_TELEMETRY_MLINK, MLINK_FUEL, 0, adress, val, UNIT_PERCENT, 0);
break;
case MLINK_CAPACITY:
setTelemetryValue(PROTOCOL_TELEMETRY_MLINK, MLINK_CAPACITY, 0, adress, val, UNIT_MAH, 0);
break;
case MLINK_FLOW:
setTelemetryValue(PROTOCOL_TELEMETRY_MLINK, MLINK_FLOW, 0, adress, val, UNIT_MILLILITERS, 0);
break;
case MLINK_DISTANCE:
setTelemetryValue(PROTOCOL_TELEMETRY_MLINK, MLINK_DISTANCE, 0, adress, val, UNIT_KM, 1);
break;
case MLINK_LQI:
uint8_t mlinkRssi = data[i + 1] >> 1;
setTelemetryValue(PROTOCOL_TELEMETRY_MLINK, MLINK_LQI, 0, 0, mlinkRssi, UNIT_RAW, 0);
telemetryData.rssi.set(mlinkRssi);
if (mlinkRssi > 0) {
telemetryStreaming = TELEMETRY_TIMEOUT10ms;
}
break;
}
}
}
else if (packet[2] == 0x03) { // Telemetry type RX-5
uint16_t mlinkRssi = (packet[4] * 100) / 35;
setTelemetryValue(PROTOCOL_TELEMETRY_MLINK, MLINK_LQI, 0, 0, mlinkRssi, UNIT_RAW, 0);
telemetryData.rssi.set(mlinkRssi);
if (mlinkRssi > 0) {
telemetryStreaming = TELEMETRY_TIMEOUT10ms;
}
setTelemetryValue(PROTOCOL_TELEMETRY_MLINK, MLINK_LOSS, 0, 0, packet[7], UNIT_RAW, 0);
}
}
void mlinkSetDefault(int index, uint16_t id, uint8_t subId, uint8_t instance)
{
TelemetrySensor &telemetrySensor = g_model.telemetrySensors[index];
telemetrySensor.id = id;
telemetrySensor.subId = subId;
telemetrySensor.instance = instance;
const MLinkSensor * sensor = getMLinkSensor(id);
if (sensor) {
TelemetryUnit unit = sensor->unit;
uint8_t prec = min<uint8_t>(2, sensor->precision);
telemetrySensor.init(sensor->name, unit, prec);
if (unit == UNIT_RPMS) {
telemetrySensor.custom.ratio = 1;
telemetrySensor.custom.offset = 1;
}
}
else {
telemetrySensor.init(id);
}
storageDirty(EE_MODEL);
}