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 "common/maths.h"
  35 #include "config/feature.h"
  36 #include "fc/runtime_config.h"
  37
  38 #include "msp/msp_protocol.h"
  39
  40 #include "rx/crsf_protocol.h"
  41 #include "rx/expresslrs_telemetry.h"
  42
  43 #include "telemetry/crsf.h"
  44 #include "telemetry/telemetry.h"
  45
  46 #include "sensors/battery.h"
  47 #include "sensors/sensors.h"
  48
  49 static uint8_t tlmBuffer[CRSF_FRAME_SIZE_MAX];
  50
  51 typedef enum {
  52     CRSF_FRAME_GPS_INDEX = 0,
  53     CRSF_FRAME_BATTERY_SENSOR_INDEX,
  54     CRSF_FRAME_ATTITUDE_INDEX,
  55     CRSF_FRAME_FLIGHT_MODE_INDEX,
  56     CRSF_FRAME_PAYLOAD_TYPES_COUNT //should be last
  57 } frameTypeIndex_e;
  58
  59 static crsfFrameType_e payloadTypes[] = {
  60     CRSF_FRAMETYPE_GPS,
  61     CRSF_FRAMETYPE_BATTERY_SENSOR,
  62     CRSF_FRAMETYPE_ATTITUDE,
  63     CRSF_FRAMETYPE_FLIGHT_MODE
  64 };
  65
  66 STATIC_UNIT_TESTED uint8_t tlmSensors = 0;
  67 STATIC_UNIT_TESTED uint8_t currentPayloadIndex;
  68
  69 static uint8_t *data = NULL;
  70 static uint8_t length = 0;
  71 static uint8_t currentOffset;
  72 static uint8_t bytesLastPayload;
  73 static uint8_t currentPackage;
  74 static bool waitUntilTelemetryConfirm;
  75 static uint16_t waitCount;
  76 static uint16_t maxWaitCount;
  77 static volatile stubbornSenderState_e senderState;
  78
  79 static void telemetrySenderResetState(void)
  80 {
  81     bytesLastPayload = 0;
  82     currentOffset = 0;
  83     currentPackage = 1;
  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 }
  90
  91 /***
  92  * Queues a message to send, will abort the current message if one is currently being transmitted
  93  ***/
  94 void setTelemetryDataToTransmit(const uint8_t lengthToTransmit, uint8_t* dataToTransmit)
  95 {
  96     length = lengthToTransmit;
  97     data = dataToTransmit;
  98     currentOffset = 0;
  99     currentPackage = 1;
 100     waitCount = 0;
 101     senderState = (senderState == ELRS_SENDER_IDLE) ? ELRS_SENDING : ELRS_RESYNC_THEN_SEND;
 102 }
 103
 104 bool isTelemetrySenderActive(void)
 105 {
 106     return senderState != ELRS_SENDER_IDLE;
 107 }
 108
 109 /***
 110  * Copy up to maxLen bytes from the current package to outData
 111  * packageIndex
 112  ***/
 113 uint8_t getCurrentTelemetryPayload(uint8_t *outData)
 114 {
 115     uint8_t packageIndex;
 116
 117     bytesLastPayload = 0;
 118     switch (senderState) {
 119     case ELRS_RESYNC:
 120     case ELRS_RESYNC_THEN_SEND:
 121         packageIndex = ELRS_TELEMETRY_MAX_PACKAGES;
 122         break;
 123     case ELRS_SENDING:
 124         bytesLastPayload = MIN((uint8_t)(length - currentOffset), ELRS_TELEMETRY_BYTES_PER_CALL);
 125         // If this is the last data chunk, and there has been at least one other packet
 126         // skip the blank packet needed for WAIT_UNTIL_NEXT_CONFIRM
 127         if (currentPackage > 1 && (currentOffset + bytesLastPayload) >= length) {
 128             packageIndex = 0;
 129         } else {
 130             packageIndex = currentPackage;
 131         }
 132         memcpy(outData, &data[currentOffset], bytesLastPayload);
 133
 134         break;
 135     default:
 136         packageIndex = 0;
 137     }
 138
 139     return packageIndex;
 140 }
 141
 142 void confirmCurrentTelemetryPayload(const bool telemetryConfirmValue)
 143 {
 144     stubbornSenderState_e nextSenderState = senderState;
 145
 146     switch (senderState) {
 147     case ELRS_SENDING:
 148         if (telemetryConfirmValue != waitUntilTelemetryConfirm) {
 149             waitCount++;
 150             if (waitCount > maxWaitCount) {
 151                 waitUntilTelemetryConfirm = !telemetryConfirmValue;
 152                 nextSenderState = ELRS_RESYNC;
 153             }
 154             break;
 155         }
 156
 157         currentOffset += bytesLastPayload;
 158         if (currentOffset >= length) {
 159             // A 0th packet is always requred so the reciver can
 160             // differentiate a new send from a resend, if this is
 161             // the first packet acked, send another, else IDLE
 162             if (currentPackage == 1) {
 163                 nextSenderState = ELRS_WAIT_UNTIL_NEXT_CONFIRM;
 164             } else {
 165                 nextSenderState = ELRS_SENDER_IDLE;
 166             }
 167         }
 168
 169         currentPackage++;
 170         waitUntilTelemetryConfirm = !waitUntilTelemetryConfirm;
 171         waitCount = 0;
 172         break;
 173
 174     case ELRS_RESYNC:
 175     case ELRS_RESYNC_THEN_SEND:
 176     case ELRS_WAIT_UNTIL_NEXT_CONFIRM:
 177         if (telemetryConfirmValue == waitUntilTelemetryConfirm) {
 178             nextSenderState = (senderState == ELRS_RESYNC_THEN_SEND) ? ELRS_SENDING : ELRS_SENDER_IDLE;
 179             waitUntilTelemetryConfirm = !telemetryConfirmValue;
 180         } else if (senderState == ELRS_WAIT_UNTIL_NEXT_CONFIRM) { // switch to resync if tx does not confirm value fast enough
 181             waitCount++;
 182             if (waitCount > maxWaitCount) {
 183                 waitUntilTelemetryConfirm = !telemetryConfirmValue;
 184                 nextSenderState = ELRS_RESYNC;
 185             }
 186         }
 187
 188         break;
 189
 190     case ELRS_SENDER_IDLE:
 191         break;
 192     }
 193
 194     senderState = nextSenderState;
 195 }
 196
 197 #ifdef USE_MSP_OVER_TELEMETRY
 198 static uint8_t *mspData = NULL;
 199 static volatile bool finishedData;
 200 static volatile uint8_t mspLength = 0;
 201 static volatile uint8_t mspCurrentOffset;
 202 static volatile uint8_t mspCurrentPackage;
 203 static volatile bool mspConfirm;
 204
 205 STATIC_UNIT_TESTED volatile bool mspReplyPending;
 206 STATIC_UNIT_TESTED volatile bool deviceInfoReplyPending;
 207
 208 void mspReceiverResetState(void)
 209 {
 210     mspCurrentOffset = 0;
 211     mspCurrentPackage = 1;
 212     mspConfirm = false;
 213     mspReplyPending = false;
 214     deviceInfoReplyPending = false;
 215 }
 216
 217 bool getCurrentMspConfirm(void)
 218 {
 219     return mspConfirm;
 220 }
 221
 222 void setMspDataToReceive(const uint8_t maxLength, uint8_t* dataToReceive)
 223 {
 224     mspLength = maxLength;
 225     mspData = dataToReceive;
 226     mspCurrentPackage = 1;
 227     mspCurrentOffset = 0;
 228     finishedData = false;
 229 }
 230
 231 void receiveMspData(const uint8_t packageIndex, const volatile uint8_t* const receiveData)
 232 {
 233     // Resync
 234     if (packageIndex == ELRS_MSP_MAX_PACKAGES) {
 235         mspConfirm = !mspConfirm;
 236         mspCurrentPackage = 1;
 237         mspCurrentOffset = 0;
 238         finishedData = false;
 239         return;
 240     }
 241
 242     if (finishedData) {
 243         return;
 244     }
 245
 246     bool acceptData = false;
 247     if (packageIndex == 0 && mspCurrentPackage > 1) {
 248         // PackageIndex 0 (the final packet) can also contain data
 249         acceptData = true;
 250         finishedData = true;
 251     } else if (packageIndex == mspCurrentPackage) {
 252         acceptData = true;
 253         mspCurrentPackage++;
 254     }
 255
 256     if (acceptData && (receiveData != NULL)) {
 257         uint8_t len = MIN((uint8_t)(mspLength - mspCurrentOffset), ELRS_MSP_BYTES_PER_CALL);
 258         memcpy(&mspData[mspCurrentOffset], (const uint8_t*) receiveData, len);
 259         mspCurrentOffset += len;
 260         mspConfirm = !mspConfirm;
 261     }
 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     case CRSF_FRAMETYPE_MSP_WRITE:
 293         if (bufferCrsfMspFrame(&packet[ELRS_MSP_PACKET_OFFSET], CRSF_FRAME_RX_MSP_FRAME_SIZE)) {
 294             handleCrsfMspFrameBuffer(&bufferMspResponse);
 295             mspReplyPending = true;
 296         }
 297         break;
 298     default:
 299         break;
 300     }
 301 }
 302 #endif
 303
 304 /*
 305  * Called when the telemetry ratio or air rate changes, calculate
 306  * the new threshold for how many times the telemetryConfirmValue
 307  * can be wrong in a row before giving up and going to RESYNC
 308  */
 309 void updateTelemetryRate(const uint16_t airRate, const uint8_t tlmRatio, const uint8_t tlmBurst)
 310 {
 311     // consipicuously unused airRate parameter, the wait count is strictly based on number
 312     // of packets, not time between the telemetry packets, or a wall clock timeout
 313     UNUSED(airRate);
 314     // The expected number of packet periods between telemetry packets
 315     uint32_t packsBetween = tlmRatio * (1 + tlmBurst) / tlmBurst;
 316     maxWaitCount = packsBetween * ELRS_TELEMETRY_MAX_MISSED_PACKETS;
 317 }
 318
 319 void initTelemetry(void)
 320 {
 321     if (!featureIsEnabled(FEATURE_TELEMETRY)) {
 322         return;
 323     }
 324
 325     if (sensors(SENSOR_ACC) && telemetryIsSensorEnabled(SENSOR_PITCH | SENSOR_ROLL | SENSOR_HEADING)) {
 326         tlmSensors |= BIT(CRSF_FRAME_ATTITUDE_INDEX);
 327     }
 328     if ((isBatteryVoltageConfigured() && telemetryIsSensorEnabled(SENSOR_VOLTAGE))
 329         || (isAmperageConfigured() && telemetryIsSensorEnabled(SENSOR_CURRENT | SENSOR_FUEL))) {
 330         tlmSensors |= BIT(CRSF_FRAME_BATTERY_SENSOR_INDEX);
 331     }
 332     if (telemetryIsSensorEnabled(SENSOR_MODE)) {
 333         tlmSensors |= BIT(CRSF_FRAME_FLIGHT_MODE_INDEX);
 334     }
 335 #ifdef USE_GPS
 336     if (featureIsEnabled(FEATURE_GPS)
 337        && telemetryIsSensorEnabled(SENSOR_ALTITUDE | SENSOR_LAT_LONG | SENSOR_GROUND_SPEED | SENSOR_HEADING)) {
 338         tlmSensors |= BIT(CRSF_FRAME_GPS_INDEX);
 339     }
 340 #endif
 341
 342     telemetrySenderResetState();
 343 #ifdef USE_MSP_OVER_TELEMETRY
 344     mspReceiverResetState();
 345 #endif
 346 }
 347
 348 bool getNextTelemetryPayload(uint8_t *nextPayloadSize, uint8_t **payloadData)
 349 {
 350 #ifdef USE_MSP_OVER_TELEMETRY
 351     if (deviceInfoReplyPending) {
 352         *nextPayloadSize = getCrsfFrame(tlmBuffer, CRSF_FRAMETYPE_DEVICE_INFO);
 353         *payloadData = tlmBuffer;
 354         deviceInfoReplyPending = false;
 355         return true;
 356     } else if (mspReplyPending) {
 357         *nextPayloadSize = mspFrameSize;
 358         *payloadData = tlmBuffer;
 359         mspReplyPending = false;
 360         return true;
 361     } else
 362 #endif
 363     if (tlmSensors & BIT(currentPayloadIndex)) {
 364         *nextPayloadSize = getCrsfFrame(tlmBuffer, payloadTypes[currentPayloadIndex]);
 365         *payloadData = tlmBuffer;
 366         currentPayloadIndex = (currentPayloadIndex + 1) % CRSF_FRAME_PAYLOAD_TYPES_COUNT;
 367         return true;
 368     } else {
 369         currentPayloadIndex = (currentPayloadIndex + 1) % CRSF_FRAME_PAYLOAD_TYPES_COUNT;
 370         *nextPayloadSize = 0;
 371         *payloadData = 0;
 372         return false;
 373     }
 374 }
 375
 376 #endif