mavlink_comm.h

Go to the documentation of this file.

/*
 * Copyright (c) 2017 Daniel Koch and James Jackson, BYU MAGICC Lab.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * * Redistributions of source code must retain the above copyright notice, this
 *   list of conditions and the following disclaimer.
 *
 * * Redistributions in binary form must reproduce the above copyright notice,
 *   this list of conditions and the following disclaimer in the documentation
 *   and/or other materials provided with the distribution.
 *
 * * Neither the name of the copyright holder nor the names of its
 *   contributors may be used to endorse or promote products derived from
 *   this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef MAVROSFLIGHT_MAVLINK_COMM_H
#define MAVROSFLIGHT_MAVLINK_COMM_H

#include <rosflight/mavrosflight/mavlink_bridge.h>
#include <rosflight/mavrosflight/mavlink_listener_interface.h>

#include <boost/asio.hpp>
#include <boost/function.hpp>
#include <boost/thread.hpp>

#include <cstdint>
#include <iostream>
#include <list>
#include <string>
#include <vector>

#define MAVLINK_SERIAL_READ_BUF_SIZE 256

namespace mavrosflight
{
class MavlinkComm
{
public:
  MavlinkComm();

  ~MavlinkComm();

  void open();

  void close();

  void register_mavlink_listener(MavlinkListenerInterface *const listener);

  void unregister_mavlink_listener(MavlinkListenerInterface *const listener);

  void send_message(const mavlink_message_t &msg);

protected:
  virtual bool is_open() = 0;
  virtual void do_open() = 0;
  virtual void do_close() = 0;
  virtual void do_async_read(const boost::asio::mutable_buffers_1 &buffer,
                             boost::function<void(const boost::system::error_code &, size_t)> handler) = 0;
  virtual void do_async_write(const boost::asio::const_buffers_1 &buffer,
                              boost::function<void(const boost::system::error_code &, size_t)> handler) = 0;

  boost::asio::io_service io_service_; 

private:
  //===========================================================================
  // definitions
  //===========================================================================

  struct WriteBuffer
  {
    uint8_t data[MAVLINK_MAX_PACKET_LEN];
    size_t len;
    size_t pos;

    WriteBuffer() : len(0), pos(0) {}

    WriteBuffer(const uint8_t *buf, uint16_t len) : len(len), pos(0)
    {
      assert(len <= MAVLINK_MAX_PACKET_LEN); 
      memcpy(data, buf, len);
    }

    const uint8_t *dpos() const { return data + pos; }

    size_t nbytes() const { return len - pos; }
  };

  typedef boost::lock_guard<boost::recursive_mutex> mutex_lock;

  //===========================================================================
  // methods
  //===========================================================================

  void async_read();

  void async_read_end(const boost::system::error_code &error, size_t bytes_transferred);

  void async_write(bool check_write_state);

  void async_write_end(const boost::system::error_code &error, size_t bytes_transferred);

  //===========================================================================
  // member variables
  //===========================================================================

  std::vector<MavlinkListenerInterface *> listeners_; 

  boost::thread io_thread_;      
  boost::recursive_mutex mutex_; 

  uint8_t sysid_;
  uint8_t compid_;

  uint8_t read_buf_raw_[MAVLINK_SERIAL_READ_BUF_SIZE];

  mavlink_message_t msg_in_;
  mavlink_status_t status_in_;

  std::list<WriteBuffer *> write_queue_; 
  bool write_in_progress_;               
};

} // namespace mavrosflight

#endif // MAVROSFLIGHT_MAVLINK_COMM_H