Program Listing for File ubx.hpp

↰ Return to documentation for file (/tmp/ws/src/ublox_dgnss/ublox_dgnss_node/include/ublox_dgnss_node/ubx/ubx.hpp)

// Copyright 2021 Australian Robotics Supplies & Technology
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef UBLOX_DGNSS_NODE__UBX__UBX_HPP_
#define UBLOX_DGNSS_NODE__UBX__UBX_HPP_
#include <sstream>
#include <iostream>
#include <iomanip>
#include <vector>
#include <cstring>
#include <string>
#include <tuple>
#include <memory>
#include "ublox_dgnss_node/usb.hpp"
#include "ublox_dgnss_node/ubx/ubx_types.hpp"
#include "ublox_dgnss_node/ubx/ubx_msg.hpp"
#include "ublox_dgnss_node/ubx/utils.hpp"
#include "ublox_dgnss_node/ubx/ubx_exceptions.hpp"

namespace ubx
{

struct Frame
{
  u1_t sync_char_1 = UBX_SYNC_CHAR_1;
  u1_t sync_char_2 = UBX_SYNC_CHAR_2;
  msg_class_t msg_class;
  msg_id_t msg_id;
  u2_t length;       // of payload
  ch_t * payload;
  u1_t ck_a;
  u1_t ck_b;
  std::vector<u1_t> buf;
  u1_t * build_frame_buf()
  {
    buf.clear();
    buf.reserve(8 + length);
    buf.push_back(sync_char_1);
    buf.push_back(sync_char_2);
    buf.push_back(msg_class);
    buf.push_back(msg_id);
    buf_append_u2(&buf, length);
    for (int i = 0; i < length; i++) {
      buf.push_back(payload[i]);
    }
    // put markers in for checksom values
    buf.push_back(ck_a);
    buf.push_back(ck_b);
    return &buf[0];
  }

  void from_buf_build()
  {
    sync_char_1 = buf[0];
    sync_char_2 = buf[1];
    msg_class = buf[2];
    msg_id = buf[3];
    length = *reinterpret_cast<u2_t *>(&buf[4]);
    payload = reinterpret_cast<ch_t *>(&buf[6]);
    ck_a = buf[buf.size() - 2];
    ck_b = buf[buf.size() - 1];
  }

