src/main/telemetry/jetiexbus.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 #include <stdbool.h>
  22 #include <stdint.h>
  23 #include <string.h>
  24
  25 #include "platform.h"
  26
  27 #ifdef USE_SERIAL_RX
  28 #ifdef USE_TELEMETRY
  29
  30 #include "build/build_config.h"
  31 #include "build/debug.h"
  32 #include "fc/runtime_config.h"
  33
  34 #include "common/utils.h"
  35 #include "common/bitarray.h"
  36
  37 #include "drivers/serial.h"
  38 #include "drivers/serial_uart.h"
  39 #include "drivers/time.h"
  40
  41 #include "flight/altitude.h"
  42 #include "flight/imu.h"
  43
  44 #include "io/serial.h"
  45 #include "rx/rx.h"
  46 #include "rx/jetiexbus.h"
  47
  48 #include "sensors/battery.h"
  49 #include "sensors/sensors.h"
  50
  51 #include "telemetry/jetiexbus.h"
  52 #include "telemetry/telemetry.h"
  53
  54 #define EXTEL_DATA_MSG      (0x40)
  55 #define EXTEL_UNMASK_TYPE   (0x3F)
  56 #define EXTEL_SYNC_LEN      1
  57 #define EXTEL_CRC_LEN       1
  58 #define EXTEL_HEADER_LEN    6
  59 #define EXTEL_MAX_LEN       28
  60 #define EXTEL_OVERHEAD      (EXTEL_SYNC_LEN + EXTEL_HEADER_LEN + EXTEL_CRC_LEN)
  61 #define EXTEL_MAX_PAYLOAD   (EXTEL_MAX_LEN - EXTEL_OVERHEAD)
  62 #define EXBUS_MAX_REQUEST_BUFFER_SIZE   (EXBUS_OVERHEAD + EXTEL_MAX_LEN)
  63
  64 enum exTelHeader_e {
  65     EXTEL_HEADER_SYNC = 0,
  66     EXTEL_HEADER_TYPE_LEN,
  67     EXTEL_HEADER_USN_LB,
  68     EXTEL_HEADER_USN_HB,
  69     EXTEL_HEADER_LSN_LB,
  70     EXTEL_HEADER_LSN_HB,
  71     EXTEL_HEADER_RES,
  72     EXTEL_HEADER_ID,
  73     EXTEL_HEADER_DATA
  74 };
  75
  76 enum {
  77     EXBUS_TRANS_ZERO = 0,
  78     EXBUS_TRANS_RX_READY,
  79     EXBUS_TRANS_RX,
  80     EXBUS_TRANS_IS_TX_COMPLETED,
  81     EXBUS_TRANS_TX
  82 };
  83
  84 enum exDataType_e {
  85     EX_TYPE_6b   = 0,                // int6_t  Data type 6b (-31 ¸31)
  86     EX_TYPE_14b  = 1,                // int14_t Data type 14b (-8191 ¸8191)
  87     EX_TYPE_22b  = 4,                // int22_t Data type 22b (-2097151 ¸2097151)
  88     EX_TYPE_DT   = 5,                // int22_t Special data type – time and date
  89     EX_TYPE_30b  = 8,                // int30_t Data type 30b (-536870911 ¸536870911)
  90     EX_TYPE_GPS  = 9,                // int30_t Special data type – GPS coordinates:  lo/hi minute - lo/hi degree.
  91     EX_TYPE_DES  = 255               // only for devicedescription
  92 };
  93
  94 const uint8_t exDataTypeLen[] = {
  95     [EX_TYPE_6b]  = 1,
  96     [EX_TYPE_14b] = 2,
  97     [EX_TYPE_22b] = 3,
  98     [EX_TYPE_DT]  = 3,
  99     [EX_TYPE_30b] = 4,
 100     [EX_TYPE_GPS] = 4
 101 };
 102
 103 typedef struct exBusSensor_s {
 104     const char *label;
 105     const char *unit;
 106     const uint8_t exDataType;
 107     const uint8_t decimals;
 108 } exBusSensor_t;
 109
 110 #define DECIMAL_MASK(decimals) (decimals << 5)
 111
 112 // list of telemetry messages
 113 // after every 15 sensors a new header has to be inserted (e.g. "BF D2")
 114 const exBusSensor_t jetiExSensors[] = {
 115     {"BF D1",       "",      EX_TYPE_DES,   0              },     // device descripton
 116     {"Voltage",     "V",     EX_TYPE_14b,   DECIMAL_MASK(1)},
 117     {"Current",     "A",     EX_TYPE_14b,   DECIMAL_MASK(2)},
 118     {"Altitude",    "m",     EX_TYPE_14b,   DECIMAL_MASK(2)},
 119     {"Capacity",    "mAh",   EX_TYPE_22b,   DECIMAL_MASK(0)},
 120     {"Power",       "W",     EX_TYPE_22b,   DECIMAL_MASK(1)},
 121     {"Roll angle",  "\xB0",  EX_TYPE_14b,   DECIMAL_MASK(1)},
 122     {"Pitch angle", "\xB0",  EX_TYPE_14b,   DECIMAL_MASK(1)},
 123     {"Heading",     "\xB0",  EX_TYPE_14b,   DECIMAL_MASK(1)},
 124     {"Vario",       "m/s",   EX_TYPE_22b,   DECIMAL_MASK(2)}
 125 };
 126
 127 // after every 15 sensors increment the step by 2 (e.g. ...EX_VAL15, EX_VAL16 = 17) to skip the device description
 128 enum exSensors_e {
 129     EX_VOLTAGE = 1,
 130     EX_CURRENT,
 131     EX_ALTITUDE,
 132     EX_CAPACITY,
 133     EX_POWER,
 134     EX_ROLL_ANGLE,
 135     EX_PITCH_ANGLE,
 136     EX_HEADING,
 137     EX_VARIO
 138 };
 139
 140 #define JETI_EX_SENSOR_COUNT (ARRAYLEN(jetiExSensors))
 141
 142 static uint8_t jetiExBusTelemetryFrame[40];
 143 static uint8_t jetiExBusTransceiveState = EXBUS_TRANS_RX;
 144 static uint8_t firstActiveSensor = 0;
 145 static uint32_t exSensorEnabled = 0;
 146
 147 static uint8_t sendJetiExBusTelemetry(uint8_t packetID, uint8_t item);
 148 static uint8_t getNextActiveSensor(uint8_t currentSensor);
 149
 150 // Jeti Ex Telemetry CRC calculations for a frame
 151 uint8_t calcCRC8(uint8_t *pt, uint8_t msgLen)
 152 {
 153     uint8_t crc=0;
 154     for (uint8_t mlen = 0; mlen < msgLen; mlen++) {
 155         crc  ^= pt[mlen];
 156         crc = crc ^ (crc << 1) ^ (crc << 2) ^ (0x0e090700 >> ((crc >> 3) & 0x18));
 157     }
 158     return(crc);
 159 }
 160
 161 /*
 162  * -----------------------------------------------
 163  *  Jeti Ex Bus Telemetry
 164  * -----------------------------------------------
 165  */
 166 void initJetiExBusTelemetry(void)
 167 {
 168     // Init Ex Bus Frame header
 169     jetiExBusTelemetryFrame[EXBUS_HEADER_SYNC] = 0x3B;       // Startbytes
 170     jetiExBusTelemetryFrame[EXBUS_HEADER_REQ] = 0x01;
 171     jetiExBusTelemetryFrame[EXBUS_HEADER_DATA_ID] = 0x3A;    // Ex Telemetry
 172
 173     // Init Ex Telemetry header
 174     uint8_t *jetiExTelemetryFrame = &jetiExBusTelemetryFrame[EXBUS_HEADER_DATA];
 175
 176     jetiExTelemetryFrame[EXTEL_HEADER_SYNC] = 0x9F;              // Startbyte
 177     jetiExTelemetryFrame[EXTEL_HEADER_USN_LB] = 0x1E;            // Serial Number 4 Byte
 178     jetiExTelemetryFrame[EXTEL_HEADER_USN_HB] = 0xA4;
 179     jetiExTelemetryFrame[EXTEL_HEADER_LSN_LB] = 0x00;            // increment by telemetry count (%16) > only 15 values per device possible
 180     jetiExTelemetryFrame[EXTEL_HEADER_LSN_HB] = 0x00;
 181     jetiExTelemetryFrame[EXTEL_HEADER_RES] = 0x00;               // reserved, by default 0x00
 182
 183     //exSensorEnabled = 0x3fe;
 184     // Check which sensors are available
 185     if (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE) {
 186         bitArraySet(&exSensorEnabled, EX_VOLTAGE);
 187     }
 188     if (batteryConfig()->currentMeterSource != CURRENT_METER_NONE) {
 189         bitArraySet(&exSensorEnabled, EX_CURRENT);
 190     }
 191     if ((batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE) && (batteryConfig()->currentMeterSource != CURRENT_METER_NONE)) {
 192         bitArraySet(&exSensorEnabled, EX_POWER);
 193         bitArraySet(&exSensorEnabled, EX_CAPACITY);
 194     }
 195     if (sensors(SENSOR_BARO)) {
 196         bitArraySet(&exSensorEnabled, EX_ALTITUDE);
 197         bitArraySet(&exSensorEnabled, EX_VARIO);
 198     }
 199     if (sensors(SENSOR_ACC)) {
 200         bitArraySet(&exSensorEnabled, EX_ROLL_ANGLE);
 201         bitArraySet(&exSensorEnabled, EX_PITCH_ANGLE);
 202     }
 203     if (sensors(SENSOR_MAG)) {
 204         bitArraySet(&exSensorEnabled, EX_HEADING);
 205     }
 206     firstActiveSensor = getNextActiveSensor(0);     // find the first active sensor
 207 }
 208
 209 void createExTelemetryTextMessage(uint8_t *exMessage, uint8_t messageID, const exBusSensor_t *sensor)
 210 {
 211     uint8_t labelLength = strlen(sensor->label);
 212     uint8_t unitLength = strlen(sensor->unit);
 213
 214     exMessage[EXTEL_HEADER_TYPE_LEN] = EXTEL_OVERHEAD + labelLength + unitLength;
 215     exMessage[EXTEL_HEADER_LSN_LB] = messageID & 0xF0;                              // Device ID
 216     exMessage[EXTEL_HEADER_ID] = messageID & 0x0F;                                  // Sensor ID (%16)
 217     exMessage[EXTEL_HEADER_DATA] = (labelLength << 3) + unitLength;
 218
 219     memcpy(&exMessage[EXTEL_HEADER_DATA + 1], sensor->label, labelLength);
 220     memcpy(&exMessage[EXTEL_HEADER_DATA + 1 + labelLength], sensor->unit, unitLength);
 221
 222     exMessage[exMessage[EXTEL_HEADER_TYPE_LEN] + EXTEL_CRC_LEN] = calcCRC8(&exMessage[EXTEL_HEADER_TYPE_LEN], exMessage[EXTEL_HEADER_TYPE_LEN]);
 223 }
 224
 225 int32_t getSensorValue(uint8_t sensor)
 226 {
 227     switch(sensor) {
 228     case EX_VOLTAGE:
 229         return getBatteryVoltageLatest();
 230         break;
 231
 232     case EX_CURRENT:
 233         return getAmperageLatest();
 234         break;
 235
 236     case EX_ALTITUDE:
 237         return getEstimatedAltitude();
 238         break;
 239
 240     case EX_CAPACITY:
 241         return getMAhDrawn();
 242         break;
 243
 244     case EX_POWER:
 245         return (getBatteryVoltageLatest() * getAmperageLatest() / 100);
 246         break;
 247
 248     case EX_ROLL_ANGLE:
 249         return attitude.values.roll;
 250         break;
 251
 252     case EX_PITCH_ANGLE:
 253         return attitude.values.pitch;
 254         break;
 255
 256     case EX_HEADING:
 257         return attitude.values.yaw;
 258         break;
 259
 260     case EX_VARIO:
 261         return getEstimatedVario();
 262         break;
 263
 264     default:
 265         return -1;
 266     }
 267 }
 268
 269 uint8_t getNextActiveSensor(uint8_t currentSensor)
 270 {
 271     while( ++currentSensor < JETI_EX_SENSOR_COUNT) {
 272         if (bitArrayGet(&exSensorEnabled, currentSensor)) {
 273             break;
 274         }
 275     }
 276     if (currentSensor == JETI_EX_SENSOR_COUNT ) {
 277         currentSensor = firstActiveSensor;
 278     }
 279     return currentSensor;
 280 }
 281
 282 uint8_t createExTelemetryValueMessage(uint8_t *exMessage, uint8_t item)
 283 {
 284     uint8_t startItem = item;
 285     uint8_t sensorItemMaxGroup = (item & 0xF0) + 0x10;
 286     uint8_t iCount;
 287     uint8_t messageSize;
 288     uint32_t sensorValue;
 289
 290     exMessage[EXTEL_HEADER_LSN_LB] = item & 0xF0;                                   // Device ID
 291     uint8_t *p = &exMessage[EXTEL_HEADER_ID];
 292
 293     while (item < sensorItemMaxGroup) {
 294         *p++ = ((item & 0x0F) << 4) | jetiExSensors[item].exDataType;   // Sensor ID (%16) | EX Data Type
 295
 296         sensorValue = getSensorValue(item);
 297         iCount = exDataTypeLen[jetiExSensors[item].exDataType];
 298
 299         while (iCount > 1) {
 300             *p++ = sensorValue;
 301             sensorValue = sensorValue >> 8;
 302             iCount--;
 303         }
 304         *p++ = (sensorValue & 0x9F) | jetiExSensors[item].decimals;
 305
 306         item = getNextActiveSensor(item);
 307
 308         if (startItem >= item) {
 309             break;
 310         }
 311
 312         if ((p - &exMessage[EXTEL_HEADER_ID]) + exDataTypeLen[jetiExSensors[item].exDataType] + 1 >= EXTEL_MAX_PAYLOAD) {
 313             break;
 314         }
 315     }
 316     messageSize = (EXTEL_HEADER_LEN + (p-&exMessage[EXTEL_HEADER_ID]));
 317     exMessage[EXTEL_HEADER_TYPE_LEN] = EXTEL_DATA_MSG | messageSize;
 318     exMessage[messageSize + EXTEL_CRC_LEN] = calcCRC8(&exMessage[EXTEL_HEADER_TYPE_LEN], messageSize);
 319
 320     return item;        // return the next item
 321 }
 322
 323 void createExBusMessage(uint8_t *exBusMessage, uint8_t *exMessage, uint8_t packetID)
 324 {
 325     uint16_t crc16;
 326
 327     exBusMessage[EXBUS_HEADER_PACKET_ID] = packetID;
 328     exBusMessage[EXBUS_HEADER_SUBLEN] = (exMessage[EXTEL_HEADER_TYPE_LEN] & EXTEL_UNMASK_TYPE) + 2;    // +2: startbyte & CRC8
 329     exBusMessage[EXBUS_HEADER_MSG_LEN] = EXBUS_OVERHEAD + exBusMessage[EXBUS_HEADER_SUBLEN];
 330
 331     crc16 = jetiExBusCalcCRC16(exBusMessage, exBusMessage[EXBUS_HEADER_MSG_LEN] - EXBUS_CRC_LEN);
 332     exBusMessage[exBusMessage[EXBUS_HEADER_MSG_LEN] - 2] = crc16;
 333     exBusMessage[exBusMessage[EXBUS_HEADER_MSG_LEN] - 1] = crc16 >> 8;
 334 }
 335
 336 void checkJetiExBusTelemetryState(void)
 337 {
 338     return;
 339 }
 340
 341 void handleJetiExBusTelemetry(void)
 342 {
 343     static uint16_t framesLost = 0; // only for debug
 344     static uint8_t item = 0;
 345     uint32_t timeDiff;
 346
 347     // Check if we shall reset frame position due to time
 348     if (jetiExBusRequestState == EXBUS_STATE_RECEIVED) {
 349
 350         // to prevent timing issues from request to answer - max. 4ms
 351         timeDiff = micros() - jetiTimeStampRequest;
 352
 353         if (timeDiff > 3000) {   // include reserved time
 354             jetiExBusRequestState = EXBUS_STATE_ZERO;
 355             framesLost++;
 356             return;
 357         }
 358
 359         if ((jetiExBusRequestFrame[EXBUS_HEADER_DATA_ID] == EXBUS_EX_REQUEST) && (jetiExBusCalcCRC16(jetiExBusRequestFrame, jetiExBusRequestFrame[EXBUS_HEADER_MSG_LEN]) == 0)) {
 360             // switch to TX mode
 361             if (serialRxBytesWaiting(jetiExBusPort) == 0) {
 362                 serialSetMode(jetiExBusPort, MODE_TX);
 363                 jetiExBusTransceiveState = EXBUS_TRANS_TX;
 364                 item = sendJetiExBusTelemetry(jetiExBusRequestFrame[EXBUS_HEADER_PACKET_ID], item);
 365                 jetiExBusRequestState = EXBUS_STATE_PROCESSED;
 366                 return;
 367             }
 368         } else {
 369             jetiExBusRequestState = EXBUS_STATE_ZERO;
 370             return;
 371         }
 372     }
 373
 374     // check the state if transmit is ready
 375     if (jetiExBusTransceiveState == EXBUS_TRANS_IS_TX_COMPLETED) {
 376         if (isSerialTransmitBufferEmpty(jetiExBusPort)) {
 377             serialSetMode(jetiExBusPort, MODE_RX);
 378             jetiExBusTransceiveState = EXBUS_TRANS_RX;
 379             jetiExBusRequestState = EXBUS_STATE_ZERO;
 380         }
 381     }
 382 }
 383
 384 uint8_t sendJetiExBusTelemetry(uint8_t packetID, uint8_t item)
 385 {
 386     static uint8_t sensorDescriptionCounter = 0xFF;
 387     static uint8_t requestLoop = 0xFF;
 388     uint8_t *jetiExTelemetryFrame = &jetiExBusTelemetryFrame[EXBUS_HEADER_DATA];
 389
 390     if (requestLoop) {
 391         while( ++sensorDescriptionCounter < JETI_EX_SENSOR_COUNT) {
 392             if (bitArrayGet(&exSensorEnabled, sensorDescriptionCounter) || (jetiExSensors[sensorDescriptionCounter].exDataType == EX_TYPE_DES)) {
 393                 break;
 394             }
 395         }
 396         if (sensorDescriptionCounter == JETI_EX_SENSOR_COUNT ) {
 397             sensorDescriptionCounter = 0;
 398         }
 399
 400         createExTelemetryTextMessage(jetiExTelemetryFrame, sensorDescriptionCounter, &jetiExSensors[sensorDescriptionCounter]);
 401         createExBusMessage(jetiExBusTelemetryFrame, jetiExTelemetryFrame, packetID);
 402         requestLoop--;
 403         if (requestLoop == 0){
 404             item = firstActiveSensor;
 405         }
 406     } else {
 407         item = createExTelemetryValueMessage(jetiExTelemetryFrame, item);
 408         createExBusMessage(jetiExBusTelemetryFrame, jetiExTelemetryFrame, packetID);
 409     }
 410
 411     serialWriteBuf(jetiExBusPort, jetiExBusTelemetryFrame, jetiExBusTelemetryFrame[EXBUS_HEADER_MSG_LEN]);
 412     jetiExBusTransceiveState = EXBUS_TRANS_IS_TX_COMPLETED;
 413
 414     return item;
 415 }
 416 #endif // TELEMETRY
 417 #endif // SERIAL_RX
 418
 419