iflytoplinuxsdk_ptdv1/src/iflytop/components/zcanreceiver/zcanhost.cpp

#include "zcanhost.hpp"

using namespace iflytop;
using namespace core;
using namespace zcr;

#define zint16p(x)  ((int16_t*)(x))
#define zuint16p(x) ((uint16_t*)(x))
#define zint32p(x)  ((int32_t*)(x))

#define CHECK_ARGC_NUM(x)                                           \
  if (argc != (x + 1)) {                                            \
    logger->error("do {},argc error {}!={}", argv[0], argc, x + 1); \
    retval = "argc num error";                                      \
    return false;                                                   \
  }

void ZCanHost::initialize(string can_if_name, int baudrate, bool enablLoopback) {
  m_zcanReceiverHost = make_shared<ZCanReceiverHost>();
  m_zcanReceiverHost->initialize(can_if_name, baudrate, enablLoopback);

  m_cmdMap["pumpctrl_c1004"] = [this](int argc, char** argv, string& retval) {
    // CHECK_ARGC_NUM(3);

    if (argc == 4) {
      uint8_t id  = atoi(argv[1]);
      int16_t acc = atoi(argv[2]);
      int16_t rpm = atoi(argv[3]);
      return pumpctrl_c1004(id, acc, rpm) != 0;
    } else if (argc == 6) {
      uint8_t id        = atoi(argv[1]);
      int16_t acc       = atoi(argv[2]);
      int16_t rpm       = atoi(argv[3]);
      int8_t  idlepower = atoi(argv[4]);
      int8_t  power     = atoi(argv[5]);
      return pumpctrl_c1004(id, acc, rpm, idlepower, power) != 0;
    }
    logger->error("do {},argc error {}!={},{}", argv[0], argc, 4, 6);
    retval = "argc num error";
    return false;
  };

  // ZCanHost::readio(int id, bool& value)
  // ZCanHost::writeio(int id, bool value)
  // ZCanHost::readadc(int id, int& value)

  m_cmdMap["readio"] = [this](int argc, char** argv, string& retval) {
    CHECK_ARGC_NUM(1);
    uint8_t id = atoi(argv[1]);
    bool    v  = false;
    if (readio(id, v) != 0) {
      logger->error("readio fail");
      return false;
    }
    retval = to_string(v);
    logger->info("readio:{},{}", id, v);
    return true;
  };

  m_cmdMap["writeio"] = [this](int argc, char** argv, string& retval) {
    CHECK_ARGC_NUM(2);
    uint8_t id = atoi(argv[1]);
    bool    v  = atoi(argv[2]);
    if (writeio(id, v) != 0) {
      logger->error("writeio fail");
      return false;
    }
    return true;
  };

  m_cmdMap["readadc"] = [this](int argc, char** argv, string& retval) {
    CHECK_ARGC_NUM(1);
    uint8_t id = atoi(argv[1]);
    int     v  = 0;
    if (readadc(id, v) != 0) {
      logger->error("readadc fail");
      return false;
    }
    logger->info("readadc:{},{}", id, v);
    retval = fmt::format("ACK:{}", v);
    return true;
  };

  m_cmdMap["hpp272_read_c1000"] = [this](int argc, char** argv, string& retval) {
    CHECK_ARGC_NUM(1);
    uint8_t       id = atoi(argv[1]);
    hpp272_data_t v  = {0};
    if (hpp272_read_c1000(id, v) != 0) {
      logger->error("hpp272_read_c1000 fail");
      return false;
    }
    logger->info("hpp272_read_c1000 {}:", id);
    logger->info("  hydrogen_peroxide_volume                 :{}", v.hydrogen_peroxide_volume);
    logger->info("  h2o_h2o2_rs                              :{}", v.h2o_h2o2_rs);
    logger->info("  temperature1                             :{}", v.temperature1);
    logger->info("  relative_humidity                        :{}", v.relative_humidity);
    logger->info("  absolute_hydrogen_peroxide               :{}", v.absolute_hydrogen_peroxide);
    logger->info("  h2o_h2o2dew_point_temperature            :{}", v.h2o_h2o2dew_point_temperature);
    logger->info("  reserved1                                :{}", v.reserved1);
    logger->info("  water_volume                             :{}", v.water_volume);
    logger->info("  water_vapor_pressure                     :{}", v.water_vapor_pressure);
    logger->info("  absolute_humidity                        :{}", v.absolute_humidity);
    logger->info("  water_vapor_saturation_pressure_h2o      :{}", v.water_vapor_saturation_pressure_h2o);
    logger->info("  temperature2                             :{}", v.temperature2);
    logger->info("  h2o2_vapor_pressure                      :{}", v.h2o2_vapor_pressure);
    logger->info("  water_vapor_saturation_pressure_h2o_h2o2 :{}", v.water_vapor_saturation_pressure_h2o_h2o2);

    // retval = fmt::format("ACK:{}", v);
    return true;
  };

  m_cmdMap["warning_light_ctrl_c1002"] = [this](int argc, char** argv, string& retval) {
    CHECK_ARGC_NUM(5);
    uint8_t id = atoi(argv[1]);
    uint8_t r  = atoi(argv[2]);
    uint8_t g  = atoi(argv[3]);
    uint8_t b  = atoi(argv[4]);
    uint8_t w  = atoi(argv[5]);

    if (warning_light_ctrl_c1002(id, r, g, b, w) != 0) {
      logger->error("warning_light_ctrl_c1002 fail");
      return false;
    }
    return true;
  };
  // huacheng_pressure_sensor_read_c1005
  m_cmdMap["huacheng_pressure_sensor_read_c1005"] = [this](int argc, char** argv, string& retval) {
    CHECK_ARGC_NUM(1);
    uint8_t                               id = atoi(argv[1]);
    huacheng_pressure_sensor_read_c1005_t v;
    if (huacheng_pressure_sensor_read_c1005(id, v) != 0) {
      logger->error("huacheng_pressure_sensor_read_c1005 fail");
      return false;
    }
    // uint8_t  precision;  // 0，1，2，3
    // uint8_t  unit;       // 0-Mpa ,1-Kpa ,2-Pa ,3-Bar ,4-Mbar ,5-kg/cm2 ,6-psi ,7-mh2o ,8-mmh2o
    // uint16_t value;      // value,  realvalue = value / 10^precision unit
    // uint16_t zero;       // 零点
    // uint16_t full;       // 满量程
    logger->info("huacheng_pressure_sensor_read_c1005 {}:", id);
    logger->info("  precision   :{}", v.precision);
    logger->info("  unit        :{}", v.unit);
    logger->info("  value       :{}", v.value);
    logger->info("  zero        :{}", v.zero);
    logger->info("  full        :{}", v.full);
    // retval = fmt::format("ACK:{}", v);
    return true;
  };

  m_cmdMap["sleep_ms"] = [this](int argc, char** argv, string& retval) {
    CHECK_ARGC_NUM(1);
    int32_t ms = atoi(argv[1]);
    logger->info("sleep_ms:{}", ms);
    std::this_thread::sleep_for(std::chrono::milliseconds(ms));
    return true;
  };
}

