Goby v2
benthos_atm900_driver_fsm.h
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 #ifndef BenthosATM900DriverFSM20161221H
24 #define BenthosATM900DriverFSM20161221H
25 
26 #include <iostream>
27 
28 #include <boost/circular_buffer.hpp>
29 #include <boost/format.hpp>
30 #include <boost/mpl/list.hpp>
31 #include <boost/statechart/custom_reaction.hpp>
32 #include <boost/statechart/deep_history.hpp>
33 #include <boost/statechart/event.hpp>
34 #include <boost/statechart/in_state_reaction.hpp>
35 #include <boost/statechart/simple_state.hpp>
36 #include <boost/statechart/state.hpp>
37 #include <boost/statechart/state_machine.hpp>
38 #include <boost/statechart/transition.hpp>
39 
40 #include "goby/acomms/acomms_constants.h"
41 #include "goby/common/logger.h"
42 #include "goby/common/time.h"
43 #include "goby/util/binary.h"
44 
45 #include "goby/acomms/protobuf/benthos_atm900.pb.h"
46 #include "goby/acomms/protobuf/modem_message.pb.h"
47 
48 namespace goby
49 {
50 namespace acomms
51 {
52 namespace benthos_fsm
53 {
54 // events
56 {
57  std::string line;
58 };
60 {
61 };
62 
64 {
65  EvAck(const std::string& response) : response_(response) {}
66 
67  std::string response_;
68 };
69 
71 {
72 };
74 {
75 };
76 
78 {
79  EvDial(int dest, int rate) : dest_(dest), rate_(rate) {}
80 
81  int dest_;
82  int rate_;
83 };
84 
86 {
87  EvRange(int dest) : dest_(dest) {}
88  int dest_;
89 };
90 
91 struct EvRequestLowPower : boost::statechart::event<EvRequestLowPower>
92 {
93 };
95 {
96 };
97 
99 {
100 };
102 {
103 };
104 
106 {
107 };
108 
109 struct EvTransmitBegun : boost::statechart::event<EvTransmitBegun>
110 {
111 };
112 
114 {
115  EvReceive(const std::string& first) : first_(first) {}
116  std::string first_;
117 };
118 
119 struct EvReceiveComplete : boost::statechart::event<EvReceiveComplete>
120 {
121 };
123 {
124 };
125 struct EvRangingComplete : boost::statechart::event<EvRangingComplete>
126 {
127 };
128 
129 struct Active;
130 struct ReceiveData;
131 struct Command;
132 struct Ready;
133 struct Configure;
134 struct SetClock;
135 struct Dial;
136 struct LowPower;
137 struct Range;
138 
139 struct Online;
140 struct Listen;
141 struct TransmitData;
142 
143 // state machine
144 struct BenthosATM900FSM : boost::statechart::state_machine<BenthosATM900FSM, Active>
145 {
146  public:
147  BenthosATM900FSM(const protobuf::DriverConfig& driver_cfg)
148  : serial_tx_buffer_(SERIAL_BUFFER_CAPACITY), received_(RECEIVED_BUFFER_CAPACITY),
149  driver_cfg_(driver_cfg), data_out_(DATA_BUFFER_CAPACITY)
150  {
151  ++count_;
152  glog_fsm_group_ = "benthosatm900::fsm::" + goby::util::as<std::string>(count_);
153  }
154 
155  void buffer_data_out(const goby::acomms::protobuf::ModemTransmission& msg);
156 
157  // messages for the serial line at next opportunity
158  boost::circular_buffer<std::string>& serial_tx_buffer() { return serial_tx_buffer_; }
159 
160  // received messages to be passed up out of the ModemDriver
161  boost::circular_buffer<protobuf::ModemTransmission>& received() { return received_; }
162 
163  // data that should (eventually) be sent out across the connection
164  boost::circular_buffer<protobuf::ModemTransmission>& data_out() { return data_out_; }
165 
166  const protobuf::DriverConfig& driver_cfg() const { return driver_cfg_; }
167 
168  const std::string& glog_fsm_group() const { return glog_fsm_group_; }
169 
170  private:
171  enum
172  {
173  SERIAL_BUFFER_CAPACITY = 10
174  };
175  enum
176  {
177  RECEIVED_BUFFER_CAPACITY = 10
178  };
179 
180  boost::circular_buffer<std::string> serial_tx_buffer_;
181  boost::circular_buffer<protobuf::ModemTransmission> received_;
182  const protobuf::DriverConfig& driver_cfg_;
183 
184  enum
185  {
186  DATA_BUFFER_CAPACITY = 5
187  };
188  boost::circular_buffer<protobuf::ModemTransmission> data_out_;
189 
190  std::string glog_fsm_group_;
191 
192  static int count_;
193 };
194 
196 {
197  StateNotify(const std::string& name) : name_(name)
198  {
199  glog.is(goby::common::logger::DEBUG1) && glog << group("benthosatm900::fsm") << name_
200  << std::endl;
201  }
202  ~StateNotify()
203  {
204  glog.is(goby::common::logger::DEBUG1) && glog << group("benthosatm900::fsm") << "~" << name_
205  << std::endl;
206  }
207 
208  private:
209  std::string name_;
210 };
211 
212 // Active
213 struct Active : boost::statechart::simple_state<Active, BenthosATM900FSM, Command,
214  boost::statechart::has_deep_history>,
216 {
217  Active() : StateNotify("Active") {}
218  ~Active() {}
219 
220  void in_state_react(const EvRxSerial&);
221 
222  typedef boost::mpl::list<
223  boost::statechart::transition<EvReset, Active>,
224  boost::statechart::in_state_reaction<EvRxSerial, Active, &Active::in_state_react>,
225  boost::statechart::transition<EvReceive, ReceiveData> >
226  reactions;
227 };
228 
229 struct ReceiveData : boost::statechart::state<ReceiveData, BenthosATM900FSM>, StateNotify
230 {
231  ReceiveData(my_context ctx);
232  ~ReceiveData() {}
233 
234  void in_state_react(const EvRxSerial&);
235 
236  typedef boost::mpl::list<
237  boost::statechart::in_state_reaction<EvRxSerial, ReceiveData, &ReceiveData::in_state_react>,
238  boost::statechart::transition<EvReceiveComplete,
239  boost::statechart::deep_history<Command> > >
240  reactions;
241 
243  unsigned reported_size_;
244  std::string encoded_bytes_; // still base 255 encoded
245 };
246 
247 // Command
248 struct Command : boost::statechart::simple_state<Command, Active, Configure,
249  boost::statechart::has_deep_history>,
251 {
252  public:
253  void in_state_react(const EvAck&);
254  void in_state_react(const EvTxSerial&);
255  boost::statechart::result react(const EvConnect&)
256  {
257  if (at_out_.empty() || at_out_.front().second != "+++")
258  {
259  at_out_.clear();
260  return transit<Online>();
261  }
262  else
263  {
264  // connect when trying to send "+++"
265  return discard_event();
266  }
267  }
268 
269  Command() : StateNotify("Command"), at_out_(AT_BUFFER_CAPACITY)
270  {
271  // in case we start up in Online mode - likely as the @OpMode=1 is the default
272  context<Command>().push_at_command("+++");
273  // the modem seems to like to reset the OpMode
274  context<Command>().push_clam_command("@OpMode=0");
275  }
276  ~Command() {}
277 
278  typedef boost::mpl::list<
279  boost::statechart::custom_reaction<EvConnect>,
280  boost::statechart::in_state_reaction<EvAck, Command, &Command::in_state_react>,
281  boost::statechart::in_state_reaction<EvTxSerial, Command, &Command::in_state_react> >
282  reactions;
283 
285  {
286  ATSentenceMeta() : last_send_time_(0), tries_(0) {}
287  double last_send_time_;
288  int tries_;
289  };
290 
291  void push_at_command(std::string cmd)
292  {
293  if (cmd != "+++")
294  cmd = "AT" + cmd;
295 
296  at_out_.push_back(std::make_pair(ATSentenceMeta(), cmd));
297  }
298  void push_clam_command(const std::string& cmd)
299  {
300  at_out_.push_back(std::make_pair(ATSentenceMeta(), cmd));
301  }
302 
303  boost::circular_buffer<std::pair<ATSentenceMeta, std::string> >& at_out() { return at_out_; }
304 
305  private:
306  enum
307  {
308  AT_BUFFER_CAPACITY = 100
309  };
310  boost::circular_buffer<std::pair<ATSentenceMeta, std::string> > at_out_;
311  enum
312  {
313  COMMAND_TIMEOUT_SECONDS = 2
314  };
315 
316  enum
317  {
318  RETRIES_BEFORE_RESET = 3
319  };
320 };
321 
322 struct Configure : boost::statechart::state<Configure, Command>, StateNotify
323 {
324  typedef boost::mpl::list<boost::statechart::transition<EvAtEmpty, SetClock> > reactions;
325 
326  Configure(my_context ctx) : my_base(ctx), StateNotify("Configure")
327  {
328  context<Command>().push_at_command("");
329 
330  // disable local echo to avoid confusing our parser
331  context<Command>().push_clam_command("@P1EchoChar=Dis");
332 
333  if (context<BenthosATM900FSM>().driver_cfg().GetExtension(
334  benthos::protobuf::BenthosATM900DriverConfig::factory_reset))
335  context<Command>().push_clam_command("factory_reset");
336 
337  if (context<BenthosATM900FSM>().driver_cfg().HasExtension(
338  benthos::protobuf::BenthosATM900DriverConfig::config_load))
339  {
340  context<Command>().push_clam_command(
341  "cfg load " + context<BenthosATM900FSM>().driver_cfg().GetExtension(
342  benthos::protobuf::BenthosATM900DriverConfig::config_load));
343  }
344 
345  for (int i = 0, n = context<BenthosATM900FSM>().driver_cfg().ExtensionSize(
346  benthos::protobuf::BenthosATM900DriverConfig::config);
347  i < n; ++i)
348  {
349  context<Command>().push_clam_command(
350  context<BenthosATM900FSM>().driver_cfg().GetExtension(
351  benthos::protobuf::BenthosATM900DriverConfig::config, i));
352  }
353 
354  // ensure serial output is the format we expect
355  context<Command>().push_clam_command("@Prompt=7");
356  context<Command>().push_clam_command("@Verbose=3");
357 
358  // Goby will handle retries
359  context<Command>().push_clam_command("@DataRetry=0");
360 
361  // Send the data immediately after we post it
362  context<Command>().push_clam_command("@FwdDelay=0.05");
363  context<Command>().push_clam_command(
364  "@LocalAddr=" +
365  goby::util::as<std::string>(context<BenthosATM900FSM>().driver_cfg().modem_id()));
366 
367  // Hex format for data
368  context<Command>().push_clam_command("@PrintHex=Ena");
369 
370  // Wake tones are required so the modem will resume from low power at packet receipt
371  context<Command>().push_clam_command("@WakeTones=Ena");
372 
373  // Receive all packets, let Goby deal with discarding them
374  context<Command>().push_clam_command("@RcvAll=Ena");
375 
376  // Show data for bad packets so we can stats
377  context<Command>().push_clam_command("@ShowBadData=Ena");
378 
379  // start up in Command mode after reboot/lowpower resume
380  context<Command>().push_clam_command("@OpMode=0");
381 
382  // store the current configuration for later inspection
383  // context<Command>().push_clam_command("cfg store /ffs/goby.ini");
384  }
385 
386  ~Configure() {}
387 };
388 
389 struct SetClock : boost::statechart::state<SetClock, Command>, StateNotify
390 {
391  typedef boost::mpl::list<boost::statechart::transition<EvAtEmpty, Ready> > reactions;
392 
393  SetClock(my_context ctx) : my_base(ctx), StateNotify("SetClock")
394  {
395  boost::posix_time::ptime p = goby::common::goby_time();
396 
397  std::string date_str = boost::str(boost::format("-d%02d/%02d/%04d") %
398  (int)p.date().month() % p.date().day() % p.date().year());
399  std::string time_str =
400  boost::str(boost::format("-t%02d:%02d:%02d") % p.time_of_day().hours() %
401  p.time_of_day().minutes() % p.time_of_day().seconds());
402 
403  context<Command>().push_clam_command("date " + time_str + " " + date_str);
404  }
405 
406  ~SetClock() {}
407 };
408 
410 {
411  private:
412  void in_state_react(const EvRequestLowPower&) { context<Command>().push_at_command("L"); }
413 
414  public:
415  Ready() : StateNotify("Ready") {}
416  ~Ready() {}
417 
418  typedef boost::mpl::list<
419  boost::statechart::transition<EvDial, Dial>, boost::statechart::transition<EvRange, Range>,
420  boost::statechart::in_state_reaction<EvRequestLowPower, Ready, &Ready::in_state_react>,
421  boost::statechart::transition<EvLowPower, LowPower> >
422  reactions;
423 };
424 
425 struct Dial : boost::statechart::state<Dial, Command>, StateNotify
426 {
427  enum
428  {
429  BENTHOS_BROADCAST_ID = 255
430  };
431  enum
432  {
433  DEFAULT_RATE = 2,
434  RATE_MIN = 2,
435  RATE_MAX = 13
436  };
437 
438  Dial(my_context ctx)
439  : my_base(ctx), StateNotify("Dial"), dest_(BENTHOS_BROADCAST_ID), rate_(DEFAULT_RATE)
440  {
441  if (const EvDial* evdial = dynamic_cast<const EvDial*>(triggering_event()))
442  {
443  dest_ = evdial->dest_;
444  if (dest_ == goby::acomms::BROADCAST_ID)
445  dest_ = BENTHOS_BROADCAST_ID;
446 
447  if (evdial->rate_ >= RATE_MIN && evdial->rate_ <= RATE_MAX)
448  rate_ = evdial->rate_;
449  }
450  context<Command>().push_clam_command("@RemoteAddr=" + goby::util::as<std::string>(dest_));
451  context<Command>().push_clam_command("@TxRate=" + goby::util::as<std::string>(rate_));
452  context<Command>().push_at_command("O");
453  }
454  ~Dial() {}
455 
456  private:
457  int dest_;
458  int rate_;
459 };
460 
461 struct LowPower : boost::statechart::state<LowPower, Command>, StateNotify
462 {
463  LowPower(my_context ctx) : my_base(ctx), StateNotify("LowPower") {}
464  ~LowPower() {}
465 };
466 
467 struct Range : boost::statechart::state<Range, Command>, StateNotify
468 {
469  void in_state_react(const EvRxSerial&);
470 
471  Range(my_context ctx) : my_base(ctx), StateNotify("Range"), dest_(0)
472  {
473  if (const EvRange* ev = dynamic_cast<const EvRange*>(triggering_event()))
474  {
475  dest_ = ev->dest_;
476  }
477  context<Command>().push_at_command("R" + goby::util::as<std::string>(dest_));
478  }
479  ~Range() {}
480 
481  typedef boost::mpl::list<
482  boost::statechart::transition<EvRangingComplete, Ready>,
483  boost::statechart::in_state_reaction<EvRxSerial, Range, &Range::in_state_react> >
484  reactions;
485 
486  private:
487  int dest_;
488 };
489 
490 // Online
491 struct Online : boost::statechart::state<Online, Active, Listen>, StateNotify
492 {
493  Online(my_context ctx) : my_base(ctx), StateNotify("Online") {}
494  ~Online() {}
495 
496  typedef boost::mpl::list<
497  boost::statechart::transition<EvShellPrompt, boost::statechart::deep_history<Command> > >
498  reactions;
499 };
500 
501 struct Listen : boost::statechart::state<Listen, Online>, StateNotify
502 {
503  Listen(my_context ctx) : my_base(ctx), StateNotify("Listen")
504  {
505  if (!context<BenthosATM900FSM>().data_out().empty())
506  post_event(EvTransmit());
507  }
508  ~Listen() {}
509 
510  typedef boost::mpl::list<boost::statechart::transition<EvTransmit, TransmitData> > reactions;
511 };
512 
513 struct TransmitData : boost::statechart::state<TransmitData, Online>, StateNotify
514 {
515  TransmitData(my_context ctx) : my_base(ctx), StateNotify("TransmitData") {}
516  ~TransmitData() {}
517 
518  void in_state_react(const EvTxSerial&);
519  void in_state_react(const EvAck&);
520 
521  typedef boost::mpl::list<
522  boost::statechart::transition<EvTransmitBegun, Ready>,
523  boost::statechart::in_state_reaction<EvTxSerial, TransmitData,
524  &TransmitData::in_state_react>,
525  boost::statechart::in_state_reaction<EvAck, TransmitData, &TransmitData::in_state_react> >
526  reactions;
527 };
528 
529 } // namespace benthos_fsm
530 } // namespace acomms
531 } // namespace goby
532 
533 #endif
ReturnType goby_time()
Returns current UTC time as a boost::posix_time::ptime.
Definition: time.h:104
const int BROADCAST_ID
special modem id for the broadcast destination - no one is assigned this address. Analogous to 192...
common::FlexOstream glog
Access the Goby logger through this object.
The global namespace for the Goby project.