src/main/telemetry/sbus2_sensors.c

   1 /*
   2  * This file is part of INAV.
   3  *
   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.
   8  *
   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.
  13  *
  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/>.
  16  */
  17
  18 #include "telemetry/sbus2.h"
  19 #include "telemetry/sbus2_sensors.h"
  20
  21
  22 #ifdef USE_TELEMETRY_SBUS2
  23 void send_RPM(uint8_t port, uint32_t RPM)
  24 {
  25    uint32_t value =  0;
  26    uint8_t bytes[2] = { };
  27
  28    value =  RPM / 6;
  29    if(value > 0xffff){
  30     value = 0xffff;
  31    }
  32    bytes[1] = value >> 8;
  33    bytes[0] = value;
  34    SBUS2_transmit_telemetry_data( port , bytes);
  35 }
  36
  37
  38 void send_temp125(uint8_t port, int16_t temp)
  39 {
  40    int16_t value=  0;
  41    uint8_t bytes[2] = { };
  42
  43    value = temp | 0x4000;
  44    bytes[0] = value >> 8;
  45    bytes[1] = value;
  46    SBUS2_transmit_telemetry_data( port , bytes);
  47 }
  48 void send_SBS01T(uint8_t port, int16_t temp){
  49   int16_t value=  0;
  50   uint8_t bytes[2] = {};
  51
  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);
  57 }
  58
  59 void send_voltage(uint8_t port,uint16_t voltage1, uint16_t voltage2)
  60 {
  61    uint16_t value = 0;
  62    uint8_t bytes[2] = { };
  63
  64    // VOLTAGE1
  65    value = voltage1 | 0x8000;
  66    if ( value > 0x9FFF ){
  67      value = 0x9FFF; // max voltage is 819.1
  68    }
  69    else if(value < 0x8000){
  70      value = 0x8000;
  71    }
  72    bytes[0] = value >> 8;
  73    bytes[1] = value;
  74    SBUS2_transmit_telemetry_data( port , bytes);
  75
  76    //VOLTAGE2
  77    value = voltage2;
  78    if ( value > 0x1FFF ){
  79      value = 0x1FFF; // max voltage is 819.1
  80    }
  81    bytes[0] = value >> 8;
  82    bytes[1] = value;
  83    SBUS2_transmit_telemetry_data( port+1 , bytes);
  84 }
  85
  86 void send_s1678_current(uint8_t port, uint16_t current, uint16_t capacity, uint16_t voltage)
  87 {
  88    uint16_t value = 0;
  89    uint32_t local = 0;
  90    uint8_t bytes[2] = { };
  91
  92
  93    // CURRENT
  94    local = ((uint32_t)current) * 1 ;
  95    value = (uint16_t)local;
  96    if ( value > 0x3FFF )
  97    {
  98       // max current is 163.83
  99       value = 0x3FFF;
 100    }
 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);
 106
 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);
 113
 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);
 120 }
 121
 122 void send_f1675_gps(uint8_t port, uint16_t speed, int16_t altitude, int16_t vario, int32_t latitude, int32_t longitude)
 123 {
 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] = {};
 130
 131
 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
 136    }
 137    else if( value1 < 0x4000){
 138      value1 = 0x4000;
 139    }
 140    bytes[0] = value1 >> 8;
 141    bytes[1] = value1;
 142    SBUS2_transmit_telemetry_data( port , bytes);
 143
 144    //HIGHT
 145    value2 = altitude | 0x4000;
 146    /*if(value2 > 0x7FFF ){              // max = +16383
 147      value2 = 0x7FFF;
 148    }
 149    else if( value2 < 0xC000){   // min = -16384
 150      value2 = 0xC000;
 151    }*/
 152    bytes[0] = value2 >> 8;
 153    bytes[1] = value2;
 154    SBUS2_transmit_telemetry_data( port+1 , bytes);
 155
 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);
 160
 161    // VARIO
 162    value2 = vario * 10;
 163    bytes[0] = value2 >> 8;
 164    bytes[1] = value2;
 165    SBUS2_transmit_telemetry_data( port+3 , bytes);
 166
 167    // LATITUDE
 168    if(latitude >= 0){
 169     latitude_pos = true;
 170    }
 171    else{
 172     latitude_pos = false;
 173     latitude = latitude * -1;
 174    }
 175    bytes[0] = (uint8_t)(latitude/1000000);
 176    value3 = (uint32_t)(latitude%1000000);
 177    if(latitude_pos){
 178     bytes[1] = ((value3 >> 16) & 0x0f);    // North
 179    }
 180    else{
 181     bytes[1] = ((value3 >> 16) | 0x1f);    // South
 182    }
 183    SBUS2_transmit_telemetry_data( port+4 , bytes);
 184
 185    bytes[0] = ((value3 >> 8) & 0xff);
 186    bytes[1] = value3 & 0xff;
 187    SBUS2_transmit_telemetry_data( port+5 , bytes);
 188
 189    // LONGITUDE
 190    if(longitude >= 0){
 191     longitude_pos = true;
 192    }
 193    else{
 194     longitude_pos = false;
 195     longitude = longitude * -1;
 196    }
 197    bytes[0] = (uint8_t)(longitude/1000000);
 198    value3 = (uint32_t)(longitude%1000000);
 199    if(longitude_pos){
 200     bytes[1] = ((value3 >> 16) & 0x0f);    // Eath
 201    }
 202    else{
 203     bytes[1] = ((value3 >> 16) | 0x1f);    // West
 204    }
 205    SBUS2_transmit_telemetry_data( port+6 , bytes);
 206
 207    bytes[0] = ((value3 >> 8) & 0xff);
 208    bytes[1] = value3 & 0xff;
 209    SBUS2_transmit_telemetry_data( port+7 , bytes);
 210 }
 211
 212 void send_f1672_vario(uint8_t port, int16_t altitude, int16_t vario)
 213 {
 214    int16_t value = 0;
 215    uint8_t bytes[2] = { };
 216
 217    // VARIO
 218    value = vario;
 219    bytes[0] = value >> 8;
 220    bytes[1] = value;
 221    SBUS2_transmit_telemetry_data( port, bytes);
 222
 223    //HIGHT
 224    value = altitude | 0x4000;
 225    bytes[0] = value >> 8;
 226    bytes[1] = value;
 227    SBUS2_transmit_telemetry_data( port + 1, bytes);
 228 }
 229
 230 void send_f1712_vario(uint8_t port, int16_t altitude, int16_t vario)
 231 {
 232    int16_t  value = 0;
 233    uint8_t bytes[2] = { };
 234
 235    // VARIO
 236    value = vario;
 237    bytes[0] = value >> 8;
 238    bytes[1] = value;
 239    SBUS2_transmit_telemetry_data( port , bytes);
 240
 241    //HIGHT
 242    value = altitude | 0x4000;
 243    bytes[0] = value >> 8;
 244    bytes[1] = value;
 245    SBUS2_transmit_telemetry_data( port + 1 , bytes);
 246
 247 }
 248
 249
 250 void send_SBS01TE(uint8_t port, int16_t temp){
 251   send_temp125(port, temp);
 252 }
 253 void send_F1713(uint8_t port, int16_t temp){
 254   send_temp125(port, temp);
 255 }
 256
 257 void send_SBS01RB(uint8_t port, uint32_t RPM){
 258   send_RPM(port, RPM);
 259 }
 260 void send_SBS01RM(uint8_t port, uint32_t RPM){
 261   send_RPM(port, RPM);
 262 }
 263 void send_SBS01RO(uint8_t port, uint32_t RPM){
 264   send_RPM(port, RPM);
 265 }
 266 void send_SBS01R(uint8_t port, uint32_t RPM){
 267   send_RPM(port, RPM);
 268 }
 269
 270 void send_F1678(uint8_t port, uint16_t current, uint16_t capacity, uint16_t voltage){
 271   send_s1678_current(port, current, capacity, voltage);
 272 }
 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));
 275 }
 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));
 278 }
 279 void send_SBS01V(uint8_t port,uint16_t voltage1, uint16_t voltage2){
 280   send_voltage(port, voltage1, voltage2);
 281 }
 282 void send_SBS01Vf(uint8_t port,float voltage1, float voltage2){
 283   send_voltage(port, (uint16_t)(voltage1 * 10), (uint16_t)(voltage2 * 10));
 284 }
 285 void send_voltagef(uint8_t port,float voltage1, float voltage2){
 286   send_voltage(port, (uint16_t)(voltage1 * 10), (uint16_t)(voltage2 * 10));
 287 }
 288 void send_SBS01C(uint8_t port, uint16_t current, uint16_t capacity, uint16_t voltage){
 289   send_s1678_current(port, current, capacity, voltage);
 290 }
 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));
 293 }
 294
 295 void send_f1712_variof(uint8_t port, int16_t altitude, float vario){
 296   send_f1712_vario(port, altitude, (int16_t)(vario * 10));
 297 }
 298 void send_f1672_variof(uint8_t port, int16_t altitude, float vario){
 299   send_f1672_vario(port, altitude, (int16_t)(vario * 100));
 300 }
 301 void send_F1712(uint8_t port, int16_t altitude, int16_t vario){
 302   send_f1712_vario(port, altitude, vario);
 303 }
 304 void send_F1712f(uint8_t port, int16_t altitude, float vario){
 305   send_f1712_vario(port, altitude, (int16_t)(vario * 10));
 306 }
 307 void send_F1672(uint8_t port, int16_t altitude, int16_t vario){
 308   send_f1672_vario(port, altitude, vario);
 309 }
 310 void send_F1672f(uint8_t port, int16_t altitude, float vario){
 311   send_f1672_vario(port, altitude, (int16_t)(vario * 100));
 312 }
 313
 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;
 320   }
 321   if(lon_deg < 0){
 322     Lon_Negative = true;
 323     lon_deg = lon_deg * -1;
 324   }
 325   if(lat_min < 0){
 326     Lat_Negative = true;
 327     lat_min = lat_min * -1;
 328   }
 329   if(lon_min < 0){
 330     Lon_Negative = true;
 331     lon_min = lon_min * -1;
 332   }
 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;
 343   }
 344   if(Lon_Negative){
 345     _longitude = _longitude * -1;
 346   }
 347   send_f1675_gps(port, speed, hight, vario, _latitude, _longitude);
 348 }
 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;
 355   }
 356   if(lon_deg < 0){
 357     Lon_Negative = true;
 358     lon_deg = lon_deg * -1;
 359   }
 360   if(lat_min < 0){
 361     Lat_Negative = true;
 362     lat_min = lat_min * -1;
 363   }
 364   if(lon_min < 0){
 365     Lon_Negative = true;
 366     lon_min = lon_min * -1;
 367   }
 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;
 376   }
 377   if(Lon_Negative){
 378     _longitude = _longitude * -1;
 379   }
 380   send_f1675_gps(port, speed, hight, vario, _latitude, _longitude);
 381 }
 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);
 394 }
 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);
 407 }
 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)
 418 {
 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;
 424    }
 425    else {
 426      lat = (-600000.0*latitude) + 0.5;
 427      // toggle south bit
 428      lat |= 0x4000000;
 429    }
 430    if (longitude > 0) {
 431      lon = (600000.0*longitude) + 0.5;
 432    }
 433    else {
 434      lon = (-600000.0*longitude) + 0.5;
 435      // toggle west bit
 436      lon |= 0x8000000;
 437    }
 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;
 444    }
 445    else {
 446     speed = 0;
 447    }
 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;
 486    }
 487    else {
 488     vario = 0;
 489    }
 490    bytes[0] = ((vario&0x001f)<<3) | ((alt&0x1c00)>>10);
 491    bytes[1] =  (vario&0x1fe0)>>5;
 492    SBUS2_transmit_telemetry_data(port+7 , bytes);
 493 }
 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)
 504 {
 505    uint32_t value = 0;
 506    uint8_t bytes[2] = { };
 507
 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);
 513
 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);
 519
 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);
 525
 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);
 531
 532    // 41 => 41 Celsius
 533    value = temp;
 534    bytes[0] = value >> 8;
 535    bytes[1] = value;
 536    SBUS2_transmit_telemetry_data( port + 4 , bytes);
 537
 538    // 21 => Bec Celsius
 539    value = becTemp;
 540    bytes[0] = value >> 8;
 541    bytes[1] = value;
 542    SBUS2_transmit_telemetry_data( port + 5 , bytes);
 543
 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);
 549
 550    // PWM output
 551    value = pwm;
 552    bytes[0] = value >> 8;
 553    bytes[1] = value;
 554    SBUS2_transmit_telemetry_data( port + 7 , bytes);
 555 }
 556
 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)
 567 {
 568    send_scorpion_kontronik(
 569     port,
 570     voltage,
 571     capacity,
 572     rpm,
 573     current,
 574     temp,
 575     becTemp,
 576     becCurrent,
 577     pwm);
 578 }
 579
 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)
 590 {
 591    send_scorpion_kontronik(
 592     port,
 593     voltage,
 594     capacity,
 595     rpm,
 596     current,
 597     temp,
 598     becTemp,
 599     becCurrent,
 600     pwm);
 601 }
 602
 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)
 619 {
 620    uint32_t value = 0;
 621    uint8_t bytes[2] = {};
 622
 623    // Actual RPM with 0x4000 Offset -> why?
 624    value = rpm / 100;
 625    value = value | 0x4000;
 626    if(value > 0xffff){
 627     value = 0xffff;
 628    }
 629    bytes[0] = value >> 8;
 630    bytes[1] = value;
 631    SBUS2_transmit_telemetry_data( port , bytes);
 632
 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);
 638
 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);
 644
 645    // Setpoint RPM without Offset
 646    value = setrpm / 100;
 647    if(value > 0xffff){
 648     value = 0xffff;
 649    }
 650    bytes[0] = value >> 8;
 651    bytes[1] = value;
 652    SBUS2_transmit_telemetry_data( port + 3 , bytes);
 653
 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);
 659
 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);
 665
 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);
 671
 672    // Altitude -> without offset?
 673    value = altitude;
 674    bytes[0] = value >> 8;
 675    bytes[1] = value;
 676    SBUS2_transmit_telemetry_data( port + 7 , bytes);
 677
 678    // Fuel Quality in %
 679    value = quality;
 680    bytes[0] = value >> 8;
 681    bytes[1] = value;
 682    SBUS2_transmit_telemetry_data( port + 8 , bytes);
 683
 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);
 689
 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);
 695
 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);
 701
 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);
 707
 708    // Second RPM without Offset
 709    value = secondrpm / 100;
 710    if(value > 0xffff){
 711     value = 0xffff;
 712    }
 713    bytes[0] = value >> 8;
 714    bytes[1] = value;
 715    SBUS2_transmit_telemetry_data( port + 13 , bytes);
 716
 717 }
 718
 719
 720 void SBUS2_transmit_telemetry_data(uint8_t slotId , const uint8_t *bytes)
 721 {
 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;
 729     }
 730
 731 }
 732 #endif