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