?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
Companion: Russian UI (#7180)
[opentx.git] / radio / src / telemetry / flysky_ibus.cpp
blob0869fb0dae5693ecfeac6dd330c221d1a4b7551d
1 /*
2 * Copyright (C) OpenTX
3 *
4 * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
5 *
6 * This program is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License version 2 as
8 * published by the Free Software Foundation.
9 *
10 * This program is distributed in the hope that it will be useful,
11 * but WITHOUT ANY WARRANTY; without even the implied warranty of
12 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 * GNU General Public License for more details.
14 */
15
16 #include "opentx.h"
17
18 /*
19 * TXID + RXID are already skipped in MULTI module to save memory+transmission time, format from Multi is:
20 * AA or AC | TX_RSSI | sensor ...
21 */
22
23
24 #define FLYSKY_TELEMETRY_LENGTH (2+7*4) // Should it be 2+7*6???
25 #define ALT_PRECISION 15
26 #define R_DIV_G_MUL_10_Q15 (uint64_t)9591506
27 #define INV_LOG2_E_Q1DOT31 (uint64_t)0x58b90bfc // Inverse log base 2 of e
28 #define PRESSURE_MASK 0x7FFFF
29
30 struct FlySkySensor
31 {
32 const uint16_t id;
33 const char * name;
34 const TelemetryUnit unit;
35 const uint8_t precision;
36 };
37
38 // telemetry sensors ID
39 enum
40 {
41 AFHDS2A_ID_VOLTAGE = 0x00, // Internal Voltage
42 AFHDS2A_ID_TEMPERATURE = 0x01, // Temperature
43 AFHDS2A_ID_MOT = 0x02, // RPM
44 AFHDS2A_ID_EXTV = 0x03, // External Voltage
45 AFHDS2A_ID_CELL_VOLTAGE = 0x04, // Avg Cell voltage
46 AFHDS2A_ID_BAT_CURR = 0x05, // battery current A * 100
47 AFHDS2A_ID_FUEL = 0x06, // remaining battery percentage / mah drawn otherwise or fuel level no unit!
48 AFHDS2A_ID_RPM = 0x07, // throttle value / battery capacity
49 AFHDS2A_ID_CMP_HEAD = 0x08, // Heading 0..360 deg, 0=north 2bytes
50 AFHDS2A_ID_CLIMB_RATE = 0x09, // 2 bytes m/s *100 signed
51 AFHDS2A_ID_COG = 0x0A, // 2 bytes Course over ground(NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. unknown max uint
52 AFHDS2A_ID_GPS_STATUS = 0x0B, // 2 bytes
53 AFHDS2A_ID_ACC_X = 0x0C, // 2 bytes m/s *100 signed
54 AFHDS2A_ID_ACC_Y = 0x0D, // 2 bytes m/s *100 signed
55 AFHDS2A_ID_ACC_Z = 0x0E, // 2 bytes m/s *100 signed
56 AFHDS2A_ID_ROLL = 0x0F, // 2 bytes deg *100 signed
57 AFHDS2A_ID_PITCH = 0x10, // 2 bytes deg *100 signed
58 AFHDS2A_ID_YAW = 0x11, // 2 bytes deg *100 signed
59 AFHDS2A_ID_VERTICAL_SPEED = 0x12, // 2 bytes m/s *100 signed
60 AFHDS2A_ID_GROUND_SPEED = 0x13, // 2 bytes m/s *100 different unit than build-in sensor
61 AFHDS2A_ID_GPS_DIST = 0x14, // 2 bytes distance from home m unsigned
62 AFHDS2A_ID_ARMED = 0x15, // 2 bytes
63 AFHDS2A_ID_FLIGHT_MODE = 0x16, // 2 bytes
64
65 AFHDS2A_ID_PRES = 0x41, // Pressure
66 AFHDS2A_ID_ODO1 = 0x7C, // Odometer1
67 AFHDS2A_ID_ODO2 = 0x7D, // Odometer2
68 AFHDS2A_ID_SPE = 0x7E, // Speed 2 bytes km/h
69 AFHDS2A_ID_TX_V = 0x7F, // TX Voltage
70
71 AFHDS2A_ID_GPS_LAT = 0x80, // 4bytes signed WGS84 in degrees * 1E7
72 AFHDS2A_ID_GPS_LON = 0x81, // 4bytes signed WGS84 in degrees * 1E7
73 AFHDS2A_ID_GPS_ALT = 0x82, // 4bytes signed!!! GPS alt m*100
74 AFHDS2A_ID_ALT = 0x83, // 4bytes signed!!! Alt m*100
75 AFHDS2A_ID_S84 = 0x84,
76 AFHDS2A_ID_S85 = 0x85,
77 AFHDS2A_ID_S86 = 0x86,
78 AFHDS2A_ID_S87 = 0x87,
79 AFHDS2A_ID_S88 = 0x88,
80 AFHDS2A_ID_S89 = 0x89,
81 AFHDS2A_ID_S8a = 0x8A,
82
83 AFHDS2A_ID_ALT_FLYSKY = 0xF9, // Altitude 2 bytes signed in m - used in FlySky native TX - SIGNED value
84 AFHDS2A_ID_RX_SNR = 0xFA, // SNR
85 AFHDS2A_ID_RX_NOISE = 0xFB, // Noise
86 AFHDS2A_ID_RX_RSSI = 0xFC, // RSSI
87 AFHDS2A_ID_RX_ERR_RATE = 0xFE, // Error rate
88 AFHDS2A_ID_END = 0xFF,
89
90 // AC type telemetry with multiple values in one packet
91 AFHDS2A_ID_GPS_FULL = 0xFD,
92 AFHDS2A_ID_VOLT_FULL = 0xF0,
93 AFHDS2A_ID_ACC_FULL = 0xEF,
94 AFHDS2A_ID_TX_RSSI = 0x200, // Pseudo id outside 1 byte range of FlySky sensors
95 };
96
97 const FlySkySensor flySkySensors[] = {
98 {AFHDS2A_ID_VOLTAGE | 0x100, ZSTR_A1, UNIT_VOLTS, 2}, // RX Voltage (remapped, really 0x0)
99 {AFHDS2A_ID_TEMPERATURE, ZSTR_TEMP1, UNIT_CELSIUS, 1}, // Temperature
100 {AFHDS2A_ID_MOT, ZSTR_RPM, UNIT_RAW, 0}, // RPM
101 {AFHDS2A_ID_EXTV, ZSTR_A3, UNIT_VOLTS, 2}, // External voltage
102 {AFHDS2A_ID_CELL_VOLTAGE, ZSTR_CELLS, UNIT_VOLTS, 2}, // Avg Cell voltage
103 {AFHDS2A_ID_BAT_CURR, ZSTR_CURR, UNIT_AMPS, 2}, // battery current A * 100
104 {AFHDS2A_ID_FUEL, ZSTR_CAPACITY, UNIT_RAW, 0}, // remaining battery percentage / mah drawn otherwise or fuel level no unit!
105 {AFHDS2A_ID_RPM, ZSTR_RPM, UNIT_RAW, 0}, // throttle value / battery capacity
106 {AFHDS2A_ID_CMP_HEAD, ZSTR_HDG, UNIT_DEGREE, 0}, // Heading 0..360 deg, 0=north 2bytes
107 {AFHDS2A_ID_CLIMB_RATE, ZSTR_VSPD, UNIT_METERS_PER_SECOND, 2}, // 2 bytes m/s *100
108 {AFHDS2A_ID_COG, ZSTR_HDG, UNIT_DEGREE, 2}, // 2 bytes Course over ground(NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. unknown max uint
109 {AFHDS2A_ID_GPS_STATUS, ZSTR_SATELLITES, UNIT_RAW, 0}, // 2 bytes
110 {AFHDS2A_ID_ACC_X, ZSTR_ACCX, UNIT_METERS_PER_SECOND, 2}, // 2 bytes m/s *100 signed
111 {AFHDS2A_ID_ACC_Y, ZSTR_ACCY, UNIT_METERS_PER_SECOND, 2}, // 2 bytes m/s *100 signed
112 {AFHDS2A_ID_ACC_Z, ZSTR_ACCZ, UNIT_METERS_PER_SECOND, 2}, // 2 bytes m/s *100 signed
113 {AFHDS2A_ID_ROLL, ZSTR_ROLL, UNIT_DEGREE, 2}, // 2 bytes deg *100 signed
114 {AFHDS2A_ID_PITCH, ZSTR_PITCH, UNIT_DEGREE, 2}, // 2 bytes deg *100 signed
115 {AFHDS2A_ID_YAW, ZSTR_YAW, UNIT_DEGREE, 2}, // 2 bytes deg *100 signed
116 {AFHDS2A_ID_VERTICAL_SPEED, ZSTR_VSPD, UNIT_METERS_PER_SECOND, 2}, // 2 bytes m/s *100
117 {AFHDS2A_ID_GROUND_SPEED, ZSTR_GSPD, UNIT_METERS_PER_SECOND, 2}, // 2 bytes m/s *100 different unit than build-in sensor
118 {AFHDS2A_ID_GPS_DIST, ZSTR_DIST, UNIT_METERS, 0}, // 2 bytes dist from home m unsigned
119 {AFHDS2A_ID_ARMED, ZSTR_ARM, UNIT_RAW, 0}, // 2 bytes
120 {AFHDS2A_ID_FLIGHT_MODE, ZSTR_FLIGHT_MODE, UNIT_RAW, 0}, // 2 bytes index
121 {AFHDS2A_ID_PRES, ZSTR_PRES, UNIT_RAW, 2}, // 4 bytes In fact Temperature + Pressure -> Altitude
122 {AFHDS2A_ID_PRES | 0x100, ZSTR_TEMP2, UNIT_CELSIUS, 1}, // 2 bytes Temperature
123 {AFHDS2A_ID_ODO1, ZSTR_ODO1, UNIT_METERS, 2}, // 2 bytes Odometer1 -- some magic with 330 needed
124 {AFHDS2A_ID_ODO2, ZSTR_ODO2, UNIT_METERS, 2}, // 2 bytes Odometer2 -- some magic with 330 needed
125 {AFHDS2A_ID_SPE, ZSTR_ASPD, UNIT_KMH, 2}, // 2 bytes Speed km/h -- some magic with 330 needed
126 {AFHDS2A_ID_TX_V, ZSTR_TXV, UNIT_VOLTS, 2}, // TX Voltage
127 {AFHDS2A_ID_GPS_LAT, ZSTR_GPS, UNIT_RAW, 7}, // 4 bytes signed WGS84 in degrees * 1E7
128 {AFHDS2A_ID_GPS_LON, ZSTR_GPS, UNIT_RAW, 7}, // 4 bytes signed WGS84 in degrees * 1E7
129 {AFHDS2A_ID_GPS_ALT, ZSTR_GPSALT, UNIT_METERS, 2}, // 4 bytes signed GPS alt m*100
130 {AFHDS2A_ID_ALT, ZSTR_ALT, UNIT_METERS, 2}, // 4 bytes signed Alt m*100
131
132 {AFHDS2A_ID_RX_SNR, ZSTR_RX_SNR, UNIT_DB, 0}, // RX SNR
133 {AFHDS2A_ID_RX_NOISE, ZSTR_RX_NOISE, UNIT_DB, 0}, // RX Noise
134 {AFHDS2A_ID_RX_RSSI, ZSTR_RSSI, UNIT_DB, 0}, // RX RSSI (0xfc)
135 {AFHDS2A_ID_RX_ERR_RATE, ZSTR_RX_QUALITY, UNIT_RAW, 0}, // RX error rate
136 {AFHDS2A_ID_TX_RSSI, ZSTR_TX_RSSI, UNIT_RAW, 0}, // Pseudo sensor for TRSSI
137
138 {0x00, NULL, UNIT_RAW, 0}, // sentinel
139 };
140
141 int32_t getALT(uint32_t value);
142
143 static void processFlySkySensor(const uint8_t * packet, uint8_t type)
144 {
145 uint8_t buffer[8];
146 uint16_t id = packet[0];
147 const uint8_t instance = packet[1];
148 int32_t value;
149
150 //Load most likely value
151 if (type == 0xAA)
152 value = (packet[3] << 8) | packet[2];
153 else
154 value = (packet[6] << 24) | (packet[5] << 16) | (packet[4] << 8) | packet[3];
155
156 if (id == 0) id = 0x100; // Some part of OpenTX does not like sensor with id and instance 0, remap to 0x100
157
158 if (id == AFHDS2A_ID_RX_NOISE || id == AFHDS2A_ID_RX_RSSI) {
159 value = -value;
160 }
161 else if (id == AFHDS2A_ID_RX_ERR_RATE) {
162 value = 100 - value;
163 telemetryData.rssi.set(value);
164 if (value > 0) telemetryStreaming = TELEMETRY_TIMEOUT10ms;
165 }
166 else if (id == AFHDS2A_ID_PRES && value) {
167 // Extract temperature to a new sensor
168 setTelemetryValue(PROTOCOL_TELEMETRY_FLYSKY_IBUS, id | 0x100, 0, instance, ((value >> 19) - 400), UNIT_CELSIUS, 1);
169 // Extract alt to a new sensor
170 setTelemetryValue(PROTOCOL_TELEMETRY_FLYSKY_IBUS, AFHDS2A_ID_ALT, 0, instance, getALT(value), UNIT_METERS, 2);
171 value &= PRESSURE_MASK;
172 }
173 else if ((id >= AFHDS2A_ID_ACC_X && id <= AFHDS2A_ID_VERTICAL_SPEED) || id == AFHDS2A_ID_CLIMB_RATE || id == AFHDS2A_ID_ALT_FLYSKY) {
174 value = (int16_t) value; // Signed value
175 }
176 else if (id == AFHDS2A_ID_GPS_STATUS) {
177 value = value >> 8;
178 }
179 else if (id == AFHDS2A_ID_GPS_FULL) {
180 //(AC FRAME)[ID][inst][size][fix][sats][LAT]x4[LON]x4[ALT]x4
181 setTelemetryValue(PROTOCOL_TELEMETRY_FLYSKY_IBUS, AFHDS2A_ID_GPS_STATUS, 0, instance, packet[4], UNIT_RAW, 0);
182 for (uint8_t sensorID = AFHDS2A_ID_GPS_LAT; sensorID <= AFHDS2A_ID_GPS_ALT; sensorID++) {
183 int index = 5 + (sensorID - AFHDS2A_ID_GPS_LAT) * 4;
184 buffer[0] = sensorID;
185 buffer[1] = instance;
186 buffer[2] = 4;
187 memcpy(buffer + 3, packet + index, 4);
188 processFlySkySensor(buffer, 0xAC);
189 }
190 return;
191 }
192 else if (id == AFHDS2A_ID_VOLT_FULL) {
193 //(AC FRAME)[ID][inst][size][ACC_X]x2[ACC_Y]x2[ACC_Z]x2[ROLL]x2[PITCH]x2[YAW]x2
194 for (uint8_t sensorID = AFHDS2A_ID_EXTV; sensorID <= AFHDS2A_ID_RPM; sensorID++) {
195 int index = 3 + (sensorID - AFHDS2A_ID_EXTV) * 2;
196 buffer[0] = sensorID;
197 buffer[1] = instance;
198 buffer[2] = packet[index];
199 buffer[3] = packet[index + 1];
200 processFlySkySensor(buffer, 0xAA);
201 }
202 return;
203 }
204 else if (id == AFHDS2A_ID_ACC_FULL) {
205 //(AC FRAME)[ID][inst][size]
206 for (uint8_t sensorID = AFHDS2A_ID_ACC_X; sensorID <= AFHDS2A_ID_YAW; sensorID++) {
207 int index = 3 + (sensorID - AFHDS2A_ID_ACC_X) * 2;
208 buffer[0] = sensorID;
209 buffer[1] = instance;
210 buffer[2] = packet[index];
211 buffer[3] = packet[index + 1];
212 processFlySkySensor(buffer, 0xAA);
213 }
214 return;
215 }
216 for (const FlySkySensor * sensor = flySkySensors; sensor->id; sensor++) {
217 if (sensor->id != id) continue;
218 if (sensor->unit == UNIT_CELSIUS) value -= 400; // Temperature sensors have 40 degree offset
219 else if (sensor->unit == UNIT_VOLTS) value = (uint16_t) value; // Voltage types are unsigned 16bit integers
220 setTelemetryValue(PROTOCOL_TELEMETRY_FLYSKY_IBUS, id, 0, instance, value, sensor->unit, sensor->precision);
221 return;
222 }
223 //unknown
224 setTelemetryValue(PROTOCOL_TELEMETRY_FLYSKY_IBUS, id, 0, instance, value, UNIT_RAW, 0);
225 }
226
227 void processFlySkyPacket(const uint8_t * packet)
228 {
229 // Set TX RSSI Value, reverse MULTIs scaling
230 setTelemetryValue(PROTOCOL_TELEMETRY_FLYSKY_IBUS, AFHDS2A_ID_TX_RSSI, 0, 0, packet[0], UNIT_RAW, 0);
231
232 const uint8_t * buffer = packet + 1;
233 int sesnor = 0;
234 while (sesnor++ < 7) {
235 if (*buffer == AFHDS2A_ID_END) break;
236 processFlySkySensor(buffer, 0xAA);
237 buffer += 4;
238 }
239 }
240
241 void processFlySkyPacketAC(const uint8_t * packet)
242 {
243 // Set TX RSSI Value, reverse MULTIs scaling
244 setTelemetryValue(PROTOCOL_TELEMETRY_FLYSKY_IBUS, AFHDS2A_ID_TX_RSSI, 0, 0, packet[0], UNIT_RAW, 0);
245 const uint8_t * buffer = packet + 1;
246 while (buffer - packet < 26) //28 + 1(multi TX rssi) - 3(ac header)
247 {
248 if (*buffer == AFHDS2A_ID_END) break;
249 uint8_t size = buffer[2];
250 processFlySkySensor(buffer, 0xAC);
251 buffer += size + 3;
252 }
253 }
254
255 void processFlySkyTelemetryData(uint8_t data, uint8_t * rxBuffer, uint8_t &rxBufferCount)
256 {
257 if (rxBufferCount == 0)
258 return;
259
260 if (data == 0xAA || data == 0xAC) {
261 TRACE("[IBUS] Packet 0x%02X", data);
262 }
263 else {
264 TRACE("[IBUS] invalid start byte 0x%02X", data);
265 rxBufferCount = 0;
266 return;
267 }
268
269 if (rxBufferCount < TELEMETRY_RX_PACKET_SIZE) {
270 rxBuffer[rxBufferCount++] = data;
271 }
272 else {
273 TRACE("[IBUS] array size %d error", rxBufferCount);
274 rxBufferCount = 0;
275 }
276
277 if (rxBufferCount >= FLYSKY_TELEMETRY_LENGTH) {
278 // debug print the content of the packets
279 #if 0
280 debugPrintf(", rssi 0x%02X: ", rxBuffer[1]);
281 for (int i=0; i<7; i++) {
282 debugPrintf("[%02X %02X %02X%02X] ", rxBuffer[i*4+2], rxBuffer[i*4 + 3],
283 rxBuffer[i*4 + 4], rxBuffer[i*4 + 5]);
284 }
285 debugPrintf("\r\n");
286 #endif
287 if (data == 0xAA) processFlySkyPacket(rxBuffer + 1);
288 else if (data == 0xAC) processFlySkyPacketAC(rxBuffer + 1);
289 rxBufferCount = 0;
290 }
291 }
292
293 const FlySkySensor * getFlySkySensor(uint16_t id)
294 {
295 for (const FlySkySensor * sensor = flySkySensors; sensor->id; sensor++) {
296 if (id == sensor->id)
297 return sensor;
298 }
299 return nullptr;
300 }
301
302 void flySkySetDefault(int index, uint16_t id, uint8_t subId, uint8_t instance)
303 {
304 TelemetrySensor &telemetrySensor = g_model.telemetrySensors[index];
305 telemetrySensor.id = id;
306 telemetrySensor.subId = subId;
307 telemetrySensor.instance = instance;
308
309 const FlySkySensor * sensor = getFlySkySensor(id);
310 if (sensor) {
311 TelemetryUnit unit = sensor->unit;
312 uint8_t prec = min<uint8_t>(2, sensor->precision);
313 telemetrySensor.init(sensor->name, unit, prec);
314
315 if (unit == UNIT_RPMS) {
316 telemetrySensor.custom.ratio = 1;
317 telemetrySensor.custom.offset = 1;
318 }
319 }
320 else {
321 telemetrySensor.init(id);
322 }
323
324 storageDirty(EE_MODEL);
325 }
326
327 uint16_t ibusTempToK(int16_t tempertureIbus)
328 {
329 return (uint16_t) (tempertureIbus - 400) + 2731;
330 }
331
332 int32_t log2fix(uint32_t x)
333 {
334 int32_t b = 1U << (ALT_PRECISION - 1);
335 int32_t y = 0;
336 while (x < 1U << ALT_PRECISION) {
337 x <<= 1;
338 y -= 1U << ALT_PRECISION;
339 }
340
341 while (x >= 2U << ALT_PRECISION) {
342 x >>= 1;
343 y += 1U << ALT_PRECISION;
344 }
345
346 uint64_t z = x;
347 for (size_t i = 0; i < ALT_PRECISION; i++) {
348 z = (z * z) >> ALT_PRECISION;
349 if (z >= 2U << ALT_PRECISION) {
350 z >>= 1;
351 y += b;
352 }
353 b >>= 1;
354 }
355 return y;
356 }
357
358 int32_t getALT(uint32_t value)
359 {
360 uint32_t pressurePa = value & PRESSURE_MASK;
361 if (pressurePa == 0) return 0;
362 uint16_t temperatureK = ibusTempToK((uint16_t) (value >> 19));
363 static uint32_t initPressure = 0;
364 static uint16_t initTemperature = 0;
365 if (initPressure <= 0) // use current pressure for ground altitude -> 0
366 {
367 initPressure = pressurePa;
368 initTemperature = temperatureK;
369 }
370 int temperature = (initTemperature + temperatureK) >> 1; //div 2
371 bool tempNegative = temperature < 0;
372 if (tempNegative) temperature = temperature * -1;
373 uint64_t helper = R_DIV_G_MUL_10_Q15;
374 helper = helper * (uint64_t) temperature;
375 helper = helper >> ALT_PRECISION;
376
377 uint32_t po_to_p = (uint32_t)(initPressure << (ALT_PRECISION - 1));
378 po_to_p = po_to_p / pressurePa;
379 //shift missing bit
380 po_to_p = po_to_p << 1;
381 if (po_to_p == 0) return 0;
382 uint64_t t = log2fix(po_to_p) * INV_LOG2_E_Q1DOT31;
383 int32_t ln = t >> 31;
384
385 bool neg = ln < 0;
386 if (neg) ln = ln * -1;
387 helper = helper * (uint64_t) ln;
388 helper = helper >> ALT_PRECISION;
389 int result = (int) helper;
390
391 if (neg ^ tempNegative) result = result * -1;
392 return result;
393 }
OpenTX is open source firmware for RC radio transmitters.