Goby v2
iver_driver.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 <boost/units/base_units/imperial/foot.hpp>
24 #include <boost/units/base_units/metric/knot.hpp>
25 
26 #include "goby/common/logger.h"
27 #include "goby/util/as.h"
28 #include "goby/util/binary.h"
29 #include "goby/util/linebasedcomms/nmea_sentence.h"
30 
31 #include "iver_driver.h"
32 
33 namespace gpb = goby::moos::protobuf;
34 using goby::glog;
36 using namespace goby::common::logger;
37 
38 const int allowed_skew = 10;
39 
40 extern "C"
41 {
42  FrontSeatInterfaceBase* frontseat_driver_load(iFrontSeatConfig* cfg)
43  {
44  return new IverFrontSeat(*cfg);
45  }
46 }
47 
48 IverFrontSeat::IverFrontSeat(const iFrontSeatConfig& cfg)
49  : FrontSeatInterfaceBase(cfg), iver_config_(cfg.GetExtension(iver_config)),
50  serial_(iver_config_.serial_port(), iver_config_.serial_baud(), "\r\n"),
51  frontseat_providing_data_(false), last_frontseat_data_time_(0),
52  frontseat_state_(gpb::FRONTSEAT_NOT_CONNECTED),
53  reported_mission_mode_(gpb::IverState::IVER_MODE_UNKNOWN)
54 {
55  goby::util::NMEASentence::enforce_talker_length = false;
56 
57  serial_.start();
58 
59  if (iver_config_.has_ntp_serial_port())
60  {
61  ntp_serial_.reset(
62  new goby::util::SerialClient(iver_config_.ntp_serial_port(), 4800, "\r\n"));
63  ntp_serial_->start();
64  }
65 }
66 
67 void IverFrontSeat::loop()
68 {
69  try_receive();
70 
71  goby::util::NMEASentence request_data("$OSD,G,C,S,P,,,,");
72  write(request_data.message());
73 
74  if (goby_time<double>() > last_frontseat_data_time_ + allowed_skew)
75  frontseat_providing_data_ = false;
76 
77  std::string in;
78  while (ntp_serial_ && ntp_serial_->readline(&in))
79  { glog.is(DEBUG2) && glog << "NTP says: " << in << std::endl; } }
80 
81 void IverFrontSeat::try_receive()
82 {
83  std::string in;
84 
85  while (serial_.readline(&in))
86  {
87  boost::trim(in);
88  try
89  {
90  process_receive(in);
91  }
92  catch (std::exception& e)
93  {
94  glog.is(DEBUG1) && glog << warn << "Failed to handle message: " << e.what()
95  << std::endl;
96  }
97  }
98 }
99 
100 void IverFrontSeat::process_receive(const std::string& s)
101 {
102  gpb::FrontSeatRaw raw_msg;
103  raw_msg.set_raw(s);
104  signal_raw_from_frontseat(raw_msg);
105 
106  // parse
107  try
108  {
109  goby::util::NMEASentence nmea(s);
110 
111  if (nmea.sentence_id() == "RMC")
112  {
113  enum RMCFields
114  {
115  TALKER = 0,
116  UTC = 1,
117  VALIDITY = 2,
118  LAT = 3,
119  LAT_HEMI = 4,
120  LON = 5,
121  LON_HEMI = 6,
122  SOG_KNOTS = 7,
123  TRACK_DEGREES = 8,
124  UT_DATE = 9,
125  MAG_VAR_DEG = 10,
126  MAG_VAR_HEMI = 11,
127  RMC_SIZE = 12
128  };
129 
130  if (nmea.size() < RMC_SIZE)
131  throw(goby::util::bad_nmea_sentence("Message too short"));
132 
133  if (ntp_serial_)
134  ntp_serial_->write(nmea.message_cr_nl());
135  }
136  else if (nmea.at(0) == "$OSI")
137  {
138  status_.Clear(); // $OSI clears the message, $C sends it
139 
140  status_.set_time(goby::common::goby_time<double>());
141 
142  enum OSIFields
143  {
144  TALKER = 0,
145  FINMOTOR = 1,
146  MODE = 2,
147  NEXTWP = 3,
148  LATITUDE = 4,
149  LONGITUDE = 5,
150  SPEED = 6,
151  DISTANCETONEXT = 7,
152  ERROR = 8,
153  ALTIMETER = 9,
154  PARKTIME = 10,
155  MAGNETICDECLINATION = 11,
156  CURRENTMISSIONNAME = 12,
157  REMAININGMISSIONTIME = 13,
158  TRUEHEADING = 14,
159  COR_DFS = 15
160  // fields after this appear to change from Remote Helm version 4->5
161  };
162 
163  status_.mutable_global_fix()->set_lat_with_units(nmea.as<double>(LATITUDE) *
164  boost::units::degree::degrees);
165  status_.mutable_global_fix()->set_lon_with_units(nmea.as<double>(LONGITUDE) *
166  boost::units::degree::degrees);
167 
168  static const boost::units::metric::knot_base_unit::unit_type knots;
169  status_.set_speed_with_units(nmea.as<double>(SPEED) * knots);
170 
171  std::string mode_str = nmea.at(MODE);
172  if (mode_str.size() >= 1 && gpb::IverState::IverMissionMode_IsValid(mode_str[0]))
173  {
174  reported_mission_mode_ = static_cast<gpb::IverState::IverMissionMode>(mode_str[0]);
175  glog.is(DEBUG1) &&
176  glog << "Iver mission mode: "
177  << gpb::IverState::IverMissionMode_Name(reported_mission_mode_)
178  << std::endl;
179  }
180  else
181  {
182  glog.is(WARN) && glog << "[Parser]: Invalid mode string [" << mode_str << "]"
183  << std::endl;
184  reported_mission_mode_ = gpb::IverState::IVER_MODE_UNKNOWN;
185  }
186 
187  switch (reported_mission_mode_)
188  {
189  case gpb::IverState::IVER_MODE_UNKNOWN:
190  case gpb::IverState::IVER_MODE_STOPPED:
191  frontseat_state_ = gpb::FRONTSEAT_IDLE;
192  break;
193 
194  case gpb::IverState::IVER_MODE_PARKING:
195  frontseat_state_ = gpb::FRONTSEAT_IN_CONTROL;
196  break;
197 
198  // all these modes can take a backseat command
199  case gpb::IverState::IVER_MODE_NORMAL:
200  case gpb::IverState::IVER_MODE_MANUAL_OVERRIDE:
201  case gpb::IverState::IVER_MODE_MANUAL_PARKING:
202  case gpb::IverState::IVER_MODE_SERVO_MODE:
203  case gpb::IverState::IVER_MODE_MISSION_MODE:
204  // no explicit handshake for frontseat command
205  frontseat_state_ = gpb::FRONTSEAT_ACCEPTING_COMMANDS;
206  break;
207  }
208 
209  static const boost::units::imperial::foot_base_unit::unit_type feet;
210  status_.mutable_global_fix()->set_depth_with_units(
211  nmea.as<double>(COR_DFS) * feet);
212  status_.mutable_global_fix()->set_altitude_with_units(
213  nmea.as<double>(ALTIMETER) * feet);
214  status_.mutable_pose()->set_heading_with_units(
215  nmea.as<double>(TRUEHEADING) * boost::units::degree::degrees);
216 
218  gpb::IverState& iver_state = *fs_data.MutableExtension(gpb::iver_state);
219  iver_state.set_mode(reported_mission_mode_);
220  signal_data_from_frontseat(fs_data);
221  }
222  else if (nmea.at(0).substr(0, 2) == "$C")
223  {
224  // $C82.8P-3.89R-2.63T20.3D3.2*78
225  enum CompassFields
226  {
227  TALKER = 0,
228  HEADING = 1,
229  PITCH = 2,
230  ROLL = 3,
231  TEMP = 4,
232  DEPTH = 5
233  };
234 
235  std::vector<std::string> cfields;
236  boost::split(cfields, nmea.at(0), boost::is_any_of("CPRTD"));
237 
238  status_.mutable_pose()->set_roll_with_units(goby::util::as<double>(cfields.at(ROLL)) *
239  boost::units::degree::degrees);
240  status_.mutable_pose()->set_pitch_with_units(goby::util::as<double>(cfields.at(PITCH)) *
241  boost::units::degree::degrees);
242 
243 
244  compute_missing(&status_);
246  data.mutable_node_status()->CopyFrom(status_);
247  signal_data_from_frontseat(data);
248  frontseat_providing_data_ = true;
249  last_frontseat_data_time_ = goby_time<double>();
250  }
251  else
252  {
253  glog.is(DEBUG1) && glog << "[Parser]: Ignoring sentence: " << s << std::endl;
254  }
255  }
257  {
258  glog.is(WARN) && glog << "[Parser]: Invalid NMEA sentence: " << e.what() << std::endl;
259  }
260 }
261 
262 void IverFrontSeat::send_command_to_frontseat(const gpb::CommandRequest& command)
263 {
264  if (command.HasExtension(gpb::iver_command))
265  {
266  const gpb::IverExtraCommands& iver_command = command.GetExtension(gpb::iver_command);
267  gpb::IverExtraCommands::IverCommand type = iver_command.command();
268  switch (type)
269  {
270  case gpb::IverExtraCommands::UNKNOWN_COMMAND: break;
271  case gpb::IverExtraCommands::START_MISSION:
272  if (iver_command.has_mission() && !iver_command.mission().empty())
273  {
274  goby::util::NMEASentence nmea("$OMSTART", goby::util::NMEASentence::IGNORE);
275  const int ignore_gps = 0;
276  const int ignore_sounder = 0;
277  const int ignore_pressure_transducer = 0;
278  const int mission_type = 0; // normal mission
279  const std::string srp_mission = "";
280  nmea.push_back(ignore_gps);
281  nmea.push_back(ignore_sounder);
282  nmea.push_back(ignore_pressure_transducer);
283  nmea.push_back(mission_type);
284  nmea.push_back(iver_command.mission());
285  nmea.push_back(srp_mission);
286  write(nmea.message());
287  }
288  else
289  {
290  glog.is(DEBUG1) && glog << "Refusing to start empty mission" << std::endl;
291  }
292  break;
293  case gpb::IverExtraCommands::STOP_MISSION:
294  {
295  // flag is always null (0)
296  goby::util::NMEASentence nmea("$OMSTOP,0", goby::util::NMEASentence::IGNORE);
297  write(nmea.message());
298  }
299  break;
300  }
301  }
302 
303  if(command.has_desired_course())
304  {
305  goby::util::NMEASentence nmea("$OMS", goby::util::NMEASentence::IGNORE);
306 
307  // degrees
308  double heading = command.desired_course().heading_with_units() / boost::units::degree::degrees;
309  while(heading >= 360) heading -= 360;
310  while(heading < 0) heading += 360;
311 
312  nmea.push_back(tenths_precision_str(heading));
313  using boost::units::quantity;
314  typedef boost::units::imperial::foot_base_unit::unit_type feet;
315 
316  // Remote Helm switched the depth field from feet -> meters in version 5
317  if (iver_config_.remote_helm_version_major() < 5)
318  nmea.push_back(tenths_precision_str(
319  command.desired_course().depth_with_units<quantity<feet> >().value())); // in feet
320  else
321  nmea.push_back(
322  tenths_precision_str(command.desired_course()
323  .depth_with_units<quantity<boost::units::si::length> >()
324  .value())); // in meters
325 
326  nmea.push_back(tenths_precision_str(iver_config_.max_pitch_angle_degrees())); // in degrees
327  typedef boost::units::metric::knot_base_unit::unit_type knots;
328  nmea.push_back(tenths_precision_str(command.desired_course().speed_with_units<quantity<knots> >().value())); // in knots
329  const int time_out = 5; // seconds
330  nmea.push_back(time_out);
331 
332  write(nmea.message());
333  }
334 }
335 
336 void IverFrontSeat::send_data_to_frontseat(const gpb::FrontSeatInterfaceData& data)
337 {
338  // no data yet to send
339 }
340 
341 void IverFrontSeat::send_raw_to_frontseat(const gpb::FrontSeatRaw& data) { write(data.raw()); }
342 
343 bool IverFrontSeat::frontseat_providing_data() const { return frontseat_providing_data_; }
344 
345 goby::moos::protobuf::FrontSeatState IverFrontSeat::frontseat_state() const
346 {
347  return frontseat_state_;
348 }
349 
350 void IverFrontSeat::write(const std::string& s)
351 {
352  gpb::FrontSeatRaw raw_msg;
353  raw_msg.set_raw(s);
354  signal_raw_to_frontseat(raw_msg);
355 
356  serial_.write(s + "\r\n");
357 }
358 
359 boost::units::quantity<boost::units::si::time> IverFrontSeat::nmea_time_to_seconds(double nmea_time,
360  int nmea_date)
361 {
362  namespace si = boost::units::si;
363 
364  using namespace boost::posix_time;
365  using namespace boost::gregorian;
366  ptime unix_epoch(date(1970, 1, 1), time_duration(0, 0, 0));
367 
368  int hours = nmea_time / 10000;
369  nmea_time -= hours * 10000;
370  int minutes = nmea_time / 100;
371  nmea_time -= minutes * 100;
372  int seconds = nmea_time;
373  long micro_s = (nmea_time - seconds) * 1e6;
374 
375  int day = 0, month = 0, year = 0;
376  // for time warp
377  if (nmea_date > 999999)
378  {
379  day = nmea_date / 100000;
380  nmea_date -= day * 100000;
381  month = nmea_date / 1000;
382  nmea_date -= month * 1000;
383  year = nmea_date;
384  }
385  else
386  {
387  day = nmea_date / 10000;
388  nmea_date -= day * 10000;
389  month = nmea_date / 100;
390  nmea_date -= month * 100;
391  year = nmea_date;
392  }
393 
394  try
395  {
396  ptime given_time(date(year + 2000, month, day),
397  time_duration(hours, minutes, seconds) + microseconds(micro_s));
398 
399  if (given_time == not_a_date_time)
400  {
401  return -1 * si::seconds;
402  }
403  else
404  {
405  date_duration date_diff = given_time.date() - date(1970, 1, 1);
406  time_duration time_diff = given_time.time_of_day();
407 
408  return (date_diff.days() * 24 * 3600 + time_diff.total_seconds() +
409  static_cast<double>(time_diff.fractional_seconds()) /
410  time_duration::ticks_per_second()) *
411  si::seconds;
412  }
413  }
414  catch (std::exception& e)
415  {
416  glog.is(DEBUG1) && glog << "Invalid time: " << e.what() << std::endl;
417  return -1 * si::seconds;
418  }
419 }
420 
421 boost::units::quantity<boost::units::degree::plane_angle>
422 IverFrontSeat::nmea_geo_to_degrees(double nmea_geo, char hemi)
423 {
424  // DDMM.MMMM
425  double deg_int = std::floor(nmea_geo / 1e2);
426  double deg_frac = (nmea_geo - (deg_int * 1e2)) / 60;
427 
428  double deg = std::numeric_limits<double>::quiet_NaN();
429 
430  if (hemi == 'N' || hemi == 'E')
431  deg = (deg_int + deg_frac);
432  else if (hemi == 'S' || hemi == 'W')
433  deg = -(deg_int + deg_frac);
434 
435  return deg * boost::units::degree::degrees;
436 }
provides a basic client for line by line text based communications over a 8N1 tty (such as an RS-232 ...
Definition: serial_client.h:35
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.