static void prase_cmd(char* input, int inputlen, int& argc, char* argv[]) {
  bool findcomment = false;
  for (size_t i = 0; input[i] == 0 || i < inputlen; i++) {
    if (input[i] == ' ' || input[i] == '\r' || input[i] == '\n' || input[i] == '\t') {
      input[i] = 0;
    }

    if (input[i] == '#') {
      findcomment = true;
    }
    if (findcomment) {
      input[i] = 0;
    }
  }

  int j = 0;
  for (size_t i = 0; input[i] == 0 || i < inputlen; i++) {
    if (input[i] != 0 && j == 0) {
      argv[argc++] = &input[i];
      j            = 1;
      continue;
    }

    if (input[i] == 0 && j == 1) {
      j = 0;
      continue;
    }
  }
}

bool ZCanHost::execcmd(string cmd, string& retval) {
  int   argc           = 0;
  char* argv[10]       = {0};
  char  cmdcache[1024] = {0};
  // context.rawcmd = cmd;
  strcpy(cmdcache, cmd.c_str());
  logger->info("do cmd:{}", cmdcache);

  for (size_t i = 0; i < cmd.size(); i++) {
    if (cmdcache[i] == '#') {
      cmdcache[i] = '\0';
    }
  }
  prase_cmd(cmdcache, strlen(cmdcache), argc, argv);
  if (argc == 0) {
    logger->error("cmd:{} prase error", cmd);
    return false;
  }

  if (m_cmdMap.find(string(argv[0])) != m_cmdMap.end()) {
    return m_cmdMap.find(string(argv[0]))->second(argc, argv, retval);
  }
  logger->error("cmd:{} not found", argv[0]);
  return false;
}

