DiverMsg_adv.hpp
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: 29.04.2013.
00036  *********************************************************************/
00037 #ifndef DIVERMSG_HPP_
00038 #define DIVERMSG_HPP_
00039 #include <labust/tritech/detail-tks/message_preprocess.hpp>
00040 
00041 #include <cstdint>
00042 #include <map>
00043 
00044 namespace labust
00045 {
00046         namespace tritech
00047         {
00048                 struct DiverMsg;
00049 
00050                 struct Packer
00051                 {
00052                         virtual ~Packer(){};
00053                         virtual uint64_t pack(DiverMsg& msg) = 0;
00054                         virtual void unpack(uint64_t data, DiverMsg& msg) = 0;
00055                 };
00056 
00057                 struct DiverMsg
00058                 {
00059                         struct AutoTopside{};
00060                         struct AutoDiver{};
00061 
00062                         DEFINE_TOPSIDE_MESSAGES(
00063                                         (PositionInit,1,0,22,0)
00064                                         (Position,2,7,18,1)
00065                                         (PositionMsg,3,7,7,23)
00066                                         (PositionDef,5,7,14,9))
00067 
00068                         DEFINE_DIVER_MESSAGES(
00069                                         (PositionInitAck,1,0,22,0)
00070                                         (Msg,2,0,0,44))
00071 
00072                         DiverMsg():
00073                                 latitude(0),
00074                                 longitude(0),
00075                                 z(0),
00076                                 depthRes(0.5),
00077                                 msgType(0)
00078                         {
00079                                 DiverMsg::registerTopsideMessages();
00080                                 DiverMsg::registerDiverMessages();
00081                         };
00082 
00083                         //\todo Add automatic extraction of lat-lon data from double values
00084                         template <class msg>
00085                         uint64_t pack()
00086                         {
00087                                 msgType = msg::type;
00088                                 fullmsg = msg::type;
00089                                 fullmsg <<= msg::depthSize;
00090                                 fullmsg |= int(z/depthRes) & boost::low_bits_mask_t<msg::depthSize>::sig_bits;
00091                                 fullmsg <<= msg::latlonSize;
00092                                 latlonToBits<msg::latlonSize>(latitude,longitude);
00093                                 fullmsg |= lat & boost::low_bits_mask_t<msg::latlonSize>::sig_bits;
00094                                 fullmsg <<= msg::latlonSize;
00095                                 fullmsg |= lon & boost::low_bits_mask_t<msg::latlonSize>::sig_bits;
00096                                 fullmsg <<= msg::msgSize;
00097                                 fullmsg |= this->msg & boost::low_bits_mask_t<msg::msgSize>::sig_bits;
00098                                 return fullmsg;
00099                         }
00100 
00101                         static inline uint8_t testType(uint64_t data, size_t msgSize = 48)
00102                         {
00103                                 return (data >> (msgSize - 4)) & 0xF;
00104                         }
00105 
00106                         template <class msg>
00107                         void unpack(uint64_t data)
00108                         {
00109                                 fullmsg=data;
00110                                 this->msg = data & boost::low_bits_mask_t<msg::msgSize>::sig_bits;
00111                                 data >>= msg::msgSize;
00112                                 lon = data & boost::low_bits_mask_t<msg::latlonSize>::sig_bits;
00113                                 data >>= msg::latlonSize;
00114                                 lat = data & boost::low_bits_mask_t<msg::latlonSize>::sig_bits;
00115                                 data >>= msg::latlonSize;
00116                                 depth = data & boost::low_bits_mask_t<msg::depthSize>::sig_bits;
00117                                 data >>= msg::depthSize;
00118                                 msgType = data & 0x0F;
00119                                 assert((msgType == msg::type) &&
00120                                                 "DiverMsg desired unpack type and received data type do not match.");
00121                         };
00122 
00123                         template <size_t precission>
00124                         std::pair<int,int> latlonToBits(double lat, double lon){return std::pair<int,int>(lat,lon);};
00125 
00126                         double latitude, longitude, z, depthRes;
00127                         uint64_t fullmsg;
00128                         uint8_t depth, msgType;
00129                         int lat,lon;
00130                         uint64_t msg;
00131                         //uint8_t noKML, def, checksum;
00132                         //int kmlX, kmlY;
00133 
00134                 private:
00135                         std::map<int, boost::shared_ptr<Packer> > topsideMap, diverMap;
00136                 };
00137 
00138                 template <>
00139                 inline uint64_t DiverMsg::pack<DiverMsg::AutoTopside>()
00140                 {
00141                         assert(topsideMap.find(msgType) != topsideMap.end());
00142                         return topsideMap[msgType]->pack(*this);
00143                 }
00144 
00145                 template <>
00146                 inline void DiverMsg::unpack<DiverMsg::AutoTopside>(uint64_t data)
00147                 {
00148                         testType(data);
00149                         assert(topsideMap.find(msgType) != topsideMap.end());
00150                         topsideMap[msgType]->unpack(data, *this);
00151                 }
00152 
00153                 template <>
00154                 inline uint64_t DiverMsg::pack<DiverMsg::AutoDiver>()
00155                 {
00156                         assert(diverMap.find(msgType) != diverMap.end());
00157                         return diverMap[msgType]->pack(*this);
00158                 }
00159 
00160                 template <>
00161                 inline void DiverMsg::unpack<DiverMsg::AutoDiver>(uint64_t data)
00162                 {
00163                         testType(data);
00164                         assert(diverMap.find(msgType) != diverMap.end());
00165                         diverMap[msgType]->unpack(data,*this);
00166                 }
00167 
00168                 template <>
00169                 inline std::pair<int,int> DiverMsg::latlonToBits<22>(double lat, double lon)
00170                 {
00171                         double min = (lon - int(lon))*60;
00172                         this->lon = int((int(lon)+180)*10000 + min*100);
00173 
00174                         min = (lat - int(lat))*60;
00175                         this->lat = int((int(lat)+90)*10000 + min*100);
00176 
00177                         return std::pair<int,int>(this->lat,this->lon);
00178                 }
00179 
00180                 template <>
00181                 inline std::pair<int,int> DiverMsg::latlonToBits<18>(double lat, double lon)
00182                 {
00183                         this->lat = int((lat - int(lat))*600000)%100000;
00184                         this->lon = int((lon - int(lon))*600000)%100000;
00185                         return std::pair<int,int>(this->lat,this->lon);
00186                 }
00187 
00188                 template <>
00189                 std::pair<int,int> DiverMsg::latlonToBits<14>(double lat, double lon)
00190                 {
00191                         double min = (lon - int(lon))*60;
00192                         this->lon = int((min - int(min))*10000);
00193 
00194                         min = (lat - int(lat))*60;
00195                         this->lat = int((min - int(min))*10000);
00196 
00197                         return std::pair<int,int>(this->lat,this->lon);
00198                 }
00199 
00200                 template <>
00201                 inline std::pair<int,int> DiverMsg::latlonToBits<7>(double lat, double lon)
00202                 {
00203                         latlonToBits<14>(lat,lon);
00204                         this->lat%=100;
00205                         this->lon%=100;
00206                         return std::pair<int,int>(this->lat,this->lon);
00207                 }
00208         }
00209 }
00210 
00211 #include <labust/tritech/detail-tks/message_preprocess_undef.hpp>
00212 
00213 /* DIVERMSG_HPP_ */
00214 #endif
00215 
00216 
00217 


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