src/main/rx/expresslrs_telemetry.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  * Based on https://github.com/ExpressLRS/ExpressLRS
  23  * Thanks to AlessandroAU, original creator of the ExpressLRS project.
  24  *
  25  * Authors:
  26  * Phobos- - Original port.
  27  */
  28
  29 #include <string.h>
  30 #include "platform.h"
  31
  32 #ifdef USE_RX_EXPRESSLRS
  33
  34 #include "config/feature.h"
  35 #include "fc/runtime_config.h"
  36
  37 #include "msp/msp_protocol.h"
  38
  39 #include "rx/crsf_protocol.h"
  40 #include "rx/expresslrs_telemetry.h"
  41
  42 #include "telemetry/crsf.h"
  43 #include "telemetry/telemetry.h"
  44
  45 #include "sensors/battery.h"
  46 #include "sensors/sensors.h"
  47
  48 static uint8_t tlmBuffer[CRSF_FRAME_SIZE_MAX];
  49
  50 typedef enum {
  51     CRSF_FRAME_GPS_INDEX = 0,
  52     CRSF_FRAME_BATTERY_SENSOR_INDEX,
  53     CRSF_FRAME_ATTITUDE_INDEX,
  54     CRSF_FRAME_FLIGHT_MODE_INDEX,
  55     CRSF_FRAME_PAYLOAD_TYPES_COUNT //should be last
  56 } frameTypeIndex_e;
  57
  58 static crsfFrameType_e payloadTypes[] = {
  59     CRSF_FRAMETYPE_GPS,
  60     CRSF_FRAMETYPE_BATTERY_SENSOR,
  61     CRSF_FRAMETYPE_ATTITUDE,
  62     CRSF_FRAMETYPE_FLIGHT_MODE
  63 };
  64
  65 STATIC_UNIT_TESTED uint8_t tlmSensors = 0;
  66 STATIC_UNIT_TESTED uint8_t currentPayloadIndex;
  67
  68 static uint8_t *data;
  69 static uint8_t length;
  70 static uint8_t bytesPerCall;
  71 static uint8_t currentOffset;
  72 static uint8_t currentPackage;
  73 static bool waitUntilTelemetryConfirm;
  74 static uint16_t waitCount;
  75 static uint16_t maxWaitCount;
  76 static volatile stubbornSenderState_e senderState;
  77
  78 static void telemetrySenderResetState(void)
  79 {
  80     data = 0;
  81     currentOffset = 0;
  82     currentPackage = 0;
  83     length = 0;
  84     waitUntilTelemetryConfirm = true;
  85     waitCount = 0;
  86     // 80 corresponds to UpdateTelemetryRate(ANY, 2, 1), which is what the TX uses in boost mode
  87     maxWaitCount = 80;
  88     senderState = ELRS_SENDER_IDLE;
  89     bytesPerCall = 1;
  90 }
  91
  92 /***
  93  * Queues a message to send, will abort the current message if one is currently being transmitted
  94  ***/
  95 void setTelemetryDataToTransmit(const uint8_t lengthToTransmit, uint8_t* dataToTransmit, const uint8_t bpc)
  96 {
  97     if (lengthToTransmit / bpc >= ELRS_TELEMETRY_MAX_PACKAGES) {
  98         return;
  99     }
 100
 101     length = lengthToTransmit;
 102     data = dataToTransmit;
 103     currentOffset = 0;
 104     currentPackage = 1;
 105     waitCount = 0;
 106     bytesPerCall = bpc;
 107     senderState = (senderState == ELRS_SENDER_IDLE) ? ELRS_SENDING : ELRS_RESYNC_THEN_SEND;
 108 }
 109
 110 bool isTelemetrySenderActive(void)
 111 {
 112     return senderState != ELRS_SENDER_IDLE;
 113 }
 114
 115 void getCurrentTelemetryPayload(uint8_t *packageIndex, uint8_t *count, uint8_t **currentData)
 116 {
 117     switch (senderState) {
 118     case ELRS_RESYNC:
 119     case ELRS_RESYNC_THEN_SEND:
 120         *packageIndex = ELRS_TELEMETRY_MAX_PACKAGES;
 121         *count = 0;
 122         *currentData = 0;
 123         break;
 124     case ELRS_SENDING:
 125         *currentData = data + currentOffset;
 126         *packageIndex = currentPackage;
 127         if (bytesPerCall > 1) {
 128             if (currentOffset + bytesPerCall <= length) {
 129                 *count = bytesPerCall;
 130             } else {
 131                 *count = length - currentOffset;
 132             }
 133         } else {
 134             *count = 1;
 135         }
 136         break;
 137     default:
 138         *count = 0;
 139         *currentData = 0;
 140         *packageIndex = 0;
 141     }
 142 }
 143
 144 void confirmCurrentTelemetryPayload(const bool telemetryConfirmValue)
 145 {
 146     stubbornSenderState_e nextSenderState = senderState;
 147
 148     switch (senderState) {
 149     case ELRS_SENDING:
 150         if (telemetryConfirmValue != waitUntilTelemetryConfirm) {
 151             waitCount++;
 152             if (waitCount > maxWaitCount) {
 153                 waitUntilTelemetryConfirm = !telemetryConfirmValue;
 154                 nextSenderState = ELRS_RESYNC;
 155             }
 156             break;
 157         }
 158
 159         currentOffset += bytesPerCall;
 160         currentPackage++;
 161         waitUntilTelemetryConfirm = !waitUntilTelemetryConfirm;
 162         waitCount = 0;
 163
 164         if (currentOffset >= length) {
 165             nextSenderState = ELRS_WAIT_UNTIL_NEXT_CONFIRM;
 166         }
 167
 168         break;
 169
 170     case ELRS_RESYNC:
 171     case ELRS_RESYNC_THEN_SEND:
 172     case ELRS_WAIT_UNTIL_NEXT_CONFIRM:
 173         if (telemetryConfirmValue == waitUntilTelemetryConfirm) {
 174             nextSenderState = (senderState == ELRS_RESYNC_THEN_SEND) ? ELRS_SENDING : ELRS_SENDER_IDLE;
 175             waitUntilTelemetryConfirm = !telemetryConfirmValue;
 176         } else if (senderState == ELRS_WAIT_UNTIL_NEXT_CONFIRM) { // switch to resync if tx does not confirm value fast enough
 177             waitCount++;
 178             if (waitCount > maxWaitCount) {
 179                 waitUntilTelemetryConfirm = !telemetryConfirmValue;
 180                 nextSenderState = ELRS_RESYNC;
 181             }
 182         }
 183
 184         break;
 185
 186     case ELRS_SENDER_IDLE:
 187         break;
 188     }
 189
 190     senderState = nextSenderState;
 191 }
 192
 193 #ifdef USE_MSP_OVER_TELEMETRY
 194 static uint8_t *mspData;
 195 static volatile bool finishedData;
 196 static volatile uint8_t mspLength;
 197 static volatile uint8_t mspBytesPerCall;
 198 static volatile uint8_t mspCurrentOffset;
 199 static volatile uint8_t mspCurrentPackage;
 200 static volatile bool mspConfirm;
 201
 202 STATIC_UNIT_TESTED volatile bool mspReplyPending;
 203 STATIC_UNIT_TESTED volatile bool deviceInfoReplyPending;
 204
 205 void mspReceiverResetState(void) {
 206     mspData = 0;
 207     mspBytesPerCall = 1;
 208     mspCurrentOffset = 0;
 209     mspCurrentPackage = 0;
 210     mspLength = 0;
 211     mspConfirm = false;
 212     mspReplyPending = false;
 213     deviceInfoReplyPending = false;
 214 }
 215
 216 bool getCurrentMspConfirm(void)
 217 {
 218     return mspConfirm;
 219 }
 220
 221 void setMspDataToReceive(const uint8_t maxLength, uint8_t* dataToReceive, const uint8_t bpc)
 222 {
 223     mspLength = maxLength;
 224     mspData = dataToReceive;
 225     mspCurrentPackage = 1;
 226     mspCurrentOffset = 0;
 227     finishedData = false;
 228     mspBytesPerCall = bpc;
 229 }
 230
 231 void receiveMspData(const uint8_t packageIndex, const volatile uint8_t* receiveData)
 232 {
 233     if  (packageIndex == 0 && mspCurrentPackage > 1) {
 234         finishedData = true;
 235         mspConfirm = !mspConfirm;
 236         return;
 237     }
 238
 239     if (packageIndex == ELRS_MSP_MAX_PACKAGES) {
 240         mspConfirm = !mspConfirm;
 241         mspCurrentPackage = 1;
 242         mspCurrentOffset = 0;
 243         finishedData = false;
 244         return;
 245     }
 246
 247     if (finishedData) {
 248         return;
 249     }
 250
 251     if (packageIndex == mspCurrentPackage) {
 252         for (uint8_t i = 0; i < mspBytesPerCall; i++) {
 253             mspData[mspCurrentOffset++] = *(receiveData + i);
 254         }
 255
 256         mspCurrentPackage++;
 257         mspConfirm = !mspConfirm;
 258         return;
 259     }
 260
 261     return;
 262 }
 263
 264 bool hasFinishedMspData(void)
 265 {
 266     return finishedData;
 267 }
 268
 269 void mspReceiverUnlock(void)
 270 {
 271     if (finishedData) {
 272         mspCurrentPackage = 1;
 273         mspCurrentOffset = 0;
 274         finishedData = false;
 275     }
 276 }
 277
 278 static uint8_t mspFrameSize = 0;
 279
 280 static void bufferMspResponse(uint8_t *payload, const uint8_t payloadSize)
 281 {
 282     mspFrameSize = getCrsfMspFrame(tlmBuffer, payload, payloadSize);
 283 }
 284
 285 void processMspPacket(uint8_t *packet)
 286 {
 287     switch (packet[2]) {
 288     case CRSF_FRAMETYPE_DEVICE_PING:
 289         deviceInfoReplyPending = true;
 290         break;
 291     case CRSF_FRAMETYPE_MSP_REQ:
 292         FALLTHROUGH;
 293     case CRSF_FRAMETYPE_MSP_WRITE:
 294         if (bufferCrsfMspFrame(&packet[ELRS_MSP_PACKET_OFFSET], CRSF_FRAME_RX_MSP_FRAME_SIZE)) {
 295             handleCrsfMspFrameBuffer(&bufferMspResponse);
 296             mspReplyPending = true;
 297         }
 298         break;
 299     default:
 300         break;
 301     }
 302 }
 303 #endif
 304
 305 /*
 306  * Called when the telemetry ratio or air rate changes, calculate
 307  * the new threshold for how many times the telemetryConfirmValue
 308  * can be wrong in a row before giving up and going to RESYNC
 309  */
 310 void updateTelemetryRate(const uint16_t airRate, const uint8_t tlmRatio, const uint8_t tlmBurst)
 311 {
 312     // consipicuously unused airRate parameter, the wait count is strictly based on number
 313     // of packets, not time between the telemetry packets, or a wall clock timeout
 314     UNUSED(airRate);
 315     // The expected number of packet periods between telemetry packets
 316     uint32_t packsBetween = tlmRatio * (1 + tlmBurst) / tlmBurst;
 317     maxWaitCount = packsBetween * ELRS_TELEMETRY_MAX_MISSED_PACKETS;
 318 }
 319
 320 void initTelemetry(void)
 321 {
 322     if (!featureIsEnabled(FEATURE_TELEMETRY)) {
 323         return;
 324     }
 325
 326     if (sensors(SENSOR_ACC) && telemetryIsSensorEnabled(SENSOR_PITCH | SENSOR_ROLL | SENSOR_HEADING)) {
 327         tlmSensors |= BIT(CRSF_FRAME_ATTITUDE_INDEX);
 328     }
 329     if ((isBatteryVoltageConfigured() && telemetryIsSensorEnabled(SENSOR_VOLTAGE))
 330         || (isAmperageConfigured() && telemetryIsSensorEnabled(SENSOR_CURRENT | SENSOR_FUEL))) {
 331         tlmSensors |= BIT(CRSF_FRAME_BATTERY_SENSOR_INDEX);
 332     }
 333     if (telemetryIsSensorEnabled(SENSOR_MODE)) {
 334         tlmSensors |= BIT(CRSF_FRAME_FLIGHT_MODE_INDEX);
 335     }
 336 #ifdef USE_GPS
 337     if (featureIsEnabled(FEATURE_GPS)
 338        && telemetryIsSensorEnabled(SENSOR_ALTITUDE | SENSOR_LAT_LONG | SENSOR_GROUND_SPEED | SENSOR_HEADING)) {
 339         tlmSensors |= BIT(CRSF_FRAME_GPS_INDEX);
 340     }
 341 #endif
 342
 343     telemetrySenderResetState();
 344 #ifdef USE_MSP_OVER_TELEMETRY
 345     mspReceiverResetState();
 346 #endif
 347 }
 348
 349 bool getNextTelemetryPayload(uint8_t *nextPayloadSize, uint8_t **payloadData)
 350 {
 351 #ifdef USE_MSP_OVER_TELEMETRY
 352     if (deviceInfoReplyPending) {
 353         *nextPayloadSize = getCrsfFrame(tlmBuffer, CRSF_FRAMETYPE_DEVICE_INFO);
 354         *payloadData = tlmBuffer;
 355         deviceInfoReplyPending = false;
 356         return true;
 357     } else if (mspReplyPending) {
 358         *nextPayloadSize = mspFrameSize;
 359         *payloadData = tlmBuffer;
 360         mspReplyPending = false;
 361         return true;
 362     } else
 363 #endif
 364     if (tlmSensors & BIT(currentPayloadIndex)) {
 365         *nextPayloadSize = getCrsfFrame(tlmBuffer, payloadTypes[currentPayloadIndex]);
 366         *payloadData = tlmBuffer;
 367         currentPayloadIndex = (currentPayloadIndex + 1) % CRSF_FRAME_PAYLOAD_TYPES_COUNT;
 368         return true;
 369     } else {
 370         currentPayloadIndex = (currentPayloadIndex + 1) % CRSF_FRAME_PAYLOAD_TYPES_COUNT;
 371         *nextPayloadSize = 0;
 372         *payloadData = 0;
 373         return false;
 374     }
 375 }
 376
 377 #endif