/*******************************************************************************
 *                                     CMD                                     *
 *******************************************************************************/
/**
 * @brief 0-1000 Command support
 */
int32_t ZCanHost::ping(int board_id) {
  shared_ptr<ZCanReceiverCMD> cmd = make_shared<ZCanReceiverCMD>();
  cmd->cmdid                      = 0;
  cmd->subcmdid                   = 0;
  cmd->data[0]                    = board_id;
  cmd->len                        = 1;
  auto rx                         = m_zcanReceiverHost->sendcmdblock(cmd, 100);
  if (rx == nullptr) {
    return err::zecode(err::knoack);
  }
  if (rx->packetType == kpt_error_ack) {
    logger->error("ping error ack,{}", *zuint16p(&rx->data[0]));
    return err::zecode(err::kfail);
  }

  return 0;
}

int32_t ZCanHost::readio(int id, bool& value) {
  shared_ptr<ZCanReceiverCMD> cmd = make_shared<ZCanReceiverCMD>();
  cmd->cmdid                      = 1;
  cmd->subcmdid                   = 0;
  cmd->data[0]                    = id;
  cmd->len                        = 1;
  auto rx                         = m_zcanReceiverHost->sendcmdblock(cmd, 100);
  if (rx == nullptr) {
    return err::zecode(err::knoack);
  }
  if (rx->packetType == kpt_error_ack) {
    logger->error("readio error ack,{}", *zuint16p(&rx->data[0]));
    return err::zecode(err::knoack);
  }

  value = rx->data[1];
  return 0;
}
int32_t ZCanHost::writeio(int id, bool value) {
  shared_ptr<ZCanReceiverCMD> cmd = make_shared<ZCanReceiverCMD>();
  cmd->cmdid                      = 2;
  cmd->subcmdid                   = 0;
  cmd->data[0]                    = id;
  cmd->data[1]                    = value;
  cmd->len                        = 2;
  auto rx                         = m_zcanReceiverHost->sendcmdblock(cmd, 100);
  if (rx == nullptr) {
    return err::zecode(err::knoack);
  }
  if (rx->packetType == kpt_error_ack) {
    logger->error("writeio error ack,{}", *zuint16p(&rx->data[0]));
    return err::zecode(err::knoack);
  }
  m_writeio_cache[id] = value;
  return 0;
}
bool ZCanHost::read_writeio_state_cache(int id) { return m_writeio_cache[id]; }

int32_t ZCanHost::readadc(int id, int& value) {
  shared_ptr<ZCanReceiverCMD> cmd = make_shared<ZCanReceiverCMD>();
  cmd->cmdid                      = 3;
  cmd->subcmdid                   = 0;
  cmd->data[0]                    = id;
  cmd->len                        = 1;
  auto rx                         = m_zcanReceiverHost->sendcmdblock(cmd, 100);
  if (rx == nullptr) {
    return err::zecode(err::knoack);
  }

  if (rx->packetType == kpt_error_ack) {
    logger->error("readadc error ack,{}", *zuint16p(&rx->data[0]));
    return err::zecode(err::knoack);
  }
  value = *(int32_t*)(&rx->data[2]);
  return 0;
}

// 1004
int32_t ZCanHost::pumpctrl_c1004(int sensorid, int16_t acc, int16_t rpm) {
  shared_ptr<ZCanReceiverCMD> cmd = make_shared<ZCanReceiverCMD>();
  cmd->cmdid                      = 1004;
  cmd->subcmdid                   = 0;
  cmd->data[0]                    = sensorid;
  *zint16p(&cmd->data[2])         = acc;
  *zint16p(&cmd->data[4])         = rpm;
  cmd->len                        = 6;
  auto rx                         = m_zcanReceiverHost->sendcmdblock(cmd, 100);
  if (rx == nullptr) {
    return err::zecode(err::knoack);
  }
  if (rx->packetType == kpt_error_ack) {
    logger->error("pumpctrl_c1004 error ack,{}", *zuint16p(&rx->data[0]));
    return err::zecode(err::kfail);
  }
  m_pumpc1004_speed_cache[sensorid] = rpm;
  return 0;
}
int16_t ZCanHost::pumpctrl_c1004_get_speed_cache(int id) { return m_pumpc1004_speed_cache[id]; }

