flight/pios/common/pios_sbus.c

   1 /**
   2  ******************************************************************************
   3  * @addtogroup PIOS PIOS Core hardware abstraction layer
   4  * @{
   5  * @addtogroup PIOS_SBus Futaba S.Bus receiver functions
   6  * @brief Code to read Futaba S.Bus receiver serial stream
   7  * @{
   8  *
   9  * @file       pios_sbus.c
  10  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011.
  11  * @brief      Code to read Futaba S.Bus receiver serial stream
  12  * @see        The GNU Public License (GPL) Version 3
  13  *
  14  *****************************************************************************/
  15 /*
  16  * This program is free software; you can redistribute it and/or modify
  17  * it under the terms of the GNU General Public License as published by
  18  * the Free Software Foundation; either version 3 of the License, or
  19  * (at your option) any later version.
  20  *
  21  * This program is distributed in the hope that it will be useful, but
  22  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  23  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  24  * for more details.
  25  *
  26  * You should have received a copy of the GNU General Public License along
  27  * with this program; if not, write to the Free Software Foundation, Inc.,
  28  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  29  */
  30
  31 #include "pios.h"
  32
  33 #ifdef PIOS_INCLUDE_SBUS
  34
  35 // Define to report number of frames since last dropped instead of weighted ave
  36 #undef SBUS_GOOD_FRAME_COUNT
  37
  38 #include <uavobjectmanager.h>
  39 #include "pios_sbus_priv.h"
  40
  41 /* Forward Declarations */
  42 static int32_t PIOS_SBus_Get(uint32_t rcvr_id, uint8_t channel);
  43 static uint16_t PIOS_SBus_RxInCallback(uint32_t context,
  44                                        uint8_t *buf,
  45                                        uint16_t buf_len,
  46                                        uint16_t *headroom,
  47                                        bool *need_yield);
  48 static void PIOS_SBus_Supervisor(uint32_t sbus_id);
  49 static uint8_t PIOS_SBus_Quality_Get(uint32_t rcvr_id);
  50
  51 /* Local Variables */
  52 const struct pios_rcvr_driver pios_sbus_rcvr_driver = {
  53     .read        = PIOS_SBus_Get,
  54     .get_quality = PIOS_SBus_Quality_Get
  55 };
  56
  57 enum pios_sbus_dev_magic {
  58     PIOS_SBUS_DEV_MAGIC = 0x53427573,
  59 };
  60
  61 struct pios_sbus_state {
  62     uint16_t channel_data[PIOS_SBUS_NUM_INPUTS];
  63     uint8_t  received_data[SBUS_FRAME_LENGTH - 2];
  64     uint8_t  receive_timer;
  65     uint8_t  failsafe_timer;
  66     uint8_t  frame_found;
  67     uint8_t  byte_count;
  68     float    quality;
  69 #ifdef SBUS_GOOD_FRAME_COUNT
  70     uint8_t  frame_count;
  71 #endif /* SBUS_GOOD_FRAME_COUNT */
  72 };
  73
  74 /* With an S.Bus frame rate of 7ms (130Hz) averaging over 26 samples
  75  * gives about a 200ms response.
  76  */
  77 #define SBUS_FL_WEIGHTED_AVE 26
  78
  79 struct pios_sbus_dev {
  80     enum pios_sbus_dev_magic   magic;
  81     const struct pios_sbus_cfg *cfg;
  82     struct pios_sbus_state     state;
  83 };
  84
  85 /* Allocate S.Bus device descriptor */
  86 #if defined(PIOS_INCLUDE_FREERTOS)
  87 static struct pios_sbus_dev *PIOS_SBus_Alloc(void)
  88 {
  89     struct pios_sbus_dev *sbus_dev;
  90
  91     sbus_dev = (struct pios_sbus_dev *)pios_malloc(sizeof(*sbus_dev));
  92     if (!sbus_dev) {
  93         return NULL;
  94     }
  95
  96     sbus_dev->magic = PIOS_SBUS_DEV_MAGIC;
  97     return sbus_dev;
  98 }
  99 #else
 100 static struct pios_sbus_dev pios_sbus_devs[PIOS_SBUS_MAX_DEVS];
 101 static uint8_t pios_sbus_num_devs;
 102 static struct pios_sbus_dev *PIOS_SBus_Alloc(void)
 103 {
 104     struct pios_sbus_dev *sbus_dev;
 105
 106     if (pios_sbus_num_devs >= PIOS_SBUS_MAX_DEVS) {
 107         return NULL;
 108     }
 109
 110     sbus_dev = &pios_sbus_devs[pios_sbus_num_devs++];
 111     sbus_dev->magic = PIOS_SBUS_DEV_MAGIC;
 112
 113     return sbus_dev;
 114 }
 115 #endif /* if defined(PIOS_INCLUDE_FREERTOS) */
 116
 117 /* Validate S.Bus device descriptor */
 118 static bool PIOS_SBus_Validate(struct pios_sbus_dev *sbus_dev)
 119 {
 120     return sbus_dev->magic == PIOS_SBUS_DEV_MAGIC;
 121 }
 122
 123 /* Reset channels in case of lost signal or explicit failsafe receiver flag */
 124 static void PIOS_SBus_ResetChannels(struct pios_sbus_state *state)
 125 {
 126     for (int i = 0; i < PIOS_SBUS_NUM_INPUTS; i++) {
 127         state->channel_data[i] = PIOS_RCVR_TIMEOUT;
 128     }
 129 }
 130
 131 /* Reset S.Bus receiver state */
 132 static void PIOS_SBus_ResetState(struct pios_sbus_state *state)
 133 {
 134     state->receive_timer  = 0;
 135     state->failsafe_timer = 0;
 136     state->frame_found    = 0;
 137     state->quality = 0.0f;
 138 #ifdef SBUS_GOOD_FRAME_COUNT
 139     state->frame_count    = 0;
 140 #endif /* SBUS_GOOD_FRAME_COUNT */
 141     PIOS_SBus_ResetChannels(state);
 142 }
 143
 144 /* Initialise S.Bus receiver interface */
 145 int32_t PIOS_SBus_Init(uint32_t *sbus_id,
 146                        const struct pios_sbus_cfg *cfg,
 147                        const struct pios_com_driver *driver,
 148                        uint32_t lower_id)
 149 {
 150     PIOS_DEBUG_Assert(sbus_id);
 151     PIOS_DEBUG_Assert(cfg);
 152     PIOS_DEBUG_Assert(driver);
 153
 154     struct pios_sbus_dev *sbus_dev;
 155
 156     sbus_dev = (struct pios_sbus_dev *)PIOS_SBus_Alloc();
 157     if (!sbus_dev) {
 158         goto out_fail;
 159     }
 160
 161     /* Bind the configuration to the device instance */
 162     sbus_dev->cfg = cfg;
 163
 164     PIOS_SBus_ResetState(&(sbus_dev->state));
 165
 166     *sbus_id = (uint32_t)sbus_dev;
 167
 168     /* Enable inverter clock and enable the inverter */
 169     (*cfg->gpio_clk_func)(cfg->gpio_clk_periph, ENABLE);
 170     GPIO_Init(cfg->inv.gpio, &cfg->inv.init);
 171     GPIO_WriteBit(cfg->inv.gpio, cfg->inv.init.GPIO_Pin, cfg->gpio_inv_enable);
 172
 173     /* Set comm driver callback */
 174     (driver->bind_rx_cb)(lower_id, PIOS_SBus_RxInCallback, *sbus_id);
 175
 176     if (!PIOS_RTC_RegisterTickCallback(PIOS_SBus_Supervisor, *sbus_id)) {
 177         PIOS_DEBUG_Assert(0);
 178     }
 179
 180     return 0;
 181
 182 out_fail:
 183     return -1;
 184 }
 185
 186 /**
 187  * Get the value of an input channel
 188  * \param[in] channel Number of the channel desired (zero based)
 189  * \output PIOS_RCVR_INVALID channel not available
 190  * \output PIOS_RCVR_TIMEOUT failsafe condition or missing receiver
 191  * \output >=0 channel value
 192  */
 193 static int32_t PIOS_SBus_Get(uint32_t rcvr_id, uint8_t channel)
 194 {
 195     struct pios_sbus_dev *sbus_dev = (struct pios_sbus_dev *)rcvr_id;
 196
 197     if (!PIOS_SBus_Validate(sbus_dev)) {
 198         return PIOS_RCVR_INVALID;
 199     }
 200
 201     /* return error if channel is not available */
 202     if (channel >= PIOS_SBUS_NUM_INPUTS) {
 203         return PIOS_RCVR_INVALID;
 204     }
 205
 206     return sbus_dev->state.channel_data[channel];
 207 }
 208
 209 /**
 210  * Compute channel_data[] from received_data[].
 211  * For efficiency it unrolls first 8 channels without loops and does the
 212  * same for other 8 channels. Also 2 discrete channels will be set.
 213  */
 214 static void PIOS_SBus_UnrollChannels(struct pios_sbus_state *state)
 215 {
 216     uint8_t *s  = state->received_data;
 217     uint16_t *d = state->channel_data;
 218
 219 #define F(v, s) (((v) >> (s)) & 0x7ff)
 220
 221     /* unroll channels 1-8 */
 222     *d++ = F(s[0] | s[1] << 8, 0);
 223     *d++ = F(s[1] | s[2] << 8, 3);
 224     *d++ = F(s[2] | s[3] << 8 | s[4] << 16, 6);
 225     *d++ = F(s[4] | s[5] << 8, 1);
 226     *d++ = F(s[5] | s[6] << 8, 4);
 227     *d++ = F(s[6] | s[7] << 8 | s[8] << 16, 7);
 228     *d++ = F(s[8] | s[9] << 8, 2);
 229     *d++ = F(s[9] | s[10] << 8, 5);
 230
 231     /* unroll channels 9-16 */
 232     *d++ = F(s[11] | s[12] << 8, 0);
 233     *d++ = F(s[12] | s[13] << 8, 3);
 234     *d++ = F(s[13] | s[14] << 8 | s[15] << 16, 6);
 235     *d++ = F(s[15] | s[16] << 8, 1);
 236     *d++ = F(s[16] | s[17] << 8, 4);
 237     *d++ = F(s[17] | s[18] << 8 | s[19] << 16, 7);
 238     *d++ = F(s[19] | s[20] << 8, 2);
 239     *d++ = F(s[20] | s[21] << 8, 5);
 240
 241     /* unroll discrete channels 17 and 18 */
 242     *d++ = (s[22] & SBUS_FLAG_DC1) ? SBUS_VALUE_MAX : SBUS_VALUE_MIN;
 243     *d++ = (s[22] & SBUS_FLAG_DC2) ? SBUS_VALUE_MAX : SBUS_VALUE_MIN;
 244 }
 245
 246 /* Update decoder state processing input byte from the S.Bus stream */
 247 static void PIOS_SBus_UpdateState(struct pios_sbus_state *state, uint8_t b)
 248 {
 249     /* should not process any data until new frame is found */
 250     if (!state->frame_found) {
 251         return;
 252     }
 253
 254     if (state->byte_count == 0) {
 255         if (b != SBUS_SOF_BYTE) {
 256             /* discard the whole frame if the 1st byte is not correct */
 257             state->frame_found = 0;
 258         } else {
 259             /* do not store the SOF byte */
 260             state->byte_count++;
 261         }
 262         return;
 263     }
 264
 265     /* do not store last frame byte as well */
 266     if (state->byte_count < SBUS_FRAME_LENGTH - 1) {
 267         /* store next byte */
 268         state->received_data[state->byte_count - 1] = b;
 269         state->byte_count++;
 270     } else {
 271         if (b == SBUS_EOF_BYTE || (b & SBUS_R7008SB_EOF_COUNTER_MASK) == 0) {
 272 #ifndef SBUS_GOOD_FRAME_COUNT
 273             /* Quality trend is towards 0% by default*/
 274             uint8_t quality_trend = 0;
 275 #endif /* SBUS_GOOD_FRAME_COUNT */
 276
 277             /* full frame received */
 278             uint8_t flags = state->received_data[SBUS_FRAME_LENGTH - 3];
 279             if (flags & SBUS_FLAG_FL) {
 280                 /* frame lost, do not update */
 281 #ifdef SBUS_GOOD_FRAME_COUNT
 282                 state->quality     = state->frame_count;
 283                 state->frame_count = 0;
 284 #endif /* SBUS_GOOD_FRAME_COUNT */
 285             } else {
 286 #ifdef SBUS_GOOD_FRAME_COUNT
 287                 if (++state->frame_count == 255) {
 288                     state->quality = state->frame_count--;
 289                 }
 290 #else /* SBUS_GOOD_FRAME_COUNT */
 291                 /* Quality trend is towards 100% */
 292                 quality_trend = 100;
 293 #endif /* SBUS_GOOD_FRAME_COUNT */
 294                 if (flags & SBUS_FLAG_FS) {
 295                     /* failsafe flag active */
 296                     PIOS_SBus_ResetChannels(state);
 297                 } else {
 298                     /* data looking good */
 299                     PIOS_SBus_UnrollChannels(state);
 300                     state->failsafe_timer = 0;
 301                 }
 302             }
 303 #ifndef SBUS_GOOD_FRAME_COUNT
 304             /* Present quality as a weighted average of good frames */
 305             state->quality = ((state->quality * (SBUS_FL_WEIGHTED_AVE - 1)) +
 306                               quality_trend) / SBUS_FL_WEIGHTED_AVE;
 307 #endif /* SBUS_GOOD_FRAME_COUNT */
 308         } else {
 309             /* discard whole frame */
 310         }
 311
 312         /* prepare for the next frame */
 313         state->frame_found = 0;
 314     }
 315 }
 316
 317 /* Comm byte received callback */
 318 static uint16_t PIOS_SBus_RxInCallback(uint32_t context,
 319                                        uint8_t *buf,
 320                                        uint16_t buf_len,
 321                                        uint16_t *headroom,
 322                                        bool *need_yield)
 323 {
 324     struct pios_sbus_dev *sbus_dev = (struct pios_sbus_dev *)context;
 325
 326     bool valid = PIOS_SBus_Validate(sbus_dev);
 327
 328     PIOS_Assert(valid);
 329
 330     struct pios_sbus_state *state = &(sbus_dev->state);
 331
 332     /* process byte(s) and clear receive timer */
 333     for (uint8_t i = 0; i < buf_len; i++) {
 334         PIOS_SBus_UpdateState(state, buf[i]);
 335         state->receive_timer = 0;
 336     }
 337
 338     /* Always signal that we can accept another byte */
 339     if (headroom) {
 340         *headroom = SBUS_FRAME_LENGTH;
 341     }
 342
 343     /* We never need a yield */
 344     *need_yield = false;
 345
 346     /* Always indicate that all bytes were consumed */
 347     return buf_len;
 348 }
 349
 350 /**
 351  * Input data supervisor is called periodically and provides
 352  * two functions: frame syncing and failsafe triggering.
 353  *
 354  * S.Bus frames come at 7ms (HS) or 14ms (FS) rate at 100000bps.
 355  * RTC timer is running at 625Hz (1.6ms). So with divider 2 it gives
 356  * 3.2ms pause between frames which is good for both S.Bus frame rates.
 357  *
 358  * Data receive function must clear the receive_timer to confirm new
 359  * data reception. If no new data received in 100ms, we must call the
 360  * failsafe function which clears all channels.
 361  */
 362 static void PIOS_SBus_Supervisor(uint32_t sbus_id)
 363 {
 364     struct pios_sbus_dev *sbus_dev = (struct pios_sbus_dev *)sbus_id;
 365
 366     bool valid = PIOS_SBus_Validate(sbus_dev);
 367
 368     PIOS_Assert(valid);
 369
 370     struct pios_sbus_state *state = &(sbus_dev->state);
 371
 372     /* waiting for new frame if no bytes were received in 3.2ms */
 373     if (++state->receive_timer > 2) {
 374         state->frame_found   = 1;
 375         state->byte_count    = 0;
 376         state->receive_timer = 0;
 377     }
 378
 379     /* activate failsafe if no frames have arrived in 102.4ms */
 380     if (++state->failsafe_timer > 64) {
 381         PIOS_SBus_ResetChannels(state);
 382         state->failsafe_timer = 0;
 383     }
 384 }
 385
 386 static uint8_t PIOS_SBus_Quality_Get(uint32_t sbus_id)
 387 {
 388     struct pios_sbus_dev *sbus_dev = (struct pios_sbus_dev *)sbus_id;
 389
 390     bool valid = PIOS_SBus_Validate(sbus_dev);
 391
 392     PIOS_Assert(valid);
 393
 394     struct pios_sbus_state *state = &(sbus_dev->state);
 395
 396     return (uint8_t)(state->quality + 0.5f);
 397 }
 398
 399 #endif /* PIOS_INCLUDE_SBUS */
 400
 401 /**
 402  * @}
 403  * @}
 404  */