src/main/io/vtx_msp.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 /* Created by phobos- */
  22
  23 #include <stdbool.h>
  24 #include <stdint.h>
  25 #include <math.h>
  26 #include <ctype.h>
  27 #include <string.h>
  28
  29 #include "platform.h"
  30
  31 #if defined(USE_VTX_MSP) && defined(USE_VTX_CONTROL) && defined(USE_VTX_COMMON)
  32
  33 #include "build/debug.h"
  34
  35 #include "cms/cms_menu_vtx_msp.h"
  36 #include "common/crc.h"
  37 #include "config/feature.h"
  38
  39 #include "drivers/vtx_common.h"
  40 #include "drivers/vtx_table.h"
  41
  42 #include "fc/runtime_config.h"
  43 #include "flight/failsafe.h"
  44
  45 #include "io/serial.h"
  46 #include "io/vtx_msp.h"
  47 #include "io/vtx_control.h"
  48 #include "io/vtx.h"
  49
  50 #include "msp/msp_protocol.h"
  51 #include "msp/msp_serial.h"
  52
  53 #include "pg/vtx_table.h"
  54
  55 #include "rx/crsf.h"
  56 #include "rx/crsf_protocol.h"
  57 #include "rx/rx.h"
  58
  59 #include "telemetry/msp_shared.h"
  60
  61 static uint16_t mspConfFreq = 0;
  62 static uint8_t mspConfBand = 0;
  63 static uint8_t mspConfChannel = 0;
  64 static uint16_t mspConfPower = 0;
  65 static uint8_t mspConfPitMode = 0;
  66 static bool mspVtxConfigChanged = false;
  67 static timeUs_t mspVtxLastTimeUs = 0;
  68 static bool prevLowPowerDisarmedState = false;
  69
  70 static const vtxVTable_t mspVTable; // forward
  71 static vtxDevice_t vtxMsp = {
  72     .vTable = &mspVTable,
  73 };
  74
  75 STATIC_UNIT_TESTED mspVtxStatus_e mspVtxStatus = MSP_VTX_STATUS_OFFLINE;
  76 static uint8_t mspVtxPortIdentifier = 255;
  77
  78 #define MSP_VTX_REQUEST_PERIOD_US (200 * 1000) // 200ms
  79
  80 static bool isCrsfPortConfig(const serialPortConfig_t *portConfig)
  81 {
  82     return portConfig->functionMask & FUNCTION_RX_SERIAL && portConfig->functionMask & FUNCTION_VTX_MSP && rxRuntimeState.serialrxProvider == SERIALRX_CRSF;
  83 }
  84
  85 static bool isLowPowerDisarmed(void)
  86 {
  87     return (!ARMING_FLAG(ARMED) && !failsafeIsActive() &&
  88         (vtxSettingsConfig()->lowPowerDisarm == VTX_LOW_POWER_DISARM_ALWAYS ||
  89         (vtxSettingsConfig()->lowPowerDisarm == VTX_LOW_POWER_DISARM_UNTIL_FIRST_ARM && !ARMING_FLAG(WAS_EVER_ARMED))));
  90 }
  91
  92 void setMspVtxDeviceStatusReady(const int descriptor)
  93 {
  94     if (mspVtxStatus != MSP_VTX_STATUS_READY && vtxTableConfig()->bands && vtxTableConfig()->channels && vtxTableConfig()->powerLevels) {
  95 #if defined(USE_MSP_OVER_TELEMETRY)
  96         if (getMspSerialPortDescriptor(mspVtxPortIdentifier) == descriptor || getMspTelemetryDescriptor() == descriptor) {
  97 #else
  98         if (getMspSerialPortDescriptor(mspVtxPortIdentifier) == descriptor) {
  99 #endif
 100             mspVtxStatus = MSP_VTX_STATUS_READY;
 101         }
 102     }
 103 }
 104
 105 void prepareMspFrame(uint8_t *mspFrame)
 106 {
 107     mspFrame[0] = VTXDEV_MSP;
 108     mspFrame[1] = vtxSettingsConfig()->band;
 109     mspFrame[2] = vtxSettingsConfig()->channel;
 110     mspFrame[3] = isLowPowerDisarmed() ? 1 : vtxSettingsConfig()->power; // index based
 111     mspFrame[4] = mspConfPitMode;
 112     mspFrame[5] = vtxSettingsConfig()->freq & 0xFF;
 113     mspFrame[6] = (vtxSettingsConfig()->freq >> 8) & 0xFF;
 114     mspFrame[7] = (mspVtxStatus == MSP_VTX_STATUS_READY) ? 1 : 0;
 115     mspFrame[8] = vtxSettingsConfig()->lowPowerDisarm;
 116     mspFrame[9] = vtxSettingsConfig()->pitModeFreq & 0xFF;
 117     mspFrame[10] = (vtxSettingsConfig()->pitModeFreq >> 8) & 0xFF;
 118 #ifdef USE_VTX_TABLE
 119     mspFrame[11] = 1;
 120     mspFrame[12] = vtxTableConfig()->bands;
 121     mspFrame[13] = vtxTableConfig()->channels;
 122     mspFrame[14] = vtxTableConfig()->powerLevels;
 123 #else
 124     mspFrame[11] = 0;
 125     mspFrame[12] = 0;
 126     mspFrame[13] = 0;
 127     mspFrame[14] = 0;
 128 #endif
 129 }
 130
 131 static void mspCrsfPush(const uint8_t mspCommand, const uint8_t *mspFrame, const uint8_t mspFrameSize)
 132 {
 133 #ifndef USE_TELEMETRY_CRSF
 134     UNUSED(mspCommand);
 135     UNUSED(mspFrame);
 136     UNUSED(mspFrameSize);
 137 #else
 138     sbuf_t crsfPayloadBuf;
 139     sbuf_t *dst = &crsfPayloadBuf;
 140
 141     uint8_t mspHeader[6] = {0x50, 0, mspCommand & 0xFF, (mspCommand >> 8) & 0xFF, mspFrameSize & 0xFF, (mspFrameSize >> 8) & 0xFF }; // MSP V2 over CRSF header
 142     uint8_t mspChecksum;
 143
 144     mspChecksum = crc8_dvb_s2_update(0, &mspHeader[1], 5); // first character is not checksummable
 145     mspChecksum = crc8_dvb_s2_update(mspChecksum, mspFrame, mspFrameSize);
 146
 147     uint8_t fullMspFrameSize = mspFrameSize + sizeof(mspHeader) + 1;  // add 1 for msp checksum
 148     uint8_t crsfFrameSize = CRSF_FRAME_LENGTH_EXT_TYPE_CRC + CRSF_FRAME_LENGTH_TYPE_CRC + fullMspFrameSize;
 149
 150     uint8_t crsfFrame[crsfFrameSize];
 151
 152     dst->ptr = crsfFrame;
 153     dst->end = ARRAYEND(crsfFrame);
 154
 155     sbufWriteU8(dst, CRSF_SYNC_BYTE);
 156     sbufWriteU8(dst, fullMspFrameSize + CRSF_FRAME_LENGTH_EXT_TYPE_CRC); // size of CRSF frame (everything except sync and size itself)
 157     sbufWriteU8(dst, CRSF_FRAMETYPE_MSP_RESP); // CRSF type
 158     sbufWriteU8(dst, CRSF_ADDRESS_CRSF_RECEIVER); // response destination is the receiver the vtx connection
 159     sbufWriteU8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER); // origin is always this device
 160     sbufWriteData(dst, mspHeader, sizeof(mspHeader));
 161     sbufWriteData(dst, mspFrame, mspFrameSize);
 162     sbufWriteU8(dst, mspChecksum);
 163     crc8_dvb_s2_sbuf_append(dst, &crsfFrame[2]); // start at byte 2, since CRC does not include device address and frame length
 164     sbufSwitchToReader(dst, crsfFrame);
 165
 166     crsfRxSendTelemetryData(); //give the FC a chance to send outstanding telemetry
 167     crsfRxWriteTelemetryData(sbufPtr(dst), sbufBytesRemaining(dst));
 168     crsfRxSendTelemetryData();
 169 #endif
 170 }
 171
 172 static uint16_t packetCounter = 0;
 173
 174 static void vtxMspProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs)
 175 {
 176     UNUSED(vtxDevice);
 177
 178     const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_MSP);
 179     uint8_t frame[15];
 180
 181     switch (mspVtxStatus) {
 182     case MSP_VTX_STATUS_OFFLINE:
 183         // wait for MSP communication from the VTX
 184 #ifdef USE_CMS
 185         mspCmsUpdateStatusString();
 186 #endif
 187         break;
 188     case MSP_VTX_STATUS_READY:
 189         if (isLowPowerDisarmed() != prevLowPowerDisarmedState) {
 190             mspVtxConfigChanged = true;
 191             prevLowPowerDisarmedState = isLowPowerDisarmed();
 192         }
 193
 194         // send an update if stuff has changed with 200ms period
 195         if (mspVtxConfigChanged && cmp32(currentTimeUs, mspVtxLastTimeUs) >= MSP_VTX_REQUEST_PERIOD_US) {
 196
 197             prepareMspFrame(frame);
 198
 199             if (isCrsfPortConfig(portConfig)) {
 200                 mspCrsfPush(MSP_VTX_CONFIG, frame, sizeof(frame));
 201             } else {
 202                 mspSerialPush((serialPortIdentifier_e) portConfig->identifier, MSP_VTX_CONFIG, frame, sizeof(frame), MSP_DIRECTION_REPLY, MSP_V2_NATIVE);
 203             }
 204             packetCounter++;
 205             mspVtxLastTimeUs = currentTimeUs;
 206             mspVtxConfigChanged = false;
 207
 208 #ifdef USE_CMS
 209             mspCmsUpdateStatusString();
 210 #endif
 211         }
 212         break;
 213     default:
 214         mspVtxStatus = MSP_VTX_STATUS_OFFLINE;
 215         break;
 216     }
 217
 218     DEBUG_SET(DEBUG_VTX_MSP, 0, packetCounter);
 219     DEBUG_SET(DEBUG_VTX_MSP, 1, isCrsfPortConfig(portConfig));
 220     DEBUG_SET(DEBUG_VTX_MSP, 2, isLowPowerDisarmed());
 221 #if defined(USE_MSP_OVER_TELEMETRY)
 222     DEBUG_SET(DEBUG_VTX_MSP, 3, isCrsfPortConfig(portConfig) ? getMspTelemetryDescriptor() : getMspSerialPortDescriptor(mspVtxPortIdentifier));
 223 #else
 224     DEBUG_SET(DEBUG_VTX_MSP, 3, getMspSerialPortDescriptor(mspVtxPortIdentifier));
 225 #endif
 226 }
 227
 228 static vtxDevType_e vtxMspGetDeviceType(const vtxDevice_t *vtxDevice)
 229 {
 230     UNUSED(vtxDevice);
 231     return VTXDEV_MSP;
 232 }
 233
 234 static bool vtxMspIsReady(const vtxDevice_t *vtxDevice)
 235 {
 236     return vtxDevice != NULL && mspVtxStatus == MSP_VTX_STATUS_READY;
 237 }
 238
 239 static void vtxMspSetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel)
 240 {
 241     UNUSED(vtxDevice);
 242     if (band != mspConfBand || channel != mspConfChannel) {
 243         mspVtxConfigChanged = true;
 244     }
 245     mspConfBand = band;
 246     mspConfChannel = channel;
 247 }
 248
 249 static void vtxMspSetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index)
 250 {
 251     uint16_t powerValue;
 252
 253     if (vtxCommonLookupPowerValue(vtxDevice, index, &powerValue)) {
 254         if (powerValue != mspConfPower) {
 255             mspVtxConfigChanged = true;
 256         }
 257         mspConfPower = powerValue;
 258     }
 259 }
 260
 261 static void vtxMspSetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff)
 262 {
 263     UNUSED(vtxDevice);
 264     if (onoff != mspConfPitMode) {
 265         mspVtxConfigChanged = true;
 266     }
 267     mspConfPitMode = onoff;
 268 }
 269
 270 static void vtxMspSetFreq(vtxDevice_t *vtxDevice, uint16_t freq)
 271 {
 272     UNUSED(vtxDevice);
 273     if (freq != mspConfFreq) {
 274         mspVtxConfigChanged = true;
 275     }
 276     mspConfFreq = freq;
 277 }
 278
 279 static bool vtxMspGetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel)
 280 {
 281     if (!vtxMspIsReady(vtxDevice)) {
 282         return false;
 283     }
 284
 285     *pBand = mspConfBand;
 286     *pChannel = mspConfChannel;
 287     return true;
 288 }
 289
 290 static bool vtxMspGetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex)
 291 {
 292     if (!vtxMspIsReady(vtxDevice)) {
 293         return false;
 294     }
 295
 296     // Special case, power not set
 297     if (mspConfPower == 0) {
 298         *pIndex = 0;
 299         return true;
 300     }
 301
 302     // Lookup value in table
 303     for (uint8_t i = 0; i < vtxTablePowerLevels; i++) {
 304         // Find value that matches current configured power level
 305         if (mspConfPower == vtxTablePowerValues[i]) {
 306             // Value found, return index
 307             *pIndex = i + 1;
 308             return true;
 309         }
 310     }
 311
 312     // Value not found in table
 313     return false;
 314 }
 315
 316 static bool vtxMspGetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFreq)
 317 {
 318     if (!vtxMspIsReady(vtxDevice)) {
 319         return false;
 320     }
 321
 322     *pFreq = vtxCommonLookupFrequency(vtxDevice, mspConfBand, mspConfChannel);
 323     return true;
 324 }
 325
 326 static bool vtxMspGetStatus(const vtxDevice_t *vtxDevice, unsigned *status)
 327 {
 328     if (!vtxMspIsReady(vtxDevice)) {
 329         return false;
 330     }
 331
 332     // Mirror configued pit mode state rather than use current pitmode as we
 333     // should, otherwise the logic in vtxProcessPitMode may not get us to the
 334     // correct state if pitmode is toggled quickly
 335     *status = (mspConfPitMode ? VTX_STATUS_PIT_MODE : 0);
 336
 337     return true;
 338 }
 339
 340 static uint8_t vtxMspGetPowerLevels(const vtxDevice_t *vtxDevice, uint16_t *levels, uint16_t *powers)
 341 {
 342     if (!vtxMspIsReady(vtxDevice)) {
 343         return 0;
 344     }
 345
 346     for (uint8_t i = 0; i < vtxTablePowerLevels; i++) {
 347         levels[i] = vtxTablePowerValues[i];
 348         uint16_t power = (uint16_t)powf(10.0f, levels[i] / 10.0f);
 349         powers[i] = power;
 350     }
 351
 352     return vtxTablePowerLevels;
 353 }
 354
 355 static const vtxVTable_t mspVTable = {
 356     .process = vtxMspProcess,
 357     .getDeviceType = vtxMspGetDeviceType,
 358     .isReady = vtxMspIsReady,
 359     .setBandAndChannel = vtxMspSetBandAndChannel,
 360     .setPowerByIndex = vtxMspSetPowerByIndex,
 361     .setPitMode = vtxMspSetPitMode,
 362     .setFrequency = vtxMspSetFreq,
 363     .getBandAndChannel = vtxMspGetBandAndChannel,
 364     .getPowerIndex = vtxMspGetPowerIndex,
 365     .getFrequency = vtxMspGetFreq,
 366     .getStatus = vtxMspGetStatus,
 367     .getPowerLevels = vtxMspGetPowerLevels,
 368     .serializeCustomDeviceStatus = NULL,
 369 };
 370
 371 bool vtxMspInit(void)
 372 {
 373     // don't bother setting up this device if we don't have MSP vtx enabled
 374     const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_MSP);
 375     if (!portConfig) {
 376         return false;
 377     }
 378
 379     mspVtxPortIdentifier = portConfig->identifier;
 380
 381     // XXX Effect of USE_VTX_COMMON should be reviewed, as following call to vtxInit will do nothing if vtxCommonSetDevice is not called.
 382 #if defined(USE_VTX_COMMON)
 383     vtxCommonSetDevice(&vtxMsp);
 384 #endif
 385
 386     vtxInit();
 387
 388     return true;
 389 }
 390
 391 #endif