mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-05 13:24:11 +01:00
209 lines
6.2 KiB
C
209 lines
6.2 KiB
C
/**
|
|
******************************************************************************
|
|
* @addtogroup PIOS PIOS Core hardware abstraction layer
|
|
* @{
|
|
* @addtogroup PIOS_COM COM MSG layer functions
|
|
* @brief Hardware communication layer
|
|
* @{
|
|
*
|
|
* @file pios_com_msg.c
|
|
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
|
* @brief COM MSG layer functions
|
|
* @see The GNU Public License (GPL) Version 3
|
|
*
|
|
*****************************************************************************/
|
|
/*
|
|
* This program is free software; you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License as published by
|
|
* the Free Software Foundation; either version 3 of the License, or
|
|
* (at your option) any later version.
|
|
*
|
|
* This program is distributed in the hope that it will be useful, but
|
|
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
|
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
|
* for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License along
|
|
* with this program; if not, write to the Free Software Foundation, Inc.,
|
|
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
|
*/
|
|
|
|
#include "pios.h"
|
|
|
|
#ifdef PIOS_INCLUDE_COM_MSG
|
|
|
|
#include "pios_com.h"
|
|
|
|
#define PIOS_COM_MSG_MAX_LEN 63
|
|
|
|
struct pios_com_msg_dev {
|
|
uint32_t lower_id;
|
|
const struct pios_com_driver *driver;
|
|
|
|
uint8_t rx_msg_buffer[PIOS_COM_MSG_MAX_LEN];
|
|
volatile bool rx_msg_full;
|
|
|
|
uint8_t tx_msg_buffer[PIOS_COM_MSG_MAX_LEN];
|
|
volatile bool tx_msg_full;
|
|
};
|
|
|
|
static struct pios_com_msg_dev com_msg_dev;
|
|
|
|
static uint16_t PIOS_COM_MSG_TxOutCallback(uint32_t context, uint8_t *buf, uint16_t buf_len, uint16_t *headroom, bool *need_yield);
|
|
static uint16_t PIOS_COM_MSG_RxInCallback(uint32_t context, uint8_t *buf, uint16_t buf_len, uint16_t *headroom, bool *need_yield);
|
|
|
|
int32_t PIOS_COM_MSG_Init(uint32_t *com_id, const struct pios_com_driver *driver, uint32_t lower_id)
|
|
{
|
|
PIOS_Assert(com_id);
|
|
PIOS_Assert(driver);
|
|
|
|
PIOS_Assert(driver->bind_tx_cb);
|
|
PIOS_Assert(driver->bind_rx_cb);
|
|
|
|
struct pios_com_msg_dev *com_dev = &com_msg_dev;
|
|
|
|
com_dev->driver = driver;
|
|
com_dev->lower_id = lower_id;
|
|
|
|
com_dev->rx_msg_full = false;
|
|
(com_dev->driver->bind_rx_cb)(lower_id, PIOS_COM_MSG_RxInCallback, (uint32_t)com_dev);
|
|
(com_dev->driver->rx_start)(com_dev->lower_id, sizeof(com_dev->rx_msg_buffer));
|
|
|
|
com_dev->tx_msg_full = false;
|
|
(com_dev->driver->bind_tx_cb)(lower_id, PIOS_COM_MSG_TxOutCallback, (uint32_t)com_dev);
|
|
|
|
*com_id = (uint32_t)com_dev;
|
|
return 0;
|
|
}
|
|
|
|
static uint16_t PIOS_COM_MSG_TxOutCallback(uint32_t context, uint8_t *buf, uint16_t buf_len, uint16_t *headroom, __attribute__((unused)) bool *need_yield)
|
|
{
|
|
struct pios_com_msg_dev *com_dev = (struct pios_com_msg_dev *)context;
|
|
|
|
PIOS_Assert(buf);
|
|
PIOS_Assert(buf_len);
|
|
|
|
uint16_t bytes_from_fifo = 0;
|
|
|
|
if (com_dev->tx_msg_full && (buf_len >= sizeof(com_dev->tx_msg_buffer))) {
|
|
/* Room for an entire message, send it */
|
|
memcpy(buf, com_dev->tx_msg_buffer, sizeof(com_dev->tx_msg_buffer));
|
|
bytes_from_fifo = sizeof(com_dev->tx_msg_buffer);
|
|
com_dev->tx_msg_full = false;
|
|
}
|
|
|
|
if (headroom) {
|
|
if (com_dev->tx_msg_full) {
|
|
*headroom = sizeof(com_dev->tx_msg_buffer);
|
|
} else {
|
|
*headroom = 0;
|
|
}
|
|
}
|
|
|
|
return bytes_from_fifo;
|
|
}
|
|
|
|
static uint16_t PIOS_COM_MSG_RxInCallback(uint32_t context, uint8_t *buf, uint16_t buf_len, uint16_t *headroom, __attribute__((unused)) bool *need_yield)
|
|
{
|
|
struct pios_com_msg_dev *com_dev = (struct pios_com_msg_dev *)context;
|
|
|
|
uint16_t bytes_into_fifo = 0;
|
|
|
|
if (!com_dev->rx_msg_full && (buf_len >= sizeof(com_dev->rx_msg_buffer))) {
|
|
memcpy(com_dev->rx_msg_buffer, buf, sizeof(com_dev->rx_msg_buffer));
|
|
bytes_into_fifo = sizeof(com_dev->rx_msg_buffer);
|
|
com_dev->rx_msg_full = true;
|
|
}
|
|
|
|
if (headroom) {
|
|
if (!com_dev->rx_msg_full) {
|
|
*headroom = sizeof(com_dev->rx_msg_buffer);
|
|
} else {
|
|
*headroom = 0;
|
|
}
|
|
}
|
|
|
|
return bytes_into_fifo;
|
|
}
|
|
|
|
int32_t PIOS_COM_MSG_Send(uint32_t com_id, const uint8_t *msg, uint16_t msg_len)
|
|
{
|
|
PIOS_Assert(msg);
|
|
PIOS_Assert(msg_len);
|
|
|
|
struct pios_com_msg_dev *com_dev = (struct pios_com_msg_dev *)com_id;
|
|
|
|
PIOS_Assert(msg_len == sizeof(com_dev->tx_msg_buffer));
|
|
|
|
/* Wait forever for room in the tx buffer */
|
|
while (com_dev->tx_msg_full) {
|
|
/* Kick the transmitter while we wait */
|
|
if (com_dev->driver->tx_start) {
|
|
(com_dev->driver->tx_start)(com_dev->lower_id, sizeof(com_dev->tx_msg_buffer));
|
|
}
|
|
}
|
|
|
|
memcpy((void *)com_dev->tx_msg_buffer, msg, msg_len);
|
|
com_dev->tx_msg_full = true;
|
|
|
|
/* Kick the transmitter now that we've queued our message */
|
|
if (com_dev->driver->tx_start) {
|
|
(com_dev->driver->tx_start)(com_dev->lower_id, sizeof(com_dev->tx_msg_buffer));
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
uint16_t PIOS_COM_MSG_Receive(uint32_t com_id, uint8_t *msg, uint16_t msg_len)
|
|
{
|
|
PIOS_Assert(msg);
|
|
PIOS_Assert(msg_len);
|
|
|
|
struct pios_com_msg_dev *com_dev = (struct pios_com_msg_dev *)com_id;
|
|
|
|
PIOS_Assert(msg_len == sizeof(com_dev->rx_msg_buffer));
|
|
|
|
if (!com_dev->rx_msg_full) {
|
|
/* There's room in our buffer, kick the receiver */
|
|
(com_dev->driver->rx_start)(com_dev->lower_id, sizeof(com_dev->rx_msg_buffer));
|
|
} else {
|
|
memcpy(msg, com_dev->rx_msg_buffer, msg_len);
|
|
com_dev->rx_msg_full = false;
|
|
|
|
return msg_len;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
/**
|
|
* Change the port speed without re-initializing
|
|
* \param[in] port COM port
|
|
* \param[in] baud Requested baud rate
|
|
* \return -1 if port not available
|
|
* \return 0 on success
|
|
*/
|
|
int32_t PIOS_COM_MSG_ChangeBaud(uint32_t com_id, uint32_t baud)
|
|
{
|
|
struct pios_com_msg_dev *com_dev = (struct pios_com_msg_dev *)com_id;
|
|
|
|
if (!com_dev) {
|
|
/* Undefined COM port for this board (see pios_board.c) */
|
|
return -1;
|
|
}
|
|
|
|
/* Invoke the driver function if it exists */
|
|
if (com_dev->driver->set_baud) {
|
|
com_dev->driver->set_baud(com_dev->lower_id, baud);
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
#endif /* PIOS_INCLUDE_COM_MSG */
|
|
|
|
/**
|
|
* @}
|
|
* @}
|
|
*/
|