radio/src/telemetry/frsky_pxx2.cpp

   1 /*
   2  * Copyright (C) OpenTX
   3  *
   4  * Based on code named
   5  *   th9x - http://code.google.com/p/th9x
   6  *   er9x - http://code.google.com/p/er9x
   7  *   gruvin9x - http://code.google.com/p/gruvin9x
   8  *
   9  * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
  10  *
  11  * This program is free software; you can redistribute it and/or modify
  12  * it under the terms of the GNU General Public License version 2 as
  13  * published by the Free Software Foundation.
  14  *
  15  * This program is distributed in the hope that it will be useful,
  16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  18  * GNU General Public License for more details.
  19  */
  20
  21 #include "opentx.h"
  22
  23 void processGetHardwareInfoFrame(uint8_t module, const uint8_t * frame)
  24 {
  25   if (moduleState[module].mode != MODULE_MODE_GET_HARDWARE_INFO) {
  26     return;
  27   }
  28
  29   ModuleInformation * destination = moduleState[module].moduleInformation;
  30
  31   uint8_t index = frame[3];
  32   uint8_t modelId = frame[4];
  33   uint8_t length = min<uint8_t>(frame[0] - 3, sizeof(PXX2HardwareInformation));
  34   if (index == PXX2_HW_INFO_TX_ID && modelId < DIM(PXX2ModulesNames)) {
  35     memcpy(&destination->information, &frame[4], length);
  36     if (destination->information.capabilities & ~((1 << MODULE_CAPABILITY_COUNT) - 1))
  37       destination->information.capabilityNotSupported = true;
  38     if (!globalData.upgradeModulePopup && destination->information.modelID == PXX2_MODULE_ISRM_S_X10S &&
  39         destination->information.swVersion.major == 0 && destination->information.swVersion.minor == 1 && destination->information.swVersion.revision < 5) {
  40       globalData.upgradeModulePopup = 1;
  41       POPUP_WARNING(STR_MODULE_UPGRADE);
  42     }
  43   }
  44   else if (index < PXX2_MAX_RECEIVERS_PER_MODULE && modelId < DIM(PXX2ReceiversNames)) {
  45     memcpy(&destination->receivers[index].information, &frame[4], length);
  46     destination->receivers[index].timestamp = get_tmr10ms();
  47     if (destination->receivers[index].information.capabilities & ~((1 << RECEIVER_CAPABILITY_COUNT) - 1))
  48       destination->information.capabilityNotSupported = true;
  49   }
  50 }
  51
  52 void processModuleSettingsFrame(uint8_t module, const uint8_t * frame)
  53 {
  54   if (moduleState[module].mode != MODULE_MODE_MODULE_SETTINGS) {
  55     return;
  56   }
  57
  58   ModuleSettings * destination = moduleState[module].moduleSettings;
  59
  60   // Flag1
  61   if (frame[4] & PXX2_TX_SETTINGS_FLAG1_EXTERNAL_ANTENNA)
  62     destination->externalAntenna = 1;
  63
  64   // Power
  65   destination->txPower = frame[5];
  66
  67   destination->state = PXX2_SETTINGS_OK;
  68   destination->timeout = 0;
  69   moduleState[module].mode = MODULE_MODE_NORMAL;
  70 }
  71
  72 void processReceiverSettingsFrame(uint8_t module, const uint8_t * frame)
  73 {
  74   if (moduleState[module].mode != MODULE_MODE_RECEIVER_SETTINGS) {
  75     return;
  76   }
  77
  78   ReceiverSettings * destination = moduleState[module].receiverSettings;
  79
  80   if (frame[4] & PXX2_RX_SETTINGS_FLAG1_FPORT)
  81     destination->fport = 1;
  82
  83   if (frame[4] & PXX2_RX_SETTINGS_FLAG1_FASTPWM)
  84     destination->pwmRate = 1;
  85
  86   if (frame[4] & PXX2_RX_SETTINGS_FLAG1_TELEMETRY_DISABLED)
  87     destination->telemetryDisabled = 1;
  88
  89   uint8_t outputsCount = min<uint8_t>(16, frame[0] - 4);
  90   destination->outputsCount = outputsCount;
  91   for (uint8_t pin = 0; pin < outputsCount; pin++) {
  92     destination->outputsMapping[pin] = frame[5 + pin];
  93   }
  94
  95   destination->state = PXX2_SETTINGS_OK;
  96   destination->timeout = 0;
  97   moduleState[module].mode = MODULE_MODE_NORMAL;
  98 }
  99
 100 void processRegisterFrame(uint8_t module, const uint8_t * frame)
 101 {
 102   if (moduleState[module].mode != MODULE_MODE_REGISTER) {
 103     return;
 104   }
 105
 106   switch(frame[3]) {
 107     case 0x00:
 108       if (reusableBuffer.moduleSetup.pxx2.registerStep == REGISTER_INIT) {
 109         // RX_NAME follows, we store it for the next step
 110         str2zchar(reusableBuffer.moduleSetup.pxx2.registerRxName, (const char *)&frame[4], PXX2_LEN_RX_NAME);
 111         reusableBuffer.moduleSetup.pxx2.registerLoopIndex = frame[12];
 112         reusableBuffer.moduleSetup.pxx2.registerStep = REGISTER_RX_NAME_RECEIVED;
 113 #if defined(COLORLCD)
 114         putEvent(EVT_REFRESH);
 115 #endif
 116       }
 117       break;
 118
 119     case 0x01:
 120       if (reusableBuffer.moduleSetup.pxx2.registerStep == REGISTER_RX_NAME_SELECTED) {
 121         // RX_NAME + PASSWORD follow, we check they are good
 122         if (cmpStrWithZchar((char *)&frame[4], reusableBuffer.moduleSetup.pxx2.registerRxName, PXX2_LEN_RX_NAME) &&
 123             cmpStrWithZchar((char *)&frame[12], g_model.modelRegistrationID, PXX2_LEN_REGISTRATION_ID)) {
 124           reusableBuffer.moduleSetup.pxx2.registerStep = REGISTER_OK;
 125           moduleState[module].mode = MODULE_MODE_NORMAL;
 126           POPUP_INFORMATION(STR_REG_OK);
 127         }
 128       }
 129       break;
 130   }
 131 }
 132
 133 void processBindFrame(uint8_t module, const uint8_t * frame)
 134 {
 135   if (moduleState[module].mode != MODULE_MODE_BIND) {
 136     return;
 137   }
 138
 139   BindInformation * destination = moduleState[module].bindInformation;
 140
 141   switch(frame[3]) {
 142     case 0x00:
 143       if (destination->step == BIND_INIT) {
 144         bool found = false;
 145         for (uint8_t i=0; i<destination->candidateReceiversCount; i++) {
 146           if (memcmp(destination->candidateReceiversNames[i], &frame[4], PXX2_LEN_RX_NAME) == 0) {
 147             found = true;
 148             break;
 149           }
 150         }
 151         if (!found && destination->candidateReceiversCount < PXX2_MAX_RECEIVERS_PER_MODULE) {
 152           memcpy(destination->candidateReceiversNames[destination->candidateReceiversCount++], &frame[4], PXX2_LEN_RX_NAME);
 153           if (moduleState[module].callback) {
 154             moduleState[module].callback();
 155           }
 156         }
 157       }
 158       break;
 159
 160     case 0x01:
 161       if (destination->step == BIND_START) {
 162         if (memcmp(&destination->candidateReceiversNames[destination->selectedReceiverIndex], &frame[4], PXX2_LEN_RX_NAME) == 0) {
 163           memcpy(g_model.moduleData[module].pxx2.receiverName[destination->rxUid], &frame[4], PXX2_LEN_RX_NAME);
 164           storageDirty(EE_MODEL);
 165           destination->step = BIND_WAIT;
 166           destination->timeout = get_tmr10ms() + 30;
 167         }
 168       }
 169       break;
 170
 171     case 0x02:
 172       if (destination->step == BIND_INFO_REQUEST) {
 173         if (memcmp(&destination->candidateReceiversNames[destination->selectedReceiverIndex], &frame[4], PXX2_LEN_RX_NAME) == 0) {
 174           memcpy(&destination->receiverInformation, &frame[12], sizeof(PXX2HardwareInformation));
 175           if (moduleState[module].callback) {
 176             moduleState[module].callback();
 177           }
 178         }
 179       }
 180       break;
 181   }
 182 }
 183
 184 void processResetFrame(uint8_t module, const uint8_t * frame)
 185 {
 186   if (moduleState[module].mode != MODULE_MODE_RESET) {
 187     return;
 188   }
 189
 190   if (reusableBuffer.moduleSetup.pxx2.resetReceiverIndex == frame[3]) {
 191     memclear(g_model.moduleData[module].pxx2.receiverName[reusableBuffer.moduleSetup.pxx2.resetReceiverIndex], PXX2_LEN_RX_NAME);
 192   }
 193
 194   moduleState[module].mode = MODULE_MODE_NORMAL;
 195 }
 196
 197 void processTelemetryFrame(uint8_t module, const uint8_t * frame)
 198 {
 199   uint8_t origin = (module << 2) + (frame[3] & 0x03);
 200   if (origin != TELEMETRY_ENDPOINT_SPORT) {
 201     sportProcessTelemetryPacketWithoutCrc(origin, &frame[4]);
 202   }
 203 }
 204
 205 #if defined(ACCESS_LIB) && !defined(SIMU)
 206 void processAuthenticationFrame(uint8_t module, const uint8_t * frame)
 207 {
 208   uint8_t cryptoType = frame[3];
 209   uint8_t messageDigest[16] = {0};
 210
 211   if (frame[0] == 4 && PXX2_AUTH_REFUSED_FLAG == frame[4]) {
 212     if (!globalData.upgradeModulePopup) {
 213       globalData.upgradeModulePopup = 1;
 214       POPUP_INFORMATION(STR_AUTH_FAILURE);
 215     }
 216     return;
 217   }
 218
 219   if (INTERNAL_MODULE == module && accessCRL(cryptoType, frame+4, messageDigest)) {
 220     moduleState[module].mode = MODULE_MODE_AUTHENTICATION;
 221     Pxx2Pulses & pxx2 = intmodulePulsesData.pxx2;
 222     pxx2.setupAuthenticationFrame(module, cryptoType, (const uint8_t *)messageDigest);
 223     intmoduleSendBuffer(pxx2.getData(), pxx2.getSize());
 224     // we remain in AUTHENTICATION mode to avoid a CHANNELS frame is sent at the end of the mixing process
 225   }
 226
 227   if (!globalData.upgradeModulePopup) {
 228     if (globalData.authenticationCount >= 2) {
 229       globalData.upgradeModulePopup = 1;
 230       POPUP_WARNING(STR_MODULE_UPGRADE);
 231     }
 232     else {
 233       globalData.authenticationCount += 1;
 234     }
 235   }
 236 }
 237 #else
 238 #define processAuthenticationFrame(module, frame)
 239 #endif
 240
 241 void processSpectrumAnalyserFrame(uint8_t module, const uint8_t * frame)
 242 {
 243   if (moduleState[module].mode != MODULE_MODE_SPECTRUM_ANALYSER) {
 244     return;
 245   }
 246
 247   uint32_t frequency = *((uint32_t *)&frame[4]);
 248   int8_t power = *((int8_t *)&frame[8]);
 249
 250   // center = 2440000000;  // 2440MHz
 251   // span = 40000000;  // 40MHz
 252   // left = 2440000000 - 20000000
 253   // step = 10000
 254
 255   int32_t offset = frequency - (reusableBuffer.spectrumAnalyser.freq - reusableBuffer.spectrumAnalyser.span / 2);
 256   TRACE("Fq=%u => %d, Pw=%d", frequency, offset, int32_t(power));
 257
 258   uint32_t x = offset / reusableBuffer.spectrumAnalyser.step;
 259   if (x < LCD_W) {
 260     reusableBuffer.spectrumAnalyser.bars[x] = max<int>(0, -SPECTRUM_ANALYSER_POWER_FLOOR + power); // we remove everything below -120dB
 261 #if defined(COLORLCD)
 262     if (reusableBuffer.spectrumAnalyser.bars[x] > reusableBuffer.spectrumAnalyser.max[x])
 263       reusableBuffer.spectrumAnalyser.max[x] = reusableBuffer.spectrumAnalyser.bars[x];
 264 #endif
 265   }
 266 }
 267
 268 void processPowerMeterFrame(uint8_t module, const uint8_t * frame)
 269 {
 270   if (moduleState[module].mode != MODULE_MODE_POWER_METER) {
 271     return;
 272   }
 273
 274   reusableBuffer.powerMeter.power = *((int16_t *)&frame[8]);
 275   if (!reusableBuffer.powerMeter.peak || reusableBuffer.powerMeter.power > reusableBuffer.powerMeter.peak) {
 276     reusableBuffer.powerMeter.peak = reusableBuffer.powerMeter.power;
 277   }
 278 }
 279
 280 void processOtaUpdateFrame(uint8_t module, const uint8_t * frame)
 281 {
 282   if (moduleState[module].mode != MODULE_MODE_OTA_UPDATE) {
 283     return;
 284   }
 285
 286   OtaUpdateInformation * destination = moduleState[module].otaUpdateInformation;
 287
 288   if (destination->step == OTA_UPDATE_START) {
 289     if (frame[3] == 0x00 && memcmp(destination->candidateReceiversNames[destination->selectedReceiverIndex], &frame[4], PXX2_LEN_RX_NAME) == 0) {
 290       destination->step = OTA_UPDATE_START_ACK;
 291     }
 292   }
 293   else if (destination->step == OTA_UPDATE_TRANSFER) {
 294     uint32_t address = *((uint32_t *)&frame[4]);
 295     if (frame[3] == 0x01 && destination->address == address) {
 296       destination->step = OTA_UPDATE_TRANSFER_ACK;
 297     }
 298   }
 299   else if (destination->step == OTA_UPDATE_EOF) {
 300     if (frame[3] == 0x02) {
 301       destination->step = OTA_UPDATE_EOF_ACK;
 302     }
 303   }
 304 }
 305
 306 void processModuleFrame(uint8_t module, const uint8_t *frame)
 307 {
 308   switch (frame[2]) {
 309     case PXX2_TYPE_ID_HW_INFO:
 310       processGetHardwareInfoFrame(module, frame);
 311       break;
 312
 313     case PXX2_TYPE_ID_TX_SETTINGS:
 314       processModuleSettingsFrame(module, frame);
 315       break;
 316
 317     case PXX2_TYPE_ID_RX_SETTINGS:
 318       processReceiverSettingsFrame(module, frame);
 319       break;
 320
 321     case PXX2_TYPE_ID_REGISTER:
 322       processRegisterFrame(module, frame);
 323       break;
 324
 325     case PXX2_TYPE_ID_BIND:
 326       processBindFrame(module, frame);
 327       break;
 328
 329     case PXX2_TYPE_ID_TELEMETRY:
 330       processTelemetryFrame(module, frame);
 331       break;
 332
 333     case PXX2_TYPE_ID_AUTHENTICATION:
 334       processAuthenticationFrame(module, frame);
 335       break;
 336
 337     case PXX2_TYPE_ID_RESET:
 338       processResetFrame(module, frame);
 339       break;
 340   }
 341 }
 342
 343 void processToolsFrame(uint8_t module, const uint8_t * frame)
 344 {
 345   switch (frame[2]) {
 346     case PXX2_TYPE_ID_POWER_METER:
 347       processPowerMeterFrame(module, frame);
 348       break;
 349
 350     case PXX2_TYPE_ID_SPECTRUM:
 351       processSpectrumAnalyserFrame(module, frame);
 352       break;
 353   }
 354 }
 355
 356 void processPXX2Frame(uint8_t module, const uint8_t * frame)
 357 {
 358   LOG_TELEMETRY_WRITE_START();
 359   for (uint8_t i = 0; i < 1 + frame[0]; i++) {
 360     LOG_TELEMETRY_WRITE_BYTE(frame[i]);
 361   }
 362
 363   switch (frame[1]) {
 364     case PXX2_TYPE_C_MODULE:
 365       processModuleFrame(module, frame);
 366       break;
 367
 368     case PXX2_TYPE_C_POWER_METER:
 369       processToolsFrame(module, frame);
 370       break;
 371
 372     case PXX2_TYPE_C_OTA:
 373       processOtaUpdateFrame(module, frame);
 374       break;
 375
 376     default:
 377       break;
 378   }
 379 }