Note: Goby version 1 (shown here) is now considered obsolete. Please use version 2 for new projects, and consider upgrading old projects.

Goby Underwater Autonomy Project  Series: 1.1, revision: 163, released on 2013-02-06 14:23:27 -0500
moos/pREMUSCodec/pREMUSCodec.cpp
00001 // t. schneider tes@mit.edu 07.25.08
00002 // ocean engineering graduate student - mit / whoi joint program
00003 // massachusetts institute of technology (mit)
00004 // laboratory for autonomous marine sensing systems (lamss)
00005 // 
00006 // this is pREMUSCodec.cpp 
00007 //
00008 // see the readme file within this directory for information
00009 // pertaining to usage and purpose of this script.
00010 //
00011 // This program is free software: you can redistribute it and/or modify
00012 // it under the terms of the GNU General Public License as published by
00013 // the Free Software Foundation, either version 3 of the License, or
00014 // (at your option) any later version.
00015 //
00016 // This software is distributed in the hope that it will be useful,
00017 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00018 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00019 // GNU General Public License for more details.
00020 //
00021 // You should have received a copy of the GNU General Public License
00022 // along with this software.  If not, see <http://www.gnu.org/licenses/>.
00023 
00024 #include "pREMUSCodec.h"
00025 
00026 #include "goby/util/string.h"
00027 #include "boost/algorithm/string.hpp"
00028 #include "goby/protobuf/modem_message.pb.h"
00029 #include "goby/moos/libmoos_util/moos_protobuf_helpers.h"
00030 
00031 using namespace std;
00032 using goby::util::as;
00033 using goby::util::glogger;
00034 
00035 pREMUSCodecConfig CpREMUSCodec::cfg_;
00036 CpREMUSCodec* CpREMUSCodec::inst_ = 0;
00037 
00038 CpREMUSCodec* CpREMUSCodec::get_instance()
00039 {
00040     if(!inst_)
00041         inst_ = new CpREMUSCodec();
00042     return inst_;
00043 }
00044 
00045 // Construction / Destruction
00046 CpREMUSCodec::CpREMUSCodec()
00047     : TesMoosApp(&cfg_),
00048       my_id(0),
00049       got_x(false),
00050       got_y(false),
00051       got_depth(false),
00052       got_speed(false),
00053       got_heading(false),
00054       status_interval(10),
00055       status_time(10) 
00056 {
00057     if(cfg_.has_modem_id_lookup_path())
00058         glogger() << modem_lookup_.read_lookup_file(cfg_.modem_id_lookup_path()) << std::endl;
00059     else
00060         glogger() << warn << "no modem_id_lookup_path in moos file. this is required for conversions between modem_id and vehicle name / type." << std::endl;
00061 
00062 
00063     south = (cfg_.common().lat_origin() < 0.0);
00064     west = (cfg_.common().lon_origin() < 0.0);
00065   
00066     // initialize m_geodesy
00067     if (!m_geodesy.Initialise(cfg_.common().lat_origin(), cfg_.common().lon_origin()))
00068     {
00069         glogger() << die << "Geodesy init failed." << std::endl;
00070     }
00071 
00072     subscribe(cfg_.mdat_state_var());
00073     subscribe(cfg_.mdat_state_out());
00074     subscribe(cfg_.mdat_ranger_var());
00075     subscribe(cfg_.mdat_ranger_out());
00076     subscribe(cfg_.mdat_redirect_var());
00077     subscribe(cfg_.mdat_redirect_out());
00078     subscribe(cfg_.mdat_alert_var());
00079     subscribe(cfg_.mdat_alert_out());
00080     subscribe(cfg_.mdat_alert2_var());
00081     subscribe(cfg_.mdat_alert2_out());
00082     subscribe("MODEM_ID");
00083     subscribe("NAV_X");
00084     subscribe("NAV_Y");
00085     subscribe("NAV_SPEED");
00086     subscribe("NAV_DEPTH");
00087     subscribe("NAV_HEADING");
00088     subscribe("OUTGOING_COMMAND");
00089 }
00090 CpREMUSCodec::~CpREMUSCodec()
00091 {
00092 }
00093 
00094 void CpREMUSCodec::inbox(const CMOOSMsg& msg)
00095 {
00096     string key   = msg.GetKey();  
00097     //bool is_dbl  = msg.IsDouble();
00098     //bool is_str  = msg.IsString();
00099     //double dval  = msg.GetDouble();
00100     string sval  = msg.GetString();
00101     
00102     // uncomment as needed
00103     // double msg_time = msg.GetTime();
00104     // string msg_src  = msg.GetSource();
00105     string msg_community = msg.GetCommunity();
00106     
00107     
00108     if(MOOSStrCmp(key, cfg_.mdat_state_var()) || MOOSStrCmp(key, cfg_.mdat_ranger_var())
00109        || MOOSStrCmp(key, cfg_.mdat_redirect_var()) || MOOSStrCmp(key, cfg_.mdat_alert_var())
00110        || MOOSStrCmp(key, cfg_.mdat_alert2_var())) // case insensitive
00111     {
00112         goby::acomms::protobuf::ModemDataTransmission msg_in;
00113         parse_for_moos(sval, &msg_in);
00114         
00115         vector<unsigned int> bytes;
00116         for (unsigned i=0; i<32; i++)
00117         {
00118             if(i < msg_in.data().size())
00119                 bytes.push_back(msg_in.data().at(i));
00120             else
00121                 bytes.push_back(0);
00122         }
00123         
00124         int node = msg_in.base().src();
00125         string name = modem_lookup_.get_name_from_id(msg_in.base().src());
00126         string type = modem_lookup_.get_type_from_id(msg_in.base().src());
00127 
00128         string human;
00129             
00130         if(MOOSStrCmp(key, cfg_.mdat_state_var()))
00131         {
00132             human = decode_mdat_state(bytes, node, name, type);
00133             m_Comms.Notify("NODE_REPORT", human);
00134         }
00135         else if(MOOSStrCmp(key, cfg_.mdat_ranger_var()))
00136         {
00137             human = decode_mdat_ranger(bytes, node, name, type);
00138             m_Comms.Notify("NODE_REPORT", human);
00139         }
00140         else if(MOOSStrCmp(key, cfg_.mdat_redirect_var()))
00141             human = decode_mdat_redirect(bytes, node, name, type);
00142         else if(MOOSStrCmp(key, cfg_.mdat_alert_var()))
00143             human = decode_mdat_alert(bytes, node, name, type);
00144         else if(MOOSStrCmp(key, cfg_.mdat_alert2_var()))
00145             human = decode_mdat_alert2(bytes, node, name, type);
00146     }
00147     else if(MOOSStrCmp(key,"MODEM_ID"))
00148     {  
00149         my_id = atoi(sval.c_str());
00150     } 
00151     else if(MOOSStrCmp(key,"NAV_X"))
00152     {  
00153         nav_x = msg.m_dfVal;
00154         got_x =true; 
00155     } 
00156     else if(MOOSStrCmp(key,"NAV_Y"))
00157     {  
00158         nav_y = msg.m_dfVal; 
00159         got_y =true; 
00160     } 
00161     else if(MOOSStrCmp(key,"NAV_DEPTH"))
00162     {  
00163         nav_depth = msg.m_dfVal; 
00164         got_depth =true; 
00165     } 
00166     else if(MOOSStrCmp(key,"NAV_SPEED"))
00167     {  
00168         nav_speed = msg.m_dfVal; 
00169         got_speed =true; 
00170     } 
00171     else if(MOOSStrCmp(key,"NAV_HEADING"))
00172     {  
00173         nav_heading = msg.m_dfVal; 
00174         got_heading =true; 
00175     } 
00176     else if(MOOSStrCmp(key,"OUTGOING_COMMAND"))
00177     {  
00178         int dest_id;
00179         // check if it is a CCL_REDIRECT and then encode
00180         if (parseRedirect(msg.m_sVal, dest_id))
00181         {
00182             MOOSTrace("parseRedirect: dest_id= %d\n", dest_id);
00183 
00184             vector<unsigned int> bytes;
00185             for (int i=0; i<32; i++)
00186                 bytes.push_back(0);
00187             if (encode_mdat_redirect(bytes))
00188             {
00189                 goby::acomms::protobuf::ModemDataTransmission msg_out;
00190                     
00191                 for (int i=0, n=bytes.size(); i<n; i++)
00192                     msg_out.mutable_data()->append(1, char(bytes[i]));
00193 
00194                 msg_out.mutable_base()->set_src(my_id);
00195                 msg_out.mutable_base()->set_dest(dest_id);
00196 
00197                 std::string out;
00198                 serialize_for_moos(&out, msg_out);
00199 
00200                 std::cerr << "publishing to " << cfg_.mdat_redirect_out() << " " << out << std::endl;
00201                 m_Comms.Notify(cfg_.mdat_redirect_out(), out);
00202             }
00203         }
00204     } 
00205 } 
00206 
00207 void CpREMUSCodec::loop()
00208 {
00209     double curr_time = MOOSTime();
00210     bool got_nav = got_x && got_y && got_heading && got_speed && got_depth;
00211 
00212     if (cfg_.create_status() && got_nav && curr_time - status_time  >= status_interval)
00213     {
00214         m_geodesy.UTM2LatLong(nav_x, nav_y, nav_lat, nav_lon);
00215       
00216         vector<unsigned int> bytes;
00217         for (int i=0; i<32; i++)
00218             bytes.push_back(0);
00219         if (encode_mdat_state(bytes))
00220  {
00221             goby::acomms::protobuf::ModemDataTransmission msg_out;
00222             
00223             for (int i=0, n=bytes.size(); i<n; i++)
00224                 msg_out.mutable_data()->append(1, char(bytes[i]));
00225 
00226             msg_out.mutable_base()->set_src(my_id);
00227             msg_out.mutable_base()->set_dest(0);
00228             
00229             std::string out;
00230             serialize_for_moos(&out, msg_out);
00231 
00232             std::cerr << "publishing to " << cfg_.mdat_state_out() << " " << out << std::endl;
00233 
00234             m_Comms.Notify(cfg_.mdat_state_out(), out);
00235             status_time = curr_time;
00236  }
00237     }  
00238 }
00239 
00240 bool CpREMUSCodec::hex_to_int_array(string h, vector<unsigned int>& c )
00241 {
00242     boost::to_upper(h);
00243     
00244     for(unsigned int i = 0; i < h.size()/2; ++i)
00245     {
00246         char buff[5];
00247         buff[0] = '0';
00248         buff[1] = 'x';
00249         buff[2] = h[2*i];
00250         buff[3] = h[2*i+1];
00251         buff[4] = '\0';
00252         
00253         sscanf(buff,"%X",&c[i]);
00254         MOOSTrace("hex_to_int_array: c[%d] = %X; buff = %s\n", i, c[i], buff);
00255     }
00256     
00257     return true;
00258 }
00259 
00260     
00261 string CpREMUSCodec::int_array_to_hex(vector<unsigned int> c )
00262 {
00263     char buff[5];
00264     string hex = "";
00265   
00266     for(unsigned int i = 0; i < c.size(); ++i)
00267     {
00268         c[i] = c[i] & 0x00ff;
00269         sprintf(buff,"%02X",c[i] & 0xff);
00270         MOOSTrace("int_array_to_hex: c[%d] = %X; buff = %s\n", i, c[i], buff);
00271         hex += MOOSFormat("%s",buff);    
00272     }
00273 
00274     
00275     return(hex);
00276 }
00277 
00278 string CpREMUSCodec::decode_mdat_ranger(vector<unsigned int> c, int node, string sender, string type)
00279 {
00280 
00281     vector<unsigned char> cc;
00282 
00283     for (unsigned int i=0; i< c.size(); i++)
00284         cc.push_back( (unsigned char) c[i]);
00285 
00286     double dlat = DecodeRangerLL(c[1],c[2],c[3],c[4],c[5]);
00287     double dlon = DecodeRangerLL(c[6],c[7],c[8],c[9],c[10]);
00288     double dhdg = DecodeRangerBCD2(c[19],c[20]) * 0.1 ;
00289     double dspd = DecodeRangerBCD(c[18])*0.1;
00290     double ddepth = DecodeRangerBCD2(c[28],c[23]);
00291   
00292     double x,y;
00293     // convert lat, long into x, y. 60 nautical miles per minute
00294     m_geodesy.LatLong2LocalUTM(dlat, dlon, y, x);
00295 
00296     // Create human-readable version
00297   
00298     string ccl_status = "MessageType=CCL_STATUS" ;
00299     ccl_status += ",Node=" + as<string>(node);
00300     ccl_status += ",Timestamp=" + as<string>(MOOSTime());
00301     ccl_status += ",Latitude=" + as<string>(dlat);
00302     ccl_status += ",Longitude=" + as<string>(dlon);
00303     ccl_status += ",Speed=" + as<string>(dspd);
00304     ccl_status += ",Heading=" + as<string>(dhdg);
00305     ccl_status += ",Depth=" + as<string>(ddepth);
00306   
00307     m_Comms.Notify("STATUS_REPORT_IN",ccl_status);
00308   
00309   
00310     // Create contact variables for collision avoidance
00311   
00312     MOOSTrace("Creating Contact fields\n");
00313   
00314     string contact_name = boost::to_upper_copy(sender);
00315     string moosvar = contact_name + "_NAV_UTC";
00316     m_Comms.Notify(moosvar,MOOSTime());
00317     moosvar = contact_name+"_NAV_X";
00318     m_Comms.Notify(moosvar,x);
00319     moosvar = contact_name+"_NAV_Y";
00320     m_Comms.Notify(moosvar,y);
00321     moosvar = contact_name+"_NAV_LAT";
00322     m_Comms.Notify(moosvar,dlat);
00323     moosvar = contact_name+"_NAV_LONG";
00324     m_Comms.Notify(moosvar,dlon);
00325     moosvar = contact_name+"_NAV_SPEED";
00326     m_Comms.Notify(moosvar,dspd);
00327     moosvar = contact_name+"_NAV_DEPTH";
00328     m_Comms.Notify(moosvar,ddepth);
00329     moosvar = contact_name+"_NAV_HEADING";
00330     m_Comms.Notify(moosvar,dhdg);
00331   
00332     return assemble_NODE_REPORT(sender,
00333                                 type,
00334                                 as<string>(-1),
00335                                 as<string>(MOOSTime()),
00336                                 as<string>(x),
00337                                 as<string>(y),
00338                                 as<string>(dlat),
00339                                 as<string>(dlon),
00340                                 as<string>(dspd),
00341                                 as<string>(dhdg),
00342                                 as<string>(ddepth),as<string>(dhdg-360));
00343   
00344 }
00345 
00346 string CpREMUSCodec::decode_mdat_state(vector<unsigned int> c, int node, string sender, string type)
00347 {
00348 // typedef struct
00349 // {
00350 //     unsigned char     mode;
00351 //     // MDAT_STATE
00352 //     LATLON_COMPRESSED latitude;
00353 //     // 3 bytes
00354 //     LATLON_COMPRESSED longitude;
00355 //       unsigned char     fix_age
00356 //       TIME_DATE         time_date;
00357 //     // 3 bytes;
00358 //     unsigned char     heading;
00359 //     // 1.5 degree resolution
00360 //     unsigned short    mission_mode_depth;
00361 //     //
00362 //     unsigned long     faults;
00363 //     unsigned char     faults_2;
00364 //     unsigned char     mission_leg;
00365 //     char              est_velocity;
00366 //     char              objective_index;
00367 //     unsigned char     watts_encoded;
00368 //     LATLON_COMPRESSED lat_goal;
00369 //     // 3 bytes
00370 //     LATLON_COMPRESSED lon_goal;
00371 //     // 3 bytes
00372 //     unsigned char     battery_percent;
00373 //     unsigned short    gfi_pitch_oil_encoded;
00374 //     // 5 bits gfi,6 bits pitch,
00375 //     // 5 bits oil
00376 // }
00377 // MODEM_MSG_DATA_STATE;
00378 //    unsigned char mode = c[0];
00379 
00380     LATLON_COMPRESSED lat, lon;
00381     lat.compressed[0] = c[1];
00382     lat.compressed[1] = c[2];
00383     lat.compressed[2] = c[3];
00384 
00385     lon.compressed[0] = c[4];
00386     lon.compressed[1] = c[5];
00387     lon.compressed[2] = c[6];
00388 
00389     double dlat = Decode_latlon(lat);
00390 
00391     double dlon = Decode_latlon(lon);
00392     if (dlon > 180.0)
00393         dlon -= 360.0;
00394 
00395     //unsigned char fix_age = c[7];
00396 
00397     TIME_DATE td;
00398     td.compressed[0] = c[8];
00399     td.compressed[1] = c[9];
00400     td.compressed[2] = c[10];
00401 
00402     unsigned char hdg = c[11];
00403 
00404     unsigned short depth = c[13];
00405     depth = (((depth << 8) | c[12]) & 0x1fff);
00406 
00407     char spd = c[20];
00408     
00409     LATLON_COMPRESSED lat_goal, lon_goal;
00410     lat_goal.compressed[0] = c[23];
00411     lat_goal.compressed[1] = c[24];
00412     lat_goal.compressed[2] = c[25];
00413 
00414     lon_goal.compressed[0] = c[26];
00415     lon_goal.compressed[1] = c[27];
00416     lon_goal.compressed[2] = c[28];
00417 
00418 
00419 
00420     unsigned long ulArg = c[30] + 256*c[31];
00421     float fGFI,fPitch, fOil;
00422     Decode_gfi_pitch_oil(ulArg, &fGFI, &fPitch, &fOil);
00423 
00424     // rest of message is not really useful for us
00425     
00426     double glat = Decode_latlon(lat_goal);
00427 
00428     double glon = Decode_latlon(lon_goal);
00429 
00430     double dhdg = Decode_heading(hdg);
00431     double ddepth = Decode_depth(depth);
00432     double dspd = Decode_est_velocity(spd);
00433     
00434     short mon, day, hour, min, sec;
00435     
00436     Decode_time_date(td, &mon, &day, &hour, &min, &sec);
00437 
00438     cout << "mon: " << mon << endl;
00439     cout << "day: " << day << endl;
00440     cout << "hour: " << hour << endl;
00441     cout << "min: " << min << endl;
00442     cout << "sec: " << sec << endl;
00443 
00444     double x,y;
00445     // convert lat, long into x, y. 60 nautical miles per minute
00446     m_geodesy.LatLong2LocalUTM(dlat, dlon, y, x);
00447 
00448 
00449     // Create human-readable version
00450 
00451     string ccl_status = "MessageType=CCL_STATUS" ;
00452     ccl_status += ",Node=" + as<string>(node);
00453     ccl_status += ",Timestamp=" + as<string>(MOOSTime());
00454     ccl_status += ",Latitude=" + as<string>(dlat);
00455     ccl_status += ",Longitude=" + as<string>(dlon);
00456     ccl_status += ",Speed=" + as<string>(dspd);
00457     ccl_status += ",Heading=" + as<string>(dhdg);
00458     ccl_status += ",Depth=" + as<string>(ddepth);
00459     ccl_status += ",GoalLatitude=" + as<string>(glat);
00460     ccl_status += ",GoalLongitude=" + as<string>(glon);
00461     ccl_status += ",Batterypercent=" + as<string>((int) c[29]);
00462     ccl_status += ",GFI=" + as<string>((double) fGFI);
00463     ccl_status += ",Pitch=" + as<string>((double) fPitch);
00464     ccl_status += ",Oil=" + as<string>((double) fOil);
00465 
00466     m_Comms.Notify("STATUS_REPORT_IN",ccl_status);
00467 
00468    
00469     // Create contact variables for collision avoidance
00470 
00471     MOOSTrace("Creating Contact fields\n");
00472     
00473     string contact_name = boost::to_upper_copy(sender);
00474     string moosvar = contact_name + "_NAV_UTC";
00475     m_Comms.Notify(moosvar,MOOSTime());
00476     moosvar = contact_name+"_NAV_X";
00477     m_Comms.Notify(moosvar,x);
00478     moosvar = contact_name+"_NAV_Y";
00479     m_Comms.Notify(moosvar,y);
00480     moosvar = contact_name+"_NAV_LAT";
00481     m_Comms.Notify(moosvar,dlat);
00482     moosvar = contact_name+"_NAV_LONG";
00483     m_Comms.Notify(moosvar,dlon);
00484     moosvar = contact_name+"_NAV_SPEED";
00485     m_Comms.Notify(moosvar,dspd);
00486     moosvar = contact_name+"_NAV_DEPTH";
00487     m_Comms.Notify(moosvar,ddepth);
00488     moosvar = contact_name+"_NAV_HEADING";
00489     m_Comms.Notify(moosvar,dhdg);
00490   
00491     return assemble_NODE_REPORT(sender,
00492                                 type,
00493                                 as<string>(-1),
00494                                 as<string>(MOOSTime()),
00495                                 as<string>(x),
00496                                 as<string>(y),
00497                                 as<string>(dlat),
00498                                 as<string>(dlon),
00499                                 as<string>(dspd),
00500                                 as<string>(dhdg),
00501                                 as<string>(ddepth),as<string>(dhdg-360));
00502 }
00503 
00504 // replaces assembleAIS. mfallon dec2010
00505 string CpREMUSCodec::assemble_NODE_REPORT(string name, string type, string db_time, string utc_time,
00506                                           string x, string y, string lat, string lon, string spd,
00507                                           string hdg, string depth, string yaw)
00508 {
00509     // Arbitary string ending: since this info is not in these REMUS messages
00510     string rest_of_string = ",LENGTH=4.0,MODE=NOMOOS,ALLSTOP=NotUsingIvP";
00511 
00512     string summary = "NAME=" + name;
00513     summary += ",TYPE=" + type;
00514     summary += ",MOOSDB_TIME=" + db_time;
00515     summary += ",UTC_TIME=" + utc_time;
00516     summary += ",X="   + x;
00517     summary += ",Y="   + y;
00518     summary += ",LAT=" + lat;
00519     summary += ",LON=" + lon;
00520     summary += ",SPD=" + spd;
00521     summary += ",HDG=" + hdg;
00522     summary += ",YAW=" + yaw;
00523     summary += ",DEPTH=" + depth;
00524     summary += rest_of_string;
00525 
00526     return summary;
00527 }
00528 
00529 bool CpREMUSCodec::encode_mdat_state(vector<unsigned int>& c)
00530 {
00531 // typedef struct
00532 // {
00533 //     unsigned char     mode;
00534 //     // MDAT_STATE
00535 //     LATLON_COMPRESSED latitude;
00536 //     // 3 bytes
00537 //     LATLON_COMPRESSED longitude;
00538 //       unsigned char     fix_age
00539 //       TIME_DATE         time_date;
00540 //     // 3 bytes;
00541 //     unsigned char     heading;
00542 //     // 1.5 degree resolution
00543 //     unsigned short    mission_mode_depth;
00544 //     //
00545 //     unsigned long     faults;
00546 //     unsigned char     faults_2;
00547 //     unsigned char     mission_leg;
00548 //     char              est_velocity;
00549 //     char              objective_index;
00550 //     unsigned char     watts_encoded;
00551 //     LATLON_COMPRESSED lat_goal;
00552 //     // 3 bytes
00553 //     LATLON_COMPRESSED lon_goal;
00554 //     // 3 bytes
00555 //     unsigned char     battery_percent;
00556 //     unsigned short    gfi_pitch_oil_encoded;
00557 //     // 5 bits gfi,6 bits pitch,
00558 //     // 5 bits oil
00559 // }
00560 // MODEM_MSG_DATA_STATE;
00561 
00562     for (unsigned int i=0; i<c.size(); i++)
00563         c[i] = 0x0000;
00564  
00565     c[0] = 0x000E;
00566 
00567     LATLON_COMPRESSED lat, lon;
00568 
00569     lat = Encode_latlon( nav_lat );
00570 
00571     lon = Encode_latlon( nav_lon );
00572 
00573     c[1] = lat.compressed[0];
00574     c[2] = lat.compressed[1];
00575     c[3] = lat.compressed[2];
00576   
00577     c[4] = lon.compressed[0];
00578     c[5] = lon.compressed[1];
00579     c[6] = lon.compressed[2];
00580   
00581     //unsigned char fix_age = c[7];
00582   
00583     TIME_DATE td;
00584 
00585     td = Encode_time_date((long) MOOSTime());
00586   
00587     c[8] = td.compressed[0];
00588     c[9] = td.compressed[1];
00589     c[10] = td.compressed[2];
00590   
00591     c[11] = Encode_heading((float) nav_heading);
00592 
00593   
00594     unsigned short depth = Encode_depth((float) nav_depth);
00595     c[12] = depth & 0x00ff;
00596     c[13] = (depth & 0xff00) >> 8;
00597  
00598     c[20] = Encode_est_velocity((float) nav_speed);
00599   
00600     return true;
00601 }
00602 
00603 string CpREMUSCodec::decode_mdat_redirect(vector<unsigned int> c, int node, string sender, string type)
00604 {
00605     //    unsigned char mode = c[0];
00606 
00607     // redirect latitude
00608 
00609     LATLON_COMPRESSED lat, lon;
00610     lat.compressed[0] = c[2];
00611     lat.compressed[1] = c[3];
00612     lat.compressed[2] = c[4];
00613 
00614     lon.compressed[0] = c[5];
00615     lon.compressed[1] = c[6];
00616     lon.compressed[2] = c[7];
00617 
00618     double dlat = Decode_latlon(lat);
00619     double dlon = Decode_latlon(lon);
00620     // convert lat, long into x, y. 60 nautical miles per minute
00621     double x,y;
00622     m_geodesy.LatLong2LocalUTM(dlat, dlon, y, x);
00623 
00624     char buff[5];
00625     sprintf(buff,"%02X",c[8] & 0xff);
00626     spd_dep_flags += MOOSFormat("%s",buff);    
00627 
00628     // Transit depth
00629     unsigned short depth = c[10];
00630     depth = ((depth << 8) | c[9]) & 0x1fff;
00631     double dtdepth = Decode_depth(depth);
00632 
00633     // Transit Speed
00634     char spd = c[11];
00635     double dtspd = Decode_speed(SPEED_MODE_MSEC,spd);
00636     
00637     // Transit Command
00638     unsigned short tcomm = c[12];
00639 
00640     // Survey depth
00641     unsigned short sdepth = c[14];
00642     double dsdepth = Decode_depth( (((sdepth << 8) | c[13]) & 0x1fff)) ;
00643     
00644     // Survey Speed
00645     spd = c[15];
00646     double dsspd = Decode_speed(SPEED_MODE_MSEC,spd);
00647     
00648     // Survey Command
00649     unsigned short scomm = c[16];
00650 
00651     // Number of Rows
00652     unsigned short nrows = c[17];
00653     // Survey row length
00654     unsigned short rowlen =  ( (c[19] << 8) | c[18] ) & 0x1fff ;
00655     // Row spacing 0
00656     unsigned short rowspace0 = c[20];
00657     // Row spacing 1
00658     unsigned short rowspace1 = c[21];
00659     // Row heading
00660     unsigned short hdg = c[22];
00661     double dshdg = Decode_heading(hdg);
00662 
00663     // Start Lat Long
00664     LATLON_COMPRESSED slat, slon;
00665     slat.compressed[0] = c[23];
00666     slat.compressed[1] = c[24];
00667     slat.compressed[2] = c[25];
00668 
00669     slon.compressed[0] = c[26];
00670     slon.compressed[1] = c[27];
00671     slon.compressed[2] = c[28];
00672 
00673     double dslat = Decode_latlon(slat);
00674     double dslon = Decode_latlon(slon);
00675     // convert lat, long into x, y. 60 nautical miles per minute
00676     double dsx,dsy;
00677     m_geodesy.LatLong2LocalUTM(dlat, dlon, dsy, dsx);
00678 
00679     // Create human-readable version
00680 
00681     string ccl_redirect = "MessageType=CCL_REDIRECT" ;
00682     ccl_redirect += ",DestinationPlatformId=" + as<string>(node);
00683     ccl_redirect += ",Timestamp=" + as<string>(MOOSTime());
00684     ccl_redirect += ",Latitude=" + as<string>(dlat);
00685     ccl_redirect += ",Longitude=" + as<string>(dlon);
00686     ccl_redirect += ",SpeedDepthFlags=" + spd_dep_flags;
00687     ccl_redirect += ",TransitSpeed=" + as<string>(dtspd);
00688     ccl_redirect += ",TransitDepth=" + as<string>(dtdepth);
00689     ccl_redirect += ",TransitCommand=" + as<string>((int) tcomm);
00690     ccl_redirect += ",SurveySpeed=" + as<string>(dsspd);
00691     ccl_redirect += ",SurveyDepth=" + as<string>(dsdepth);
00692     ccl_redirect += ",SurveyCommand=" + as<string>((int) scomm);
00693     ccl_redirect += ",NumberOfRows=" + as<string>((int) nrows);
00694     ccl_redirect += ",RowLength=" + as<string>((int) rowlen);
00695     ccl_redirect += ",RowSpacing0=" + as<string>((int) rowspace0);
00696     ccl_redirect += ",RowSpacing1=" + as<string>((int) rowspace1);
00697     ccl_redirect += ",RowHeading=" + as<string>(dshdg);
00698     ccl_redirect += ",StartLatitude=" + as<string>(dslat);
00699     ccl_redirect += ",StartLongitude=" + as<string>(dslon);
00700 
00701 
00702     m_Comms.Notify("CCL_REDIRECT_IN",ccl_redirect);
00703 
00704     return(ccl_redirect);
00705 }
00706 
00707 bool CpREMUSCodec::encode_mdat_redirect(vector<unsigned int>& c)
00708 {
00709     for (unsigned int i=0; i<c.size(); i++)
00710         c[i] = 0x0000;
00711  
00712     c[0] = MDAT_REDIRECT;
00713 
00714     LATLON_COMPRESSED lat, lon;
00715 
00716     lat = Encode_latlon( transit_lat );
00717   
00718     lon = Encode_latlon( transit_lon );
00719 
00720     c[2] = lat.compressed[0];
00721     c[3] = lat.compressed[1];
00722     c[4] = lat.compressed[2];
00723   
00724     c[5] = lon.compressed[0];
00725     c[6] = lon.compressed[1];
00726     c[7] = lon.compressed[2];
00727   
00728     // SpeedDepthFlags
00729     c[8] = strtol(spd_dep_flags.c_str(),NULL,16);
00730 
00731 
00732     // Transit depth
00733     unsigned short depth = Encode_depth((float) transit_depth);
00734     c[9] = depth & 0x00ff;
00735     c[10] = (depth & 0xff00) >> 8;
00736  
00737     // Transit Speed
00738     c[11] = Encode_speed(SPEED_MODE_MSEC,(float) transit_speed);
00739   
00740     // Transit Command
00741     c[12] = transit_command;
00742   
00743 
00744     // Survey depth
00745     depth = Encode_depth((float) survey_depth);
00746     c[13] = depth & 0x00ff;
00747     c[14] = (depth & 0xff00) >> 8;
00748  
00749     // Survey Speed
00750     c[15] = Encode_speed(SPEED_MODE_MSEC,(float) survey_speed);
00751   
00752     // Survey Command
00753     c[16] = survey_command;
00754   
00755     // Number of Survey Rows
00756     c[17] = survey_rows;
00757 
00758     // Survey row length
00759     c[18] = int(row_length) & 0x00ff;
00760     c[19] = (int(row_length) & 0xff00) >> 8;
00761   
00762     // Row spacing 0
00763     c[20] = int(row_spacing_0);
00764     // Row spacing 1
00765     c[21] = int(row_spacing_1);
00766 
00767     // Survey Row Headings  
00768     c[22] = Encode_heading((float) row_heading);
00769 
00770     lat = Encode_latlon( start_lat );
00771   
00772     lon = Encode_latlon( start_lon );
00773 
00774     c[23] = lat.compressed[0];
00775     c[24] = lat.compressed[1];
00776     c[25] = lat.compressed[2];
00777   
00778     c[26] = lon.compressed[0];
00779     c[27] = lon.compressed[1];
00780     c[28] = lon.compressed[2];
00781   
00782     return true;
00783 }
00784 
00785 string CpREMUSCodec::decode_mdat_position(vector<unsigned int> c, int node, string sender, string type)
00786 {
00787 
00788     string sType;
00789 
00790     switch (c[1]) {
00791         case PS_DISABLED:
00792             sType="Disabled";
00793             break;
00794         case PS_SHIPS_POLE:
00795             sType="Ship's pole";
00796             break;
00797         case PS_GATEWAY_BUOY:
00798             sType="Gateway Buoy";
00799             break;
00800         case PS_NAMED_TRANSPONDER:
00801             sType="Transponder";
00802             break;
00803         case PS_VEHICLE_POSITION:
00804             sType="Vehicle position";
00805             break;
00806         case PS_LAST:
00807             sType="Last";
00808             break;
00809         default:
00810             sType="Invalid";
00811     }
00812 
00813     LATLON_COMPRESSED lat, lon;
00814     lat.compressed[0] = c[2];
00815     lat.compressed[1] = c[3];
00816     lat.compressed[2] = c[4];
00817 
00818     lon.compressed[0] = c[5];
00819     lon.compressed[1] = c[6];
00820     lon.compressed[2] = c[7];
00821 
00822     double dlat = Decode_latlon(lat);
00823 
00824     double dlon = Decode_latlon(lon);
00825 
00826     double x,y;
00827     // convert lat, long into x, y. 60 nautical miles per minute
00828     m_geodesy.LatLong2LocalUTM(dlat, dlon, y, x);
00829 
00830     // No speed heading depth info
00831     double dspd = 0;
00832     double dhdg = 0;
00833     double ddepth = 0;
00834     // Create human-readable version
00835 
00836     string ccl_status = "MessageType=CCL_POSITION" ;
00837     ccl_status += ",Node=" + as<string>(node);
00838     ccl_status += ",Type=" + sType;
00839     ccl_status += ",Timestamp=" + as<string>(MOOSTime());
00840     ccl_status += ",Latitude=" + as<string>(dlat);
00841     ccl_status += ",Longitude=" + as<string>(dlon);
00842     // make sure label is terminated
00843     //c[28] = 0;
00844     ccl_status += ",Label=" ;
00845     for (unsigned int i=15; i<28; i++)
00846     {
00847  unsigned char cc = c[i];
00848  ccl_status.push_back(cc);
00849     }
00850 
00851     m_Comms.Notify("CCL_POSITION_IN",ccl_status);
00852 
00853    
00854     // Create contact variables for collision avoidance
00855 
00856     MOOSTrace("Creating Contact fields\n");
00857     
00858     string contact_name = boost::to_upper_copy(sender);
00859     string moosvar = contact_name + "_NAV_UTC";
00860     m_Comms.Notify(moosvar,MOOSTime());
00861     moosvar = contact_name+"_NAV_X";
00862     m_Comms.Notify(moosvar,x);
00863     moosvar = contact_name+"_NAV_Y";
00864     m_Comms.Notify(moosvar,y);
00865     moosvar = contact_name+"_NAV_LAT";
00866     m_Comms.Notify(moosvar,dlat);
00867     moosvar = contact_name+"_NAV_LONG";
00868     m_Comms.Notify(moosvar,dlon);
00869     
00870     return assemble_NODE_REPORT(sender,
00871                                 type,
00872                                 as<string>(-1),
00873                                 as<string>(MOOSTime()),
00874                                 as<string>(x),
00875                                 as<string>(y),
00876                                 as<string>(dlat),
00877                                 as<string>(dlon),
00878                                 as<string>(dspd),
00879                                 as<string>(dhdg),
00880                                 as<string>(ddepth),as<string>(dhdg-360));
00881 
00882 }
00883 
00884 bool CpREMUSCodec::encode_mdat_position(vector<unsigned int>& c)
00885 {
00886 
00887     for (unsigned int i=0; i<c.size(); i++)
00888         c[i] = 0x0000;
00889  
00890     c[0] = MDAT_POSITION;
00891     c[1] = PS_VEHICLE_POSITION;
00892 
00893     LATLON_COMPRESSED lat, lon;
00894 
00895     lat = Encode_latlon( nav_lat );
00896   
00897     lon = Encode_latlon( nav_lon );
00898   
00899     c[2] = lat.compressed[0];
00900     c[3] = lat.compressed[1];
00901     c[4] = lat.compressed[2];
00902   
00903     c[5] = lon.compressed[0];
00904     c[6] = lon.compressed[1];
00905     c[7] = lon.compressed[2];
00906   
00907     return true;
00908 }
00909 
00910 string CpREMUSCodec::decode_mdat_alert(vector<unsigned int> c, int node, string sender, string type)
00911 {
00912     // OASIS Timestamp
00913     unsigned long oasis_time = (c[1]) +
00914         (c[2]<<8) +
00915         (c[3]<<16) +
00916         (c[4]<<24) ; 
00917   
00918     LATLON_COMPRESSED lat, lon;
00919 
00920     lat.compressed[0]=c[5];
00921     lat.compressed[1]=c[6];
00922     lat.compressed[2]=c[7];
00923     double dlat = Decode_latlon(lat);
00924 
00925     lon.compressed[0]=c[8];
00926     lon.compressed[1]=c[9];
00927     lon.compressed[2]=c[10];
00928     double dlon = Decode_latlon(lon);
00929 
00930     unsigned char hdg = c[11];
00931     double dhdg = Decode_heading(hdg);
00932 
00933     string ccl_alert = "MessageType=CCL_ALERT" ;
00934     ccl_alert += ",Node=" + as<string>(node);
00935     ccl_alert += ",Timestamp=" + as<string>(double(oasis_time));
00936     ccl_alert += ",Latitude=" + as<string>(dlat);
00937     ccl_alert += ",Longitude=" + as<string>(dlon);
00938     ccl_alert += ",Heading=" + as<string>(dhdg);
00939     ccl_alert += ",Contact1Id=" + as<string>( c[12] + (c[13]*256) );
00940     ccl_alert += ",Contact1Age=" + as<string>( c[14] );
00941     ccl_alert += ",Contact1Bearing=" + as<string>( c[15] );
00942     ccl_alert += ",Contact1Signature1=" + as<string>( c[16] );
00943     ccl_alert += ",Contact1Signature2=" + as<string>( c[17] );
00944     ccl_alert += ",Contact2Id=" + as<string>( c[18] + (c[19]*256) );
00945     ccl_alert += ",Contact2Age=" + as<string>( c[20] );
00946     ccl_alert += ",Contact2Bearing=" + as<string>( c[21] );
00947     ccl_alert += ",Contact2Signature1=" + as<string>( c[22] );
00948     ccl_alert += ",Contact2Signature2=" + as<string>( c[23] );
00949     ccl_alert += ",Contact3Id=" + as<string>( c[24] + (c[25]*256) );
00950     ccl_alert += ",Contact3Age=" + as<string>( c[26] );
00951     ccl_alert += ",Contact3Bearing=" + as<string>( c[27] );
00952     ccl_alert += ",Contact3Signature1=" + as<string>( c[28] );
00953     ccl_alert += ",Contact3Signature2=" + as<string>( c[29] );
00954     
00955     m_Comms.Notify("CCL_ALERT_IN",ccl_alert);
00956   
00957     return(ccl_alert);
00958 }
00959 
00960 string CpREMUSCodec::decode_mdat_alert2(vector<unsigned int> c, int node, string sender, string type)
00961 {
00962     // OASIS Timestamp
00963     unsigned long oasis_time = (c[1]) +
00964         (c[2]<<8) +
00965         (c[3]<<16) +
00966         (c[4]<<24) ; 
00967   
00968     LATLON_COMPRESSED lat, lon;
00969 
00970     lat.compressed[0]=c[5];
00971     lat.compressed[1]=c[6];
00972     lat.compressed[2]=c[7];
00973     double dlat = Decode_latlon(lat);
00974 
00975     lon.compressed[0]=c[8];
00976     lon.compressed[1]=c[9];
00977     lon.compressed[2]=c[10];
00978     double dlon = Decode_latlon(lon);
00979 
00980     unsigned char hdg1 = c[15];
00981     double dhdg1 = Decode_heading(hdg1);
00982     string hdgtyp1 = "Vehicle";
00983     if (c[13] & 0x80)
00984         hdgtyp1 = "Absolute";
00985 
00986     unsigned char hdg2 = c[22];
00987     double dhdg2 = Decode_heading(hdg2);
00988     string hdgtyp2 = "Vehicle";
00989     if (c[20] & 0x80)
00990         hdgtyp2 = "Absolute";
00991 
00992     unsigned char hdg3 = c[29];
00993     double dhdg3 = Decode_heading(hdg3);
00994     string hdgtyp3 = "Vehicle";
00995     if (c[27] & 0x80)
00996         hdgtyp3 = "Absolute";
00997 
00998     string ccl_alert2 = "MessageType=CCL_ALERT2" ;
00999     ccl_alert2 += ",Node=" + as<string>(node);
01000     ccl_alert2 += ",Timestamp=" + as<string>(double(oasis_time));
01001     ccl_alert2 += ",Latitude=" + as<string>(dlat);
01002     ccl_alert2 += ",Longitude=" + as<string>(dlon);
01003     ccl_alert2 += ",Contact1Id=" + as<string>( c[11] + (c[12]*256) );
01004     ccl_alert2 += ",Contact1Age=" + as<string>( c[13] );
01005     ccl_alert2 += ",Contact1Bearing=" + as<string>( c[14] );
01006     ccl_alert2 += ",Contact1HeadingType=" + hdgtyp1;
01007     ccl_alert2 += ",Contact1Heading=" + as<string>(dhdg1);
01008     ccl_alert2 += ",Contact1Signature1=" + as<string>( c[16] );
01009     ccl_alert2 += ",Contact1Signature2=" + as<string>( c[17] );
01010     ccl_alert2 += ",Contact2Id=" + as<string>( c[18] + (c[19]*256) );
01011     ccl_alert2 += ",Contact2Age=" + as<string>( c[20] );
01012     ccl_alert2 += ",Contact2Bearing=" + as<string>( c[21] );
01013     ccl_alert2 += ",Contact2HeadingType=" + hdgtyp2;
01014     ccl_alert2 += ",Contact2Heading=" + as<string>(dhdg2);
01015     ccl_alert2 += ",Contact2Signature1=" + as<string>( c[23] );
01016     ccl_alert2 += ",Contact2Signature2=" + as<string>( c[24] );
01017     ccl_alert2 += ",Contact3Id=" + as<string>( c[25] + (c[26]*256) );
01018     ccl_alert2 += ",Contact3Age=" + as<string>( c[27] );
01019     ccl_alert2 += ",Contact3Bearing=" + as<string>( c[28] );
01020     ccl_alert2 += ",Contact3HeadingType=" + hdgtyp3;
01021     ccl_alert2 += ",Contact3Heading=" + as<string>(dhdg3);
01022     ccl_alert2 += ",Contact3Signature1=" + as<string>( c[30] );
01023     ccl_alert2 += ",Contact3Signature2=" + as<string>( c[31] );
01024     
01025     m_Comms.Notify("CCL_ALERT2_IN",ccl_alert2);
01026   
01027     return(ccl_alert2);
01028 }
01029 
01030 bool CpREMUSCodec::parseRedirect(string msg, int& node)
01031 {
01032 
01033     string bite;
01034     double dummy;
01035 
01036     MOOSValFromString(bite,msg,"MessageType");
01037     if (!MOOSStrCmp(bite,"CCL_REDIRECT"))
01038         return(false);
01039 
01040     MOOSValFromString(dummy,msg,"DestinationPlatformId");
01041     node = int(dummy);
01042     MOOSTrace("parseRedirect: node= %d\n", node);
01043 
01044     MOOSValFromString(dummy,msg,"Timestamp");
01045 
01046     MOOSValFromString(transit_lat,msg,"Latitude");
01047     MOOSValFromString(transit_lon,msg,"Longitude");
01048   
01049     MOOSValFromString(spd_dep_flags,msg,"SpeedDepthFlags");
01050     MOOSValFromString(transit_depth,msg,"TransitDepth");
01051     MOOSValFromString(transit_speed,msg,"TransitSpeed");
01052     MOOSValFromString(dummy,msg,"TransitCommand");
01053     transit_command = int(dummy);
01054 
01055     MOOSValFromString(survey_depth,msg,"SurveyDepth");
01056     MOOSValFromString(survey_speed,msg,"SurveySpeed");
01057     MOOSValFromString(dummy,msg,"SurveyCommand");
01058     survey_command = int(dummy);
01059     MOOSValFromString(dummy,msg,"NumberOfRows");
01060     survey_rows = int(dummy);
01061     MOOSValFromString(row_length,msg,"RowLength");
01062     MOOSValFromString(row_spacing_0,msg,"RowSpacing0");
01063     MOOSValFromString(row_spacing_1,msg,"RowSpacing1");
01064     MOOSValFromString(row_heading,msg,"RowHeading");
01065 
01066     MOOSValFromString(start_lat,msg,"StartLatitude");
01067     MOOSValFromString(start_lon,msg,"StartLongitude");
01068 
01069     return(true);
01070 }
01071 
01072 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends