.. _program_listing_file__tmp_ws_src_ublox_dgnss_ublox_dgnss_node_include_ublox_dgnss_node_ubx_ubx.hpp:

Program Listing for File ubx.hpp
================================

|exhale_lsh| :ref:`Return to documentation for file <file__tmp_ws_src_ublox_dgnss_ublox_dgnss_node_include_ublox_dgnss_node_ubx_ubx.hpp>` (``/tmp/ws/src/ublox_dgnss/ublox_dgnss_node/include/ublox_dgnss_node/ubx/ubx.hpp``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   // 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_