int32_t ZCanHost::pumpctrl_c1004(int sensorid, int16_t acc, int16_t rpm, int8_t idlepower, int8_t power) {
  shared_ptr<ZCanReceiverCMD> cmd = make_shared<ZCanReceiverCMD>();
  cmd->cmdid                      = 1004;
  cmd->subcmdid                   = 1;
  cmd->data[0]                    = sensorid;
  *zint16p(&cmd->data[2])         = acc;
  *zint16p(&cmd->data[4])         = rpm;
  cmd->data[6]                    = idlepower;
  cmd->data[7]                    = power;
  cmd->len                        = 8;
  auto rx                         = m_zcanReceiverHost->sendcmdblock(cmd, 100);
  if (rx == nullptr) {
    return err::zecode(err::knoack);
  }
  if (rx->packetType == kpt_error_ack) {
    logger->error("pumpctrl_c1004 error ack,{}", *zuint16p(&rx->data[0]));
    return err::zecode(err::kfail);
  }

  m_pumpc1004_speed_cache[sensorid] = rpm;
  return 0;
}

/**
 * @brief 报警三色指示灯控制
 *
 * @param sensorid
 * @param r
 * @param g
 * @param b
 * @param w
 */
int32_t ZCanHost::warning_light_ctrl_c1002(uint8_t sensorid, uint8_t r, uint8_t g, uint8_t b, uint8_t w) {
  shared_ptr<ZCanReceiverCMD> cmd = make_shared<ZCanReceiverCMD>();
  cmd->cmdid                      = 1002;
  cmd->subcmdid                   = 0;
  cmd->data[0]                    = sensorid;
  cmd->data[2]                    = r;
  cmd->data[3]                    = g;
  cmd->data[4]                    = b;
  cmd->data[5]                    = w;
  cmd->len                        = 6;

  auto rx = m_zcanReceiverHost->sendcmdblock(cmd, 100);

  if (!rx) {
    return err::zecode(err::knoack);
  }

  if (rx->packetType == kpt_error_ack) {
    logger->error("warning_light_ctrl_c1002 error ack,{}", *zuint16p(&rx->data[0]));
    return err::zecode(err::kfail);
  }

  return 0;
}

/**
 * @brief 读取华成压力传感器数值
 *
 * @param sensorid
 * @param value
 */

int32_t ZCanHost::huacheng_pressure_sensor_read_c1005(int sensorid, huacheng_pressure_sensor_read_c1005_t& value) {
  /**
   * @brief
   */

  shared_ptr<ZCanReceiverCMD> cmd = make_shared<ZCanReceiverCMD>();
  cmd->cmdid                      = 1005;
  cmd->subcmdid                   = 0;
  cmd->data[0]                    = sensorid;
  cmd->len                        = 2;

  auto rx = m_zcanReceiverHost->sendcmdblock(cmd, 100);
  if (!rx) {
    return err::zecode(err::knoack);
  }

  if (rx->packetType == kpt_error_ack) {
    logger->error("huacheng_pressure_sensor_read_c1005 error ack");
    return err::zecode(err::kfail);
  }

  if (rx->len != 10) {
    logger->warn("huacheng_pressure_sensor_read_c1005 rx len error:{}", rx->len);
    return err::zecode(err::kfail);
  }

  value.precision = rx->data[2];
  value.unit      = rx->data[3];
  value.value     = *(int16_t*)(&rx->data[4]);
  value.zero      = *(int16_t*)(&rx->data[6]);
  value.full      = *(int16_t*)(&rx->data[8]);

  return 0;
}

int32_t ZCanHost::hpp272_read_c1000(int sensorid, hpp272_data_t& value) {
  memset(&value, 0, sizeof(hpp272_data_t));
  shared_ptr<ZCanReceiverCMD> cmd = make_shared<ZCanReceiverCMD>();
  cmd->cmdid                      = 1000;
  cmd->subcmdid                   = 0;
  cmd->data[0]                    = sensorid;
  cmd->len                        = 2;

  auto rx = m_zcanReceiverHost->sendcmdblock(cmd, 300);
  if (!rx) {
    logger->error("hpp272_read_c1000 timeout");
    return err::zecode(err::knoack);
  }

  if (rx->packetType == kpt_error_ack) {
    logger->error("hpp272_read_c1000 error ack");
    return err::zecode(err::kfail);
  }

  if (rx->len != sizeof(hpp272_data_t) + 2) {
    logger->warn("hpp272_read_c1000 rx len error:{}!=", rx->len, sizeof(hpp272_data_t) + 2);
    return err::zecode(err::kfail);
  }
  memcpy(&value, &rx->data[2], sizeof(hpp272_data_t));
  return 0;
}