diff --git a/src/algorithms/PVT/libs/rtcm.cc b/src/algorithms/PVT/libs/rtcm.cc
index bbd0abeb1..6023f19ee 100644
--- a/src/algorithms/PVT/libs/rtcm.cc
+++ b/src/algorithms/PVT/libs/rtcm.cc
@@ -32,7 +32,6 @@
 #include <cmath>      // for std::fmod, std::lround
 #include <cstdlib>    // for strtol
 #include <iostream>   // for std::cout
-#include <sstream>    // for std::stringstream
 
 
 Rtcm::Rtcm(uint16_t port) : RTCM_port(port), server_is_running(false)
diff --git a/src/algorithms/PVT/libs/rtcm.h b/src/algorithms/PVT/libs/rtcm.h
index 91b7ac275..2fe68299c 100644
--- a/src/algorithms/PVT/libs/rtcm.h
+++ b/src/algorithms/PVT/libs/rtcm.h
@@ -30,17 +30,18 @@
 #include <boost/asio.hpp>
 #include <boost/date_time/posix_time/posix_time.hpp>
 #include <glog/logging.h>
-#include <algorithm>  // for min
+#include <algorithm>  // for std::max, std::min, std::copy_n
 #include <array>
 #include <bitset>
 #include <cstddef>  // for size_t
 #include <cstdint>
-#include <cstring>  // for memcpy
 #include <deque>
+#include <iomanip>  // for std::setw
 #include <list>
 #include <map>
 #include <memory>
 #include <set>
+#include <sstream>  // for std::stringstream
 #include <string>
 #include <thread>
 #include <utility>
@@ -612,8 +613,10 @@ private:
         inline void encode_header()
         {
             char header[header_length + 1] = "";
-            std::snprintf(header, header_length + 1, "GS%4d", std::max(std::min(static_cast<int>(body_length_), static_cast<int>(max_body_length)), 0));
-            std::memcpy(data_.data(), header, header_length);
+            std::stringstream ss;
+            ss << "GS" << std::setw(4) << std::max(std::min(static_cast<int>(body_length_), static_cast<int>(max_body_length)), 0);
+            std::copy_n(ss.str().c_str(), header_length + 1, header);
+            std::copy_n(header, header_length, data_.data());
         }
 
     private:
@@ -895,7 +898,7 @@ private:
 
                     const char* char_msg = message.c_str();
                     msg.body_length(message.length());
-                    std::memcpy(msg.body(), char_msg, msg.body_length());
+                    std::copy_n(char_msg, msg.body_length(), msg.body());
                     msg.encode_header();
                     c->write(msg);
                 }