MsgEncoder.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2010, LABUST, UNIZG-FER
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the LABUST nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *
00034 *  Author: Dula Nad
00035 *  Created: 14.02.2013.
00036 *********************************************************************/
00037 #include <labust/tritech/mtMessages.hpp>
00038 #include <labust/tritech/mmcMessages.hpp>
00039 
00040 #include <boost/asio.hpp>
00041 #include <boost/archive/binary_oarchive.hpp>
00042 #include <boost/archive/binary_iarchive.hpp>
00043 
00044 #include <string>
00045 #include <vector>
00046 #include <cstdint>
00047 #include <bitset>
00048 #include <iostream>
00049 #include <boost/integer/integer_mask.hpp>
00050 #include <cassert>
00051 #include <boost/regex.hpp>
00052 
00053 #define ADD_DIVER_MESSAGE(NAME, CODE, DEPTHSIZE, \
00054         LATLONSIZE, DEFSIZE, MSGSIZE, KMLNOSIZE, KMLSIZE, IMGSIZE, VOIDSIZE, CHKSIZE) \
00055         struct NAME\
00056         {\
00057           enum{type=CODE};\
00058                 enum{depthSize=DEPTHSIZE,latlonSize=LATLONSIZE,defSize=DEFSIZE, msgSize=MSGSIZE, kmlNoSize=KMLNOSIZE, \
00059           kmlSize=KMLSIZE, imgSize=IMGSIZE, voidSize=VOIDSIZE, chkSize=CHKSIZE};\
00060         };\
00061 
00062 
00063 struct DiverMsg
00064 {
00065         //Topside messages
00066         ADD_DIVER_MESSAGE(PositionInit,         0,      0,      22,     0,      0,      0,      0,      0,      0,      0);
00067         ADD_DIVER_MESSAGE(Position_18,          1,      7,      18,     0,      0,      0,      0,      0,      1,      0);
00068         ADD_DIVER_MESSAGE(PositionMsg,          2,      7,      7,      0,      23,     0,      0,      0,      0,      0);
00069         ADD_DIVER_MESSAGE(PositionImg,          3,      7,      7,      0,      0,      0,      0,      23,     0,      0);
00070         ADD_DIVER_MESSAGE(Position_14Def,       4,      7,      14,     5,      0,      0,      0,      0,      4,      0);
00071         ADD_DIVER_MESSAGE(PositionMsgDef,       5,      7,      7,      5,      18,     0,      0,      0,      0,      0);
00072         ADD_DIVER_MESSAGE(PositionImgDef,       6,      7,      7,      5,      0,      0,      0,      18,     0,      0);
00073         ADD_DIVER_MESSAGE(PositionKml,          7,      7,      7,      0,      0,      3,      10,     0,      0,      0);
00074         ADD_DIVER_MESSAGE(PositionChk,          8,      7,      7,      0,      0,      0,      0,      0,      17,     6);
00075         ADD_DIVER_MESSAGE(PositionMsgChk,       9,      7,      7,      0,      12,     0,      0,      0,      5,      6);
00076         ADD_DIVER_MESSAGE(PositionImgChk,       10,     7,      7,      0,      0,      0,      0,      17,     0,      6);
00077         ADD_DIVER_MESSAGE(PositionDefChk,       11,     7,      7,      5,      0,      0,      0,      0,      12,     6);
00078         ADD_DIVER_MESSAGE(PositionMsgDefChk,12, 7,      7,      5,      12,     0,      0,      0,      0,      6);
00079         ADD_DIVER_MESSAGE(PositionImgDefChk,13, 7,      7,      5,      0,      0,      0,      12,     0,      6);
00080 
00081         //Diver messages
00082         ADD_DIVER_MESSAGE(PositonInitAck,       0,      0,      22,     0,      0,      0,      0,      0,      0,      0);
00083         ADD_DIVER_MESSAGE(Msg,                          1,      0,      0,      0,      0,      0,      0,      0,      44,     0);
00084 
00085         DiverMsg():
00086                 latitude(0),
00087                 longitude(0),
00088                 z(0),
00089                 depthRes(0.5){};
00090 
00091         //\todo Add automatic extraction of lat-lon data from double values
00092         template <class msg>
00093         uint64_t pack()
00094         {
00095                 fullmsg = msg::type;
00096                 fullmsg <<= msg::depthSize;
00097                 fullmsg |= int(z/depthRes) & boost::low_bits_mask_t<msg::depthSize>::sig_bits;
00098                 fullmsg <<= msg::latlonSize;
00099                 latlonToBits<msg::latlonSize>(latitude,longitude);
00100                 fullmsg |= lat & boost::low_bits_mask_t<msg::latlonSize>::sig_bits;
00101                 fullmsg <<= msg::latlonSize;
00102                 fullmsg |= lon & boost::low_bits_mask_t<msg::latlonSize>::sig_bits;
00103                 fullmsg <<= msg::msgSize;
00104                 fullmsg |= this->msg & boost::low_bits_mask_t<msg::msgSize>::sig_bits;
00105                 return fullmsg;
00106         }
00107 
00108         static inline uint8_t testType(uint64_t data, size_t msgSize = 48)
00109         {
00110                 return (data >> (msgSize - 4)) & 0xF;
00111         }
00112 
00113         template <class msg>
00114         bool unpack(uint64_t data)
00115         {
00116                 fullmsg=data;
00117                 this->msg = data & boost::low_bits_mask_t<msg::msgSize>::sig_bits;
00118                 data >>= msg::msgSize;
00119                 lon = data & boost::low_bits_mask_t<msg::latlonSize>::sig_bits;
00120                 data >>= msg::latlonSize;
00121                 lat = data & boost::low_bits_mask_t<msg::latlonSize>::sig_bits;
00122                 data >>= msg::latlonSize;
00123                 depth = data & boost::low_bits_mask_t<msg::depthSize>::sig_bits;
00124                 data >>= msg::depthSize;
00125 
00126                 assert(((data & 0x0F) == msg::type) &&
00127                                 "DiverMsg desired unpack type and received data type do not match.");
00128         };
00129 
00130         template <size_t precission>
00131         std::pair<int,int> latlonToBits(double lat, double lon){return std::pair<int,int>();};
00132 
00133         double latitude, longitude, z, depthRes;
00134         uint64_t fullmsg;
00135         uint8_t depth;
00136         int lat,lon;
00137         uint64_t msg;
00138         //uint8_t noKML, def, checksum;
00139         //int kmlX, kmlY;
00140 };
00141 
00142 template <>
00143 inline std::pair<int,int> DiverMsg::latlonToBits<22>(double lat, double lon)
00144 {
00145         double min = (lon - int(lon))*60;
00146         this->lon = int((int(lon)+180)*10000 + min*100);
00147 
00148         min = (lat - int(lat))*60;
00149         this->lat = int((int(lat)+90)*10000 + min*100);
00150 
00151         return std::pair<int,int>(this->lat,this->lon);
00152 }
00153 
00154 template <>
00155 inline std::pair<int,int> DiverMsg::latlonToBits<18>(double lat, double lon)
00156 {
00157         this->lat = int((lat - int(lat))*600000)%100000;
00158         this->lon = int((lon - int(lon))*600000)%100000;
00159         return std::pair<int,int>(this->lat,this->lon);
00160 }
00161 
00162 template <>
00163 std::pair<int,int> DiverMsg::latlonToBits<14>(double lat, double lon)
00164 {
00165         double min = (lon - int(lon))*60;
00166         this->lon = int((min - int(min))*10000);
00167 
00168         min = (lat - int(lat))*60;
00169         this->lat = int((min - int(min))*10000);
00170 
00171         return std::pair<int,int>(this->lat,this->lon);
00172 }
00173 
00174 template <>
00175 inline std::pair<int,int> DiverMsg::latlonToBits<7>(double lat, double lon)
00176 {
00177         latlonToBits<14>(lat,lon);
00178         this->lat%=100;
00179         this->lon%=100;
00180         return std::pair<int,int>(this->lat,this->lon);
00181 }
00182 
00183 void sendToModem(boost::asio::serial_port& port, DiverMsg& msg)
00184 {
00185         using namespace labust::tritech;
00186         MTMsgPtr tmsg(new MTMsg());
00187         tmsg->txNode = labust::tritech::Nodes::SlaveModem;
00188         tmsg->rxNode = labust::tritech::Nodes::Surface;
00189         tmsg->node = labust::tritech::Nodes::SlaveModem;
00190         tmsg->msgType = MTMsg::mtMiniModemData;
00191         MMCMsg mmsg;
00192         mmsg.msgType = labust::tritech::mmcGetRangeTxRxBits48;
00193         mmsg.data[0] = 48;
00194 
00195         const char* p=reinterpret_cast<const char*>(&msg.fullmsg);
00196         std::cout<<"Payload:";
00197 //      for (int i=5; i>=0; --i)
00198 //      {
00199 //              mmsg.data[i+1] = p[i];
00200 //              std::cout<<int(p[i])<<",";
00201 //      }
00202         for (int i=0; i<6; ++i)
00203         {
00204                 mmsg.data[i+1] = p[5-i];
00205                 std::cout<<int(p[5-i])<<",";
00206         }
00207         std::cout<<std::endl;
00208 
00209         std::bitset<48> pyl(msg.fullmsg);
00210         std::cout<<"Payload 2:"<<pyl<<std::endl;
00211 
00212         boost::archive::binary_oarchive ar(*tmsg->data, boost::archive::no_header);
00213         ar<<mmsg;
00214 
00215         tmsg->setup();
00216         boost::asio::streambuf output;
00217         std::ostream out(&output);
00218         //prepare header
00219   out<<'@';
00220   out.width(4);
00221   out.fill('0');
00222   out<<std::uppercase<<std::hex<<tmsg->size;
00223   boost::archive::binary_oarchive dataSer(output, boost::archive::no_header);
00224   dataSer << (*tmsg);
00225 
00226   //write header
00227   boost::asio::write(port, output.data());
00228   //write data
00229   boost::asio::write(port, tmsg->data->data());
00230 }
00231 
00232 void writeLatLon(DiverMsg& msg)
00233 {
00234         double lat = msg.latitude;
00235         double lon = msg.longitude;
00236         std::cout<<"Sending lat-lon:"<<int(lat)<<"\° "<<(lat-int(lat))*60<<"', ";
00237         std::cout<<int(lon)<<"\° "<<(lon-int(lon))*60<<std::endl;
00238         std::cout<<"\t encoded lat-lon:"<<msg.lat<<", "<<msg.lon<<std::endl;
00239 }
00240 
00241 int main(int argc, char* argv[])
00242 {
00243         boost::asio::io_service io;
00244         boost::asio::serial_port port(io);
00245 
00246         port.open("/dev/rfcomm0");
00247         port.set_option(boost::asio::serial_port::baud_rate(57600));
00248         port.set_option(boost::asio::serial_port::flow_control(boost::asio::serial_port::flow_control::none));
00249 
00250         //boost::asio::streambuf incoming;
00251         //boost::asio::read_until(port, incoming, boost::regex("\n"));
00252 
00253         if (port.is_open()) std::cout<<"Port is open."<<std::endl;
00254 
00255         uint8_t test_data[] = {0x40,0x30,0x30,0x31,0x46,0x1F,0x00,0x56,0xFF,0x1A,0x4F,0x80,0x56,0x21,0x02,0x00,
00256                         0x00,0x00,0xB8,0x0B,0x30,0x22,0xC3,0xF3,0x38,0x78,0x38,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
00257 
00258         using namespace labust::tritech;
00259 
00260         double depth = 5;
00261         double lat=45.769216667, lon=16.001851667;
00262         //init
00263         DiverMsg msg;
00264         msg.latitude = lat;
00265         msg.longitude = lon;
00266         msg.z = depth;
00267         msg.msg = 0;
00268         msg.pack<DiverMsg::PositionInit>();
00269         writeLatLon(msg);
00270         sendToModem(port,msg);
00271         usleep(1000*1000);
00272 
00273         //7bit
00274         double step = 0.003/60;
00275         for (int i=0; i<4; ++i)
00276         {
00277                 msg.latitude += step;
00278                 msg.longitude += step;
00279                 msg.pack<DiverMsg::PositionMsg>();
00280                 writeLatLon(msg);
00281                 sendToModem(port, msg);
00282                 usleep(1000*1000);
00283         }
00284         for (int i=0; i<8; ++i)
00285         {
00286                 msg.latitude -= step;
00287                 msg.longitude -= step;
00288                 msg.pack<DiverMsg::PositionMsg>();
00289                 writeLatLon(msg);
00290                 sendToModem(port, msg);
00291                 usleep(1000*1000);
00292         }
00293         //14 bit
00294         /*step = 0.3/60;
00295         for (int i=0; i<4; ++i)
00296         {
00297                 msg.latitude += step;
00298                 msg.longitude += step;
00299                 msg.pack<DiverMsg::PositionDef>();
00300                 writeLatLon(msg);
00301                 sendToModem(port, msg);
00302                 usleep(1000*1000);
00303         }
00304         step = 0.3/60;
00305         for (int i=0; i<8; ++i)
00306         {
00307                 msg.latitude -= step;
00308                 msg.longitude -= step;
00309                 msg.pack<DiverMsg::PositionDef>();
00310                 writeLatLon(msg);
00311                 sendToModem(port, msg);
00312                 usleep(1000*1000);
00313         }*/
00314         //18 bit
00315 
00316         usleep(5000*1000);
00317 
00318         step = 3./60;
00319         for (int i=0; i<4; ++i)
00320         {
00321                 msg.latitude += step;
00322                 msg.longitude += step;
00323                 msg.pack<DiverMsg::Position_18>();
00324                 writeLatLon(msg);
00325                 sendToModem(port, msg);
00326                 usleep(1000*1000);
00327         }
00328         step = 3./60;
00329         for (int i=0; i<8; ++i)
00330         {
00331                 msg.latitude -= step;
00332                 msg.longitude -= step;
00333                 msg.pack<DiverMsg::Position_18>();
00334                 writeLatLon(msg);
00335                 sendToModem(port, msg);
00336                 usleep(1000*1000);
00337         }
00338 
00339         return 0;
00340 }
00341 
00342 
00343 


usbl
Author(s): Gyula Nagy
autogenerated on Thu Jul 10 2014 10:34:21