pios_hsum.c

Go to the documentation of this file.

/*
 * 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, see <http://www.gnu.org/licenses/>
 */
/* Project Includes */
#include "pios_hsum_priv.h"

#if defined(PIOS_INCLUDE_HSUM)

#if !defined(PIOS_INCLUDE_RTC)
#error PIOS_INCLUDE_RTC must be used to use HSUM
#endif

/* HSUM frame size and contents definitions */
#define HSUM_HEADER_LENGTH 3
#define HSUM_CRC_LENGTH 2
#define HSUM_MAX_CHANNELS_PER_FRAME 32
#define HSUM_OVERHEAD_LENGTH (HSUM_HEADER_LENGTH+HSUM_CRC_LENGTH)
#define HSUM_MAX_FRAME_LENGTH (HSUM_MAX_CHANNELS_PER_FRAME*2+HSUM_OVERHEAD_LENGTH)

#define HSUM_GRAUPNER_ID 0xA8
#define HSUM_STATUS_LIVING_SUMH 0x00
#define HSUM_STATUS_LIVING_SUMD 0x01
#define HSUM_STATUS_FAILSAFE 0x81

/* Forward Declarations */
static int32_t PIOS_HSUM_Get(uintptr_t rcvr_id, uint8_t channel);
static uint16_t PIOS_HSUM_RxInCallback(uintptr_t context,
                                       uint8_t *buf,
                                       uint16_t buf_len,
                                       uint16_t *headroom,
                                       bool *need_yield);
static void PIOS_HSUM_Supervisor(uintptr_t hsum_id);

/* Local Variables */
const struct pios_rcvr_driver pios_hsum_rcvr_driver = {
        .read = PIOS_HSUM_Get,
};

enum pios_hsum_dev_magic {
        PIOS_HSUM_DEV_MAGIC = 0x4853554D,
};

struct pios_hsum_state {
        uint16_t channel_data[PIOS_HSUM_NUM_INPUTS];
        uint8_t received_data[HSUM_MAX_FRAME_LENGTH];
        uint8_t receive_timer;
        uint8_t failsafe_timer;
        uint8_t frame_found;
        uint8_t tx_connected;
        uint8_t byte_count;
        uint8_t frame_length;
};

struct pios_hsum_dev {
        enum pios_hsum_dev_magic magic;
        const struct pios_hsum_cfg *cfg;
        enum pios_hsum_proto proto;
        struct pios_hsum_state state;
};

/* Allocate HSUM device descriptor */
static struct pios_hsum_dev *PIOS_HSUM_Alloc(void)
{
        struct pios_hsum_dev *hsum_dev;

        hsum_dev = (struct pios_hsum_dev *)PIOS_malloc(sizeof(*hsum_dev));
        if (!hsum_dev)
                return NULL;

        hsum_dev->magic = PIOS_HSUM_DEV_MAGIC;
        return hsum_dev;
}

/* Validate HSUM device descriptor */
static bool PIOS_HSUM_Validate(struct pios_hsum_dev *hsum_dev)
{
        return (hsum_dev->magic == PIOS_HSUM_DEV_MAGIC);
}

/* Reset channels in case of lost signal or explicit failsafe receiver flag */
static void PIOS_HSUM_ResetChannels(struct pios_hsum_dev *hsum_dev)
{
        struct pios_hsum_state *state = &(hsum_dev->state);
        for (int i = 0; i < PIOS_HSUM_NUM_INPUTS; i++) {
                state->channel_data[i] = PIOS_RCVR_TIMEOUT;
        }
}

/* Reset HSUM receiver state */
static void PIOS_HSUM_ResetState(struct pios_hsum_dev *hsum_dev)
{
        struct pios_hsum_state *state = &(hsum_dev->state);
        state->receive_timer = 0;
        state->failsafe_timer = 0;
        state->frame_found = 0;
        state->tx_connected = 0;
        PIOS_HSUM_ResetChannels(hsum_dev);
}

