← Back to branch summary

~tes/goby/2.1-git

« back to all changes in this revision

Viewing changes to src/apps/moos/iFrontSeat/legacy_translator.cpp

  • Committer: GitHub
  • Author(s): Toby Schneider
  • Date: 2017-06-06 17:21:21 UTC
  • mfrom: (656.1.1)
  • Revision ID: git-v1:266d3647326d3ab512c4ed4f613578d4ed570afb
Merge pull request #28 from GobySoft/2.1-clean-up-gps-surface-bluefin

Simply GPS surfacing on Bluefin vehicles

Show diffs side-by-side

added added

removed removed

Lines of Context:
src/apps/moos/iFrontSeat/legacy_translator.cpp
34
34
 
35
35
FrontSeatLegacyTranslator::FrontSeatLegacyTranslator(iFrontSeat* fs)
36
36
    : ifs_(fs),
37
 
      last_pending_surface_(-1),
38
 
      gps_in_progress_(false),
39
 
      gps_request_id_(0),
40
37
      request_id_(0)
41
38
{
42
39
 
87
84
        ifs_->subscribe("FRONTSEAT_BHVOFF", &FrontSeatLegacyTranslator::handle_mail_frontseat_bhvoff, this);
88
85
        ifs_->subscribe("FRONTSEAT_SILENT", &FrontSeatLegacyTranslator::handle_mail_frontseat_silent, this);
89
86
        ifs_->subscribe("BACKSEAT_ABORT", &FrontSeatLegacyTranslator::handle_mail_backseat_abort, this);
90
 
        ifs_->subscribe("PENDING_SURFACE", &FrontSeatLegacyTranslator::handle_mail_gps_request, this);
91
 
 
92
 
        goby::acomms::connect(&ifs_->frontseat_->signal_command_response,
93
 
                              this, &FrontSeatLegacyTranslator::handle_gps_command_response);   
94
87
    }
95
88
    
96
89
    
144
137
        if(status.global_fix().has_altitude()) 
145
138
            ifs_->publish("NAV_ALTITUDE", status.global_fix().altitude());
146
139
 
 
140
        // surface for GPS variable
 
141
        if(status.global_fix().lat_source() == goby::moos::protobuf::GPS &&
 
142
           status.global_fix().lon_source() == goby::moos::protobuf::GPS)
 
143
        {
 
144
            std::stringstream ss;
 
145
            ss << "Timestamp=" << std::setprecision(15) << status.time();
 
146
            ifs_->publish("GPS_UPDATE_RECEIVED", ss.str());
 
147
        }
147
148
    }    
148
149
 
149
150
 
296
297
        ifs_->publish("BACKSEAT_READY", 0);
297
298
}
298
299
 
299
 
void FrontSeatLegacyTranslator::handle_mail_gps_request(const CMOOSMsg& msg)
300
 
{
301
 
    double pending_surface = msg.GetDouble();
302
 
 
303
 
    // pending surface crossing from positive to negative triggers the GPS request
304
 
    if(pending_surface < 0 && last_pending_surface_ >= 0 && !gps_in_progress_)
305
 
    {
306
 
        // post GPS request
307
 
        gpb::CommandRequest command;
308
 
        command.set_response_requested(true);
309
 
        gps_request_id_ = LEGACY_REQUEST_IDENTIFIER + request_id_++;
310
 
        command.set_request_id(gps_request_id_);
311
 
        gpb::BluefinExtraCommands* bluefin_command = command.MutableExtension(gpb::bluefin_command);
312
 
        bluefin_command->set_command(gpb::BluefinExtraCommands::GPS_REQUEST);
313
 
        publish_command(command);
314
 
        
315
 
        gps_in_progress_ = true;
316
 
    }
317
 
    // if pending surface resets to positive, and we're still getting GPS, cancel it
318
 
    else if(pending_surface >= 0 && gps_in_progress_)
319
 
    {
320
 
        glog.is(DEBUG1) && glog << warn << "Canceling GPS update ... no acknowledgement within max time at surface." << std::endl;
321
 
 
322
 
        gpb::CommandRequest command;
323
 
        command.set_cancel_request_id(gps_request_id_);
324
 
 
325
 
        publish_command(command);
326
 
        
327
 
        gps_in_progress_ = false;        
328
 
    }
329
 
    last_pending_surface_ = pending_surface;
330
 
}
331
 
 
332
 
void FrontSeatLegacyTranslator::handle_gps_command_response(const goby::moos::protobuf::CommandResponse& response)
333
 
{
334
 
    if(gps_in_progress_ && response.request_id() == gps_request_id_)
335
 
    {
336
 
        std::stringstream ss;
337
 
        ss << "Timestamp=" << std::setprecision(15) << goby::common::goby_time<double>();
338
 
        ifs_->publish("GPS_UPDATE_RECEIVED", ss.str());
339
 
        gps_in_progress_ = false;
340
 
    }
341
 
}
342
 
 
343
300
void FrontSeatLegacyTranslator::handle_mail_buoyancy_control(const CMOOSMsg& msg)
344
301
{
345
302