src/main/telemetry/ibus_shared.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight 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  * Cleanflight 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 Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdbool.h>
  19 #include <stdint.h>
  20 #include <stdlib.h>
  21 #include "platform.h"
  22 #include "telemetry/ibus_shared.h"
  23
  24 #if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_IBUS)
  25
  26 #include "common/maths.h"
  27 #include "common/axis.h"
  28
  29 #include "drivers/serial.h"
  30
  31 #include "fc/fc_core.h"
  32 #include "fc/rc_controls.h"
  33 #include "fc/runtime_config.h"
  34 #include "scheduler/scheduler.h"
  35
  36 #include "io/osd.h"
  37 #include "io/serial.h"
  38
  39 #include "sensors/barometer.h"
  40 #include "sensors/temperature.h"
  41 #include "sensors/acceleration.h"
  42 #include "sensors/battery.h"
  43 #include "sensors/sensors.h"
  44 #include "sensors/pitotmeter.h"
  45
  46 #include "flight/imu.h"
  47 #include "flight/failsafe.h"
  48 #include "flight/mixer.h"
  49
  50 #include "navigation/navigation.h"
  51
  52 #include "telemetry/ibus.h"
  53 #include "telemetry/telemetry.h"
  54 #include "fc/config.h"
  55 #include "config/feature.h"
  56 #include "io/gps.h"
  57 #define IBUS_TEMPERATURE_OFFSET (0x0190)
  58
  59 typedef uint8_t ibusAddress_t;
  60
  61 typedef enum {
  62     IBUS_COMMAND_DISCOVER_SENSOR      = 0x80,
  63     IBUS_COMMAND_SENSOR_TYPE          = 0x90,
  64     IBUS_COMMAND_MEASUREMENT          = 0xA0
  65 } ibusCommand_e;
  66
  67 typedef struct IBUS_SENSOR {
  68     uint8_t type;
  69     uint8_t size;
  70     uint8_t value;
  71 } IBUS_SENSOR;
  72 static IBUS_SENSOR SENSOR_ADDRESS_TYPE_LOOKUP[] = {
  73     {.type = IBUS_MEAS_TYPE_INTERNAL_VOLTAGE, .size = 2, .value = IBUS_MEAS_VALUE_NONE },             // Address 0, sensor 1, not usable since it is reserved for internal voltage
  74     {.type = IBUS_MEAS_TYPE_EXTERNAL_VOLTAGE, .size = 2, .value = IBUS_MEAS_VALUE_EXTERNAL_VOLTAGE }, // Address 1 ,sensor 2, VBAT
  75     {.type = IBUS_MEAS_TYPE_TEMPERATURE,      .size = 2, .value = IBUS_MEAS_VALUE_TEMPERATURE },      // Address 2, sensor 3, Baro/Gyro Temp
  76     {.type = IBUS_MEAS_TYPE_RPM,              .size = 2, .value = IBUS_MEAS_VALUE_STATUS },           // Address 3, sensor 4, Status AS RPM
  77     {.type = IBUS_MEAS_TYPE_RPM,              .size = 2, .value = IBUS_MEAS_VALUE_ACC_Z },            // Address 4, sensor 5, MAG_COURSE in deg AS RPM
  78     {.type = IBUS_MEAS_TYPE_EXTERNAL_VOLTAGE, .size = 2, .value = IBUS_MEAS_VALUE_CURRENT },          // Address 5, sensor 6, Current in A AS ExtV
  79     {.type = IBUS_MEAS_TYPE_EXTERNAL_VOLTAGE, .size = 2, .value = IBUS_MEAS_VALUE_ALT },              // Address 6, sensor 7, Baro Alt in cm AS ExtV
  80     {.type = IBUS_MEAS_TYPE_RPM,              .size = 2, .value = IBUS_MEAS_VALUE_HEADING },          // Address 7, sensor 8, HOME_DIR in deg AS RPM
  81     {.type = IBUS_MEAS_TYPE_RPM,              .size = 2, .value = IBUS_MEAS_VALUE_DIST },             // Address 8, sensor 9, HOME_DIST in m AS RPM
  82     {.type = IBUS_MEAS_TYPE_RPM,              .size = 2, .value = IBUS_MEAS_VALUE_COG },              // Address 9, sensor 10,GPS_COURSE in deg AS RPM
  83     {.type = IBUS_MEAS_TYPE_RPM,              .size = 2, .value = IBUS_MEAS_VALUE_GALT },             // Address 10,sensor 11,GPS_ALT in m AS RPM (ALT m)
  84     {.type = IBUS_MEAS_TYPE_RPM,              .size = 2, .value = IBUS_MEAS_VALUE_GPS_LAT2 },         // Address 11,sensor 12,GPS_LAT2 AS RPM 5678 (-12.3456789 N)
  85     {.type = IBUS_MEAS_TYPE_RPM,              .size = 2, .value = IBUS_MEAS_VALUE_GPS_LON2 },         // Address 12,sensor 13,GPS_LON2 AS RPM 6789 (-123.4567890 E)
  86     {.type = IBUS_MEAS_TYPE_EXTERNAL_VOLTAGE, .size = 2, .value = IBUS_MEAS_VALUE_GPS_LAT1 },         // Address 13,sensor 14,GPS_LAT1 AS ExtV -12.45 (-12.3456789 N)
  87     {.type = IBUS_MEAS_TYPE_EXTERNAL_VOLTAGE, .size = 2, .value = IBUS_MEAS_VALUE_GPS_LON1 },         // Address 14,sensor 15,GPS_LON1 AS ExtV -123.45 (-123.4567890 E)
  88     {.type = IBUS_MEAS_TYPE_RPM,              .size = 2, .value = IBUS_MEAS_VALUE_SPE }               // Address 15,sensor 16,GPS_SPEED in km/h AS RPM (SPE km\h)
  89 };
  90
  91 static serialPort_t *ibusSerialPort = NULL;
  92
  93 static uint8_t transmitIbusPacket(uint8_t ibusPacket[static IBUS_MIN_LEN], size_t packetLength) {
  94     uint16_t checksum = ibusCalculateChecksum(ibusPacket, packetLength);
  95     ibusPacket[packetLength - IBUS_CHECKSUM_SIZE] = (checksum & 0xFF);
  96     ibusPacket[packetLength - IBUS_CHECKSUM_SIZE + 1] = (checksum >> 8);
  97     for (size_t i = 0; i < packetLength; i++) {
  98         serialWrite(ibusSerialPort, ibusPacket[i]);
  99     }
 100     return packetLength;
 101 }
 102
 103 static uint8_t sendIbusCommand(ibusAddress_t address) {
 104     uint8_t sendBuffer[] = { 0x04, IBUS_COMMAND_DISCOVER_SENSOR | address, 0x00, 0x00 };
 105     return transmitIbusPacket(sendBuffer, sizeof sendBuffer);
 106 }
 107
 108 static uint8_t sendIbusSensorType(ibusAddress_t address) {
 109     uint8_t sendBuffer[] = { 0x06, IBUS_COMMAND_SENSOR_TYPE | address, SENSOR_ADDRESS_TYPE_LOOKUP[address].type, SENSOR_ADDRESS_TYPE_LOOKUP[address].size, 0x0, 0x0 };
 110     return transmitIbusPacket(sendBuffer, sizeof sendBuffer);
 111 }
 112
 113 static uint8_t sendIbusMeasurement2(ibusAddress_t address, uint16_t measurement) {
 114     uint8_t sendBuffer[] = { 0x06, IBUS_COMMAND_MEASUREMENT | address, measurement & 0xFF, (measurement >> 8) & 0xFF, 0x0, 0x0 };
 115     return transmitIbusPacket(sendBuffer, sizeof sendBuffer);
 116 }
 117
 118 static uint8_t sendIbusMeasurement4(ibusAddress_t address, int32_t measurement) {
 119     uint8_t sendBuffer[] = { 0x08, IBUS_COMMAND_MEASUREMENT | address,
 120         measurement & 0xFF, (measurement >> 8) & 0xFF, (measurement >> 16) & 0xFF, (measurement >> 24) & 0xFF,
 121         0x0, 0x0 };
 122     return transmitIbusPacket(sendBuffer, sizeof sendBuffer);
 123 }
 124
 125 static bool isCommand(ibusCommand_e expected, uint8_t ibusPacket[static IBUS_MIN_LEN]) {
 126     return (ibusPacket[1] & 0xF0) == expected;
 127 }
 128
 129 static ibusAddress_t getAddress(uint8_t ibusPacket[static IBUS_MIN_LEN]) {
 130     return (ibusPacket[1] & 0x0F);
 131 }
 132
 133 // MANUAL, ACRO, AIR, ANGLE, HRZN, ALTHOLD, POSHOLD, RTH, WP, CRUISE, LAUNCH, FAILSAFE
 134 static uint8_t flightModeToIBusTelemetryMode1[FLM_COUNT] = { 0, 1, 1, 3, 2, 5, 6, 7, 4, 4, 8, 9 };
 135 static uint8_t flightModeToIBusTelemetryMode2[FLM_COUNT] = { 5, 1, 1, 0, 7, 2, 8, 6, 3, 3, 4, 9 };
 136 static uint8_t dispatchMeasurementRequest(ibusAddress_t address) {
 137 #if defined(USE_GPS)
 138     uint8_t fix = 0;
 139     if (sensors(SENSOR_GPS)
 140 #ifdef USE_GPS_FIX_ESTIMATION
 141         || STATE(GPS_ESTIMATED_FIX)
 142 #endif
 143         ) {
 144         if (gpsSol.fixType == GPS_NO_FIX) fix = 1;
 145         else if (gpsSol.fixType == GPS_FIX_2D) fix = 2;
 146         else if (gpsSol.fixType == GPS_FIX_3D) fix = 3;
 147     }
 148 #endif
 149     if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_TEMPERATURE) { //BARO_TEMP\GYRO_TEMP
 150         int16_t temperature;
 151         const bool temp_valid = sensors(SENSOR_BARO) ? getBaroTemperature(&temperature) : getIMUTemperature(&temperature);
 152         if (!temp_valid || (temperature < -400)) temperature = -400; // Minimum reported temperature is -40°C
 153         return sendIbusMeasurement2(address, (uint16_t)(temperature  + IBUS_TEMPERATURE_OFFSET));
 154     } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_RPM) {
 155         return sendIbusMeasurement2(address, (uint16_t)getThrottlePercent(osdUsingScaledThrottle()) );
 156     } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_EXTERNAL_VOLTAGE) { //VBAT
 157         if (telemetryConfig()->report_cell_voltage) {
 158             return sendIbusMeasurement2(address, getBatteryAverageCellVoltage());
 159         } else {
 160             return sendIbusMeasurement2(address, getBatteryVoltage());
 161         }
 162     } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_CURRENT) { //CURR in 10*mA, 1 = 10 mA
 163         if (isAmperageConfigured()) return sendIbusMeasurement2(address, (uint16_t) getAmperage()); //int32_t
 164         else return sendIbusMeasurement2(address, 0);
 165     } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_FUEL) { //capacity in mAh
 166         if (isAmperageConfigured()) return sendIbusMeasurement2(address, (uint16_t) getMAhDrawn()); //int32_t
 167         else return sendIbusMeasurement2(address, 0);
 168     } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_CLIMB) {
 169         return sendIbusMeasurement2(address, (int16_t) (getEstimatedActualVelocity(Z))); //
 170     } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_ACC_Z) { //MAG_COURSE 0-360*, 0=north
 171         return sendIbusMeasurement2(address, (uint16_t) (attitude.values.yaw * 10)); //in ddeg -> cdeg, 1ddeg = 10cdeg
 172     } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_ACC_Y) { //PITCH in
 173         return sendIbusMeasurement2(address, (uint16_t) (-attitude.values.pitch * 10)); //in ddeg -> cdeg, 1ddeg = 10cdeg
 174     } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_ACC_X) { //ROLL in
 175         return sendIbusMeasurement2(address, (uint16_t) (attitude.values.roll * 10)); //in ddeg -> cdeg, 1ddeg = 10cdeg
 176     } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_VSPEED) { //Speed cm/s
 177 #ifdef USE_PITOT
 178         if (sensors(SENSOR_PITOT) && pitotIsHealthy()) return sendIbusMeasurement2(address, (uint16_t)getAirspeedEstimate()); //int32_t
 179         else
 180 #endif
 181         return sendIbusMeasurement2(address, 0);
 182     } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_ARMED) { //motorArmed
 183         if ((telemetryConfig()->ibusTelemetryType & 0x7F) < 8) {
 184             return sendIbusMeasurement2(address, ARMING_FLAG(ARMED) ? 0 : 1);
 185         } else {
 186             return sendIbusMeasurement2(address, ARMING_FLAG(ARMED) ? 1 : 0);
 187         }
 188     } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_MODE) {
 189         uint16_t flightMode = flightModeToIBusTelemetryMode2[getFlightModeForTelemetry()];
 190         return sendIbusMeasurement2(address, flightMode);
 191     } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_PRES) { //PRESSURE in dPa -> 9876 is 987.6 hPa
 192         if (sensors(SENSOR_BARO)) return sendIbusMeasurement2(address, (int16_t) (baro.baroPressure / 10)); //int32_t
 193         else return sendIbusMeasurement2(address, 0);
 194     } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_ALT) { //BARO_ALT in cm => m
 195         if (sensors(SENSOR_BARO)) return sendIbusMeasurement2(address, (uint16_t) baro.BaroAlt); //int32_t
 196         else return sendIbusMeasurement2(address, 0);
 197     } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_ALT4) { //BARO_ALT //In cm => m
 198         if (sensors(SENSOR_BARO)) return sendIbusMeasurement4(address, (int32_t) baro.BaroAlt); //int32_t
 199         else return sendIbusMeasurement4(address, 0);
 200     } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_STATUS) { //STATUS sat num AS #0, FIX AS 0, HDOP AS 0, Mode AS 0
 201         uint16_t status = flightModeToIBusTelemetryMode1[getFlightModeForTelemetry()];
 202 #if defined(USE_GPS)
 203         if (sensors(SENSOR_GPS)
 204 #ifdef USE_GPS_FIX_ESTIMATION
 205             || STATE(GPS_ESTIMATED_FIX)
 206 #endif
 207             ) {
 208             status += gpsSol.numSat * 1000;
 209             status += fix * 100;
 210             if (STATE(GPS_FIX_HOME)) status += 500;
 211             status += constrain(gpsSol.hdop / 1000, 0, 9) * 10;
 212         }
 213 #endif
 214         return sendIbusMeasurement2(address, status);
 215     } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_HEADING) { //HOME_DIR 0-360deg
 216 #if defined(USE_GPS)
 217         if (sensors(SENSOR_GPS)
 218 #ifdef USE_GPS_FIX_ESTIMATION
 219             || STATE(GPS_ESTIMATED_FIX)
 220 #endif
 221             ) return sendIbusMeasurement2(address, (uint16_t) GPS_directionToHome); else //int16_t
 222 #endif
 223         return sendIbusMeasurement2(address, 0);
 224     } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_DIST) { //HOME_DIST in m
 225 #if defined(USE_GPS)
 226         if (sensors(SENSOR_GPS)
 227 #ifdef USE_GPS_FIX_ESTIMATION
 228                 || STATE(GPS_ESTIMATED_FIX)
 229 #endif
 230             ) return sendIbusMeasurement2(address, (uint16_t) GPS_distanceToHome); else //uint16_t
 231 #endif
 232         return sendIbusMeasurement2(address, 0);
 233     } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_SPE) { //GPS_SPEED in cm/s => km/h, 1cm/s = 0.036 km/h
 234 #if defined(USE_GPS)
 235         if (sensors(SENSOR_GPS)
 236 #ifdef USE_GPS_FIX_ESTIMATION
 237                 || STATE(GPS_ESTIMATED_FIX)
 238 #endif
 239             ) return sendIbusMeasurement2(address, (uint16_t) gpsSol.groundSpeed * 36 / 100); else //int16_t
 240 #endif
 241         return sendIbusMeasurement2(address, 0);
 242     } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_SPEED) {//SPEED in cm/s
 243 #if defined(USE_GPS)
 244         if (sensors(SENSOR_GPS)
 245 #ifdef USE_GPS_FIX_ESTIMATION
 246             || STATE(GPS_ESTIMATED_FIX)
 247 #endif
 248         ) return sendIbusMeasurement2(address, (uint16_t) gpsSol.groundSpeed); //int16_t
 249 #endif
 250         return sendIbusMeasurement2(address, 0);
 251     } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_COG) { //GPS_COURSE (0-360deg, 0=north)
 252 #if defined(USE_GPS)
 253         if (sensors(SENSOR_GPS)
 254 #ifdef USE_GPS_FIX_ESTIMATION
 255             || STATE(GPS_ESTIMATED_FIX)
 256 #endif
 257         ) return sendIbusMeasurement2(address, (uint16_t) (gpsSol.groundCourse / 10)); else //int16_t
 258 #endif
 259         return sendIbusMeasurement2(address, 0);
 260     } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_STATUS) { //GPS_STATUS fix sat
 261 #if defined(USE_GPS)
 262         if (sensors(SENSOR_GPS)
 263 #ifdef USE_GPS_FIX_ESTIMATION
 264             || STATE(GPS_ESTIMATED_FIX)
 265 #endif
 266         ) return sendIbusMeasurement2(address, (((uint16_t)fix)<<8) + gpsSol.numSat); else //uint8_t, uint8_t
 267 #endif
 268         return sendIbusMeasurement2(address, 0);
 269     } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LAT) { //4byte
 270 #if defined(USE_GPS)
 271         if (sensors(SENSOR_GPS)
 272 #ifdef USE_GPS_FIX_ESTIMATION
 273             || STATE(GPS_ESTIMATED_FIX)
 274 #endif
 275         ) return sendIbusMeasurement4(address, (int32_t)gpsSol.llh.lat); else //int32_t
 276 #endif
 277         return sendIbusMeasurement4(address, 0);
 278     } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LON) { //4byte
 279 #if defined(USE_GPS)
 280         if (sensors(SENSOR_GPS)
 281 #ifdef USE_GPS_FIX_ESTIMATION
 282             || STATE(GPS_ESTIMATED_FIX)
 283 #endif
 284         ) return sendIbusMeasurement4(address, (int32_t)gpsSol.llh.lon); else //int32_t
 285 #endif
 286         return sendIbusMeasurement4(address, 0);
 287     } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LAT1) { //GPS_LAT1 //Lattitude * 1e+7
 288 #if defined(USE_GPS)
 289         if (sensors(SENSOR_GPS)
 290 #ifdef USE_GPS_FIX_ESTIMATION
 291             || STATE(GPS_ESTIMATED_FIX)
 292 #endif
 293         ) return sendIbusMeasurement2(address, (uint16_t) (gpsSol.llh.lat / 100000)); else
 294 #endif
 295         return sendIbusMeasurement2(address, 0);
 296     } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LON1) { //GPS_LON1 //Longitude * 1e+7
 297 #if defined(USE_GPS)
 298         if (sensors(SENSOR_GPS)
 299 #ifdef USE_GPS_FIX_ESTIMATION
 300             || STATE(GPS_ESTIMATED_FIX)
 301 #endif
 302         ) return sendIbusMeasurement2(address, (uint16_t) (gpsSol.llh.lon / 100000)); else
 303 #endif
 304         return sendIbusMeasurement2(address, 0);
 305     } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LAT2) { //GPS_LAT2 //Lattitude * 1e+7
 306 #if defined(USE_GPS)
 307         if (sensors(SENSOR_GPS)
 308 #ifdef USE_GPS_FIX_ESTIMATION
 309             || STATE(GPS_ESTIMATED_FIX)
 310 #endif
 311         ) return sendIbusMeasurement2(address, (uint16_t) ((gpsSol.llh.lat % 100000)/10));
 312 #endif
 313         return sendIbusMeasurement2(address, 0);
 314     } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LON2) { //GPS_LON2 //Longitude * 1e+7
 315 #if defined(USE_GPS)
 316         if (sensors(SENSOR_GPS)
 317 #ifdef USE_GPS_FIX_ESTIMATION
 318             || STATE(GPS_ESTIMATED_FIX)
 319 #endif
 320         ) return sendIbusMeasurement2(address, (uint16_t) ((gpsSol.llh.lon % 100000)/10)); else
 321 #endif
 322         return sendIbusMeasurement2(address, 0);
 323     } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GALT4) { //GPS_ALT //In cm => m
 324 #if defined(USE_GPS)
 325         if (sensors(SENSOR_GPS)
 326 #ifdef USE_GPS_FIX_ESTIMATION
 327             || STATE(GPS_ESTIMATED_FIX)
 328 #endif
 329         ) return sendIbusMeasurement4(address, (int32_t) (gpsSol.llh.alt)); else //int32_t
 330 #endif
 331         return sendIbusMeasurement4(address, 0);
 332     } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GALT) { //GPS_ALT //In cm => m
 333 #if defined(USE_GPS)
 334         if (sensors(SENSOR_GPS)
 335 #ifdef USE_GPS_FIX_ESTIMATION
 336             || STATE(GPS_ESTIMATED_FIX)
 337 #endif
 338         ) return sendIbusMeasurement2(address, (uint16_t) (gpsSol.llh.alt / 100)); else //int32_t
 339 #endif
 340         return sendIbusMeasurement2(address, 0);
 341     }
 342     else return 0;
 343 }
 344
 345 uint8_t respondToIbusRequest(uint8_t ibusPacket[static IBUS_RX_BUF_LEN]) {
 346     ibusAddress_t returnAddress = getAddress(ibusPacket);
 347     if (returnAddress < sizeof SENSOR_ADDRESS_TYPE_LOOKUP) {
 348         if (isCommand(IBUS_COMMAND_DISCOVER_SENSOR, ibusPacket)) {
 349             return sendIbusCommand(returnAddress);
 350         } else if (isCommand(IBUS_COMMAND_SENSOR_TYPE, ibusPacket)) {
 351             return sendIbusSensorType(returnAddress);
 352         } else if (isCommand(IBUS_COMMAND_MEASUREMENT, ibusPacket)) {
 353             return dispatchMeasurementRequest(returnAddress);
 354         }
 355     }
 356     return 0;
 357 }
 358
 359 void initSharedIbusTelemetry(serialPort_t *port) {
 360     ibusSerialPort = port;
 361 }
 362
 363 void changeTypeIbusTelemetry(uint8_t id, uint8_t type, uint8_t value) {
 364     SENSOR_ADDRESS_TYPE_LOOKUP[id].type = type;
 365     SENSOR_ADDRESS_TYPE_LOOKUP[id].value = value;
 366     if (value == IBUS_MEAS_VALUE_GPS) SENSOR_ADDRESS_TYPE_LOOKUP[id].size = 14;
 367     else if (value == IBUS_MEAS_VALUE_GPS_LAT || value == IBUS_MEAS_VALUE_GPS_LON || value == IBUS_MEAS_VALUE_ALT4 || value == IBUS_MEAS_VALUE_GALT4)
 368       SENSOR_ADDRESS_TYPE_LOOKUP[id].size = 4;
 369     else SENSOR_ADDRESS_TYPE_LOOKUP[id].size = 2;
 370 }
 371
 372 #endif //defined(USE_TELEMETRY) && defined(USE_TELEMETRY_IBUS)