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
acomms/libmodemdriver/mm_driver.cpp
00001 // copyright 2009-2011 t. schneider tes@mit.edu
00002 // 
00003 // this file is part of the goby-acomms WHOI Micro-Modem driver.
00004 // goby-acomms is a collection of libraries 
00005 // for acoustic underwater networking
00006 //
00007 // This program is free software: you can redistribute it and/or modify
00008 // it under the terms of the GNU General Public License as published by
00009 // the Free Software Foundation, either version 3 of the License, or
00010 // (at your option) any later version.
00011 //
00012 // This software is distributed in the hope that it will be useful,
00013 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00014 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00015 // GNU General Public License for more details.
00016 //
00017 // You should have received a copy of the GNU General Public License
00018 // along with this software.  If not, see <http://www.gnu.org/licenses/>.
00019 
00020 #include <sstream>
00021 
00022 #include <boost/foreach.hpp>
00023 #include <boost/assign.hpp>
00024 #include <boost/algorithm/string.hpp>
00025 
00026 #include "goby/util/logger.h"
00027 
00028 #include "mm_driver.h"
00029 #include "driver_exception.h"
00030 
00031 using goby::util::NMEASentence;
00032 using goby::util::goby_time;
00033 using goby::util::ptime2unix_double;
00034 using goby::util::as;
00035 using google::protobuf::uint32;
00036 using namespace goby::util::tcolor;
00037 
00038 
00039 boost::posix_time::time_duration goby::acomms::MMDriver::MODEM_WAIT = boost::posix_time::seconds(3);
00040 boost::posix_time::time_duration goby::acomms::MMDriver::WAIT_AFTER_REBOOT = boost::posix_time::seconds(2);
00041 boost::posix_time::time_duration goby::acomms::MMDriver::ALLOWED_SKEW = boost::posix_time::seconds(2);
00042 boost::posix_time::time_duration goby::acomms::MMDriver::HYDROID_GATEWAY_GPS_REQUEST_INTERVAL = boost::posix_time::seconds(30);
00043 std::string goby::acomms::MMDriver::SERIAL_DELIMITER = "\r";
00044 unsigned goby::acomms::MMDriver::PACKET_FRAME_COUNT [] = { 1, 3, 3, 2, 2, 8 };
00045 unsigned goby::acomms::MMDriver::PACKET_SIZE [] = { 32, 32, 64, 256, 256, 256 };
00046 
00047 
00048 //
00049 // INITIALIZATION
00050 //
00051 
00052 goby::acomms::MMDriver::MMDriver(std::ostream* log /*= 0*/)
00053     : ModemDriverBase(log),
00054       log_(log),
00055       last_write_time_(goby_time()),
00056       waiting_for_modem_(false),
00057       startup_done_(false),
00058       global_fail_count_(0),
00059       present_fail_count_(0),
00060       clock_set_(false),
00061       last_hydroid_gateway_gps_request_(goby_time()),
00062       is_hydroid_gateway_(false),
00063       local_cccyc_(false)
00064 {
00065     initialize_talkers();
00066 }
00067 
00068 void goby::acomms::MMDriver::startup(const protobuf::DriverConfig& cfg)
00069 {
00070     if(startup_done_)
00071     {
00072         if(log_) *log_ << warn << group("modem_out") << "startup() called but driver is already started." << std::endl;
00073         return;
00074     }
00075         
00076     // store a copy for us later
00077     driver_cfg_ = cfg;
00078     
00079     if(!cfg.has_line_delimiter())
00080         driver_cfg_.set_line_delimiter(SERIAL_DELIMITER);
00081 
00082     if(!cfg.has_serial_baud())
00083         driver_cfg_.set_serial_baud(DEFAULT_BAUD);
00084 
00085     // support the non-standard Hydroid gateway buoy
00086     if(driver_cfg_.HasExtension(MicroModemConfig::hydroid_gateway_id))
00087         set_hydroid_gateway_prefix(driver_cfg_.GetExtension(MicroModemConfig::hydroid_gateway_id));
00088 
00089     
00090     modem_start(driver_cfg_);
00091     
00092     set_clock();
00093     
00094     write_cfg();
00095     
00096     // so that we know what the modem has for all the NVRAM values, not just the ones we set
00097     query_all_cfg();
00098 
00099     startup_done_ = true;
00100 }
00101 
00102 
00103 
00104 
00105 void goby::acomms::MMDriver::initialize_talkers()
00106 {
00107     boost::assign::insert (sentence_id_map_)
00108         ("ACK",ACK)("DRQ",DRQ)("RXA",RXA)("RXD",RXD)
00109         ("RXP",RXP)("TXD",TXD)("TXA",TXA)("TXP",TXP) 
00110         ("TXF",TXF)("CYC",CYC)("MPC",MPC)("MPA",MPA)
00111         ("MPR",MPR)("RSP",RSP)("MSC",MSC)("MSA",MSA)
00112         ("MSR",MSR)("EXL",EXL)("MEC",MEC)("MEA",MEA) 
00113         ("MER",MER)("MUC",MUC)("MUA",MUA)("MUR",MUR) 
00114         ("PDT",PDT)("PNT",PNT)("TTA",TTA)("MFD",MFD) 
00115         ("CLK",CLK)("CFG",CFG)("AGC",AGC)("BBD",BBD) 
00116         ("CFR",CFR)("CST",CST)("MSG",MSG)("REV",REV) 
00117         ("DQF",DQF)("SHF",SHF)("MFD",MFD)("SNR",SNR) 
00118         ("DOP",DOP)("DBG",DBG)("FFL",FFL)("FST",FST) 
00119         ("ERR",ERR)("TOA",TOA);
00120 
00121     boost::assign::insert (talker_id_map_)
00122         ("CC",CC)("CA",CA)("SN",SN)("GP",GP); 
00123 
00124     // from Micro-Modem Software Interface Guide v. 3.04
00125     boost::assign::insert (description_map_)
00126         ("$CAACK","Acknowledgment of a transmitted packet")
00127         ("$CADRQ","Data request message, modem to host")
00128         ("$CARXA","Received ASCII message, modem to host")
00129         ("$CARXD","Received binary message, modem to host")
00130         ("$CARXP","Incoming packet detected, modem to host")
00131         ("$CCTXD","Transmit binary data message, host to modem")
00132         ("$CCTXA","Transmit ASCII data message, host to modem")
00133         ("$CATXD","Echo back of transmit binary data message")
00134         ("$CATXA","Echo back of transmit ASCII data message")
00135         ("$CATXP","Start of packet transmission, modem to host")
00136         ("$CATXF","End of packet transmission, modem to host")
00137         ("$CCCYC","Network Cycle Initialization Command")
00138         ("$CACYC","Echo of Network Cycle Initialization command")
00139         ("$CCMPC","Mini-Packet Ping command, host to modem")
00140         ("$CAMPC","Echo of Ping command, modem to host")
00141         ("$CAMPA","A Ping has been received, modem to host")
00142         ("$CAMPR","Reply to Ping has been received, modem to host")
00143         ("$CCRSP","Pinging with an FM sweep")
00144         ("$CARSP","Respose to FM sweep ping command")
00145         ("$CCMSC","Sleep command, host to modem")
00146         ("$CAMSC","Echo of Sleep command, modem to host")
00147         ("$CAMSA","A Sleep was received acoustically, modem to host")
00148         ("$CAMSR","A Sleep reply was received, modem to host")
00149         ("$CCEXL","External hardware control command, local modem only")
00150         ("$CCMEC","External hardware control command, host to modem")
00151         ("$CAMEC","Echo of hardware control command, modem to host")
00152         ("$CAMEA","Hardware control command received acoustically")
00153         ("$CAMER","Hardware control command reply received")
00154         ("$CCMUC","User Mini-Packet command, host to modem")
00155         ("$CAMUC","Echo of user Mini-Packet, modem to host")
00156         ("$CAMUA","Mini-Packet received acoustically, modem to host")
00157         ("$CAMUR","Reply to Mini-Packet received, modem to host")
00158         ("$CCPDT","Ping REMUS digital transponder, host to modem")
00159         ("$CCPNT","Ping narrowband transponder, host to modem")
00160         ("$SNTTA","Transponder travel times, modem to host")
00161         ("$SNMFD","Nav matched filter information, modem to host")
00162         ("$CCCLK","Set clock, host to modem")
00163         ("$CCCFG","Set NVRAM configuration parameter, host to modem")
00164         ("$CCCFQ","Query configuration parameter, host to modem")
00165         ("$CCAGC","Set automatic gain control")
00166         ("$CABBD","Dump of baseband data to serial port, modem to host")
00167         ("$CCCFR","Measure noise level at receiver, host to modem")
00168         ("$SNCFR","Noise report, modem to host")
00169         ("$CACST","Communication cycle receive statistics")
00170         ("$CAXST","Communication cycle transmit statistics")
00171         ("$CAMSG","Transaction message, modem to host")
00172         ("$CAREV","Software revision message, modem to host")
00173         ("$CADQF","Data quality factor information, modem to host")
00174         ("$CASHF","Shift information, modem to host")
00175         ("$CAMFD","Comms matched filter information, modem to host")
00176         ("$CACLK","Time/Date message, modem to host")
00177         ("$CASNR","SNR statistics on the incoming PSK packet")
00178         ("$CADOP","Doppler speed message, modem to host")
00179         ("$CADBG","Low level debug message, modem to host")
00180         ("$CAERR","Error message, modem to host")
00181         ("$CATOA","Message from modem to host reporting time of arrival of the previous packet, and the synchronous timing mode used to determine that time.");
00182 
00183     // from Micro-Modem Software Interface Guide v. 3.04
00184     boost::assign::insert (cfg_map_)
00185         ("AGC","Turn on automatic gain control")
00186         ("AGN","Analog Gain (50 is 6 dB, 250 is 30 dB)")
00187         ("ASD","Always Send Data. Tells the modem to send test data when the user does not provide any.")
00188         ("BBD","PSK Baseband data dump to serial port")
00189         ("BND","Frequency Bank (1, 2, 3 for band A, B, or C, 0 for user-defined PSK only band)")
00190         ("BR1","Baud rate for serial port 1 (3 = 19200)")
00191         ("BR2","Baud rate for serial port 2 (3 = 19200)")
00192         ("BRN","Run bootloader at next revert")
00193         ("BSP","Boot loader serial port")
00194         ("BW0","Bandwidth for Band 0 PSK CPR 0-1 Coprocessor power toggle switch 1")
00195         ("CRL","Cycle init reverb lockout (ms) 50")
00196         ("CST","Cycle statistics message 1")
00197         ("CTO","Cycle init timeout (sec) 10")
00198         ("DBG","Enable low-level debug messages 0")
00199         ("DGM","Diagnostic messaging 0")
00200         ("DOP","Whether or not to send the $CADOP message")
00201         ("DQF","Whether or not to send the $CADQF message")
00202         ("DTH","Matched filter signal threshold, FSK")
00203         ("DTO","Data request timeout (sec)")
00204         ("DTP","Matched filter signal threshold, PSK")
00205         ("ECD","Int Delay at end of cycle (ms)")
00206         ("EFF","Feedforward taps for the LMS equalizer")
00207         ("EFB","Feedback taps for the LMS equalizer")
00208         ("FMD","PSK FM probe direction,0 up, 1 down")
00209         ("FML","PSK FM probe length, symbols")
00210         ("FC0","Carrier at Band 0 PSK only")
00211         ("GPS","GPS parser on aux. serial port")
00212         ("HFC","Hardware flow control on main serial port")
00213         ("MCM","Enable current mode hydrophone power supply on Rev. C Multi-Channel Analog Board. Must be set to 1 for Rev. B Multi-Channel Analog Board.")
00214         ("MFD","Whether or not to send the MFD messages")
00215         ("IRE","Print impulse response of FM sweep")
00216         ("MFC","MFD calibration value (samples)")
00217         ("MFD","Whether or not to send the MFD messages")
00218         ("MOD","0 sends FSK minipacket, 1 sends PSK minipacket")
00219         ("MPR","Enable power toggling on Multi-Channel Analog Board")
00220         ("MSE","Print symbol mean squared error (dB) from the LMS equalizer")
00221         ("MVM","Enable voltage mode hydrophone power supply on Multi-Channel Analog Board")
00222         ("NDT","Detect threshold for nav detector") 
00223         ("NPT","Power threshold for nav detector")
00224         ("NRL","Navigation reverb lockout (ms)")
00225         ("NRV","Number of CTOs before hard reboot")
00226         ("PAD","Power-amp delay (ms)")
00227         ("PCM","Passband channel mask")
00228         ("POW","Detection power threshold (dB) PRL Int Packet reverb lockout (ms)")
00229         ("PTH","Matched filter detector power threshold")
00230         ("PTO","Packet timeout (sec)")
00231         ("REV","Whether or not to send the $CAREV message")
00232         ("SGP","Show GPS messages on main serial port")
00233         ("RXA","Whether or not to send the $CARXA message")
00234         ("RXD","Whether or not to send the $CARXD message")
00235         ("RXP","Whether or not to send the $CARXP message") 
00236         ("SCG","Set clock from GPS")
00237         ("SHF","Whether or not to send the $CASHF message")
00238         ("SNR","Turn on SNR stats for PSK comms")
00239         ("SNV","Synchronous transmission of packets")
00240         ("SRC","Default Source Address")
00241         ("TAT","Navigation turn-around-time (msec)")
00242         ("TOA","Display time of arrival of a packet (sec)")
00243         ("TXD","Delay before transmit (ms)")
00244         ("TXP","Turn on start of transmit message")
00245         ("TXF","Turn on end of transmit message")
00246         ("XST","Turn on transmit stats message, CAXST");    
00247     
00248 }
00249 
00250 void goby::acomms::MMDriver::set_hydroid_gateway_prefix(int id)
00251 {
00252     is_hydroid_gateway_ = true;
00253     // If the buoy is in use, make the prefix #M<ID>
00254     hydroid_gateway_gps_request_ = "#G" + as<std::string>(id) + "\r\n";        
00255     hydroid_gateway_modem_prefix_ = "#M" + as<std::string>(id);
00256     
00257     if(log_) *log_ << "Setting the hydroid_gateway buoy prefix: out=" << hydroid_gateway_modem_prefix_ << std::endl;
00258 }
00259 
00260 
00261 void goby::acomms::MMDriver::set_clock()
00262 {
00263     NMEASentence nmea("$CCCLK", NMEASentence::IGNORE);
00264     boost::posix_time::ptime p = goby_time();
00265     
00266     nmea.push_back(int(p.date().year()));
00267     nmea.push_back(int(p.date().month()));
00268     nmea.push_back(int(p.date().day()));
00269     nmea.push_back(int(p.time_of_day().hours()));
00270     nmea.push_back(int(p.time_of_day().minutes()));
00271     nmea.push_back(int(p.time_of_day().seconds()));
00272     
00273     static protobuf::ModemMsgBase base_msg;
00274     base_msg.Clear();
00275     base_msg.set_time(as<std::string>(p));
00276     append_to_write_queue(nmea, &base_msg);
00277 
00278     // take a breath to let the clock be set 
00279     sleep(1);
00280 }
00281 
00282 void goby::acomms::MMDriver::write_cfg()
00283 {
00284     // reset nvram if requested and not a Hydroid buoy
00285     // as this resets the baud to 19200 and the buoy
00286     // requires 4800
00287     if(!is_hydroid_gateway_ && driver_cfg_.GetExtension(MicroModemConfig::reset_nvram))
00288         write_single_cfg("ALL,0");
00289 
00290     write_single_cfg("SRC," + as<std::string>(driver_cfg_.modem_id()));
00291     
00292     
00293     for(int i = 0, n = driver_cfg_.ExtensionSize(MicroModemConfig::nvram_cfg); i < n; ++i)
00294     {
00295         write_single_cfg(driver_cfg_.GetExtension(MicroModemConfig::nvram_cfg, i));
00296     }    
00297 }
00298 
00299 void goby::acomms::MMDriver::write_single_cfg(const std::string &s)
00300 {
00301         
00302     NMEASentence nmea("$CCCFG", NMEASentence::IGNORE);        
00303     nmea.push_back(boost::to_upper_copy(s));
00304 
00305     // set our map now so we know various values immediately (like SRC)
00306     nvram_cfg_[nmea[1]] = nmea.as<int>(2);
00307         
00308     static protobuf::ModemMsgBase base_msg;
00309     base_msg.Clear();
00310     append_to_write_queue(nmea, &base_msg);
00311     
00312 }
00313 
00314 
00315 
00316 void goby::acomms::MMDriver::query_all_cfg()
00317 {
00318     NMEASentence nmea("$CCCFQ,ALL", NMEASentence::IGNORE);
00319 
00320     static protobuf::ModemMsgBase base_msg;
00321     base_msg.Clear();
00322     append_to_write_queue(nmea, &base_msg);
00323 }
00324 
00325 
00326 //
00327 // SHUTDOWN
00328 //
00329 
00330 
00331 void goby::acomms::MMDriver::shutdown()
00332 {
00333     startup_done_ = false;
00334     modem_close();
00335 }
00336 
00337 goby::acomms::MMDriver::~MMDriver()
00338 { }
00339 
00340 
00341 //
00342 // LOOP
00343 //
00344 
00345 void goby::acomms::MMDriver::do_work()
00346 {    
00347     // don't try to set the clock if we already have outgoing
00348     // messages queued since the time will be wrong by the time
00349     // we can send
00350     if(!clock_set_ && out_.empty())
00351         set_clock();
00352     
00353     // keep trying to send stuff to the modem
00354     try_send();
00355 
00356     // read any incoming messages from the modem
00357     std::string in;
00358     while(modem_read(&in))
00359     {
00360         boost::trim(in);
00361  // Check for whether the hydroid_gateway buoy is being used and if so, remove the prefix
00362  if (is_hydroid_gateway_) in.erase(0, HYDROID_GATEWAY_PREFIX_LENGTH);
00363 
00364         // try to handle the received message, posting appropriate signals
00365         try
00366         {
00367             NMEASentence nmea(in, NMEASentence::VALIDATE);
00368             process_receive(nmea);
00369         }
00370         catch(std::exception& e)
00371         {
00372             if(log_) *log_ << group("modem_in") << warn << e.what() << std::endl;
00373         }
00374     }
00375 
00376     // if we're using a hydroid buoy query it for its GPS position
00377     if(is_hydroid_gateway_ &&
00378        last_hydroid_gateway_gps_request_ + HYDROID_GATEWAY_GPS_REQUEST_INTERVAL < goby_time())
00379     {
00380         modem_write(hydroid_gateway_gps_request_);
00381         last_hydroid_gateway_gps_request_ = goby_time();
00382     }
00383     
00384 }
00385 
00386 //
00387 // HANDLE MAC SIGNALS
00388 //
00389 
00390 void goby::acomms::MMDriver::handle_initiate_transmission(protobuf::ModemMsgBase* base_msg)
00391 {
00392     // we initiated this cycle so don't grab data *again* on the CACYC (in cyc()) 
00393     local_cccyc_ = true;
00394     protobuf::ModemDataInit init_msg;
00395     init_msg.mutable_base()->CopyFrom(*base_msg);
00396     init_msg.set_num_frames(PACKET_FRAME_COUNT[base_msg->rate()]);
00397     cache_outgoing_data(init_msg);
00398     
00399     // don't start a local cycle if we have no data
00400     const bool is_local_cycle = init_msg.base().src() == driver_cfg_.modem_id();
00401     if(!(is_local_cycle && cached_data_msgs_.empty()))
00402     {
00403         //$CCCYC,CMD,ADR1,ADR2,Packet Type,ACK,Npkt*CS
00404         NMEASentence nmea("$CCCYC", NMEASentence::IGNORE);
00405         nmea.push_back(0); // CMD: deprecated field
00406         nmea.push_back(init_msg.base().src()); // ADR1
00407         
00408         
00409         nmea.push_back(is_local_cycle
00410                        ? cached_data_msgs_.begin()->second.base().dest()
00411                        : init_msg.base().dest()); // ADR2        
00412         
00413         nmea.push_back(init_msg.base().rate()); // Packet Type (transmission rate)
00414         nmea.push_back(is_local_cycle
00415                        ? static_cast<int>(cached_data_msgs_.begin()->second.ack_requested())
00416                        : 1); // ACK: deprecated field, but still dictates the value provided by CADRQ
00417         nmea.push_back(init_msg.num_frames()); // number of frames we want
00418 
00419         append_to_write_queue(nmea, base_msg);
00420     }
00421     else
00422     {
00423         if(log_)
00424             *log_ << group("modem_out") << "Not initiating transmission because we have no data to send" << std::endl;
00425     }
00426     
00427 }
00428 
00429 void goby::acomms::MMDriver::handle_initiate_ranging(protobuf::ModemRangingRequest* request_msg)
00430 {
00431     switch(request_msg->type())
00432     {
00433         case protobuf::MODEM_ONE_WAY_SYNCHRONOUS:
00434         {
00435             if(log_) *log_ << warn << group("modem_out") << "Cannot initiate ONE_WAY_SYNCHRONOUS ping manually. You must enable NVRAM cfg \"TOA,1\" and one-way synchronous messages will be reported on all relevant acoustic transactions" << std::endl;
00436             break;
00437         }
00438         
00439         case protobuf::MODEM_TWO_WAY_PING:
00440         {
00441             //$CCMPC,SRC,DEST*CS
00442             NMEASentence nmea("$CCMPC", NMEASentence::IGNORE);
00443             nmea.push_back(request_msg->base().src()); // ADR1
00444             nmea.push_back(request_msg->base().dest()); // ADR2
00445             
00446             append_to_write_queue(nmea, request_msg->mutable_base());
00447             break;
00448         }
00449         
00450 
00451         case protobuf::REMUS_LBL_RANGING:
00452         {
00453             // $CCPDT,GRP,CHANNEL,SF,STO,Timeout,AF,BF,CF,DF*CS
00454             NMEASentence nmea("$CCPDT", NMEASentence::IGNORE);
00455             nmea.push_back(1); // GRP 1 is the only group right now 
00456             nmea.push_back(request_msg->base().src() % 4 + 1); // can only use 1-4
00457             nmea.push_back(0); // synchronize may not work?
00458             nmea.push_back(0); // synchronize may not work?
00459             // REMUS LBL is 50 ms turn-around time, assume 1500 m/s speed of sound
00460             nmea.push_back(int((request_msg->max_range()*2.0 / ROUGH_SPEED_OF_SOUND)*1000 + REMUS_LBL_TURN_AROUND_MS));
00461             nmea.push_back(request_msg->enable_beacons() >> 0 & 1);
00462             nmea.push_back(request_msg->enable_beacons() >> 1 & 1);
00463             nmea.push_back(request_msg->enable_beacons() >> 2 & 1);
00464             nmea.push_back(request_msg->enable_beacons() >> 3 & 1);
00465 
00466             append_to_write_queue(nmea, request_msg->mutable_base());
00467             break;
00468         }
00469         
00470     }
00471 }
00472 
00473 //
00474 // OUTGOING NMEA
00475 //
00476 
00477 
00478 void goby::acomms::MMDriver::try_send()
00479 {
00480     if(out_.empty()) return;
00481 
00482     const protobuf::ModemMsgBase& base_msg = out_.front().second;
00483     
00484     bool resend = waiting_for_modem_ && (last_write_time_ <= (goby_time() - MODEM_WAIT));
00485     if(!waiting_for_modem_)
00486     {
00487         mm_write(base_msg);
00488     }
00489     else if(resend)
00490     {
00491         if(log_) *log_ << group("modem_out") << warn << "resending last command; no serial ack in " << (goby_time() - last_write_time_).total_seconds() << " second(s). " << std::endl;
00492         ++global_fail_count_;
00493         
00494         if(global_fail_count_ == MAX_FAILS_BEFORE_DEAD)
00495         {
00496             modem_close();
00497             throw(driver_exception("modem appears to not be responding!"));
00498         }
00499         
00500         try
00501         {
00502             // try to increment the present (current NMEA sentence) fail counter
00503             // will throw if fail counter exceeds RETRIES
00504             increment_present_fail();
00505             // assuming we're still ok, write the line again
00506             mm_write(base_msg);
00507         }
00508         catch(driver_exception& e)
00509         {
00510             present_fail_exceeds_retries();
00511         }
00512     }
00513 }
00514 
00515 void goby::acomms::MMDriver::increment_present_fail()
00516 {
00517     ++present_fail_count_;
00518     if(present_fail_count_ >= RETRIES)
00519         throw(driver_exception("Fail count exceeds RETRIES"));
00520 }
00521 
00522 void goby::acomms::MMDriver::present_fail_exceeds_retries()
00523 {
00524     if(log_) *log_  << group("modem_out") << warn << "modem did not respond to our command even after " << RETRIES << " retries. continuing onwards anyway..." << std::endl;
00525     pop_out();    
00526 }
00527 
00528 
00529 
00530 void goby::acomms::MMDriver::mm_write(const protobuf::ModemMsgBase& base_msg)
00531 {
00532     if(log_) *log_ << group("modem_out") << hydroid_gateway_modem_prefix_
00533                    << base_msg.raw() << "\n" << "^ "
00534                    << magenta << base_msg.description() << nocolor << std::endl;
00535 
00536     signal_all_outgoing(base_msg);    
00537  
00538     modem_write(hydroid_gateway_modem_prefix_ + base_msg.raw() + "\r\n");
00539 
00540     waiting_for_modem_ = true;
00541     last_write_time_ = goby_time();
00542 }
00543 
00544 
00545 
00546 
00547 void goby::acomms::MMDriver::pop_out()
00548 {
00549     waiting_for_modem_ = false;
00550 
00551     if(!out_.empty())
00552         out_.pop_front();
00553     else
00554     {
00555         if(log_) *log_ << group("modem_out") << warn
00556                        << "Expected to pop outgoing NMEA message but out_ deque is empty" << std::endl;
00557     }
00558     
00559     present_fail_count_ = 0;
00560 }
00561 
00562 void goby::acomms::MMDriver::append_to_write_queue(const NMEASentence& nmea, protobuf::ModemMsgBase* base_msg)
00563 {
00564     base_msg->set_raw(nmea.message());
00565     
00566     if(!base_msg->has_time())
00567         base_msg->set_time(as<std::string>(goby_time()));
00568     
00569     if(!base_msg->has_description() && description_map_.count(nmea.front()))
00570         base_msg->set_description(description_map_[nmea.front()]);
00571 
00572     
00573     out_.push_back(make_pair(nmea, *base_msg));
00574     try_send(); // try to push it now without waiting for the next call to do_work();
00575 }
00576 
00577 //
00578 // INCOMING NMEA
00579 //
00580 
00581 
00582 void goby::acomms::MMDriver::process_receive(const NMEASentence& nmea)
00583 {
00584     // need to print this first so raw log messages appear causal (as they are)
00585     if(log_)
00586     {
00587         *log_ << group("modem_in") << nmea.message() << std::endl;
00588         if(description_map_.count(nmea.front()))
00589             *log_ << group("modem_in") << "^ " << blue << description_map_[nmea.front()] << nocolor << std::endl;
00590     }
00591 
00592     global_fail_count_ = 0;
00593     
00594     protobuf::ModemMsgBase* this_base_msg = 0;
00595     static protobuf::ModemMsgBase base_msg;
00596     static protobuf::ModemDataInit init_msg;
00597     static protobuf::ModemDataTransmission data_msg;
00598     static protobuf::ModemDataAck ack_msg;
00599     static protobuf::ModemRangingReply ranging_msg;
00600     
00601     base_msg.Clear();
00602     // look at the sentence id (last three characters of the NMEA 0183 talker)
00603     switch(sentence_id_map_[nmea.sentence_id()])
00604     {
00605         //
00606         // local modem
00607         //
00608         case REV: rev(nmea); break; // software revision
00609         case ERR: err(nmea); break; // error message
00610         case DRQ: drq(nmea); break; // data request
00611         case CFG: cfg(nmea, &base_msg); break; // configuration
00612         case CLK: clk(nmea, &base_msg); break; // clock
00613             
00614         //
00615         // data cycle
00616         //
00617         case CYC:  // cycle init
00618         {
00619             init_msg.Clear();
00620             cyc(nmea, &init_msg);
00621             this_base_msg = init_msg.mutable_base();
00622             // can't trust ADR1 to be SRC, so we trash this CATOA
00623             ranging_msg.Clear();
00624             break;
00625         }
00626 
00627         case RXD:  // data receive
00628         {
00629             data_msg.Clear();
00630             rxd(nmea, &data_msg);
00631             this_base_msg = data_msg.mutable_base();
00632             flush_toa(*this_base_msg, &ranging_msg);
00633             break;
00634         }
00635         
00636         case ACK:  // acknowledge
00637         {
00638             ack_msg.Clear();
00639             ack(nmea, &ack_msg);
00640             this_base_msg = ack_msg.mutable_base();
00641             flush_toa(*this_base_msg, &ranging_msg);
00642             break;
00643         }
00644 
00645         //
00646         // ranging
00647         //
00648         case MPR:  // ping response
00649         {
00650             ranging_msg.Clear();
00651             mpr(nmea, &ranging_msg);
00652             this_base_msg = ranging_msg.mutable_base();
00653             break;
00654         }
00655         
00656         case TTA: // remus lbl times
00657         {
00658             ranging_msg.Clear();
00659             tta(nmea, &ranging_msg);
00660             this_base_msg = ranging_msg.mutable_base();
00661             break; 
00662         }
00663         
00664         case TOA: // one way synchronous Time-Of-Arrival
00665         {
00666             ranging_msg.Clear();
00667             toa(nmea, &ranging_msg);
00668             this_base_msg = ranging_msg.mutable_base();
00669             break; 
00670         }
00671         case RXP:
00672         {
00673             // clear out any pending TOA that didn't get flushed
00674             if(ranging_msg.type() == protobuf::MODEM_ONE_WAY_SYNCHRONOUS)
00675             {
00676                 if(log_) *log_ << group("modem_in") << warn << "failed to flush: " << ranging_msg << std::endl;
00677                 ranging_msg.Clear();
00678             }
00679         }    
00680 
00681         
00682         default: break;
00683     }
00684 
00685     if(!this_base_msg) this_base_msg = &base_msg;
00686 
00687     if(log_ && this_base_msg->has_description())
00688         *log_ << group("modem_in") << "^ " << blue << this_base_msg->description() << nocolor << std::endl;
00689     
00690     this_base_msg->set_raw(nmea.message());
00691     if(!this_base_msg->has_description() && description_map_.count(nmea.front()))
00692         this_base_msg->set_description(description_map_[nmea.front()]);
00693     
00694     signal_all_incoming(*this_base_msg);
00695     
00696     // clear the last send given modem acknowledgement
00697     if(!out_.empty() && out_.front().first.sentence_id() == nmea.sentence_id())
00698         pop_out();    
00699 }
00700 
00701 void goby::acomms::MMDriver::ack(const NMEASentence& nmea, protobuf::ModemDataAck* m)
00702 {
00703     uint32 frame = as<uint32>(nmea[3])-1;
00704     
00705     if(frames_waiting_for_ack_.count(frame))
00706     {
00707         
00708         m->mutable_base()->set_time(as<std::string>(goby_time()));
00709         m->mutable_base()->set_src(as<uint32>(nmea[1]));
00710         m->mutable_base()->set_dest(as<uint32>(nmea[2]));
00711         // WHOI counts starting at 1, Goby counts starting at 0
00712         m->set_frame(frame);
00713         
00714         signal_ack(*m);
00715 
00716         frames_waiting_for_ack_.erase(frame);
00717     }
00718     else
00719     {
00720         if(log_)
00721             *log_ << warn << "Received acknowledgement for Micro-Modem frame " << frame + 1 << " (Goby frame " << frame << ") that we were not expecting." << std::endl;
00722     }
00723 }
00724 
00725 void goby::acomms::MMDriver::drq(const NMEASentence& nmea_in)
00726 {
00727     //$CADRQ,HHMMSS,SRC,DEST,ACK,N,F#*CS
00728 
00729     NMEASentence nmea_out("$CCTXD", NMEASentence::IGNORE);        
00730 
00731     // WHOI counts frames from 1, we count from 0
00732     int frame = as<int>(nmea_in[6])-1;
00733     std::map<unsigned, protobuf::ModemDataTransmission>::iterator it =
00734         cached_data_msgs_.find(frame);
00735     
00736     if(it != cached_data_msgs_.end())
00737     {
00738         // use the cached data
00739         protobuf::ModemDataTransmission& data_msg = it->second;
00740         nmea_out.push_back(data_msg.base().src());
00741         nmea_out.push_back(data_msg.base().dest());
00742         nmea_out.push_back(int(data_msg.ack_requested()));
00743         nmea_out.push_back(hex_encode(data_msg.data()));
00744         cached_data_msgs_.erase(it);
00745         
00746         if(data_msg.ack_requested())
00747             frames_waiting_for_ack_.insert(frame);
00748     }
00749     else
00750     {
00751         // send a blank message to supress further DRQ
00752         nmea_out.push_back(nmea_in[2]); // SRC
00753         nmea_out.push_back(nmea_in[3]); // DEST
00754         nmea_out.push_back(nmea_in[4]); // ACK
00755         nmea_out.push_back(""); // no data
00756     }
00757     static protobuf::ModemMsgBase base_msg;
00758     base_msg.Clear();
00759     append_to_write_queue(nmea_out, &base_msg);    
00760 }
00761 
00762 void goby::acomms::MMDriver::rxd(const NMEASentence& nmea, protobuf::ModemDataTransmission* m)
00763 {
00764     m->mutable_base()->set_time(as<std::string>(goby_time()));
00765     m->mutable_base()->set_src(as<uint32>(nmea[1]));
00766     m->mutable_base()->set_dest(as<uint32>(nmea[2]));
00767     m->set_ack_requested(as<bool>(nmea[3]));
00768     // WHOI counts from 1, we count from 0
00769     m->set_frame(as<uint32>(nmea[4])-1);
00770     m->set_data(hex_decode(nmea[5]));
00771 
00772     signal_receive(*m);
00773 }
00774 
00775 
00776 void goby::acomms::MMDriver::cfg(const NMEASentence& nmea, protobuf::ModemMsgBase* base_msg)
00777 {
00778     nvram_cfg_[nmea[1]] = nmea.as<int>(2);
00779     base_msg->set_description("Configuration: " + cfg_map_[nmea.at(1)]);
00780     
00781     if(out_.empty() ||
00782        (out_.front().first.sentence_id() != "CFG" && out_.front().first.sentence_id() != "CFQ"))
00783         return;
00784     
00785     if(out_.front().first.sentence_id() == "CFQ") pop_out();
00786 }
00787 
00788 void goby::acomms::MMDriver::clk(const NMEASentence& nmea, protobuf::ModemMsgBase* base_msg)
00789 {
00790     if(out_.empty() || out_.front().first.sentence_id() != "CLK")
00791         return;
00792     
00793     using namespace boost::posix_time;
00794     using namespace boost::gregorian;
00795     // modem responds to the previous second, which is why we subtract one second from the current time
00796     ptime expected = goby_time();
00797     ptime reported = ptime(date(nmea.as<int>(1),
00798                                 nmea.as<int>(2),
00799                                 nmea.as<int>(3)),
00800                            time_duration(nmea.as<int>(4),
00801                                          nmea.as<int>(5),
00802                                          nmea.as<int>(6),
00803                                          0));
00804     if(log_) *log_ << "reported time: " << reported << std::endl;
00805     
00806     
00807     base_msg->set_time(as<std::string>(reported));
00808     base_msg->set_time_source(protobuf::ModemMsgBase::MODEM_TIME);
00809     
00810     // make sure the modem reports its time as set at the right time
00811     // we may end up oversetting the clock, but better safe than sorry...
00812     if(reported >= (expected - ALLOWED_SKEW))
00813         clock_set_ = true;    
00814     
00815 }
00816 
00817 void goby::acomms::MMDriver::mpr(const NMEASentence& nmea, protobuf::ModemRangingReply* m)
00818 {
00819     m->mutable_base()->set_time(as<std::string>(goby_time()));
00820     
00821     // $CAMPR,SRC,DEST,TRAVELTIME*CS
00822     // reverse src and dest so they match the original request
00823     m->mutable_base()->set_src(as<uint32>(nmea[2]));
00824     m->mutable_base()->set_dest(as<uint32>(nmea[1]));
00825 
00826     if(nmea.size() > 3)
00827         m->add_one_way_travel_time(as<double>(nmea[3]));
00828 
00829     m->set_type(protobuf::MODEM_TWO_WAY_PING);
00830 
00831     
00832     signal_range_reply(*m);
00833 }
00834 
00835 void goby::acomms::MMDriver::rev(const NMEASentence& nmea)
00836 {
00837     if(nmea[2] == "INIT")
00838     {
00839         // reboot
00840         sleep(WAIT_AFTER_REBOOT.total_seconds());
00841         clock_set_ = false;
00842     }
00843     else if(nmea[2] == "AUV")
00844     {
00845         //check the clock
00846         using boost::posix_time::ptime;
00847         ptime expected = goby_time();
00848         ptime reported = nmea_time2ptime(nmea[1]);
00849 
00850         if(reported < (expected - ALLOWED_SKEW))
00851             clock_set_ = false;
00852     }
00853 }
00854 
00855 void goby::acomms::MMDriver::err(const NMEASentence& nmea)
00856 {
00857     *log_ << group("modem_out") << warn << "modem reports error: " << nmea.message() << std::endl;
00858 
00859     
00860     // recover quicker if old firmware does not understand one of our commands
00861     if(nmea.at(2) == "NMEA")
00862     {
00863         waiting_for_modem_ = false;
00864 
00865         try
00866         {
00867             increment_present_fail(); 
00868         }
00869         catch(driver_exception& e)
00870         {
00871             present_fail_exceeds_retries();
00872         }
00873     }
00874 }
00875 
00876 
00877 
00878 void goby::acomms::MMDriver::cyc(const NMEASentence& nmea, protobuf::ModemDataInit* init_msg)
00879 {
00880     init_msg->mutable_base()->set_time(as<std::string>(goby_time()));
00881 
00882     // somewhat "loose" interpretation of some of the fields
00883     init_msg->mutable_base()->set_src(as<uint32>(nmea[2])); // ADR1
00884     init_msg->mutable_base()->set_dest(as<uint32>(nmea[3])); // ADR2
00885     init_msg->mutable_base()->set_rate(as<uint32>(nmea[4])); // Rate
00886     init_msg->set_num_frames(as<uint32>(nmea[6])); // Npkts, number of packets
00887     
00888     
00889     // if we're supposed to send and we didn't initiate the cycle
00890     if(!local_cccyc_)
00891         cache_outgoing_data(*init_msg);
00892     else // clear flag for next cycle
00893         local_cccyc_ = false;
00894 }
00895 
00896 void goby::acomms::MMDriver::cache_outgoing_data(const protobuf::ModemDataInit& init_msg)
00897 {
00898     if(init_msg.base().src() == driver_cfg_.modem_id())
00899     {
00900         if(!cached_data_msgs_.empty())
00901         {
00902             if(log_) *log_ << warn << "flushing " << cached_data_msgs_.size() << " messages that were never sent in response to a $CADRQ." << std::endl;
00903             cached_data_msgs_.clear();
00904         }
00905 
00906         if(!frames_waiting_for_ack_.empty())
00907         {
00908             if(log_) *log_ << warn << "flushing " << frames_waiting_for_ack_.size() << " expected acknowledgments that were never received." << std::endl;
00909             frames_waiting_for_ack_.clear();
00910         }
00911         
00912     
00913         static protobuf::ModemDataRequest request_msg;
00914         request_msg.Clear();
00915         // make a data request in anticipation that we will need to send
00916         request_msg.mutable_base()->set_time(as<std::string>(goby_time()));
00917     
00918         request_msg.mutable_base()->set_src(init_msg.base().src());
00919         request_msg.mutable_base()->set_dest(init_msg.base().dest());
00920     
00921         // nmea_in[4] == ack
00922         request_msg.set_max_bytes(PACKET_SIZE[init_msg.base().rate()]);
00923         static protobuf::ModemDataTransmission data_msg;
00924         for(unsigned i = 0, n = init_msg.num_frames(); i < n; ++i)
00925         {
00926             request_msg.set_frame(i);
00927             data_msg.Clear();
00928             
00929             signal_data_request(request_msg, &data_msg);
00930 
00931             if(!validate_data(request_msg, &data_msg))
00932                continue;
00933 
00934             // no more data to send
00935             if(data_msg.data().empty())
00936                 break;
00937             
00938             cached_data_msgs_[i] = data_msg;
00939         }
00940     }
00941 }
00942 
00943 
00944 
00945 bool goby::acomms::MMDriver::validate_data(const protobuf::ModemDataRequest& request_msg,
00946                                            protobuf::ModemDataTransmission* data_msg)
00947 {
00948     if(!data_msg->base().has_src())
00949     {
00950         data_msg->mutable_base()->set_src(request_msg.base().src());
00951     }
00952     else if(data_msg->base().src() != request_msg.base().src())
00953     {
00954         if(log_)
00955             *log_ << warn << "ModemDataTransmission::ModemMsgBase::src must equal ModemDataRequest::ModemMsgBase::dest" << std::endl;
00956         return false;
00957     }
00958             
00959     if(!data_msg->base().has_dest())
00960     {
00961         data_msg->mutable_base()->set_dest(request_msg.base().dest());
00962     }
00963     else if(!is_valid_destination(data_msg->base().dest()))
00964     {
00965         if(log_)
00966             *log_ << warn << "ModemDataTransmission::ModemMsgBase::src must equal ModemDataRequest::ModemMsgBase::dest" << std::endl;
00967         return false;
00968     }
00969     
00970     return true;
00971 }
00972 
00973 
00974 void goby::acomms::MMDriver::toa(const NMEASentence& nmea, protobuf::ModemRangingReply* m)
00975 {
00976     const unsigned SYNCHED_TO_PPS_AND_CCCLK_GOOD = 3;
00977     if(nmea.as<unsigned>(2) == SYNCHED_TO_PPS_AND_CCCLK_GOOD) // good timing
00978     {
00979         boost::posix_time::ptime toa = nmea_time2ptime(nmea[1]);
00980         double frac_sec = double(toa.time_of_day().fractional_seconds())/toa.time_of_day().ticks_per_second();
00981         //ambiguous because we don't know when the message was sent. report out to MAX_RANGE and the user has to have a smarter way to differentiate
00982         const int MAX_RANGE = 10000;
00983         for(int i = 0; i <= MAX_RANGE / ROUGH_SPEED_OF_SOUND; ++i)
00984             m->add_one_way_travel_time(frac_sec + i);
00985 
00986         m->set_type(protobuf::MODEM_ONE_WAY_SYNCHRONOUS);
00987         m->mutable_base()->set_time(as<std::string>(toa));
00988         m->mutable_base()->set_time_source(protobuf::ModemMsgBase::MODEM_TIME);
00989     }
00990 }
00991 
00992 void goby::acomms::MMDriver::flush_toa(const protobuf::ModemMsgBase& base_msg, protobuf::ModemRangingReply* ranging_msg)
00993 {
00994     if(ranging_msg->type() == protobuf::MODEM_ONE_WAY_SYNCHRONOUS)
00995     {
00996         ranging_msg->mutable_base()->set_dest(driver_cfg_.modem_id());
00997         ranging_msg->mutable_base()->set_src(base_msg.src());
00998         signal_range_reply(*ranging_msg);
00999         ranging_msg->Clear();
01000     }
01001 }
01002 
01003 void goby::acomms::MMDriver::tta(const NMEASentence& nmea, protobuf::ModemRangingReply* m)
01004 {
01005     m->add_one_way_travel_time(as<double>(nmea[1]));
01006     m->add_one_way_travel_time(as<double>(nmea[2]));
01007     m->add_one_way_travel_time(as<double>(nmea[3]));
01008     m->add_one_way_travel_time(as<double>(nmea[4]));
01009     
01010     m->set_type(protobuf::REMUS_LBL_RANGING);
01011 
01012     m->mutable_base()->set_src(driver_cfg_.modem_id());
01013     m->mutable_base()->set_time(as<std::string>(nmea_time2ptime(nmea[5])));
01014     m->mutable_base()->set_time_source(protobuf::ModemMsgBase::MODEM_TIME);
01015     
01016     signal_range_reply(*m);    
01017 }
01018 
01019 //
01020 // UTILITY
01021 //
01022 
01023 boost::posix_time::ptime goby::acomms::MMDriver::nmea_time2ptime(const std::string& mt)
01024 {   
01025     using namespace boost::posix_time;
01026     using namespace boost::gregorian;
01027 
01028     // must be at least HHMMSS
01029     if(mt.length() < 6)
01030         return ptime(not_a_date_time);  
01031     else
01032     {
01033         std::string s_hour = mt.substr(0,2), s_min = mt.substr(2,2), s_sec = mt.substr(4,2), s_fs = "0";
01034 
01035         // has some fractional seconds
01036         if(mt.length() > 7)
01037             s_fs = mt.substr(7); // everything after the "."
01038          
01039         try
01040         {
01041             int hour = boost::lexical_cast<int>(s_hour);
01042             int min = boost::lexical_cast<int>(s_min);
01043             int sec = boost::lexical_cast<int>(s_sec);
01044             int micro_sec = boost::lexical_cast<int>(s_fs)*pow(10, 6-s_fs.size());
01045            
01046      boost::gregorian::date return_date(boost::gregorian::day_clock::universal_day());
01047      boost::posix_time::time_duration return_duration(boost::posix_time::time_duration(hour, min, sec, 0) + microseconds(micro_sec));
01048      boost::posix_time::ptime return_time(return_date, return_duration);
01049             return return_time;
01050         }
01051         catch (boost::bad_lexical_cast&)
01052         {
01053             return ptime(not_a_date_time);
01054         }        
01055     }
01056 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends