Handles the communication between host computer and the HighLevel Processor (HLP) of the AscTec Autopilot board. More...
#include <comm.h>
Public Member Functions | |
void | close () |
closes the serial port(s) | |
Comm () | |
bool | connect (const std::string &port_rx, const std::string &port_tx, uint32_t baudrate) |
connects to the specified serial port(s) with the given baudrate. The HLP sets it's baudrate automatically. | |
uint32_t | getRxPackets () |
uint32_t | getRxPacketsGood () |
void | loopOnce () |
template<class T > | |
void | registerCallback (uint8_t id, void(T::*cb_func)(uint8_t *, uint32_t), T *p_obj) |
registers a class method that gets called when the packet specified by id was sent from the HLP | |
template<typename T > | |
bool | sendPacket (uint8_t packet_id, const T &data) |
sends a packet to the HLP | |
template<typename T > | |
bool | sendPacketAck (uint8_t packet_id, const T &data, const double &timeout=1.0) |
sends a packet to the HLP and blocks until it got acknowledged or until the timeout is reached | |
~Comm () | |
Private Types | |
typedef boost::asio::serial_port | SerialPort |
typedef boost::shared_ptr < SerialPort > | SerialPortPtr |
Private Member Functions | |
bool | configurePort (SerialPortPtr &serial_port, uint32_t *baudrate) |
bool | connect (SerialPortPtr &serial_port, const std::string &port, uint32_t baudrate) |
uint16_t | crc16 (void *data, uint16_t cnt, uint16_t crc=0xff) |
uint16_t | crc_update (uint16_t crc, uint8_t data) |
void | processPacketAck (uint8_t *buf, uint32_t length) |
void | rxReadCallback (const boost::system::error_code &error, size_t bytes_transferred) |
void | rxReadStart (uint32_t length, uint32_t timeout=0) |
void | rxTimeoutCallback (const boost::system::error_code &error) |
void | serializePacket (uint8_t packet_id, const void *data, const int32_t &packet_size, uint8_t flag=0) |
bool | validateChecksum (uint8_t *buf, uint32_t length, uint16_t checksum) |
bool | waitForPacketAck (uint8_t ack_id, const double &timeout=1.0) |
Private Attributes | |
CommCallbackMap | callbacks_ |
std::vector< uint8_t > | packet_acks_ |
SerialPortPtr | port_rx_ |
std::string | port_rx_name_ |
SerialPortPtr | port_tx_ |
std::string | port_tx_name_ |
uint8_t | rx_buffer_ [256] |
uint32_t | rx_packet_cnt_ |
uint32_t | rx_packet_good_cnt_ |
int | rx_state_ |
boost::asio::deadline_timer | rx_timeout_ |
bool | rx_timeout_occurred_ |
uint8_t | tx_buffer_ [256] |
uint32_t | tx_bytes2send_ |
const int | TX_HEADER_SIZE |
boost::mutex | tx_mutex_ |
boost::asio::io_service | uart_service_ |
boost::thread | uart_thread_ [2] |
Handles the communication between host computer and the HighLevel Processor (HLP) of the AscTec Autopilot board.
Can connect to either one or two (e.g. for separate rx/tx wireless links) serial ports. The default baudrate can be changed up to 921600 baud. Messages can be sent with Comm::sendPacket or Comm::sendPacketAck . Messages are received by setting a callback function with Comm::registerCallback for the message id to receive.
typedef boost::asio::serial_port Comm::SerialPort [private] |
typedef boost::shared_ptr<SerialPort> Comm::SerialPortPtr [private] |
Comm::Comm | ( | ) |
Comm::~Comm | ( | ) |
void Comm::close | ( | ) |
bool Comm::configurePort | ( | SerialPortPtr & | serial_port, |
uint32_t * | baudrate | ||
) | [private] |
bool Comm::connect | ( | SerialPortPtr & | serial_port, |
const std::string & | port, | ||
uint32_t | baudrate | ||
) | [private] |
bool Comm::connect | ( | const std::string & | port_rx, |
const std::string & | port_tx, | ||
uint32_t | baudrate | ||
) |
connects to the specified serial port(s) with the given baudrate. The HLP sets it's baudrate automatically.
The port names can be equal, then it connects only to one serial port (most common operation). It can also connect to different ports for rx and tx. This is useful when e.g. two wireless modules (one for rx, one for tx) such as XBee are used to connect to the helicopter and bandwidth of one link is to low. rx/tx is seen from the host computer running the communication node. Any baudrate can be set. If it doesn't match a standard baudrate, the closest standard baudrate is chosen. If the HLP doesn't "hear" anything from the node for ~10s, it will go into autobaud mode. During connection setup, the node sends 'a' to the HLP, which will then configure its baudrate automatically.
port_rx | port to use for rx |
port_tx | port to use for tx |
baudrate | baudrate to connect with. equal for both ports. The baudrate finally chosen is written to baudrate. |
uint16_t Comm::crc16 | ( | void * | data, |
uint16_t | cnt, | ||
uint16_t | crc = 0xff |
||
) | [private] |
uint16_t Comm::crc_update | ( | uint16_t | crc, |
uint8_t | data | ||
) | [inline, private] |
uint32_t Comm::getRxPackets | ( | ) | [inline] |
uint32_t Comm::getRxPacketsGood | ( | ) | [inline] |
void Comm::loopOnce | ( | ) | [inline] |
void Comm::processPacketAck | ( | uint8_t * | buf, |
uint32_t | length | ||
) | [private] |
void Comm::registerCallback | ( | uint8_t | id, |
void(T::*)(uint8_t *, uint32_t) | cb_func, | ||
T * | p_obj | ||
) | [inline] |
registers a class method that gets called when the packet specified by id was sent from the HLP
Currently, only one method/function can be registered per message id. The method has to be prototyped as follows (see e.g. Comm::processBaudrate):
void class::method(uint8_t *buffer, uint32_t length)
id | the message id to subscribe to |
cb_func | the method that gets called |
p_obj | pointer to the object whose cb_func should get called |
void Comm::rxReadCallback | ( | const boost::system::error_code & | error, |
size_t | bytes_transferred | ||
) | [private] |
void Comm::rxReadStart | ( | uint32_t | length, |
uint32_t | timeout = 0 |
||
) | [private] |
void Comm::rxTimeoutCallback | ( | const boost::system::error_code & | error | ) | [private] |
bool Comm::sendPacket | ( | uint8_t | packet_id, |
const T & | data | ||
) | [inline] |
bool Comm::sendPacketAck | ( | uint8_t | packet_id, |
const T & | data, | ||
const double & | timeout = 1.0 |
||
) | [inline] |
void Comm::serializePacket | ( | uint8_t | packet_id, |
const void * | data, | ||
const int32_t & | packet_size, | ||
uint8_t | flag = 0 |
||
) | [private] |
bool Comm::validateChecksum | ( | uint8_t * | buf, |
uint32_t | length, | ||
uint16_t | checksum | ||
) | [private] |
bool Comm::waitForPacketAck | ( | uint8_t | ack_id, |
const double & | timeout = 1.0 |
||
) | [private] |
CommCallbackMap Comm::callbacks_ [private] |
std::vector<uint8_t> Comm::packet_acks_ [private] |
SerialPortPtr Comm::port_rx_ [private] |
std::string Comm::port_rx_name_ [private] |
SerialPortPtr Comm::port_tx_ [private] |
std::string Comm::port_tx_name_ [private] |
uint8_t Comm::rx_buffer_[256] [private] |
uint32_t Comm::rx_packet_cnt_ [private] |
uint32_t Comm::rx_packet_good_cnt_ [private] |
int Comm::rx_state_ [private] |
boost::asio::deadline_timer Comm::rx_timeout_ [private] |
bool Comm::rx_timeout_occurred_ [private] |
uint8_t Comm::tx_buffer_[256] [private] |
uint32_t Comm::tx_bytes2send_ [private] |
const int Comm::TX_HEADER_SIZE [private] |
boost::mutex Comm::tx_mutex_ [private] |
boost::asio::io_service Comm::uart_service_ [private] |
boost::thread Comm::uart_thread_[2] [private] |