  std::tuple<u1_t, u1_t> ubx_check_sum()
  {
    build_frame_buf();
    size_t n = buf.size() - 2;             // dont include the two checksum fields
    u1_t ck_a = 0;
    u1_t ck_b = 0;
    for (size_t i = 2; i < n; i++) {
      ck_a = ck_a + buf[i];
      ck_b = ck_b + ck_a;
    }

    return std::make_tuple(ck_a, ck_b);
  }
  std::string to_hex()
  {
    std::ostringstream os;
    os << "size: " << buf.size() << " '0x";
    for (const auto & u1 : buf) {
      os << ubx::to_hex(u1);
    }
    os << "'";
    return os.str();
  }
  std::string payload_to_hex()
  {
    std::ostringstream os;
    os << "length: " << length << " '0x";
    for (size_t i = 0; i < length; i++) {
      const u1_t u1 = (u1_t)payload[i];
      os << ubx::to_hex(u1);
    }
    os << "'";
    return os.str();
  }
};

using FramePoll = Frame;
using FramePolled = Frame;
using FrameValSet = Frame;

std::shared_ptr<FramePolled> get_polled_frame(
  std::shared_ptr<usb::Connection> usbc,
  std::shared_ptr<FramePoll> poll_frame)
{
  usbc->write_buffer(&poll_frame->buf[0], poll_frame->buf.size());

  auto polled_frame = std::make_shared<FramePolled>();

  int max_retries = 1000 / usbc->timeout_ms();       // max retries per second
  int i = 0;
  static u_char buf[64 * 100 + 1];
  int len;
  do {
    std::memset(buf, 0, sizeof(buf));
    try {
      len = usbc->read_chars(buf, sizeof(buf));
    } catch (usb::UsbException & e) {
      throw e;
    } catch (usb::TimeoutException & e) {
      // timeout is set in usbc_
      continue;
    } catch (std::exception & e) {
      throw e;
    }


    if (len > 0) {
      if (buf[0] == UBX_SYNC_CHAR_1 && buf[1] == UBX_SYNC_CHAR_2) {
        polled_frame->buf.resize(len);
        memcpy(&polled_frame->buf[0], &buf[0], len);
        polled_frame->from_buf_build();

        // make sure checksums match
        u1_t ck_a, ck_b;
        std::tie(ck_a, ck_b) = polled_frame->ubx_check_sum();
        if (ck_a != polled_frame->ck_a && ck_b != polled_frame->ck_b) {
          throw UbxAckNackException("polled frame checksum failed");
        }

        // if device failed to acknowledge the poll request then Nak it
        if (polled_frame->msg_class == UBX_ACK && polled_frame->msg_id == UBX_ACK_NAK) {
          std::ostringstream msg_oss;
          msg_oss << "UBX_ACK_NAK fail";
          msg_oss << " sent poll_frame.msg_class: " << "0x" << std::setfill('0') << std::setw(2) <<
            std::right << std::hex << +poll_frame->msg_class;
          msg_oss << " poll_frame.msg_id: " << "0x" << std::setfill('0') << std::setw(2) <<
            std::right << std::hex << +poll_frame->msg_id;
          msg_oss << " repsonse polled_frame.msg_class: " << "0x" << std::setfill('0') << std::setw(
            2) << std::right << std::hex << +polled_frame->msg_class;
          msg_oss << " polled_frame.msg_id: " << "0x" << std::setfill('0') << std::setw(2) <<
            std::right << std::hex << +polled_frame->msg_id;
          throw UbxAckNackException(msg_oss.str());
        }

        // exit while loop
        break;
      }
    }
  } while (++i < max_retries);

  if (i >= max_retries) {
    std::ostringstream msg_oss;
    msg_oss << "UBX_ACK_NAK wasnt received after " << i << " tries";
    msg_oss << " sent poll_frame.msg_class: " << "0x" << std::setfill('0') << std::setw(2) <<
      std::right << std::hex << +poll_frame->msg_class;
    msg_oss << " poll_frame.msg_id: " << "0x" << std::setfill('0') << std::setw(2) << std::right <<
      std::hex << +poll_frame->msg_id;

    throw UbxAckNackException(msg_oss.str());
  }

  return polled_frame;
}

class UBXPayloadBase
{
protected:
  msg_class_t msg_class_;
  msg_id_t msg_id_;
  std::vector<u1_t> payload_;

  UBXPayloadBase(msg_class_t msg_class, msg_id_t msg_id)
  : msg_class_(msg_class), msg_id_(msg_id)
  {
    payload_.clear();
  }
  UBXPayloadBase()
  : msg_class_(-1), msg_id_(-1)
  {
  }
};

class UBXPayload : UBXPayloadBase
{
protected:
  msg_class_t msg_class_;
  msg_id_t msg_id_;
  std::vector<u1_t> payload_;

  UBXPayload(msg_class_t msg_class, msg_id_t msg_id)
  : UBXPayloadBase(msg_class, msg_id), msg_class_(msg_class), msg_id_(msg_id)
  {
    payload_.clear();
  }
  UBXPayload()
  : msg_class_(-1), msg_id_(-1)
  {
  }

public:
  virtual std::tuple<u1_t *, size_t> make_poll_payload()
  {
    payload_.clear();
    return std::make_tuple(payload_.data(), payload_.size());
  }
};

class UBXPayloadPoll : UBXPayloadBase
{
public:
  UBXPayloadPoll()
  : UBXPayloadBase(-1, -1)
  {
  }
  UBXPayloadPoll(msg_class_t msg_class, msg_id_t msg_id)
  : UBXPayloadBase(msg_class, msg_id)
  {
  }
// zero length payload
  msg_class_t msg_class()
  {
    return msg_class_;
  }
  msg_id_t msg_id()
  {
    return msg_id_;
  }
};


class UBXPayloadOutputPrint : UBXPayloadBase
{
public:
  UBXPayloadOutputPrint()
  : UBXPayloadBase(-1, -1)
  {
  }
  UBXPayloadOutputPrint(msg_class_t msg_class, msg_id_t msg_id)
  : UBXPayloadBase(msg_class, msg_id)
  {
  }