static int PIOS_HSUM_UnrollChannels(struct pios_hsum_dev *hsum_dev)
{
        struct pios_hsum_state *state = &(hsum_dev->state);

        /* check the header and crc for a valid HoTT SUM stream */
        uint8_t vendor = state->received_data[0];
        uint8_t status = state->received_data[1];
        if (vendor != HSUM_GRAUPNER_ID)
                /* Graupner ID was expected */
                goto stream_error;

        switch (status) {
        case HSUM_STATUS_LIVING_SUMH:
        case HSUM_STATUS_LIVING_SUMD:
        case HSUM_STATUS_FAILSAFE:
                /* check crc before processing */
                if (hsum_dev->proto == PIOS_HSUM_PROTO_SUMD) {
                        /* SUMD has 16 bit CCITT CRC */
                        uint16_t crc = 0;
                        uint8_t *s = &(state->received_data[0]);
                        int len = state->byte_count - 2;
                        for (int n = 0; n < len; n++) {
                                crc ^= (uint16_t)s[n] << 8;
                                for (int i = 0; i < 8; i++)
                                        crc = (crc & 0x8000) ? (crc << 1) ^ 0x1021 : (crc << 1);
                        }
                        if (crc ^ (((uint16_t)s[len] << 8) | s[len + 1]))
                                /* wrong crc checksum found */
                                goto stream_error;
                }
                if (hsum_dev->proto == PIOS_HSUM_PROTO_SUMH) {
                        /* SUMH has only 8 bit added CRC */
                        uint8_t crc = 0;
                        uint8_t *s = &(state->received_data[0]);
                        int len = state->byte_count - 1;
                        for (int n = 0; n < len; n++)
                                crc += s[n];
                        if (crc ^ s[len])
                                /* wrong crc checksum found */
                                goto stream_error;
                }
                /* check for a living connect */
                state->tx_connected |= (status != HSUM_STATUS_FAILSAFE);
                break;
        default:
                /* wrong header format */
                goto stream_error;
        }

        /* check initial connection since reset or timeout */
        if (!(state->tx_connected)) {
                /* these are failsafe data without a first connect. ignore it */
                PIOS_HSUM_ResetChannels(hsum_dev);
                return 0;
        }
        
        /* unroll channels */
        uint8_t n_channels = state->received_data[2];
        uint8_t *s = &(state->received_data[3]);
        uint16_t word;

        for (int i = 0; i < HSUM_MAX_CHANNELS_PER_FRAME; i++) {
                if (i < n_channels) {
                        word = ((uint16_t)s[0] << 8) | s[1];
                        s += sizeof(uint16_t);
                        /* save the channel value */
                        if (i < PIOS_HSUM_NUM_INPUTS) {
                                state->channel_data[i] = word;
                        }
                } else
                        /* this channel was not received */
                        state->channel_data[i] = PIOS_RCVR_INVALID;
        }

        PIOS_RCVR_ActiveFromISR();

        /* all channels processed */
        return 0;

stream_error:
        /* either SUMD selected with SUMH stream found, or vice-versa */
        return -1;
}

/* Update decoder state processing input byte from the HoTT stream */
static void PIOS_HSUM_UpdateState(struct pios_hsum_dev *hsum_dev, uint8_t byte)
{
        struct pios_hsum_state *state = &(hsum_dev->state);
        if (state->frame_found) {
                /* receiving the data frame */
                if (state->byte_count < HSUM_MAX_FRAME_LENGTH) {
                        /* store next byte */
                        state->received_data[state->byte_count++] = byte;
                        if (state->byte_count == HSUM_HEADER_LENGTH) {
                                /* 3rd byte contains the number of channels. calculate frame size */
                                state->frame_length = HSUM_OVERHEAD_LENGTH + 2 * byte;
                        }
                        if (state->byte_count == state->frame_length) {
                                /* full frame received - process and wait for new one */
                                if (!PIOS_HSUM_UnrollChannels(hsum_dev))
                                        /* data looking good */
                                        state->failsafe_timer = 0;
                                /* prepare for the next frame */
                                state->frame_found = 0;
                        }
                }
        }
}

