edgetx-gx12/radio/src/io/multi_protolist.cpp

/*
 * Copyright (C) EdgeTX
 *
 * Based on code named
 *   opentx - https://github.com/opentx/opentx
 *   th9x - http://code.google.com/p/th9x
 *   er9x - http://code.google.com/p/er9x
 *   gruvin9x - http://code.google.com/p/gruvin9x
 *
 * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 */

#include "multi_protolist.h"
#include "multi_rfprotos.h"
#include "strhelpers.h"
#include "audio.h"
#include "translations.h"
#include "pulses/modules_helpers.h"
#include "pulses/pulses.h"
#include "gui_common.h"

#include "opentx.h" // reusableBuffer

#include <algorithm>

MultiRfProtocols* MultiRfProtocols::_instance[NUM_MODULES] = {};

// MPM telemetry packet format type = MultiProtoDef (0x11)
//
// data[0]     = protocol number, 0xFF is an invalid list entry
//               (Option value too large) and nothing sent after
// data[1..n]  = protocol name null terminated
// data[n+1]   = flags
//                flags>>4 Option text number to be displayed
//                         (check multi status for description)
//                flags&0x01 failsafe supported
//                flags&0x02 Channel Map Disabled supported
// data[n+2]   = number of sub protocols
// data[n+3]   = sub protocols text length, only sent if nbr_sub != 0
// data[n+4..] = sub protocol names, only sent if nbr_sub != 0

constexpr uint8_t MULTI_INVALID_PROTO = 0xFF;
constexpr uint32_t MULTI_PROTOLIST_TIMEOUT = 100; // 100ms
constexpr uint32_t MULTI_PROTOLIST_START_TIMEOUT = 3000; // 3s

bool MultiRfProtocols::RfProto::parse(const uint8_t* data, uint8_t len)
{
  const char* s = (const char*)data;

  uint8_t subProtoNr = 0;
  uint8_t subProtoLen = 0;

  // special case handling for Frsky protos
  if (proto == MODULE_SUBTYPE_MULTI_FRSKY) {
    const mm_protocol_definition* def = getMultiProtocolDefinition(proto);
    if (!def) return false;

    label = "Frsky";
    flags = 0x20;
    subProtoNr = def->maxSubtype + 1;
    subProtoLen = def->subTypeString[0];
    s = def->subTypeString + 1;

  } else {
    // proto label string
    while (*s && len--) s++;
    if (*s || !len) return false;
    label = (const char*)data;
    s++;
    len--;

    // flags, subProtoNr, subProto label max length
    if (len < 2) return false;
    flags = (uint8_t) * (s++);
    //TRACE("flags: 0x%02X", flags);

    subProtoNr = (uint8_t) * (s++);
    len -= 2;

    if (!len) return true;
    subProtoLen = (uint8_t) * (s++);
    len--;

    if (len < subProtoNr * subProtoLen) return false;
  }

  fillSubProtoList(s, subProtoNr, subProtoLen);
  return true;
}

void MultiRfProtocols::RfProto::fillSubProtoList(const char* str, int n, int len)
{
  char tmp[len + 1];
  subProtos.reserve(n);

  for (int i = 0; i < n; i++, str += len) {
    strncpy(tmp, str, len);
    tmp[len] = '\0';
    subProtos.emplace_back((const char*)tmp);
    //TRACE("%s: '%s'", label.c_str(), subProtos.back().c_str());
  }
}

uint8_t MultiRfProtocols::RfProto::getOption() const
{
  uint8_t opt = flags >> 4;
  if (opt >= getMaxMultiOptions()) {
    opt = 1; // Unknown options are defaulted to type 1 (basic option)
  }
  return opt;
}

const char* MultiRfProtocols::RfProto::getOptionStr() const
{
  return mm_options_strings::options[getOption()];
}

MultiRfProtocols* MultiRfProtocols::instance(unsigned int moduleIdx)
{
  if (moduleIdx >= NUM_MODULES) return nullptr;
  if (!_instance[moduleIdx]) {
    _instance[moduleIdx] = new MultiRfProtocols(moduleIdx);
  }
  return _instance[moduleIdx];
}

void MultiRfProtocols::removeInstance(unsigned int moduleIdx)
{
  delete _instance[moduleIdx];
  _instance[moduleIdx] = nullptr;
}

int MultiRfProtocols::getIndex(unsigned int proto) const
{
  auto it = proto2idx.find(proto);
  return it != proto2idx.end() ? it->second : -1;
}

const MultiRfProtocols::RfProto* MultiRfProtocols::getProto(unsigned int proto) const
{
  int idx = getIndex(proto);
  if (idx >= 0 && (unsigned)idx < protoList.size())
    return &protoList[idx];

  return nullptr;
}

std::string MultiRfProtocols::getProtoLabel(unsigned int proto) const
{
  if (scanState != ScanEnd) {
    MultiModuleStatus& status = getMultiModuleStatus(moduleIdx);
    if (status.protocolName[0] && status.isValid()) {
      return std::string(status.protocolName);
    } else if (proto <= MODULE_SUBTYPE_MULTI_LAST) {
      char tmp[8];
      getStringAtIndex(tmp, STR_MULTI_PROTOCOLS, proto);
      return std::string(tmp);
    }
  } else {
    int idx = getIndex(proto);
    if (idx >= 0 && (unsigned)idx < protoList.size()) {
      return protoList[idx].label;
    }
  }
  return std::string();
}