  std::string to_hex()
  {
    u1_t * buf = payload_.data();
    size_t size = payload_.size();
    std::ostringstream os;

    os << "size: " << size << " '0x";
    for (size_t i = 0; i < size; i++) {
      os << std::setfill('0') << std::setw(2) << std::right << std::hex << +buf[i];
    }
    os << "'";
    return os.str();
  }
};

template<typename T>
class Payload : public T, public UBXPayloadOutputPrint
{
public:
  Payload(ch_t * payload_polled, u2_t size)
  : T(payload_polled, size), UBXPayloadOutputPrint(T::MSG_CLASS, T::MSG_ID)
  {
  }
  Payload()
  : T(), UBXPayloadOutputPrint(T::MSG_CLASS, T::MSG_ID)
  {
  }
};

template<typename T>
class PayloadPoll : public T, public UBXPayloadPoll
{
public:
  PayloadPoll()
  : T(), UBXPayloadPoll(T::MSG_CLASS, T::MSG_ID)
  {
  }
};


template<typename T>
class FrameContainer
{
private:
  msg_class_t msg_class_;
  msg_id_t msg_id_;
  std::shared_ptr<Frame> frame_;
  std::shared_ptr<FramePoll> frame_poll_;
  std::shared_ptr<Payload<T>> payload_;
  std::shared_ptr<PayloadPoll<T>> payload_poll_;

public:
  FrameContainer()
  {
    payload_poll_ = std::make_shared<PayloadPoll<T>>();
    payload_ = std::make_shared<Payload<T>>();
    this->msg_class_ = payload_poll_->msg_class();
    this->msg_id_ = payload_poll_->msg_id();
  }

  msg_class_t msg_class()
  {
    return msg_class_;
  }
  msg_id_t msg_id()
  {
    return msg_id_;
  }
  std::shared_ptr<Frame> frame()
  {
    return frame_;
  }
  std::shared_ptr<FramePoll> frame_poll()
  {
    if (frame_poll_.use_count() == 0) {
      make_frame_poll();
    }
    return frame_poll_;
  }
  std::shared_ptr<Payload<T>> payload()
  {
    return payload_;
  }
  std::shared_ptr<PayloadPoll<T>> payload_poll()
  {
    return payload_poll_;
  }

  std::shared_ptr<ubx::FramePoll> make_frame_poll()
  {
    u1_t * payload;
    size_t payload_size;

    if (payload_poll_.use_count() == 0) {
      throw UbxPayloadException("No poll payload set!");
    }
    std::tie(payload, payload_size) = payload_poll_->make_poll_payload();

    frame_poll_ = std::make_shared<ubx::FramePoll>();
    frame_poll_->msg_class = T::MSG_CLASS;
    frame_poll_->msg_id = T::MSG_ID;
    frame_poll_->payload = reinterpret_cast<ch_t *>(payload);
    frame_poll_->length = payload_size;
    std::tie(frame_poll_->ck_a, frame_poll_->ck_b) = frame_poll_->ubx_check_sum();
    frame_poll_->build_frame_buf();

    return frame_poll_;
  }

  void frame(std::shared_ptr<Frame> frame)
  {
    if (frame->msg_class != msg_class_ || frame->msg_id != msg_id_) {
      throw UbxValueException("msg class & id for frame dont match frame type's");
    }
    frame_ = frame;
    payload_ = std::make_shared<Payload<T>>(frame->payload, frame->length);
  }
};

template<class T, class U>
class UBXFrameComms
{
private:
  std::shared_ptr<FrameContainer<T>> container_;
  std::shared_ptr<U> comms_;

public:
  explicit UBXFrameComms(std::shared_ptr<U> comms)
  {
    comms_ = comms;
    container_ = std::make_shared<FrameContainer<T>>();
  }
  std::shared_ptr<ubx::Frame> frame()
  {
    return container_->frame();
  }
  std::shared_ptr<Payload<T>> payload()
  {
    return container_->payload();
  }
  void frame(std::shared_ptr<ubx::Frame> frame)
  {
    container_->frame(frame);
  }
  void poll_async()
  {
    auto frame_poll = container_->frame_poll();
    comms_->write_buffer_async(frame_poll->buf.data(), frame_poll->buf.size(), NULL);
  }
};
}  // namespace ubx

#endif  // UBLOX_DGNSS_NODE__UBX__UBX_HPP_