src/main/telemetry/ibus_shared.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 /*
  22  * FlySky iBus telemetry implementation by CraigJPerry.
  23  * Unit tests and some additions by Unitware
  24  *
  25  * Many thanks to Dave Borthwick's iBus telemetry dongle converter for
  26  * PIC 12F1572 (also distributed under GPLv3) which was referenced to
  27  * clarify the protocol.
  28  */
  29
  30
  31 #include <stdbool.h>
  32 #include <stdint.h>
  33 #include <stdlib.h>
  34 #include <limits.h>
  35
  36 #include "platform.h"
  37 #include "telemetry/telemetry.h"
  38 #include "telemetry/ibus_shared.h"
  39
  40 static uint16_t calculateChecksum(const uint8_t *ibusPacket);
  41
  42 #if defined(USE_TELEMETRY_IBUS)
  43 #include "config/feature.h"
  44 #include "pg/pg.h"
  45 #include "pg/pg_ids.h"
  46 #include "sensors/battery.h"
  47 #include "fc/rc_controls.h"
  48 #include "fc/config.h"
  49 #include "sensors/gyro.h"
  50 #include "drivers/accgyro/accgyro.h"
  51 #include "fc/runtime_config.h"
  52 #include "sensors/acceleration.h"
  53 #include "sensors/sensors.h"
  54 #include "sensors/barometer.h"
  55 #include "flight/imu.h"
  56 #include "flight/position.h"
  57 #include "io/gps.h"
  58
  59
  60 #define IBUS_TEMPERATURE_OFFSET     400
  61 #define INVALID_IBUS_ADDRESS        0
  62 #define IBUS_BUFFSIZE               33 // biggest iBus message seen so far + 1
  63 #define IBUS_HEADER_FOOTER_SIZE     4
  64 #define IBUS_2BYTE_SESNSOR          2
  65 #define IBUS_4BYTE_SESNSOR          4
  66
  67 typedef uint8_t ibusAddress_t;
  68
  69 typedef enum {
  70     IBUS_COMMAND_DISCOVER_SENSOR      = 0x80,
  71     IBUS_COMMAND_SENSOR_TYPE          = 0x90,
  72     IBUS_COMMAND_MEASUREMENT          = 0xA0
  73 } ibusCommand_e;
  74
  75 typedef union ibusTelemetry {
  76     uint16_t uint16;
  77     uint32_t uint32;
  78     int16_t int16;
  79     int32_t int32;
  80     uint8_t byte[4];
  81 } ibusTelemetry_s;
  82
  83 #if defined(USE_GPS)
  84
  85 const uint8_t GPS_IDS[] = {
  86     IBUS_SENSOR_TYPE_GPS_STATUS,
  87     IBUS_SENSOR_TYPE_SPE,
  88     IBUS_SENSOR_TYPE_GPS_LAT,
  89     IBUS_SENSOR_TYPE_GPS_LON,
  90     IBUS_SENSOR_TYPE_GPS_ALT,
  91     IBUS_SENSOR_TYPE_GROUND_SPEED,
  92     IBUS_SENSOR_TYPE_ODO1,
  93     IBUS_SENSOR_TYPE_ODO2,
  94     IBUS_SENSOR_TYPE_GPS_DIST,
  95     IBUS_SENSOR_TYPE_COG,
  96 };
  97 #endif
  98
  99 #if defined(USE_TELEMETRY_IBUS_EXTENDED)
 100
 101 const uint8_t FULL_GPS_IDS[] = {
 102     IBUS_SENSOR_TYPE_GPS_STATUS,
 103     IBUS_SENSOR_TYPE_GPS_LAT,
 104     IBUS_SENSOR_TYPE_GPS_LON,
 105     IBUS_SENSOR_TYPE_GPS_ALT,
 106 };
 107
 108 const uint8_t FULL_VOLT_IDS[] = {
 109     IBUS_SENSOR_TYPE_EXTERNAL_VOLTAGE,
 110     IBUS_SENSOR_TYPE_CELL,
 111     IBUS_SENSOR_TYPE_BAT_CURR,
 112     IBUS_SENSOR_TYPE_FUEL,
 113     IBUS_SENSOR_TYPE_RPM,
 114 };
 115
 116 const uint8_t FULL_ACC_IDS[] = {
 117     IBUS_SENSOR_TYPE_ACC_X,
 118     IBUS_SENSOR_TYPE_ACC_Y,
 119     IBUS_SENSOR_TYPE_ACC_Z,
 120     IBUS_SENSOR_TYPE_ROLL,
 121     IBUS_SENSOR_TYPE_PITCH,
 122     IBUS_SENSOR_TYPE_YAW,
 123 };
 124
 125 #endif //defined(USE_TELEMETRY_IBUS_EXTENDED)
 126
 127 static serialPort_t *ibusSerialPort = NULL;
 128 static ibusAddress_t ibusBaseAddress = INVALID_IBUS_ADDRESS;
 129 static uint8_t sendBuffer[IBUS_BUFFSIZE];
 130
 131
 132 static void setValue(uint8_t* bufferPtr, uint8_t sensorType, uint8_t length);
 133
 134 static uint8_t getSensorID(ibusAddress_t address)
 135 {
 136     //all checks are done in theAddressIsWithinOurRange
 137     uint32_t index = address - ibusBaseAddress;
 138     return telemetryConfig()->flysky_sensors[index];
 139 }
 140
 141 static uint8_t getSensorLength(uint8_t sensorID)
 142 {
 143     if (sensorID == IBUS_SENSOR_TYPE_PRES || (sensorID >= IBUS_SENSOR_TYPE_GPS_LAT && sensorID <= IBUS_SENSOR_TYPE_ALT_MAX)) {
 144         return IBUS_4BYTE_SESNSOR;
 145     }
 146 #if defined(USE_TELEMETRY_IBUS_EXTENDED)
 147     if (sensorID == IBUS_SENSOR_TYPE_GPS_FULL) {
 148         return 14;
 149     }
 150     if (sensorID == IBUS_SENSOR_TYPE_VOLT_FULL) {
 151         return 10;
 152     }
 153     if (sensorID == IBUS_SENSOR_TYPE_VOLT_FULL) {
 154         return 12;
 155     }
 156 #endif
 157     return IBUS_2BYTE_SESNSOR;
 158 }
 159
 160 static uint8_t transmitIbusPacket()
 161 {
 162     unsigned frameLength = sendBuffer[0];
 163     if (frameLength == INVALID_IBUS_ADDRESS) {
 164         return 0;
 165     }
 166     unsigned payloadLength = frameLength - IBUS_CHECKSUM_SIZE;
 167     uint16_t checksum = calculateChecksum(sendBuffer);
 168     for (unsigned i = 0; i < payloadLength; i++) {
 169         serialWrite(ibusSerialPort, sendBuffer[i]);
 170     }
 171     serialWrite(ibusSerialPort, checksum & 0xFF);
 172     serialWrite(ibusSerialPort, checksum >> 8);
 173     return frameLength;
 174 }
 175
 176 static void setIbusDiscoverSensorReply(ibusAddress_t address)
 177 {
 178     sendBuffer[0] = IBUS_HEADER_FOOTER_SIZE;
 179     sendBuffer[1] = IBUS_COMMAND_DISCOVER_SENSOR | address;
 180 }
 181
 182 static void setIbusSensorType(ibusAddress_t address)
 183 {
 184     uint8_t sensorID = getSensorID(address);
 185     uint8_t sensorLength = getSensorLength(sensorID);
 186     sendBuffer[0] = IBUS_HEADER_FOOTER_SIZE + 2;
 187     sendBuffer[1] = IBUS_COMMAND_SENSOR_TYPE | address;
 188     sendBuffer[2] = sensorID;
 189     sendBuffer[3] = sensorLength;
 190 }
 191
 192 static uint16_t getVoltage()
 193 {
 194     uint16_t voltage = getBatteryVoltage();
 195     if (telemetryConfig()->report_cell_voltage) {
 196         voltage /= getBatteryCellCount();
 197     }
 198     return voltage;
 199 }
 200
 201 static uint16_t getTemperature()
 202 {
 203     uint16_t temperature = gyroGetTemperature() * 10;
 204 #if defined(USE_BARO)
 205     if (sensors(SENSOR_BARO)) {
 206         temperature = (uint16_t) ((baro.baroTemperature + 50) / 10);
 207     }
 208 #endif
 209     return temperature + IBUS_TEMPERATURE_OFFSET;
 210 }
 211
 212
 213 static uint16_t getFuel()
 214 {
 215     uint16_t fuel = 0;
 216     if (batteryConfig()->batteryCapacity > 0) {
 217         fuel = (uint16_t)calculateBatteryPercentageRemaining();
 218     } else {
 219         fuel = (uint16_t)constrain(getMAhDrawn(), 0, 0xFFFF);
 220     }
 221     return fuel;
 222 }
 223
 224 static uint16_t getRPM()
 225 {
 226     uint16_t rpm = 0;
 227     if (ARMING_FLAG(ARMED)) {
 228         const throttleStatus_e throttleStatus = calculateThrottleStatus();
 229         rpm = rcCommand[THROTTLE];  // / BLADE_NUMBER_DIVIDER;
 230         if (throttleStatus == THROTTLE_LOW && featureIsEnabled(FEATURE_MOTOR_STOP)) rpm = 0;
 231     } else {
 232         rpm = (uint16_t)(batteryConfig()->batteryCapacity); //  / BLADE_NUMBER_DIVIDER
 233     }
 234     return rpm;
 235 }
 236
 237 static uint16_t getMode()
 238 {
 239     uint16_t flightMode = 1; //Acro
 240     if (FLIGHT_MODE(ANGLE_MODE)) {
 241          flightMode = 0; //Stab
 242     }
 243     if (FLIGHT_MODE(PASSTHRU_MODE)) {
 244         flightMode = 3; //Auto
 245     }
 246     if (FLIGHT_MODE(HEADFREE_MODE) || FLIGHT_MODE(MAG_MODE)) {
 247         flightMode = 4; //Guided! (there in no HEAD, MAG so use Guided)
 248     }
 249     if (FLIGHT_MODE(HORIZON_MODE)) {
 250         flightMode = 7; //Circle! (there in no horizon so use Circle)
 251     }
 252     if (FLIGHT_MODE(FAILSAFE_MODE)) {
 253         flightMode = 9; //Land
 254     }
 255     return flightMode;
 256 }
 257
 258 #if defined(USE_ACC)
 259 static int16_t getACC(uint8_t index)
 260 {
 261     return (int16_t)((acc.accADC[index] * acc.dev.acc_1G_rec) * 1000);
 262 }
 263 #endif
 264
 265 #if defined(USE_TELEMETRY_IBUS_EXTENDED)
 266 static void setCombinedFrame(uint8_t* bufferPtr, const uint8_t* structure, uint8_t itemCount)
 267 {
 268     uint8_t offset = 0;
 269     uint8_t size = 0;
 270     for (unsigned i = 0; i < itemCount; i++) {
 271         size = getSensorLength(structure[i]);
 272         setValue(bufferPtr + offset, structure[i], size);
 273         offset += size;
 274     }
 275 }
 276 #endif
 277
 278
 279
 280 #if defined(USE_GPS)
 281 static bool setGPS(uint8_t sensorType, ibusTelemetry_s* value)
 282 {
 283     bool result = false;
 284     for (unsigned i = 0; i < sizeof(GPS_IDS); i++) {
 285         if (sensorType == GPS_IDS[i]) {
 286             result = true;
 287             break;
 288         }
 289     }
 290     if (!result) return result;
 291
 292     uint16_t gpsFixType = 0;
 293     uint16_t sats = 0;
 294     if (sensors(SENSOR_GPS)) {
 295         gpsFixType = !STATE(GPS_FIX) ? 1 : (gpsSol.numSat < 5 ? 2 : 3);
 296         sats = gpsSol.numSat;
 297         if (STATE(GPS_FIX) || sensorType == IBUS_SENSOR_TYPE_GPS_STATUS) {
 298             result = true;
 299             switch (sensorType) {
 300             case IBUS_SENSOR_TYPE_SPE:
 301                 value->uint16 = gpsSol.groundSpeed * 36 / 100;
 302                 break;
 303             case IBUS_SENSOR_TYPE_GPS_LAT:
 304                 value->int32 = gpsSol.llh.lat;
 305                 break;
 306             case IBUS_SENSOR_TYPE_GPS_LON:
 307                 value->int32 = gpsSol.llh.lon;
 308                 break;
 309             case IBUS_SENSOR_TYPE_GPS_ALT:
 310                 value->int32 = (int32_t)gpsSol.llh.altCm;
 311                 break;
 312             case IBUS_SENSOR_TYPE_GROUND_SPEED:
 313                 value->uint16 = gpsSol.groundSpeed;
 314                 break;
 315             case IBUS_SENSOR_TYPE_ODO1:
 316             case IBUS_SENSOR_TYPE_ODO2:
 317             case IBUS_SENSOR_TYPE_GPS_DIST:
 318                 value->uint16 = GPS_distanceToHome;
 319                 break;
 320             case IBUS_SENSOR_TYPE_COG:
 321                 value->uint16 = gpsSol.groundCourse * 100;
 322                 break;
 323             case IBUS_SENSOR_TYPE_GPS_STATUS:
 324                 value->byte[0] = gpsFixType;
 325                 value->byte[1] = sats;
 326                 break;
 327             }
 328         }
 329     }
 330     return result;
 331 }
 332 #endif //defined(USE_GPS)
 333
 334 static void setValue(uint8_t* bufferPtr, uint8_t sensorType, uint8_t length)
 335 {
 336     ibusTelemetry_s value;
 337
 338 #if defined(USE_TELEMETRY_IBUS_EXTENDED)
 339     const uint8_t* structure = 0;
 340     uint8_t itemCount;
 341     if (sensorType == IBUS_SENSOR_TYPE_GPS_FULL) {
 342         structure = FULL_GPS_IDS;
 343         itemCount = sizeof(FULL_GPS_IDS);
 344     }
 345     if (sensorType == IBUS_SENSOR_TYPE_VOLT_FULL) {
 346         structure = FULL_VOLT_IDS;
 347         itemCount = sizeof(FULL_VOLT_IDS);
 348     }
 349     if (sensorType == IBUS_SENSOR_TYPE_ACC_FULL) {
 350         structure = FULL_ACC_IDS;
 351         itemCount = sizeof(FULL_ACC_IDS);
 352     }
 353     if (structure != 0) {
 354         setCombinedFrame(bufferPtr, structure, sizeof(itemCount));
 355         return;
 356     }
 357 #endif //defined(USE_TELEMETRY_IBUS_EXTENDED)
 358
 359 #if defined(USE_GPS)
 360     if (setGPS(sensorType, &value)) {
 361         return;
 362     }
 363 #endif //defined(USE_TELEMETRY_IBUS_EXTENDED)
 364
 365     for (unsigned i = 0; i < length; i++) {
 366         bufferPtr[i] = value.byte[i] = 0;
 367     }
 368     switch (sensorType) {
 369         case IBUS_SENSOR_TYPE_EXTERNAL_VOLTAGE:
 370             value.uint16 = getVoltage();
 371             break;
 372         case IBUS_SENSOR_TYPE_TEMPERATURE:
 373             value.uint16 = getTemperature();
 374             break;
 375         case IBUS_SENSOR_TYPE_RPM_FLYSKY:
 376             value.int16 = (int16_t)rcCommand[THROTTLE];
 377             break;
 378         case IBUS_SENSOR_TYPE_FUEL:
 379             value.uint16 = getFuel();
 380             break;
 381         case IBUS_SENSOR_TYPE_RPM:
 382             value.uint16 = getRPM();
 383             break;
 384         case IBUS_SENSOR_TYPE_FLIGHT_MODE:
 385             value.uint16 = getMode();
 386             break;
 387         case IBUS_SENSOR_TYPE_CELL:
 388             value.uint16 = (uint16_t)(getBatteryAverageCellVoltage());
 389             break;
 390         case IBUS_SENSOR_TYPE_BAT_CURR:
 391             value.uint16 = (uint16_t)getAmperage();
 392             break;
 393 #if defined(USE_ACC)
 394         case IBUS_SENSOR_TYPE_ACC_X:
 395         case IBUS_SENSOR_TYPE_ACC_Y:
 396         case IBUS_SENSOR_TYPE_ACC_Z:
 397             value.int16 = getACC(sensorType - IBUS_SENSOR_TYPE_ACC_X);
 398             break;
 399 #endif
 400         case IBUS_SENSOR_TYPE_ROLL:
 401         case IBUS_SENSOR_TYPE_PITCH:
 402         case IBUS_SENSOR_TYPE_YAW:
 403             value.int16 = attitude.raw[sensorType - IBUS_SENSOR_TYPE_ROLL] *10;
 404             break;
 405         case IBUS_SENSOR_TYPE_ARMED:
 406             value.uint16 = ARMING_FLAG(ARMED) ? 1 : 0;
 407             break;
 408 #if defined(USE_TELEMETRY_IBUS_EXTENDED)
 409         case IBUS_SENSOR_TYPE_CMP_HEAD:
 410             value.uint16 = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
 411             break;
 412 #ifdef USE_VARIO
 413         case IBUS_SENSOR_TYPE_VERTICAL_SPEED:
 414         case IBUS_SENSOR_TYPE_CLIMB_RATE:
 415             value.int16 = (int16_t) constrain(getEstimatedVario(), SHRT_MIN, SHRT_MAX);
 416             break;
 417 #endif
 418 #ifdef USE_BARO
 419         case IBUS_SENSOR_TYPE_ALT:
 420         case IBUS_SENSOR_TYPE_ALT_MAX:
 421             value.int32 = baro.BaroAlt;
 422             break;
 423         case IBUS_SENSOR_TYPE_PRES:
 424             value.uint32 = baro.baroPressure | (((uint32_t)getTemperature()) << 19);
 425             break;
 426 #endif
 427 #endif //defined(TELEMETRY_IBUS_EXTENDED)
 428     }
 429     for (unsigned i = 0; i < length; i++) {
 430         bufferPtr[i] = value.byte[i];
 431     }
 432 }
 433 static void setIbusMeasurement(ibusAddress_t address)
 434 {
 435     uint8_t sensorID = getSensorID(address);
 436     uint8_t sensorLength = getSensorLength(sensorID);
 437     sendBuffer[0] = IBUS_HEADER_FOOTER_SIZE + sensorLength;
 438     sendBuffer[1] = IBUS_COMMAND_MEASUREMENT | address;
 439     setValue(sendBuffer + 2, sensorID, sensorLength);
 440 }
 441
 442 static bool isCommand(ibusCommand_e expected, const uint8_t *ibusPacket)
 443 {
 444     return (ibusPacket[1] & 0xF0) == expected;
 445 }
 446
 447 static ibusAddress_t getAddress(const uint8_t *ibusPacket)
 448 {
 449     return (ibusPacket[1] & 0x0F);
 450 }
 451
 452 static void autodetectFirstReceivedAddressAsBaseAddress(ibusAddress_t returnAddress)
 453 {
 454     if ((INVALID_IBUS_ADDRESS == ibusBaseAddress) &&
 455     (INVALID_IBUS_ADDRESS != returnAddress)) {
 456         ibusBaseAddress = returnAddress;
 457     }
 458 }
 459
 460 static bool theAddressIsWithinOurRange(ibusAddress_t returnAddress)
 461 {
 462     return (returnAddress >= ibusBaseAddress) &&
 463     (ibusAddress_t)(returnAddress - ibusBaseAddress) < ARRAYLEN(telemetryConfig()->flysky_sensors) &&
 464     telemetryConfig()->flysky_sensors[(returnAddress - ibusBaseAddress)] != IBUS_SENSOR_TYPE_NONE;
 465 }
 466
 467 uint8_t respondToIbusRequest(uint8_t const * const ibusPacket)
 468 {
 469     ibusAddress_t returnAddress = getAddress(ibusPacket);
 470     autodetectFirstReceivedAddressAsBaseAddress(returnAddress);
 471     //set buffer to invalid
 472     sendBuffer[0] = INVALID_IBUS_ADDRESS;
 473
 474     if (theAddressIsWithinOurRange(returnAddress)) {
 475         if (isCommand(IBUS_COMMAND_DISCOVER_SENSOR, ibusPacket)) {
 476             setIbusDiscoverSensorReply(returnAddress);
 477         } else if (isCommand(IBUS_COMMAND_SENSOR_TYPE, ibusPacket)) {
 478             setIbusSensorType(returnAddress);
 479         } else if (isCommand(IBUS_COMMAND_MEASUREMENT, ibusPacket)) {
 480             setIbusMeasurement(returnAddress);
 481         }
 482     }
 483     //transmit if content was set
 484     return transmitIbusPacket();
 485 }
 486
 487
 488 void initSharedIbusTelemetry(serialPort_t *port)
 489 {
 490     ibusSerialPort = port;
 491     ibusBaseAddress = INVALID_IBUS_ADDRESS;
 492 }
 493
 494
 495 #endif //defined(USE_TELEMETRY) && defined(USE_TELEMETRY_IBUS)
 496
 497 static uint16_t calculateChecksum(const uint8_t *ibusPacket)
 498 {
 499     uint16_t checksum = 0xFFFF;
 500     uint8_t dataSize = ibusPacket[0] - IBUS_CHECKSUM_SIZE;
 501     for (unsigned i = 0; i < dataSize; i++) {
 502         checksum -= ibusPacket[i];
 503     }
 504
 505     return checksum;
 506 }
 507
 508 bool isChecksumOkIa6b(const uint8_t *ibusPacket, const uint8_t length)
 509 {
 510     uint16_t calculatedChecksum = calculateChecksum(ibusPacket);
 511
 512     // Note that there's a byte order swap to little endian here
 513     return (calculatedChecksum >> 8) == ibusPacket[length - 1]
 514            && (calculatedChecksum & 0xFF) == ibusPacket[length - 2];
 515 }