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