예제 #1
0
int
main(int argc, char** argv)
{
  if (argc != 3)
  {
    std::cerr << "Usage: " << argv[0] << "FILE.lsf[.gz]" << "FILE.ubx" << std::endl;
    return 1;
  }

  std::istream* is = 0;
  Compression::Methods method = Compression::Factory::detect(argv[1]);
  if (method == METHOD_UNKNOWN)
    is = new std::ifstream(argv[1], std::ios::binary);
  else
    is = new Compression::FileInput(argv[1], method);

  std::ofstream ofs(argv[2], std::ios::binary);

  DUNE::IMC::Message* msg = 0;

  try
  {
    while ((msg = DUNE::IMC::Packet::deserialize(*is)) != 0)
    {
      if (msg->getId() == DUNE_IMC_DEVDATABINARY)
      {
        DUNE::IMC::DevDataBinary* ddb = (DUNE::IMC::DevDataBinary*)msg;

        if (ddb->value.size() < 2)
          continue;

        if ((uint8_t)ddb->value[0] == c_ubx_sync0 && (uint8_t)ddb->value[1] == c_ubx_sync1)
          ofs.write(&ddb->value[0], ddb->value.size());
      }
    }
  }
  catch (std::runtime_error& e)
  {
    std::cerr << "ERROR: " << e.what() << std::endl;
  }

  delete is;

  return 0;
}
예제 #2
0
int
main(int argc, char** argv)
{
  if (argc != 2)
  {
    std::cerr << "Usage: " << argv[0] << " Data.lsf[.gz]" << std::endl;
    return 1;
  }

  std::istream* is = 0;
  Compression::Methods method = Compression::Factory::detect(argv[1]);
  if (method == METHOD_UNKNOWN)
    is = new std::ifstream(argv[1], std::ios::binary);
  else
    is = new Compression::FileInput(argv[1], method);

  DUNE::IMC::Message* msg = NULL;

  Path folder = Path(argv[1]).dirname();

  try
  {
    while ((msg = DUNE::IMC::Packet::deserialize(*is)) != 0)
    {
      if (msg->getId() == DUNE_IMC_COMPRESSEDIMAGE)
      {
        DUNE::IMC::CompressedImage* img = dynamic_cast<DUNE::IMC::CompressedImage*>(msg);

        Path fname = folder / String::str("%0.4f.jpg", img->getTimeStamp());
        std::ofstream ofs(fname.c_str(), std::ios::binary);
        ofs.write(&(img->data[0]), img->data.size());
      }

      delete msg;
    }
  }
  catch (std::runtime_error& e)
  {
    std::cerr << "ERROR: " << e.what() << std::endl;
  }

  delete is;

  return 0;
}
int
main(int32_t argc, char** argv)
{
  if (argc <= 1)
  {
    std::cerr << "Usage: " << argv[0] << " <path_to_log_1/Data.lsf[.gz]> ... <path_to_log_n/Data.lsf[.gz]>"
              << std::endl;
    std::cerr << "Or: " << argv[0] << "-e <Voltage Entity Label> <Current Entity Label> <path_to_log_1/Data.lsf[.gz]> ... <path_to_log_n/Data.lsf[.gz]>"
              << std::endl;
    return 1;
  }

  std::string volt_label;
  std::string curr_label;

  unsigned start_index = 1;

  if (strcmp(argv[1], "-e") == 0)
  {
    if (argc < 5)
    {
      std::cerr << "Too few arguments" << std::endl;
      return 1;
    }

    volt_label = argv[2];
    curr_label = argv[3];

    start_index = 4;
  }
  else
  {
    volt_label = c_label;
    curr_label = c_label;
  }

  // Total of energy spent
  double total_accum = 0.0;
  // Total energy spent while the motor was on
  double motor_total_accum = 0.0;

  // Moving average window sizes
  unsigned wsizes[Monitors::FuelLevel::BatteryData::BM_TOTAL];

  for (unsigned k = 0; k < Monitors::FuelLevel::BatteryData::BM_TOTAL; k++)
    wsizes[k] = c_samples;

  for (int32_t i = start_index; i < argc; ++i)
  {
    std::istream* is = 0;
    DUNE::Compression::Methods method = DUNE::Compression::Factory::detect(argv[i]);
    if (method == DUNE::Compression::METHOD_UNKNOWN)
      is = new std::ifstream(argv[i], std::ios::binary);
    else
      is = new DUNE::Compression::FileInput(argv[i], method);

    DUNE::IMC::Message* msg = NULL;

    bool got_name = false;
    std::string log_name = "unknown";

    // Energy computation related data
    Monitors::FuelLevel::BatteryData bdata(wsizes);
    bool volt_entity_set = false;
    bool curr_entity_set = false;
    bool entities_set = false;
    unsigned eids[Monitors::FuelLevel::BatteryData::BM_TOTAL] = {0};
    unsigned samples = 0;
    double last_timestamp = 0.0;
    double accum = 0.0;

    // Current rpm value
    float rpm = 0.0;

    // Ignore some logs
    bool ignore = false;

    try
    {
      while ((msg = DUNE::IMC::Packet::deserialize(*is)) != 0)
      {

        if (msg->getId() == DUNE_IMC_LOGGINGCONTROL)
        {
          if (!got_name)
          {
            DUNE::IMC::LoggingControl* ptr = static_cast<DUNE::IMC::LoggingControl*>(msg);

            if (ptr->op == DUNE::IMC::LoggingControl::COP_STARTED)
            {
              log_name = ptr->name;
              got_name = true;
            }
          }
        }
        else if (msg->getId() == DUNE_IMC_ENTITYINFO)
        {
          DUNE::IMC::EntityInfo* ptr = static_cast<DUNE::IMC::EntityInfo*>(msg);

          if (ptr->label.compare(volt_label) == 0)
          {
            eids[Monitors::FuelLevel::BatteryData::BM_VOLTAGE] = ptr->id;
            volt_entity_set = true;
          }

          if (ptr->label.compare(curr_label) == 0)
          {
            eids[Monitors::FuelLevel::BatteryData::BM_CURRENT] = ptr->id;
            curr_entity_set = true;
          }

          if (!entities_set && volt_entity_set && curr_entity_set)
          {
            bdata.setEntities(eids);
            entities_set = true;
          }
        }
        else if (msg->getId() == DUNE_IMC_VOLTAGE)
        {
          if (entities_set)
          {
            DUNE::IMC::Voltage* ptr = static_cast<DUNE::IMC::Voltage*>(msg);
            bdata.update(ptr);
            ++samples;

            if (samples > c_min_samples)
            {
              float drop = bdata.getEnergyDrop(msg->getTimeStamp() - last_timestamp);
              accum += drop;

              if (rpm > c_min_rpm)
                motor_total_accum += drop;
            }
          }

          last_timestamp = msg->getTimeStamp();
        }
        else if (msg->getId() == DUNE_IMC_CURRENT)
        {
          if (entities_set)
          {
            DUNE::IMC::Current* ptr = static_cast<DUNE::IMC::Current*>(msg);
            bdata.update(ptr);
          }
        }
        else if (msg->getId() == DUNE_IMC_RPM)
        {
          DUNE::IMC::Rpm* ptr = static_cast<DUNE::IMC::Rpm*>(msg);
          rpm = ptr->value;
        }
        else if (msg->getId() == DUNE_IMC_SIMULATEDSTATE)
        {
          // since it has simulated state let us ignore this log
          ignore = true;
          delete msg;
          std::cerr << "this is a simulated log";
          break;
        }

        delete msg;
      }
    }
    catch (std::runtime_error& e)
    {
      std::cerr << "ERROR: " << e.what() << std::endl;
    }

    delete is;

    if (ignore)
    {
      std::cerr << "... ignoring" << std::endl;
      continue;
    }

    std::cerr << "Consumed " << accum << " in " << log_name << "." << std::endl;

    total_accum += accum;
  }

  std::cerr << "Total energy consumed is " << total_accum << "Wh" << std::endl
            << "The amount of " << motor_total_accum
            << std::fixed << std::setprecision(1)
            << "Wh (" << motor_total_accum / total_accum * 100.0
            << "%) was consumed while the motor was on" << std::endl;

  return 0;
}
예제 #4
0
int
main(int32_t argc, char** argv)
{
  if (argc != 3)
  {
    std::cerr << "Usage: " << argv[0] << " <config ini file> <path_to_log.lsf[.gz]>"
              << std::endl;
    return 1;
  }

  Arguments m_args;

  readArgs(argv[1], m_args);
  m_args.filter_args.decay_factor *= 0.01f;

  //! Array of entities
  unsigned m_eids[BatteryData::BM_TOTAL];

  // Filter pointer
  FuelFilter* m_fuel_filter = NULL;

  bool resolved_entities[BatteryData::BM_TOTAL];
  for (unsigned i = 0; i < BatteryData::BM_TOTAL; ++i)
    resolved_entities[i] = false;

  EPMap m_epower;

  bool got_entities = false;

  PseudoTimer timer;
  PeriodicRun pr(1.0);

  ByteBuffer buffer;
  std::ofstream lsf("NewFuel.lsf", std::ios::binary);

  std::istream* is = 0;
  DUNE::Compression::Methods method = DUNE::Compression::Factory::detect(argv[2]);
  if (method == DUNE::Compression::METHOD_UNKNOWN)
    is = new std::ifstream(argv[2], std::ios::binary);
  else
    is = new DUNE::Compression::FileInput(argv[2], method);

  if (!is)
  {
    std::cerr << "bad file" << std::endl;
    return 1;
  }

  is->seekg(0, is->end);
  unsigned file_length = is->tellg();
  is->seekg(0, is->beg);
  Time::Counter<float> prog_timer(5.0);

  DUNE::IMC::Message* msg = NULL;

  DUNE::IMC::FuelLevel* ptr = NULL;

  bool got_first = false;

  try
  {
    while ((msg = DUNE::IMC::Packet::deserialize(*is)) != 0)
    {
      bool log_it = false;

      timer.update(msg->getTimeStamp());

      if (!got_first)
      {
        IMC::EstimatedState state;
        state.setTimeStamp(msg->getTimeStamp());
        IMC::Packet::serialize(&state, buffer);
        lsf.write(buffer.getBufferSigned(), buffer.getSize());
        got_first = true;

        std::cerr << "got first timestamp" << std::endl;

        m_fuel_filter = new FuelFilter(&m_args.filter_args, m_eids, &m_epower,
                                       NULL, true, msg->getTimeStamp());
      }

      if (msg->getId() == DUNE_IMC_ENTITYINFO)
      {
        IMC::EntityInfo* ent = static_cast<IMC::EntityInfo*>(msg);

        if (!got_entities)
        {
          bool got_all = true;

          for (unsigned i = 0; i < BatteryData::BM_TOTAL; ++i)
          {
            if (ent->label == m_args.elb[i])
            {
              m_eids[i] = ent->id;
              resolved_entities[i] = true;
            }

            if (resolved_entities[i] == false)
              got_all = false;
          }

          got_entities = got_all;

          if (got_entities)
            std::cerr << "Got all entities" << std::endl;
        }

        for (unsigned i = 0; i < m_args.est_list.size(); i++)
        {
          if (ent->label == m_args.est_list[i])
            m_epower.insert(EPPair(ent->id, EntityPower(m_args.est_power[i])));
        }

        if (m_args.est_list.size() < m_epower.size())
          std::cerr << "TOO MANY ENTRIES!" << std::endl;

        IMC::Packet::serialize(msg, buffer);
        lsf.write(buffer.getBufferSigned(), buffer.getSize());
        delete msg;
        continue;
      }

      if (msg->getId() == DUNE_IMC_VOLTAGE)
      {
        m_fuel_filter->onVoltage(static_cast<IMC::Voltage*>(msg));
        log_it = true;
      }
      else if (msg->getId() == DUNE_IMC_CURRENT)
      {
        m_fuel_filter->onCurrent(static_cast<IMC::Current*>(msg));
        log_it = true;
      }
      else if (msg->getId() == DUNE_IMC_TEMPERATURE)
      {
        m_fuel_filter->onTemperature(static_cast<IMC::Temperature*>(msg));
        log_it = true;
      }
      else if (msg->getId() == DUNE_IMC_VEHICLESTATE)
      {
        m_fuel_filter->onVehicleState(static_cast<IMC::VehicleState*>(msg));
        log_it = true;
      }
      else if (msg->getId() == DUNE_IMC_FUELLEVEL)
      {
        log_it = true;
      }
      else if (msg->getId() == DUNE_IMC_ENTITYACTIVATIONSTATE)
      {
        m_fuel_filter->onEntityActivationState(static_cast<IMC::EntityActivationState*>(msg));
        log_it = true;
      }

      if (timer.isValid() && pr.doRun(timer.getTime()))
      {
        // Update fuel filter
        if (m_fuel_filter->update())
        {
          IMC::FuelLevel fl;
          fl.setSourceEntity(250);
          fl.setTimeStamp(msg->getTimeStamp());

          m_fuel_filter->fillMessage(fl, m_args.op_labels, m_args.op_values);

          if (ptr != NULL)
          {
            float diff = ptr->value - fl.value;
            char sign = (diff > 0)? '-' : '+';
            if (std::fabs(diff) > 1.0)
              std::cerr << "jumped " << sign
                        << std::fabs(diff) / 100 * m_args.filter_args.full_capacity
                        << " (" << std::fabs(diff) << "%)" << std::endl;
          }

          Memory::clear(ptr);
          ptr = static_cast<IMC::FuelLevel*>(fl.clone());

          IMC::Packet::serialize(&fl, buffer);
          lsf.write(buffer.getBufferSigned(), buffer.getSize());
        }
      }

      if (log_it)
      {
        IMC::Packet::serialize(msg, buffer);
        lsf.write(buffer.getBufferSigned(), buffer.getSize());
      }

      if (prog_timer.overflow())
      {
        std::cerr << (float)is->tellg() / file_length * 100.0 << "%" << std::endl;
        prog_timer.reset();
      }

      delete msg;
    }
  }
  catch (std::runtime_error& e)
  {
    std::cerr << "ERROR: " << e.what() << std::endl;
  }

  Memory::clear(m_fuel_filter);
  Memory::clear(ptr);

  delete is;

  return 0;
}