mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-11 19:24:10 +01:00
257 lines
7.3 KiB
C
257 lines
7.3 KiB
C
/**
|
|
******************************************************************************
|
|
* @file pios_ibus.c
|
|
* @author dRonin, http://dRonin.org/, Copyright (C) 2016
|
|
* @addtogroup PIOS PIOS Core hardware abstraction layer
|
|
* @{
|
|
* @addtogroup PIOS_IBus PiOS IBus receiver driver
|
|
* @{
|
|
* @brief Receives and decodes IBus protocol reciever packets
|
|
*****************************************************************************/
|
|
/*
|
|
* 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
|
|
*
|
|
* Additional note on redistribution: The copyright and license notices above
|
|
* must be maintained in each individual source file that is a derivative work
|
|
* of this source file; otherwise redistribution is prohibited.
|
|
*/
|
|
|
|
#include "pios_ibus_priv.h"
|
|
|
|
#ifdef PIOS_INCLUDE_IBUS
|
|
|
|
// private
|
|
#define PIOS_IBUS_CHANNELS 10
|
|
// 1 sync byte, 1 unknown byte, 10x channels (uint16_t), 8 unknown bytes, 2 crc bytes
|
|
#define PIOS_IBUS_BUFLEN (1 + 1 + PIOS_IBUS_CHANNELS * 2 + 8 + 2)
|
|
#define PIOS_IBUS_SYNCBYTE 0x20
|
|
#define PIOS_IBUS_MAGIC 0x84fd9a39
|
|
|
|
/**
|
|
* @brief IBus receiver driver internal state data
|
|
*/
|
|
struct pios_ibus_dev {
|
|
uint32_t magic;
|
|
int buf_pos;
|
|
int rx_timer;
|
|
int failsafe_timer;
|
|
uint16_t checksum;
|
|
uint16_t channel_data[PIOS_IBUS_CHANNELS];
|
|
uint8_t rx_buf[PIOS_IBUS_BUFLEN];
|
|
};
|
|
|
|
/**
|
|
* @brief Allocates a driver instance
|
|
* @retval pios_ibus_dev pointer on success, NULL on failure
|
|
*/
|
|
static struct pios_ibus_dev *PIOS_IBUS_Alloc(void);
|
|
/**
|
|
* @brief Validate a driver instance
|
|
* @param[in] dev device driver instance pointer
|
|
* @retval true on success, false on failure
|
|
*/
|
|
static bool PIOS_IBUS_Validate(const struct pios_ibus_dev *ibus_dev);
|
|
/**
|
|
* @brief Read a channel from the last received frame
|
|
* @param[in] id Driver instance
|
|
* @param[in] channel 0-based channel index
|
|
* @retval raw channel value, or error value (see pios_rcvr.h)
|
|
*/
|
|
static int32_t PIOS_IBUS_Read(uint32_t id, uint8_t channel);
|
|
/**
|
|
* @brief Set all channels in the last frame buffer to a given value
|
|
* @param[in] dev Driver instance
|
|
* @param[in] value channel value
|
|
*/
|
|
static void PIOS_IBUS_SetAllChannels(struct pios_ibus_dev *ibus_dev, uint16_t value);
|
|
/**
|
|
* @brief Serial receive callback
|
|
* @param[in] context Driver instance handle
|
|
* @param[in] buf Receive buffer
|
|
* @param[in] buf_len Number of bytes available
|
|
* @param[out] headroom Number of bytes remaining in internal buffer
|
|
* @param[out] task_woken Did we awake a task?
|
|
* @retval Number of bytes consumed from the buffer
|
|
*/
|
|
static uint16_t PIOS_IBUS_Receive(uint32_t context, uint8_t *buf, uint16_t buf_len,
|
|
uint16_t *headroom, bool *task_woken);
|
|
/**
|
|
* @brief Reset the internal receive buffer state
|
|
* @param[in] ibus_dev device driver instance pointer
|
|
*/
|
|
static void PIOS_IBUS_ResetBuffer(struct pios_ibus_dev *ibus_dev);
|
|
/**
|
|
* @brief Unpack a frame from the internal receive buffer to the channel buffer
|
|
* @param[in] ibus_dev device driver instance pointer
|
|
*/
|
|
static void PIOS_IBUS_UnpackFrame(struct pios_ibus_dev *ibus_dev);
|
|
/**
|
|
* @brief RTC tick callback
|
|
* @param[in] context Driver instance handle
|
|
*/
|
|
static void PIOS_IBUS_Supervisor(uint32_t context);
|
|
|
|
// public
|
|
const struct pios_rcvr_driver pios_ibus_rcvr_driver = {
|
|
.read = PIOS_IBUS_Read,
|
|
};
|
|
|
|
|
|
static struct pios_ibus_dev *PIOS_IBUS_Alloc(void)
|
|
{
|
|
struct pios_ibus_dev *ibus_dev;
|
|
|
|
ibus_dev = (struct pios_ibus_dev *)pios_malloc(sizeof(*ibus_dev));
|
|
|
|
if (!ibus_dev) {
|
|
return NULL;
|
|
}
|
|
|
|
memset(ibus_dev, 0, sizeof(*ibus_dev));
|
|
ibus_dev->magic = PIOS_IBUS_MAGIC;
|
|
|
|
return ibus_dev;
|
|
}
|
|
|
|
static bool PIOS_IBUS_Validate(const struct pios_ibus_dev *ibus_dev)
|
|
{
|
|
return ibus_dev && ibus_dev->magic == PIOS_IBUS_MAGIC;
|
|
}
|
|
|
|
int32_t PIOS_IBUS_Init(uint32_t *ibus_id, const struct pios_com_driver *driver,
|
|
uint32_t lower_id)
|
|
{
|
|
struct pios_ibus_dev *ibus_dev = PIOS_IBUS_Alloc();
|
|
|
|
if (!ibus_dev) {
|
|
return -1;
|
|
}
|
|
|
|
*ibus_id = (uint32_t)ibus_dev;
|
|
|
|
PIOS_IBUS_SetAllChannels(ibus_dev, PIOS_RCVR_INVALID);
|
|
|
|
if (!PIOS_RTC_RegisterTickCallback(PIOS_IBUS_Supervisor, *ibus_id)) {
|
|
PIOS_Assert(0);
|
|
}
|
|
|
|
(driver->bind_rx_cb)(lower_id, PIOS_IBUS_Receive, *ibus_id);
|
|
|
|
return 0;
|
|
}
|
|
|
|
static int32_t PIOS_IBUS_Read(uint32_t context, uint8_t channel)
|
|
{
|
|
if (channel > PIOS_IBUS_CHANNELS) {
|
|
return PIOS_RCVR_INVALID;
|
|
}
|
|
|
|
struct pios_ibus_dev *ibus_dev = (struct pios_ibus_dev *)context;
|
|
if (!PIOS_IBUS_Validate(ibus_dev)) {
|
|
return PIOS_RCVR_NODRIVER;
|
|
}
|
|
|
|
return ibus_dev->channel_data[channel];
|
|
}
|
|
|
|
static void PIOS_IBUS_SetAllChannels(struct pios_ibus_dev *ibus_dev, uint16_t value)
|
|
{
|
|
for (int i = 0; i < PIOS_IBUS_CHANNELS; i++) {
|
|
ibus_dev->channel_data[i] = value;
|
|
}
|
|
}
|
|
|
|
static uint16_t PIOS_IBUS_Receive(uint32_t context, uint8_t *buf, uint16_t buf_len,
|
|
uint16_t *headroom, bool *task_woken)
|
|
{
|
|
struct pios_ibus_dev *ibus_dev = (struct pios_ibus_dev *)context;
|
|
|
|
if (!PIOS_IBUS_Validate(ibus_dev)) {
|
|
goto out_fail;
|
|
}
|
|
|
|
for (int i = 0; i < buf_len; i++) {
|
|
if (ibus_dev->buf_pos == 0 && buf[i] != PIOS_IBUS_SYNCBYTE) {
|
|
continue;
|
|
}
|
|
|
|
ibus_dev->rx_buf[ibus_dev->buf_pos++] = buf[i];
|
|
if (ibus_dev->buf_pos <= PIOS_IBUS_BUFLEN - 2) {
|
|
ibus_dev->checksum -= buf[i];
|
|
} else if (ibus_dev->buf_pos == PIOS_IBUS_BUFLEN) {
|
|
PIOS_IBUS_UnpackFrame(ibus_dev);
|
|
}
|
|
}
|
|
|
|
ibus_dev->rx_timer = 0;
|
|
|
|
*headroom = PIOS_IBUS_BUFLEN - ibus_dev->buf_pos;
|
|
*task_woken = false;
|
|
return buf_len;
|
|
|
|
out_fail:
|
|
*headroom = 0;
|
|
*task_woken = false;
|
|
return 0;
|
|
}
|
|
|
|
static void PIOS_IBUS_ResetBuffer(struct pios_ibus_dev *ibus_dev)
|
|
{
|
|
ibus_dev->checksum = 0xffff;
|
|
ibus_dev->buf_pos = 0;
|
|
}
|
|
|
|
static void PIOS_IBUS_UnpackFrame(struct pios_ibus_dev *ibus_dev)
|
|
{
|
|
uint16_t rxsum = ibus_dev->rx_buf[PIOS_IBUS_BUFLEN - 1] << 8 |
|
|
ibus_dev->rx_buf[PIOS_IBUS_BUFLEN - 2];
|
|
|
|
if (ibus_dev->checksum != rxsum) {
|
|
goto out_fail;
|
|
}
|
|
|
|
uint16_t *chan = (uint16_t *)&ibus_dev->rx_buf[2];
|
|
for (int i = 0; i < PIOS_IBUS_CHANNELS; i++) {
|
|
ibus_dev->channel_data[i] = *chan++;
|
|
}
|
|
|
|
ibus_dev->failsafe_timer = 0;
|
|
|
|
out_fail:
|
|
PIOS_IBUS_ResetBuffer(ibus_dev);
|
|
}
|
|
|
|
static void PIOS_IBUS_Supervisor(uint32_t context)
|
|
{
|
|
struct pios_ibus_dev *ibus_dev = (struct pios_ibus_dev *)context;
|
|
|
|
PIOS_Assert(PIOS_IBUS_Validate(ibus_dev));
|
|
|
|
if (++ibus_dev->rx_timer > 3) {
|
|
PIOS_IBUS_ResetBuffer(ibus_dev);
|
|
}
|
|
|
|
if (++ibus_dev->failsafe_timer > 32) {
|
|
PIOS_IBUS_SetAllChannels(ibus_dev, PIOS_RCVR_TIMEOUT);
|
|
}
|
|
}
|
|
|
|
#endif // PIOS_INCLUDE_IBUS
|
|
|
|
/**
|
|
* @}
|
|
* @}
|
|
*/
|