Goby v2
bluefin_incoming.cpp
1 // Copyright 2009-2018 Toby Schneider (http://gobysoft.org/index.wt/people/toby)
2 // GobySoft, LLC (2013-)
3 // Massachusetts Institute of Technology (2007-2014)
4 // Community contributors (see AUTHORS file)
5 //
6 //
7 // This file is part of the Goby Underwater Autonomy Project Libraries
8 // ("The Goby Libraries").
9 //
10 // The Goby Libraries are free software: you can redistribute them and/or modify
11 // them under the terms of the GNU Lesser General Public License as published by
12 // the Free Software Foundation, either version 2.1 of the License, or
13 // (at your option) any later version.
14 //
15 // The Goby Libraries are distributed in the hope that they will be useful,
16 // but WITHOUT ANY WARRANTY; without even the implied warranty of
17 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 // GNU Lesser General Public License for more details.
19 //
20 // You should have received a copy of the GNU Lesser General Public License
21 // along with Goby. If not, see <http://www.gnu.org/licenses/>.
22 
23 #include "goby/common/logger.h"
24 
25 #include "bluefin.h"
26 
27 namespace gpb = goby::moos::protobuf;
28 using goby::glog;
31 using namespace goby::common::logger;
32 using namespace goby::common::tcolor;
33 
34 void BluefinFrontSeat::bfack(const goby::util::NMEASentence& nmea)
35 {
36  frontseat_providing_data_ = true;
37  last_frontseat_data_time_ = goby_time<double>();
38 
39  enum
40  {
41  TIMESTAMP = 1,
42  COMMAND_NAME = 2,
43  TIMESTAMP_OF_COMMAND = 3,
44  BEHAVIOR_INSERT_ID = 4,
45  ACK_STATUS = 5,
46  FUTURE_USE = 6,
47  DESCRIPTION = 7,
48  };
49 
50  enum AckStatus
51  {
52  INVALID_REQUEST = 0,
53  REQUEST_UNSUCCESSFULLY_PROCESSED = 1,
54  REQUEST_SUCCESSFULLY_PROCESSED = 2,
55  REQUEST_PENDING = 3
56  };
57 
58  AckStatus status = static_cast<AckStatus>(nmea.as<int>(ACK_STATUS));
59 
60  std::string acked_sentence = nmea.at(COMMAND_NAME);
61 
62  switch (status)
63  {
64  case INVALID_REQUEST:
65  glog.is(DEBUG1) && glog << warn << "Huxley reports that we sent an invalid "
66  << acked_sentence << " request." << std::endl;
67  break;
68  case REQUEST_UNSUCCESSFULLY_PROCESSED:
69  glog.is(DEBUG1) && glog << warn
70  << "Huxley reports that it unsuccessfully processed our "
71  << acked_sentence << " request: "
72  << "\"" << nmea.at(DESCRIPTION) << "\"" << std::endl;
73  break;
74  case REQUEST_SUCCESSFULLY_PROCESSED: break;
75  case REQUEST_PENDING:
76  glog.is(DEBUG1) && glog << "Huxley reports that our " << acked_sentence
77  << " request is pending." << std::endl;
78  if (!out_.empty())
79  pending_.push_back(out_.front().message());
80  break;
81  }
82 
83  boost::to_upper(acked_sentence);
84  // we expect it to be the front of either the out_ or pending_ queues
85  if (!out_.empty() && boost::iequals(out_.front().sentence_id(), acked_sentence))
86  {
87  out_.pop_front();
88  }
89  else if (!pending_.empty() && boost::iequals(pending_.front().sentence_id(), acked_sentence))
90  {
91  pending_.pop_front();
92  }
93  else
94  {
95  glog.is(DEBUG1) && glog << warn << "Received NMEA Ack for message that was the front "
96  << "of neither the outgoing or pending queue. Clearing our queues "
97  "and attempting to carry on ..."
98  << std::endl;
99  out_.clear();
100  pending_.clear();
101  return;
102  }
103 
104  // handle response to outstanding requests
105  if (status != REQUEST_PENDING)
106  {
107  gpb::BluefinExtraCommands::BluefinCommand type = gpb::BluefinExtraCommands::UNKNOWN_COMMAND;
108  switch (sentence_id_map_.left.at(acked_sentence))
109  {
110  case RMB: type = gpb::BluefinExtraCommands::DESIRED_COURSE; break;
111  case BOY: type = gpb::BluefinExtraCommands::BUOYANCY_ADJUST; break;
112  case TRM: type = gpb::BluefinExtraCommands::TRIM_ADJUST; break;
113  case SIL: type = gpb::BluefinExtraCommands::SILENT_MODE; break;
114  case RCB: type = gpb::BluefinExtraCommands::CANCEL_CURRENT_BEHAVIOR; break;
115  default: break;
116  }
117 
118  if (outstanding_requests_.count(type))
119  {
120  gpb::CommandResponse response;
121  response.set_request_successful(status == REQUEST_SUCCESSFULLY_PROCESSED);
122  response.set_request_id(outstanding_requests_[type].request_id());
123  if (!response.request_successful())
124  {
125  response.set_error_code(status);
126  response.set_error_string(nmea.at(DESCRIPTION));
127  }
128  outstanding_requests_.erase(type);
129  signal_command_response(response);
130  }
131  }
132  waiting_for_huxley_ = false;
133 }
134 
135 void BluefinFrontSeat::bfmsc(const goby::util::NMEASentence& nmea)
136 {
137  // TODO: See if there is something to the message contents
138  // BF manual says: Arbitrary textual message. Semantics determined by the payload.
139  if (bf_config_.accepting_commands_hook() == BluefinFrontSeatConfig::BFMSC_TRIGGER)
140  frontseat_state_ = gpb::FRONTSEAT_ACCEPTING_COMMANDS;
141 }
142 
143 void BluefinFrontSeat::bfnvg(const goby::util::NMEASentence& nmea)
144 {
145  frontseat_providing_data_ = true;
146  last_frontseat_data_time_ = goby_time<double>();
147 
148  enum
149  {
150  TIMESTAMP = 1,
151  LATITUDE = 2,
152  LAT_HEMISPHERE = 3,
153  LONGITUDE = 4,
154  LON_HEMISPHERE = 5,
155  QUALITY_OF_POSITION = 6,
156  ALTITUDE = 7,
157  DEPTH = 8,
158  HEADING = 9,
159  ROLL = 10,
160  PITCH = 11,
161  COMPUTED_TIMESTAMP = 12
162  };
163 
164  // parse out the message
165  status_.Clear(); // NVG clears the message, NVR sends it
166  status_.set_time(
167  goby::util::as<double>(goby::common::nmea_time2ptime(nmea.at(COMPUTED_TIMESTAMP))));
168 
169  const std::string& lat_string = nmea.at(LATITUDE);
170  if (lat_string.length() > 2)
171  {
172  double lat_deg = goby::util::as<double>(lat_string.substr(0, 2));
173  double lat_min = goby::util::as<double>(lat_string.substr(2, lat_string.size()));
174  double lat = lat_deg + lat_min / 60;
175  status_.mutable_global_fix()->set_lat((nmea.at(LAT_HEMISPHERE) == "S") ? -lat : lat);
176  }
177  else
178  {
179  status_.mutable_global_fix()->set_lat(std::numeric_limits<double>::quiet_NaN());
180  }
181 
182  const std::string& lon_string = nmea.at(LONGITUDE);
183  if (lon_string.length() > 2)
184  {
185  double lon_deg = goby::util::as<double>(lon_string.substr(0, 3));
186  double lon_min = goby::util::as<double>(lon_string.substr(3, nmea.at(4).size()));
187  double lon = lon_deg + lon_min / 60;
188  status_.mutable_global_fix()->set_lon((nmea.at(LON_HEMISPHERE) == "W") ? -lon : lon);
189  }
190  else
191  {
192  status_.mutable_global_fix()->set_lon(std::numeric_limits<double>::quiet_NaN());
193  }
194 
195  if (nmea.as<int>(QUALITY_OF_POSITION) == 1)
196  {
197  status_.mutable_global_fix()->set_lat_source(goby::moos::protobuf::GPS);
198  status_.mutable_global_fix()->set_lon_source(goby::moos::protobuf::GPS);
199  }
200 
201  status_.mutable_global_fix()->set_altitude(nmea.as<double>(ALTITUDE));
202  status_.mutable_global_fix()->set_depth(nmea.as<double>(DEPTH));
203  status_.mutable_pose()->set_heading(nmea.as<double>(HEADING));
204  status_.mutable_pose()->set_roll(nmea.as<double>(ROLL));
205  status_.mutable_pose()->set_pitch(nmea.as<double>(PITCH));
206 }
207 
208 void BluefinFrontSeat::bfnvr(const goby::util::NMEASentence& nmea)
209 {
210  enum
211  {
212  TIMESTAMP = 1,
213  EAST_VELOCITY = 2,
214  NORTH_VELOCITY = 3,
215  DOWN_VELOCITY = 4,
216  PITCH_RATE = 5,
217  ROLL_RATE = 6,
218  YAW_RATE = 7,
219  };
220 
221  double dt =
222  goby::util::as<double>(goby::common::nmea_time2ptime(nmea.at(TIMESTAMP))) - status_.time();
223  double east_speed = nmea.as<double>(EAST_VELOCITY);
224  double north_speed = nmea.as<double>(NORTH_VELOCITY);
225 
226  status_.mutable_pose()->set_pitch_rate(nmea.as<double>(PITCH_RATE));
227  status_.mutable_pose()->set_roll_rate(nmea.as<double>(ROLL_RATE));
228  status_.mutable_pose()->set_heading_rate(nmea.as<double>(YAW_RATE));
229  status_.set_speed(std::sqrt(north_speed * north_speed + east_speed * east_speed));
230 
231  status_.mutable_pose()->set_roll_rate_time_lag(dt);
232  status_.mutable_pose()->set_pitch_rate_time_lag(dt);
233  status_.mutable_pose()->set_heading_rate_time_lag(dt);
234  status_.set_speed_time_lag(dt);
235 
236  // fill in the local X, Y
237  compute_missing(&status_);
238 
240  data.mutable_node_status()->CopyFrom(status_);
241  signal_data_from_frontseat(data);
242 }
243 
244 void BluefinFrontSeat::bfsvs(const goby::util::NMEASentence& nmea)
245 {
246  // If the Bluefin vehicle is equipped with a sound velocity sensor, this message will provide the raw output of that sensor. If not, then an estimated value will be provided.
247 
248  // We don't use this, choosing to calculate it ourselves from the CTD
249 }
250 
251 void BluefinFrontSeat::bfrvl(const goby::util::NMEASentence& nmea)
252 {
253  // Vehicle velocity through water as estimated from thruster RPM, may be empty if no lookup table is implemented (m/s)
254 }
255 
256 void BluefinFrontSeat::bfsht(const goby::util::NMEASentence& nmea)
257 {
258  glog.is(WARN) && glog << "Bluefin sent us the SHT message: they are shutting down!"
259  << std::endl;
260 }
261 
262 void BluefinFrontSeat::bfmbs(const goby::util::NMEASentence& nmea)
263 {
264  // This message is sent when the Bluefin vehicle is just beginning a new behavior in the current mission. It can be used by payloads for record-keeping or to synchronize actions with the current mission. Use of the (d--d) dive file field is considered deprecated in favor of getting the same information from BFMIS. See also the BFPLN message below.
265 
266  enum
267  {
268  TIMESTAMP = 1,
269  CURRENT_DIVE_FILE = 2,
270  DEPRECATED_BEHAVIOR_NUMBER = 3,
271  PAYLOAD_BEHAVIOR_IDENTIFIER = 4,
272  BEHAVIOR_TYPE = 5,
273  };
274 
275  std::string behavior_type = nmea.at(BEHAVIOR_TYPE);
276  glog.is(DEBUG1) && glog << "Bluefin began frontseat mission: " << behavior_type << std::endl;
277 }
278 
279 void BluefinFrontSeat::bfboy(const goby::util::NMEASentence& nmea)
280 {
281  enum
282  {
283  TIMESTAMP = 1,
284  STATUS = 2,
285  ERROR_CODE = 3,
286  DEBUG_STRING = 4,
287  BUOYANCY_ESTIMATE_NEWTONS = 5
288  };
289 
291  gpb::BuoyancyStatus* buoy_status =
292  data.MutableExtension(gpb::bluefin_data)->mutable_buoyancy_status();
293 
294  int status = nmea.as<int>(STATUS);
295  if (gpb::BuoyancyStatus::Status_IsValid(status))
296  buoy_status->set_status(static_cast<gpb::BuoyancyStatus::Status>(status));
297 
298  int error = nmea.as<int>(ERROR_CODE);
299  if (gpb::BuoyancyStatus::Error_IsValid(error))
300  buoy_status->set_error(static_cast<gpb::BuoyancyStatus::Error>(error));
301  buoy_status->set_debug_string(nmea.at(DEBUG_STRING));
302  buoy_status->set_buoyancy_newtons(nmea.as<double>(BUOYANCY_ESTIMATE_NEWTONS));
303  signal_data_from_frontseat(data);
304 }
305 
306 void BluefinFrontSeat::bftrm(const goby::util::NMEASentence& nmea)
307 {
308  enum
309  {
310  TIMESTAMP = 1,
311  STATUS = 2,
312  ERROR_CODE = 3,
313  DEBUG_STRING = 4,
314  PITCH_DEGREES = 5,
315  ROLL_DEGREES = 6
316  };
317 
319  gpb::TrimStatus* trim_status = data.MutableExtension(gpb::bluefin_data)->mutable_trim_status();
320 
321  int status = nmea.as<int>(STATUS);
322  int error = nmea.as<int>(ERROR_CODE);
323 
324  if (gpb::TrimStatus::Status_IsValid(status))
325  trim_status->set_status(static_cast<gpb::TrimStatus::Status>(status));
326  if (gpb::TrimStatus::Error_IsValid(error))
327  trim_status->set_error(static_cast<gpb::TrimStatus::Error>(error));
328  trim_status->set_debug_string(nmea.at(DEBUG_STRING));
329  trim_status->set_pitch_trim_degrees(nmea.as<double>(PITCH_DEGREES));
330  trim_status->set_roll_trim_degrees(nmea.as<double>(ROLL_DEGREES));
331 
332  signal_data_from_frontseat(data);
333 }
334 
335 void BluefinFrontSeat::bfmbe(const goby::util::NMEASentence& nmea)
336 {
337  enum
338  {
339  TIMESTAMP = 1,
340  CURRENT_DIVE_FILE = 2,
341  DEPRECATED_BEHAVIOR_NUMBER = 3,
342  PAYLOAD_BEHAVIOR_IDENTIFIER = 4,
343  BEHAVIOR_TYPE = 5,
344  };
345 
346  std::string behavior_type = nmea.at(BEHAVIOR_TYPE);
347 
348  glog.is(DEBUG1) && glog << "Bluefin ended frontseat mission: " << behavior_type << std::endl;
349 }
350 
351 void BluefinFrontSeat::bftop(const goby::util::NMEASentence& nmea)
352 {
353  // Topside Message (Not Implemented)
354  // Delivery of a message sent from the topside.
355 }
356 
357 void BluefinFrontSeat::bfdvl(const goby::util::NMEASentence& nmea)
358 {
359  // Raw DVL Data
360 }
361 
362 void BluefinFrontSeat::bfmis(const goby::util::NMEASentence& nmea)
363 {
364  std::string running = nmea.at(3);
365  if (running.find("Running") != std::string::npos)
366  {
367  switch (bf_config_.accepting_commands_hook())
368  {
369  case BluefinFrontSeatConfig::BFMIS_RUNNING_TRIGGER:
370  frontseat_state_ = gpb::FRONTSEAT_ACCEPTING_COMMANDS;
371  break;
372 
373  case BluefinFrontSeatConfig::BFCTL_TRIGGER:
374  case BluefinFrontSeatConfig::BFMSC_TRIGGER:
375  if (frontseat_state_ != gpb::FRONTSEAT_ACCEPTING_COMMANDS)
376  frontseat_state_ = gpb::FRONTSEAT_IN_CONTROL;
377  break;
378 
379  default: break;
380  }
381  }
382  else
383  {
384  frontseat_state_ = gpb::FRONTSEAT_IDLE;
385  }
386 }
387 
388 void BluefinFrontSeat::bfctd(const goby::util::NMEASentence& nmea)
389 {
391  goby::moos::protobuf::CTDSample* ctd_sample = data.mutable_ctd_sample();
392 
393  enum
394  {
395  TIMESTAMP_SENT = 1,
396  CONDUCTIVITY = 2,
397  TEMPERATURE = 3,
398  PRESSURE = 4,
399  TIMESTAMP_DATA = 5
400  };
401 
402  // Conductivity (uSiemens/cm -> Siemens/meter)
403  ctd_sample->set_conductivity(nmea.as<double>(CONDUCTIVITY) / 1e4);
404 
405  // Temperature (degrees Celsius)
406  ctd_sample->set_temperature(nmea.as<double>(TEMPERATURE));
407 
408  // Pressure (kPa -> Pascals)
409  ctd_sample->set_pressure(nmea.as<double>(PRESSURE) * 1e3);
410  compute_missing(ctd_sample);
411  signal_data_from_frontseat(data);
412 }
413 
414 void BluefinFrontSeat::bfctl(const goby::util::NMEASentence& nmea)
415 {
416  if (bf_config_.accepting_commands_hook() == BluefinFrontSeatConfig::BFCTL_TRIGGER)
417  {
418  enum
419  {
420  TIMESTAMP = 1,
421  CONTROL = 2
422  };
423 
424  bool control = nmea.as<bool>(CONTROL);
425  if (control)
426  frontseat_state_ = gpb::FRONTSEAT_ACCEPTING_COMMANDS;
427  else if (frontseat_state_ == gpb::FRONTSEAT_ACCEPTING_COMMANDS)
428  frontseat_state_ = gpb::FRONTSEAT_IN_CONTROL;
429  }
430 }
Contains functions for adding color to Terminal window streams.
Definition: term_color.h:54
double goby_time< double >()
Returns current UTC time as seconds and fractional seconds since 1970-01-01 00:00:00.
Definition: time.h:130
ReturnType goby_time()
Returns current UTC time as a boost::posix_time::ptime.
Definition: time.h:104
common::FlexOstream glog
Access the Goby logger through this object.