std::string MultiRfProtocols::getLastProtoLabel() const
{
  if (!protoList.empty())
    return protoList.back().label;

  return std::string();
}

float MultiRfProtocols::getProgress() const
{
  constexpr float WAIT_TIME_RATIO = 0.7; // wait time accounts for 70%
  constexpr float PROTOS_RATIO = 1.0 - WAIT_TIME_RATIO;
  
  if (scanState == ScanStop)  return 0.0;
  if (scanState == ScanBegin) {
    float t = (float)(RTOS_GET_MS() - scanStart) / (float)MULTI_PROTOLIST_START_TIMEOUT;
    return t * WAIT_TIME_RATIO;
  }

  return WAIT_TIME_RATIO + ((float)getNProtos() / (float)totalProtos) * PROTOS_RATIO;
}

void MultiRfProtocols::fillList(std::function<void(const RfProto&)> addProto) const
{
  for (const auto& p : protoList) {
    addProto(p);
  }
}

bool MultiRfProtocols::triggerScan()
{
  if (scanState == ScanStop
      && moduleState[moduleIdx].mode == MODULE_MODE_NORMAL) {

    proto2idx.clear();
    protoList.clear();
    scanState = ScanBegin;
    currentProto = MULTI_INVALID_PROTO;
    moduleState[moduleIdx].mode = MODULE_MODE_GET_HARDWARE_INFO;
    scanStart = lastScan = RTOS_GET_MS();
    return true;
  }

  return false;
}

bool MultiRfProtocols::scanReply(const uint8_t* packet, uint8_t len)
{
  switch (scanState) {
    case ScanBegin:
    case Scanning:
      if (packet && len) {
        uint8_t replyProtoId = *packet;
        len--;
        packet++;

        // new status received
        if (replyProtoId != MULTI_INVALID_PROTO) {
          if (currentProto == MULTI_INVALID_PROTO) {
            //TRACE("# of protos: %d", totalProtos);
            totalProtos = replyProtoId;
            scanState = Scanning;
            protoList.reserve(totalProtos);
          } else {
            //TRACE("Proto = %d; label = '%s'", replyProtoId,
            //      (const char*)packet);

            int proto = convertMultiToOtx(replyProtoId);
            if (proto != MODULE_SUBTYPE_MULTI_CONFIG &&
                proto != MODULE_SUBTYPE_MULTI_SCANNER) {
              bool insertProto = true;
              if (proto == MODULE_SUBTYPE_MULTI_FRSKY) {
                auto it = std::find_if(protoList.begin(), protoList.end(),
                                       [=](const RfProto& p) {
                                         return p.proto == (const int)proto;
                                       });
                if (it != protoList.end()) {
                  // FRSKY proto already added
                  insertProto = false;
                }
              }

              if (insertProto) {
                RfProto rfProto(proto);
                if (rfProto.parse(packet, len)) {
                  proto2idx[proto] = protoList.size();
                  protoList.emplace_back(rfProto);
                } else {
                  TRACE("Error parsing proto [%d]", proto);
                }
              }
            } else {
              // do not count excluded protocols
              totalProtos--;
            }
          }

          currentProto++;
          lastScan = RTOS_GET_MS();
          return true;

        } else {
          scanState = ScanEnd;
          setModuleMode(moduleIdx, MODULE_MODE_NORMAL);
          break;
        }
      } else {
        uint32_t timeout = MULTI_PROTOLIST_TIMEOUT;

        if (scanState == ScanBegin) {
          // Timeout = 3s solely because of the very slow start time of the
          // MPM...
          timeout = MULTI_PROTOLIST_START_TIMEOUT;
        }

        if (RTOS_GET_MS() - lastScan >= timeout) {
          TRACE("proto scan timeout!");
          scanState = ScanInvalid;
        }
      }
      break;

    case ScanInvalid: {
      // TODO: fill with static list (sorted)
      const mm_protocol_definition* pdef =
          getMultiProtocolDefinition(MODULE_SUBTYPE_MULTI_FIRST);

      // build the list of static protos
      protoList.clear();
      protoList.reserve(MODULE_SUBTYPE_MULTI_LAST - MODULE_SUBTYPE_MULTI_FIRST + 1);
      for (; pdef->protocol != 0xfe; pdef++) {
        RfProto rfProto(pdef->protocol);

        char tmp[8];
        rfProto.label =
            getStringAtIndex(tmp, STR_MULTI_PROTOCOLS, pdef->protocol);
        rfProto.flags =
            (pdef->failsafe ? 0x01 : 0) | (pdef->disable_ch_mapping ? 0x02 : 0);

        if (pdef->subTypeString) {
          int st_len = pdef->subTypeString[0];
          const char* s = pdef->subTypeString + 1;

          rfProto.fillSubProtoList(s, pdef->maxSubtype + 1, st_len);
        }

        protoList.emplace_back(rfProto);
      }

      // sort it by label
      std::sort(
          protoList.begin(), protoList.end(),
          [](const RfProto& a, const RfProto& b) { return a.label < b.label; });

      // index the sorted list
      proto2idx.clear();
      for (unsigned int i = 0; i < protoList.size(); i++)
        proto2idx[protoList[i].proto] = i;

      scanState = ScanEnd;
      setModuleMode(moduleIdx, MODULE_MODE_NORMAL);
    } break;

    default:
      break;
  }

  return false;
}