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