/* Initialise HoTT receiver interface */
int32_t PIOS_HSUM_Init(uintptr_t *hsum_id,
                       const struct pios_com_driver *driver,
                       uintptr_t lower_id,
                       enum pios_hsum_proto proto)
{
        PIOS_DEBUG_Assert(hsum_id);
        PIOS_DEBUG_Assert(driver);

        struct pios_hsum_dev *hsum_dev;

        hsum_dev = (struct pios_hsum_dev *)PIOS_HSUM_Alloc();
        if (!hsum_dev)
                return -1;

        /* Bind the configuration to the device instance */
        hsum_dev->proto = proto;

        PIOS_HSUM_ResetState(hsum_dev);

        *hsum_id = (uintptr_t)hsum_dev;

        /* Set comm driver callback */
        (driver->bind_rx_cb)(lower_id, PIOS_HSUM_RxInCallback, *hsum_id);

        if (!PIOS_RTC_RegisterTickCallback(PIOS_HSUM_Supervisor, *hsum_id)) {
                PIOS_DEBUG_Assert(0);
        }

        return 0;
}

/* Comm byte received callback */
static uint16_t PIOS_HSUM_RxInCallback(uintptr_t context,
                                       uint8_t *buf,
                                       uint16_t buf_len,
                                       uint16_t *headroom,
                                       bool *need_yield)
{
        struct pios_hsum_dev *hsum_dev = (struct pios_hsum_dev *)context;

        bool valid = PIOS_HSUM_Validate(hsum_dev);
        PIOS_Assert(valid);

        /* process byte(s) and clear receive timer */
        for (uint8_t i = 0; i < buf_len; i++) {
                PIOS_HSUM_UpdateState(hsum_dev, buf[i]);
                hsum_dev->state.receive_timer = 0;
        }

        /* Always signal that we can accept more data */
        if (headroom)
                *headroom = HSUM_MAX_FRAME_LENGTH;

        /* We never need a yield */
        *need_yield = false;

        /* Always indicate that all bytes were consumed */
        return buf_len;
}

static int32_t PIOS_HSUM_Get(uintptr_t rcvr_id, uint8_t channel)
{
        struct pios_hsum_dev *hsum_dev = (struct pios_hsum_dev *)rcvr_id;

        if (!PIOS_HSUM_Validate(hsum_dev))
                return PIOS_RCVR_INVALID;

        /* return error if channel is not available */
        if (channel >= PIOS_HSUM_NUM_INPUTS)
                return PIOS_RCVR_INVALID;

        /* may also be PIOS_RCVR_TIMEOUT set by other function */
        return hsum_dev->state.channel_data[channel];
}

static void PIOS_HSUM_Supervisor(uintptr_t hsum_id)
{
        struct pios_hsum_dev *hsum_dev = (struct pios_hsum_dev *)hsum_id;

        bool valid = PIOS_HSUM_Validate(hsum_dev);
        PIOS_Assert(valid);

        struct pios_hsum_state *state = &(hsum_dev->state);

        /* waiting for new frame if no bytes were received in 8ms */
        if (++state->receive_timer > 4) {
                state->frame_found = 1;
                state->byte_count = 0;
                state->receive_timer = 0;
                state->frame_length = HSUM_MAX_FRAME_LENGTH;
        }

        /* activate failsafe if no frames have arrived in ~250ms */
        if (++state->failsafe_timer > 156) {
                PIOS_HSUM_ResetChannels(hsum_dev);
                state->failsafe_timer = 0;
                state->tx_connected = 0;
        }
}

#endif  /* PIOS_INCLUDE_HSUM */