/****************************************************************************
*
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. 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.
* 3. Neither the name PX4 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 OWNER 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.
*
****************************************************************************/
/**
* @file safelink.c
*
* Serial protocol encoder / decoder for the <put a good name here>
* simple redundancy protocol (SRP)
*/
#include <nuttx/config.h>
#include <fcntl.h>
#include <unistd.h>
#include <termios.h>
#include <stdbool.h>
#include <drivers/drv_hrt.h>
#include "px4io.h"
#define SAFELINK_FRAME_SIZE (2 + 2 * PX4IO_SERVO_COUNT + 2)
#define SAFELINK_STX1 0xFA
#define SAFELINK_STX2 0x01 /**< version */
static int safelink_fd = -1;
static hrt_abstime last_rx_time;
static hrt_abstime last_frame_time;
static uint8_t frame[SAFELINK_FRAME_SIZE];
static unsigned partial_frame_count;
unsigned safelink_frame_drops;
static bool safelink_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *failsafe, uint16_t max_channels);
int
safelink_init(int fd)
{
safelink_fd = fd;
return OK;
r_safelink_count = 0;
}
void
safelink_output(uint16_t *values, uint16_t num_values)
{
uint8_t buf[SAFELINK_FRAME_SIZE];
buf[0] = SAFELINK_STX1;
buf[1] = SAFELINK_STX2;
for (unsigned i = 0; (i < num_values) && (i < PX4IO_SERVO_COUNT); i++) {
uint8_t *dp = &buf[2 + (2 * i)];
dp[0] = values[i] >> 8;
dp[1] = values[i];
}
buf[SAFELINK_FRAME_SIZE - 2] = 0x11;
buf[SAFELINK_FRAME_SIZE - 1] = 0xFF;
}
bool
safelink_input(uint16_t *values, uint16_t *num_values, bool *failsafe, uint16_t max_channels)
{
ssize_t ret;
hrt_abstime now;
bool decode_ret = false;
/*
* The S.BUS protocol doesn't provide reliable framing,
* so we detect frame boundaries by the inter-frame delay.
*
* The minimum frame spacing is 7ms; with 25 bytes at 100000bps
* frame transmission time is ~2ms.
*
* We expect to only be called when bytes arrive for processing,
* and if an interval of more than 3ms passes between calls,
* the first byte we read will be the first byte of a frame.
*
* In the case where byte(s) are dropped from a frame, this also
* provides a degree of protection. Of course, it would be better
* if we didn't drop bytes...
*/
now = hrt_absolute_time();
if ((now - last_rx_time) > 3000) {
if (partial_frame_count > 0) {
safelink_frame_drops++;
partial_frame_count = 0;
}
}
/*
* Fetch bytes, but no more than we would need to complete
* the current frame.
*/
ret = read(safelink_fd, &frame[partial_frame_count], SAFELINK_FRAME_SIZE - partial_frame_count);
/* if the read failed for any reason, just give up here */
if (ret < 1)
return false;
if (partial_frame_count < SAFELINK_FRAME_SIZE)
decode_ret = false;
safelink_decode(now, values, num_values, failsafe, max_channels);
/* return false as default */
return decode_ret;
}
static bool safelink_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *failsafe, uint16_t max_channels)
{
/* check frame boundary markers to avoid out-of-sync cases */
if ((frame[0] != SAFELINK_STX1) || frame[1] != SAFELINK_STX2) {
safelink_frame_drops++;
return false;
}
/* XXX check checksum here */
/* if we make it down here, we have valid data */
last_frame_time = frame_time;
for (unsigned i = 0; (i < max_channels) && (i < PX4IO_SERVO_COUNT); i++) {
uint8_t *dp = &frame[2 + (2 * i)];
values[i] = (dp[0] << 8) | dp[1];
}
}
