Merge pull request #10531 from b14ckyy/master
[inav.git] / src / main / telemetry / sbus2_sensors.c
blobf2e94f51ac16cd34ec99e397242485622f513757
1 /*
2 * This file is part of INAV.
4 * INAV is free software: you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation, either version 3 of the License, or
7 * (at your option) any later version.
9 * INAV is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU General Public License for more details.
14 * You should have received a copy of the GNU General Public License
15 * along with INAV. If not, see <http://www.gnu.org/licenses/>.
18 #include "telemetry/sbus2.h"
19 #include "telemetry/sbus2_sensors.h"
22 #ifdef USE_TELEMETRY_SBUS2
23 void send_RPM(uint8_t port, uint32_t RPM)
25 uint32_t value = 0;
26 uint8_t bytes[2] = { };
28 value = RPM / 6;
29 if(value > 0xffff){
30 value = 0xffff;
32 bytes[1] = value >> 8;
33 bytes[0] = value;
34 SBUS2_transmit_telemetry_data( port , bytes);
38 void send_temp125(uint8_t port, int16_t temp)
40 int16_t value= 0;
41 uint8_t bytes[2] = { };
43 value = temp | 0x4000;
44 bytes[0] = value >> 8;
45 bytes[1] = value;
46 SBUS2_transmit_telemetry_data( port , bytes);
48 void send_SBS01T(uint8_t port, int16_t temp){
49 int16_t value= 0;
50 uint8_t bytes[2] = {};
52 value = temp | 0x8000;
53 value = value + 100;
54 bytes[0] = value;// >> 8;
55 bytes[1] = value >> 8;
56 SBUS2_transmit_telemetry_data(port , bytes);
59 void send_voltage(uint8_t port,uint16_t voltage1, uint16_t voltage2)
61 uint16_t value = 0;
62 uint8_t bytes[2] = { };
64 // VOLTAGE1
65 value = voltage1 | 0x8000;
66 if ( value > 0x9FFF ){
67 value = 0x9FFF; // max voltage is 819.1
69 else if(value < 0x8000){
70 value = 0x8000;
72 bytes[0] = value >> 8;
73 bytes[1] = value;
74 SBUS2_transmit_telemetry_data( port , bytes);
76 //VOLTAGE2
77 value = voltage2;
78 if ( value > 0x1FFF ){
79 value = 0x1FFF; // max voltage is 819.1
81 bytes[0] = value >> 8;
82 bytes[1] = value;
83 SBUS2_transmit_telemetry_data( port+1 , bytes);
86 void send_s1678_current(uint8_t port, uint16_t current, uint16_t capacity, uint16_t voltage)
88 uint16_t value = 0;
89 uint32_t local = 0;
90 uint8_t bytes[2] = { };
93 // CURRENT
94 local = ((uint32_t)current) * 1 ;
95 value = (uint16_t)local;
96 if ( value > 0x3FFF )
98 // max current is 163.83
99 value = 0x3FFF;
101 bytes[0] = value >> 8;
102 bytes[0] = bytes[0] | 0x40;
103 bytes[0] = bytes[0] & 0x7F;
104 bytes[1] = value;
105 SBUS2_transmit_telemetry_data( port , bytes);
107 //VOLTAGE
108 local = ((uint32_t)voltage) * 1;
109 value = (uint16_t)local;
110 bytes[0] = value >> 8;
111 bytes[1] = value;
112 SBUS2_transmit_telemetry_data( port+1 , bytes);
114 // CAPACITY
115 local = (uint32_t)capacity;
116 value = (uint16_t)local;
117 bytes[0] = value >> 8;
118 bytes[1] = value;
119 SBUS2_transmit_telemetry_data( port+2 , bytes);
122 void send_f1675_gps(uint8_t port, uint16_t speed, int16_t altitude, int16_t vario, int32_t latitude, int32_t longitude)
124 uint16_t value1 = 0;
125 int16_t value2 = 0;
126 uint32_t value3 = 0;
127 bool latitude_pos = false;
128 bool longitude_pos = false;
129 uint8_t bytes[2] = {};
132 // SPEED -> Bit 14(bytes[1] bit7) -> GPS Valid or not
133 value1 = speed | 0x4000;
134 if (value1 > 0x43E7 ){
135 value1 = 0x43E7; // max Speed is 999 km/h
137 else if( value1 < 0x4000){
138 value1 = 0x4000;
140 bytes[0] = value1 >> 8;
141 bytes[1] = value1;
142 SBUS2_transmit_telemetry_data( port , bytes);
144 //HIGHT
145 value2 = altitude | 0x4000;
146 /*if(value2 > 0x7FFF ){ // max = +16383
147 value2 = 0x7FFF;
149 else if( value2 < 0xC000){ // min = -16384
150 value2 = 0xC000;
152 bytes[0] = value2 >> 8;
153 bytes[1] = value2;
154 SBUS2_transmit_telemetry_data( port+1 , bytes);
156 //TIME -> 12:34:56 Uhr = 12*60*60 + 34*60 + 56 = 45296 = 0xB0F0
157 bytes[0] = 0xB0;
158 bytes[1] = 0xF0;
159 SBUS2_transmit_telemetry_data( port+2 , bytes);
161 // VARIO
162 value2 = vario * 10;
163 bytes[0] = value2 >> 8;
164 bytes[1] = value2;
165 SBUS2_transmit_telemetry_data( port+3 , bytes);
167 // LATITUDE
168 if(latitude >= 0){
169 latitude_pos = true;
171 else{
172 latitude_pos = false;
173 latitude = latitude * -1;
175 bytes[0] = (uint8_t)(latitude/1000000);
176 value3 = (uint32_t)(latitude%1000000);
177 if(latitude_pos){
178 bytes[1] = ((value3 >> 16) & 0x0f); // North
180 else{
181 bytes[1] = ((value3 >> 16) | 0x1f); // South
183 SBUS2_transmit_telemetry_data( port+4 , bytes);
185 bytes[0] = ((value3 >> 8) & 0xff);
186 bytes[1] = value3 & 0xff;
187 SBUS2_transmit_telemetry_data( port+5 , bytes);
189 // LONGITUDE
190 if(longitude >= 0){
191 longitude_pos = true;
193 else{
194 longitude_pos = false;
195 longitude = longitude * -1;
197 bytes[0] = (uint8_t)(longitude/1000000);
198 value3 = (uint32_t)(longitude%1000000);
199 if(longitude_pos){
200 bytes[1] = ((value3 >> 16) & 0x0f); // Eath
202 else{
203 bytes[1] = ((value3 >> 16) | 0x1f); // West
205 SBUS2_transmit_telemetry_data( port+6 , bytes);
207 bytes[0] = ((value3 >> 8) & 0xff);
208 bytes[1] = value3 & 0xff;
209 SBUS2_transmit_telemetry_data( port+7 , bytes);
212 void send_f1672_vario(uint8_t port, int16_t altitude, int16_t vario)
214 int16_t value = 0;
215 uint8_t bytes[2] = { };
217 // VARIO
218 value = vario;
219 bytes[0] = value >> 8;
220 bytes[1] = value;
221 SBUS2_transmit_telemetry_data( port, bytes);
223 //HIGHT
224 value = altitude | 0x4000;
225 bytes[0] = value >> 8;
226 bytes[1] = value;
227 SBUS2_transmit_telemetry_data( port + 1, bytes);
230 void send_f1712_vario(uint8_t port, int16_t altitude, int16_t vario)
232 int16_t value = 0;
233 uint8_t bytes[2] = { };
235 // VARIO
236 value = vario;
237 bytes[0] = value >> 8;
238 bytes[1] = value;
239 SBUS2_transmit_telemetry_data( port , bytes);
241 //HIGHT
242 value = altitude | 0x4000;
243 bytes[0] = value >> 8;
244 bytes[1] = value;
245 SBUS2_transmit_telemetry_data( port + 1 , bytes);
250 void send_SBS01TE(uint8_t port, int16_t temp){
251 send_temp125(port, temp);
253 void send_F1713(uint8_t port, int16_t temp){
254 send_temp125(port, temp);
257 void send_SBS01RB(uint8_t port, uint32_t RPM){
258 send_RPM(port, RPM);
260 void send_SBS01RM(uint8_t port, uint32_t RPM){
261 send_RPM(port, RPM);
263 void send_SBS01RO(uint8_t port, uint32_t RPM){
264 send_RPM(port, RPM);
266 void send_SBS01R(uint8_t port, uint32_t RPM){
267 send_RPM(port, RPM);
270 void send_F1678(uint8_t port, uint16_t current, uint16_t capacity, uint16_t voltage){
271 send_s1678_current(port, current, capacity, voltage);
273 void send_s1678_currentf(uint8_t port, float current, uint16_t capacity, float voltage){
274 send_s1678_current(port, (uint16_t)(current * 100), capacity, (uint16_t)(voltage * 100));
276 void send_F1678f(uint8_t port, float current, uint16_t capacity, float voltage){
277 send_s1678_current(port, (uint16_t)(current * 100), capacity, (uint16_t)(voltage * 100));
279 void send_SBS01V(uint8_t port,uint16_t voltage1, uint16_t voltage2){
280 send_voltage(port, voltage1, voltage2);
282 void send_SBS01Vf(uint8_t port,float voltage1, float voltage2){
283 send_voltage(port, (uint16_t)(voltage1 * 10), (uint16_t)(voltage2 * 10));
285 void send_voltagef(uint8_t port,float voltage1, float voltage2){
286 send_voltage(port, (uint16_t)(voltage1 * 10), (uint16_t)(voltage2 * 10));
288 void send_SBS01C(uint8_t port, uint16_t current, uint16_t capacity, uint16_t voltage){
289 send_s1678_current(port, current, capacity, voltage);
291 void send_SBS01Cf(uint8_t port, float current, uint16_t capacity, float voltage){
292 send_s1678_current(port, (uint16_t)(current * 100), capacity, (uint16_t)(voltage * 100));
295 void send_f1712_variof(uint8_t port, int16_t altitude, float vario){
296 send_f1712_vario(port, altitude, (int16_t)(vario * 10));
298 void send_f1672_variof(uint8_t port, int16_t altitude, float vario){
299 send_f1672_vario(port, altitude, (int16_t)(vario * 100));
301 void send_F1712(uint8_t port, int16_t altitude, int16_t vario){
302 send_f1712_vario(port, altitude, vario);
304 void send_F1712f(uint8_t port, int16_t altitude, float vario){
305 send_f1712_vario(port, altitude, (int16_t)(vario * 10));
307 void send_F1672(uint8_t port, int16_t altitude, int16_t vario){
308 send_f1672_vario(port, altitude, vario);
310 void send_F1672f(uint8_t port, int16_t altitude, float vario){
311 send_f1672_vario(port, altitude, (int16_t)(vario * 100));
314 void send_F1675minf(uint8_t port, uint16_t speed, int16_t hight, int16_t vario, int8_t lat_deg, float lat_min, int8_t lon_deg, float lon_min){
315 bool Lat_Negative = false;
316 bool Lon_Negative = false;
317 if(lat_deg < 0){
318 Lat_Negative = true;
319 lat_deg = lat_deg * -1;
321 if(lon_deg < 0){
322 Lon_Negative = true;
323 lon_deg = lon_deg * -1;
325 if(lat_min < 0){
326 Lat_Negative = true;
327 lat_min = lat_min * -1;
329 if(lon_min < 0){
330 Lon_Negative = true;
331 lon_min = lon_min * -1;
333 int32_t _latitude_deg = lat_deg;
334 int32_t _longitude_deg = lon_deg;
335 int32_t _latitude_min = lat_min * 10000;
336 int32_t _longitude_min = lon_min * 10000;
337 int32_t _latitude = _latitude_deg * 1000000;
338 int32_t _longitude = _longitude_deg * 1000000;
339 _latitude = _latitude + _latitude_min;
340 _longitude = _longitude + _longitude_min;
341 if(Lat_Negative){
342 _latitude = _latitude * -1;
344 if(Lon_Negative){
345 _longitude = _longitude * -1;
347 send_f1675_gps(port, speed, hight, vario, _latitude, _longitude);
349 void send_F1675min(uint8_t port, uint16_t speed, int16_t hight, int16_t vario, int8_t lat_deg, int32_t lat_min, int8_t lon_deg, int32_t lon_min){
350 bool Lat_Negative = false;
351 bool Lon_Negative = false;
352 if(lat_deg < 0){
353 Lat_Negative = true;
354 lat_deg = lat_deg * -1;
356 if(lon_deg < 0){
357 Lon_Negative = true;
358 lon_deg = lon_deg * -1;
360 if(lat_min < 0){
361 Lat_Negative = true;
362 lat_min = lat_min * -1;
364 if(lon_min < 0){
365 Lon_Negative = true;
366 lon_min = lon_min * -1;
368 int32_t _latitude_deg = lat_deg;
369 int32_t _longitude_deg = lon_deg;
370 int32_t _latitude = _latitude_deg * 1000000;
371 int32_t _longitude = _longitude_deg * 1000000;
372 _latitude = _latitude + lat_min;
373 _longitude = _longitude + lon_min;
374 if(Lat_Negative){
375 _latitude = _latitude * -1;
377 if(Lon_Negative){
378 _longitude = _longitude * -1;
380 send_f1675_gps(port, speed, hight, vario, _latitude, _longitude);
382 void send_F1675(uint8_t port, uint16_t speed, int16_t hight, int16_t vario, int32_t latitude, int32_t longitude){
383 int32_t _latitude = latitude;
384 int32_t _longitude = longitude;
385 int32_t _latitude_deg = _latitude/1000000;
386 int32_t _longitude_deg = _longitude/1000000;
387 int32_t _latitude_min = _latitude%1000000;
388 int32_t _longitude_min = _longitude%1000000;
389 _latitude = _latitude_deg * 1000000;
390 _longitude = _longitude_deg * 1000000;
391 _latitude = _latitude + ((_latitude_min * 60)/100);
392 _longitude = _longitude + ((_longitude_min * 60)/100);
393 send_f1675_gps(port, speed, hight, vario, _latitude, _longitude);
395 void send_F1675f(uint8_t port, uint16_t speed, int16_t hight, int16_t vario, float latitude, float longitude){
396 int32_t _latitude = latitude * 1000000;
397 int32_t _longitude = longitude * 1000000;
398 int32_t _latitude_deg = _latitude/1000000;
399 int32_t _longitude_deg = _longitude/1000000;
400 int32_t _latitude_min = _latitude%1000000;
401 int32_t _longitude_min = _longitude%1000000;
402 _latitude = _latitude_deg * 1000000;
403 _longitude = _longitude_deg * 1000000;
404 _latitude = _latitude + ((_latitude_min * 60)/100);
405 _longitude = _longitude + ((_longitude_min * 60)/100);
406 send_f1675_gps(port, speed, hight, vario, _latitude, _longitude);
408 void send_SBS10G(
409 uint8_t port,
410 uint16_t hours, // 0 to 24
411 uint16_t minutes, // 0 to 60
412 uint16_t seconds, // 0 to 60
413 float latitude, // decimal degrees (i.e. 52.520833; negative value for southern hemisphere)
414 float longitude, // decimal degrees (i.e. 13.409430; negative value for western hemisphere)
415 float altitudeMeters, // meters (valid range: -1050 to 4600)
416 uint16_t speed, // km/h (valid range 0 to 511)
417 float gpsVario) // m/s (valid range: -150 to 260)
419 uint32_t utc = (hours*3600) + (minutes*60) + seconds;
420 uint32_t lat, lon;
421 // scale latitude/longitude (add 0.5 for correct rounding)
422 if (latitude > 0) {
423 lat = (600000.0*latitude) + 0.5;
425 else {
426 lat = (-600000.0*latitude) + 0.5;
427 // toggle south bit
428 lat |= 0x4000000;
430 if (longitude > 0) {
431 lon = (600000.0*longitude) + 0.5;
433 else {
434 lon = (-600000.0*longitude) + 0.5;
435 // toggle west bit
436 lon |= 0x8000000;
438 // convert altitude (add 0.5 for correct rounding)
439 uint16_t alt = (altitudeMeters>=-820 && altitudeMeters<=4830) ?(1.25*(altitudeMeters+820)) + 0.5 : 0;
440 // error check speed
441 if (speed < 512) {
442 // set speed enable bit
443 speed |= 0x200;
445 else {
446 speed = 0;
448 // initialize buffer
449 uint8_t bytes[2] = { };
450 // slot 0 (utc)
451 bytes[0] = (utc&0x00ff);
452 bytes[1] = (utc&0xff00)>>8;
453 SBUS2_transmit_telemetry_data(port , bytes);
454 // slot 1 (latitude & utc)
455 bytes[0] = ((lat&0x007f)<<1) | ((utc&0x10000)>>16);
456 bytes[1] = (lat&0x7f80)>>7;
457 SBUS2_transmit_telemetry_data(port+1 , bytes);
458 // slot 2 (latitude & longitude)
459 bytes[0] = (lat&0x07f8000)>>15;
460 bytes[1] = ((lat&0x7800000)>>23) | (lon&0x0f)<<4;
461 SBUS2_transmit_telemetry_data(port+2 , bytes);
462 // slot 3 (longitude)
463 bytes[0] = (lon&0x00ff0)>>4;
464 bytes[1] = (lon&0xff000)>>12;
465 SBUS2_transmit_telemetry_data(port+3 , bytes);
466 // slot 4 (longitude & speed)
467 bytes[0] = ((lon&0xff00000)>>20);
468 bytes[1] = (speed&0xff);
469 SBUS2_transmit_telemetry_data(port+4 , bytes);
470 // slot 5 (pressure & speed)
471 bytes[0] = ((speed&0x300)>>8);
472 bytes[1] = 0x00;
473 SBUS2_transmit_telemetry_data(port+5 , bytes);
474 // slot 6 (altitude & pressure)
475 bytes[0] = ((alt&0x003)<<6);
476 bytes[1] = (alt&0x3fc)>>2;
477 SBUS2_transmit_telemetry_data(port+6 , bytes);
478 // slot (7 (vario & altitude)
479 uint16_t vario;
480 // error check vario
481 if (gpsVario >= -150 && gpsVario <= 260) {
482 // scale vario (add 0.5 for correct rounding)
483 vario = (10.0*(gpsVario + 150)) + 0.5;
484 // set vario enable
485 vario |= 0x1000;
487 else {
488 vario = 0;
490 bytes[0] = ((vario&0x001f)<<3) | ((alt&0x1c00)>>10);
491 bytes[1] = (vario&0x1fe0)>>5;
492 SBUS2_transmit_telemetry_data(port+7 , bytes);
494 void send_scorpion_kontronik(
495 uint8_t port,
496 uint16_t voltage,
497 uint16_t capacity,
498 uint32_t rpm,
499 uint16_t current,
500 uint16_t temp,
501 uint16_t becTemp,
502 uint16_t becCurrent,
503 uint16_t pwm)
505 uint32_t value = 0;
506 uint8_t bytes[2] = { };
508 // voltage 41.1 = 4110
509 value = voltage | 0x8000;
510 bytes[0] = value >> 8;
511 bytes[1] = value;
512 SBUS2_transmit_telemetry_data( port , bytes);
514 // 1330 mah => 1.33 Ah
515 value = capacity;
516 bytes[0] = value >> 8;
517 bytes[1] = value;
518 SBUS2_transmit_telemetry_data( port + 1 , bytes);
520 // 2250 rpm => 2250
521 value = rpm / 6;
522 bytes[0] = value >> 8;
523 bytes[1] = value;
524 SBUS2_transmit_telemetry_data( port + 2 , bytes);
526 // 13310 => 133.1 A
527 value = current;
528 bytes[0] = value >> 8;
529 bytes[1] = value;
530 SBUS2_transmit_telemetry_data( port + 3 , bytes);
532 // 41 => 41 Celsius
533 value = temp;
534 bytes[0] = value >> 8;
535 bytes[1] = value;
536 SBUS2_transmit_telemetry_data( port + 4 , bytes);
538 // 21 => Bec Celsius
539 value = becTemp;
540 bytes[0] = value >> 8;
541 bytes[1] = value;
542 SBUS2_transmit_telemetry_data( port + 5 , bytes);
544 // 650 => 6,5 Bec Current
545 value = becCurrent;
546 bytes[0] = value >> 8;
547 bytes[1] = value;
548 SBUS2_transmit_telemetry_data( port + 6 , bytes);
550 // PWM output
551 value = pwm;
552 bytes[0] = value >> 8;
553 bytes[1] = value;
554 SBUS2_transmit_telemetry_data( port + 7 , bytes);
557 void send_scorpion(
558 uint8_t port,
559 uint16_t voltage,
560 uint16_t capacity,
561 uint32_t rpm,
562 uint16_t current,
563 uint16_t temp,
564 uint16_t becTemp,
565 uint16_t becCurrent,
566 uint16_t pwm)
568 send_scorpion_kontronik(
569 port,
570 voltage,
571 capacity,
572 rpm,
573 current,
574 temp,
575 becTemp,
576 becCurrent,
577 pwm);
580 void send_kontronik(
581 uint8_t port,
582 uint16_t voltage,
583 uint16_t capacity,
584 uint32_t rpm,
585 uint16_t current,
586 uint16_t temp,
587 uint16_t becTemp,
588 uint16_t becCurrent,
589 uint16_t pwm)
591 send_scorpion_kontronik(
592 port,
593 voltage,
594 capacity,
595 rpm,
596 current,
597 temp,
598 becTemp,
599 becCurrent,
600 pwm);
603 void send_jetcat(
604 uint8_t port,
605 uint32_t rpm,
606 uint16_t egt,
607 uint16_t pump_volt,
608 uint32_t setrpm,
609 uint16_t thrust,
610 uint16_t fuel,
611 uint16_t fuelflow,
612 uint16_t altitude,
613 uint16_t quality,
614 uint16_t volt,
615 uint16_t current,
616 uint16_t speed,
617 uint16_t status,
618 uint32_t secondrpm)
620 uint32_t value = 0;
621 uint8_t bytes[2] = {};
623 // Actual RPM with 0x4000 Offset -> why?
624 value = rpm / 100;
625 value = value | 0x4000;
626 if(value > 0xffff){
627 value = 0xffff;
629 bytes[0] = value >> 8;
630 bytes[1] = value;
631 SBUS2_transmit_telemetry_data( port , bytes);
633 // EGT Abgastemperatur in °C
634 value = egt;
635 bytes[0] = value >> 8;
636 bytes[1] = value;
637 SBUS2_transmit_telemetry_data( port + 1 , bytes);
639 // Pump Voltage 12.34V = 1234
640 value = pump_volt;
641 bytes[0] = value >> 8;
642 bytes[1] = value;
643 SBUS2_transmit_telemetry_data( port + 2 , bytes);
645 // Setpoint RPM without Offset
646 value = setrpm / 100;
647 if(value > 0xffff){
648 value = 0xffff;
650 bytes[0] = value >> 8;
651 bytes[1] = value;
652 SBUS2_transmit_telemetry_data( port + 3 , bytes);
654 // Thrust 123.4N = 1234
655 value = thrust;
656 bytes[0] = value >> 8;
657 bytes[1] = value;
658 SBUS2_transmit_telemetry_data( port + 4 , bytes);
660 // Fuel (remain) in ml
661 value = fuel;
662 bytes[0] = value >> 8;
663 bytes[1] = value;
664 SBUS2_transmit_telemetry_data( port + 5 , bytes);
666 // Fuel Flow in ml/min
667 value = fuelflow;
668 bytes[0] = value >> 8;
669 bytes[1] = value;
670 SBUS2_transmit_telemetry_data( port + 6 , bytes);
672 // Altitude -> without offset?
673 value = altitude;
674 bytes[0] = value >> 8;
675 bytes[1] = value;
676 SBUS2_transmit_telemetry_data( port + 7 , bytes);
678 // Fuel Quality in %
679 value = quality;
680 bytes[0] = value >> 8;
681 bytes[1] = value;
682 SBUS2_transmit_telemetry_data( port + 8 , bytes);
684 // Voltage 12.34V = 1234
685 value = volt;
686 bytes[0] = value >> 8;
687 bytes[1] = value;
688 SBUS2_transmit_telemetry_data( port + 9 , bytes);
690 // Current 123.4A = 1234
691 value = current;
692 bytes[0] = value >> 8;
693 bytes[1] = value;
694 SBUS2_transmit_telemetry_data( port + 10 , bytes);
696 // Speed in km/h
697 value = speed;
698 bytes[0] = value >> 8;
699 bytes[1] = value;
700 SBUS2_transmit_telemetry_data( port + 11 , bytes);
702 // Status and Error Code
703 value = status;
704 bytes[0] = value >> 8;
705 bytes[1] = value;
706 SBUS2_transmit_telemetry_data( port + 12 , bytes);
708 // Second RPM without Offset
709 value = secondrpm / 100;
710 if(value > 0xffff){
711 value = 0xffff;
713 bytes[0] = value >> 8;
714 bytes[1] = value;
715 SBUS2_transmit_telemetry_data( port + 13 , bytes);
720 void SBUS2_transmit_telemetry_data(uint8_t slotId , const uint8_t *bytes)
722 if(slotId > 0 && slotId < SBUS2_SLOT_COUNT) {
723 sbusTelemetryData[slotId].payload.data[0] = bytes[0];
724 sbusTelemetryData[slotId].payload.data[1] = bytes[1];
725 sbusTelemetryData[slotId].slotId = sbus2SlotIds[slotId];
726 //sbusTelemetryData[i].payload.data[0] = 0x81;
727 //sbusTelemetryData[i].payload.data[1] = 0x80;
728 sbusTelemetryDataUsed[slotId] = 1;
732 #endif