src/marnav/seatalk/message_89.cpp

   1 #include <marnav/seatalk/message_89.hpp>
   2 #include <cmath>
   3
   4 namespace marnav
   5 {
   6 namespace seatalk
   7 {
   8
   9 message_89::message_89()
  10         : message(ID)
  11         , value_(0.0)
  12 {
  13 }
  14
  15 std::unique_ptr<message> message_89::parse(const raw & data)
  16 {
  17         check_size(data, SIZE);
  18
  19         std::unique_ptr<message> result = utils::make_unique<message_89>();
  20         message_89 & msg = static_cast<message_89 &>(*result);
  21
  22         //  89 U2 VW XY 2Z
  23         // raw  1  2  3  4
  24
  25         double t = 0.0;
  26         t += 90.0 * ((data[1] & 0x30) >> 4);
  27         t += 2.0 * (data[2] & 0x3f);
  28         t += 0.5 * ((data[1] & 0xc0) >> 6);
  29
  30         msg.value_ = std::fmod(t, 360.0);
  31
  32         return result;
  33 }
  34
  35 raw message_89::get_data() const
  36 {
  37         uint8_t u = static_cast<uint8_t>(std::floor(value_ / 90.0)) & 0x03;
  38         uint8_t vw = static_cast<uint8_t>(std::floor((value_ - (90.0 * u)) / 2.0));
  39         u += static_cast<uint8_t>(std::floor(std::fmod(value_, 2.0) * 2.0)) << 2;
  40
  41         return raw{static_cast<uint8_t>(ID), static_cast<uint8_t>(0x02 | (u << 4)), vw, 0x00, 0x20};
  42 }
  43
  44 /// Sets the heading, this value will be truncated to the interval `[0.0 .. 359.5]`,
  45 /// in steps of `0.5` degrees.
  46 ///
  47 /// @param[in] t Heading in degrees, steps of `0.5`. The absolute value will be used.
  48 void message_89::set_heading(double t)
  49 {
  50         value_ = std::fmod(std::fabs(t), 360.0);
  51 }
  52 }
  53 }