flight/pios/stm32f10x/pios_dsm.c

   1 /**
   2  ******************************************************************************
   3  * @addtogroup PIOS PIOS Core hardware abstraction layer
   4  * @{
   5  * @addtogroup PIOS_DSM Spektrum/JR DSMx satellite receiver functions
   6  * @brief Code to bind and read Spektrum/JR DSMx satellite receiver serial stream
   7  * @{
   8  *
   9  * @file       pios_dsm.c
  10  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011.
  11  * @brief      Code bind and read Spektrum/JR DSMx satellite 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_DSM
  34
  35 #include "pios_dsm_priv.h"
  36
  37 // *** UNTESTED CODE ***
  38 #undef DSM_LINK_QUALITY
  39
  40 /* Forward Declarations */
  41 static int32_t PIOS_DSM_Get(uint32_t rcvr_id, uint8_t channel);
  42 static uint8_t PIOS_DSM_Quality_Get(uint32_t rcvr_id);
  43 static uint16_t PIOS_DSM_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_DSM_Supervisor(uint32_t dsm_id);
  49
  50 /* Local Variables */
  51 const struct pios_rcvr_driver pios_dsm_rcvr_driver = {
  52     .read        = PIOS_DSM_Get,
  53     .get_quality = PIOS_DSM_Quality_Get
  54 };
  55
  56 enum pios_dsm_dev_magic {
  57     PIOS_DSM_DEV_MAGIC = 0x44534d78,
  58 };
  59
  60 struct pios_dsm_state {
  61     uint16_t channel_data[PIOS_DSM_NUM_INPUTS];
  62     uint8_t  received_data[DSM_FRAME_LENGTH];
  63     uint8_t  receive_timer;
  64     uint8_t  failsafe_timer;
  65     uint8_t  frame_found;
  66     uint8_t  byte_count;
  67     uint8_t  frames_lost_last;
  68     float    quality;
  69 };
  70
  71 /* With an DSM frame rate of 11ms (90Hz) averaging over 18 samples
  72  * gives about a 200ms response.
  73  */
  74 #define DSM_FL_WEIGHTED_AVE 18
  75
  76 struct pios_dsm_dev {
  77     enum pios_dsm_dev_magic   magic;
  78     const struct pios_dsm_cfg *cfg;
  79     struct pios_dsm_state     state;
  80 };
  81
  82 /* Allocate DSM device descriptor */
  83 #if defined(PIOS_INCLUDE_FREERTOS)
  84 static struct pios_dsm_dev *PIOS_DSM_Alloc(void)
  85 {
  86     struct pios_dsm_dev *dsm_dev;
  87
  88     dsm_dev = (struct pios_dsm_dev *)pios_malloc(sizeof(*dsm_dev));
  89     if (!dsm_dev) {
  90         return NULL;
  91     }
  92
  93     dsm_dev->magic = PIOS_DSM_DEV_MAGIC;
  94     return dsm_dev;
  95 }
  96 #else
  97 static struct pios_dsm_dev pios_dsm_devs[PIOS_DSM_MAX_DEVS];
  98 static uint8_t pios_dsm_num_devs;
  99 static struct pios_dsm_dev *PIOS_DSM_Alloc(void)
 100 {
 101     struct pios_dsm_dev *dsm_dev;
 102
 103     if (pios_dsm_num_devs >= PIOS_DSM_MAX_DEVS) {
 104         return NULL;
 105     }
 106
 107     dsm_dev = &pios_dsm_devs[pios_dsm_num_devs++];
 108     dsm_dev->magic = PIOS_DSM_DEV_MAGIC;
 109
 110     return dsm_dev;
 111 }
 112 #endif /* if defined(PIOS_INCLUDE_FREERTOS) */
 113
 114 /* Validate DSM device descriptor */
 115 static bool PIOS_DSM_Validate(struct pios_dsm_dev *dsm_dev)
 116 {
 117     return dsm_dev->magic == PIOS_DSM_DEV_MAGIC;
 118 }
 119
 120 /* Try to bind DSMx satellite using specified number of pulses */
 121 static void PIOS_DSM_Bind(struct pios_dsm_dev *dsm_dev, uint8_t bind)
 122 {
 123     const struct pios_dsm_cfg *cfg = dsm_dev->cfg;
 124
 125     GPIO_InitTypeDef GPIO_InitStructure;
 126
 127     GPIO_InitStructure.GPIO_Pin   = cfg->bind.init.GPIO_Pin;
 128     GPIO_InitStructure.GPIO_Speed = cfg->bind.init.GPIO_Speed;
 129     GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_IPU;
 130
 131     /* just to limit bind pulses */
 132     if (bind > 10) {
 133         bind = 10;
 134     }
 135
 136     GPIO_Init(cfg->bind.gpio, &cfg->bind.init);
 137
 138     /* RX line, set high */
 139     GPIO_SetBits(cfg->bind.gpio, cfg->bind.init.GPIO_Pin);
 140
 141     /* on CC works up to 140ms, guess bind window is around 20-140ms after power up */
 142     PIOS_DELAY_WaitmS(20);
 143
 144     for (int i = 0; i < bind; i++) {
 145         /* RX line, drive low for 120us */
 146         GPIO_ResetBits(cfg->bind.gpio, cfg->bind.init.GPIO_Pin);
 147         PIOS_DELAY_WaituS(120);
 148         /* RX line, drive high for 120us */
 149         GPIO_SetBits(cfg->bind.gpio, cfg->bind.init.GPIO_Pin);
 150         PIOS_DELAY_WaituS(120);
 151     }
 152     /* RX line, set input and wait for data */
 153     GPIO_Init(cfg->bind.gpio, &GPIO_InitStructure);
 154 }
 155
 156 /* Reset channels in case of lost signal or explicit failsafe receiver flag */
 157 static void PIOS_DSM_ResetChannels(struct pios_dsm_dev *dsm_dev)
 158 {
 159     struct pios_dsm_state *state = &(dsm_dev->state);
 160
 161     for (int i = 0; i < PIOS_DSM_NUM_INPUTS; i++) {
 162         state->channel_data[i] = PIOS_RCVR_TIMEOUT;
 163     }
 164 }
 165
 166 /* Reset DSM receiver state */
 167 static void PIOS_DSM_ResetState(struct pios_dsm_dev *dsm_dev)
 168 {
 169     struct pios_dsm_state *state = &(dsm_dev->state);
 170
 171     state->receive_timer    = 0;
 172     state->failsafe_timer   = 0;
 173     state->frame_found      = 0;
 174     state->quality = 0.0f;
 175     state->frames_lost_last = 0;
 176     PIOS_DSM_ResetChannels(dsm_dev);
 177 }
 178
 179 /**
 180  * Check and unroll complete frame data.
 181  * \output 0 frame data accepted
 182  * \output -1 frame error found
 183  */
 184 static int PIOS_DSM_UnrollChannels(struct pios_dsm_dev *dsm_dev)
 185 {
 186     struct pios_dsm_state *state = &(dsm_dev->state);
 187     /* Fix resolution for detection. */
 188     static uint8_t resolution    = 11;
 189     uint32_t channel_log = 0;
 190
 191     // *** UNTESTED CODE ***
 192 #ifdef DSM_LINK_QUALITY
 193     /* increment the lost frame counter */
 194     uint8_t frames_lost = state->received_data[0];
 195
 196     /* We only get a lost frame count when the next good frame comes in */
 197     /* Present quality as a weighted average of good frames */
 198     /* First consider the bad frames */
 199     for (int i = 0; i < frames_lost - state->frames_lost_last; i++) {
 200         state->quality = (state->quality * (DSM_FL_WEIGHTED_AVE - 1)) /
 201                          DSM_FL_WEIGHTED_AVE;
 202     }
 203     /* And now the good frame */
 204     state->quality = ((state->quality * (DSM_FL_WEIGHTED_AVE - 1)) +
 205                       100) / DSM_FL_WEIGHTED_AVE;
 206
 207     state->frames_lost_last = frames_lost;
 208 #endif /* DSM_LINK_QUALITY */
 209
 210     /* unroll channels */
 211     uint8_t *s    = &(state->received_data[2]);
 212     uint16_t mask = (resolution == 10) ? 0x03ff : 0x07ff;
 213
 214     for (int i = 0; i < DSM_CHANNELS_PER_FRAME; i++) {
 215         uint16_t word = ((uint16_t)s[0] << 8) | s[1];
 216         s += 2;
 217
 218         /* skip empty channel slot */
 219         if (word == 0xffff) {
 220             continue;
 221         }
 222
 223         /* minimal data validation */
 224         if ((i > 0) && (word & DSM_2ND_FRAME_MASK)) {
 225             /* invalid frame data, ignore rest of the frame */
 226             goto stream_error;
 227         }
 228
 229         /* extract and save the channel value */
 230         uint8_t channel_num = (word >> resolution) & 0x0f;
 231         if (channel_num < PIOS_DSM_NUM_INPUTS) {
 232             if (channel_log & (1 << channel_num)) {
 233                 /* Found duplicate. This should happen when in 11 bit */
 234                 /* mode and the data is 10 bits */
 235                 if (resolution == 10) {
 236                     return -1;
 237                 }
 238                 resolution = 10;
 239                 return PIOS_DSM_UnrollChannels(dsm_dev);
 240             }
 241
 242             if ((channel_log & 0xFF) == 0x55) {
 243                 /* This pattern indicates 10 bit pattern */
 244                 if (resolution == 11) {
 245                     return -1;
 246                 }
 247                 resolution = 11;
 248                 return PIOS_DSM_UnrollChannels(dsm_dev);
 249             }
 250
 251             state->channel_data[channel_num] = (word & mask);
 252             /* keep track of this channel */
 253             channel_log |= (1 << channel_num);
 254         }
 255     }
 256
 257     /* all channels processed */
 258     return 0;
 259
 260 stream_error:
 261     /* either DSM2 selected with DSMX stream found, or vice-versa */
 262     return -1;
 263 }
 264
 265 /* Update decoder state processing input byte from the DSMx stream */
 266 static void PIOS_DSM_UpdateState(struct pios_dsm_dev *dsm_dev, uint8_t byte)
 267 {
 268     struct pios_dsm_state *state = &(dsm_dev->state);
 269
 270     if (state->frame_found) {
 271         /* receiving the data frame */
 272         if (state->byte_count < DSM_FRAME_LENGTH) {
 273             /* store next byte */
 274             state->received_data[state->byte_count++] = byte;
 275             if (state->byte_count == DSM_FRAME_LENGTH) {
 276                 /* full frame received - process and wait for new one */
 277                 if (!PIOS_DSM_UnrollChannels(dsm_dev)) {
 278                     /* data looking good */
 279                     state->failsafe_timer = 0;
 280                 }
 281
 282                 /* prepare for the next frame */
 283                 state->frame_found = 0;
 284             }
 285         }
 286     }
 287 }
 288
 289 /* Initialise DSM receiver interface */
 290 int32_t PIOS_DSM_Init(uint32_t *dsm_id,
 291                       const struct pios_dsm_cfg *cfg,
 292                       const struct pios_com_driver *driver,
 293                       uint32_t lower_id,
 294                       uint8_t bind)
 295 {
 296     PIOS_DEBUG_Assert(dsm_id);
 297     PIOS_DEBUG_Assert(cfg);
 298     PIOS_DEBUG_Assert(driver);
 299
 300     struct pios_dsm_dev *dsm_dev;
 301
 302     dsm_dev = (struct pios_dsm_dev *)PIOS_DSM_Alloc();
 303     if (!dsm_dev) {
 304         return -1;
 305     }
 306
 307     /* Bind the configuration to the device instance */
 308     dsm_dev->cfg = cfg;
 309
 310     /* Bind the receiver if requested */
 311     if (bind) {
 312         PIOS_DSM_Bind(dsm_dev, bind);
 313     }
 314
 315     PIOS_DSM_ResetState(dsm_dev);
 316
 317     *dsm_id = (uint32_t)dsm_dev;
 318
 319     /* Set comm driver callback */
 320     (driver->bind_rx_cb)(lower_id, PIOS_DSM_RxInCallback, *dsm_id);
 321
 322     if (!PIOS_RTC_RegisterTickCallback(PIOS_DSM_Supervisor, *dsm_id)) {
 323         PIOS_DEBUG_Assert(0);
 324     }
 325
 326     return 0;
 327 }
 328
 329 /* Comm byte received callback */
 330 static uint16_t PIOS_DSM_RxInCallback(uint32_t context,
 331                                       uint8_t *buf,
 332                                       uint16_t buf_len,
 333                                       uint16_t *headroom,
 334                                       bool *need_yield)
 335 {
 336     struct pios_dsm_dev *dsm_dev = (struct pios_dsm_dev *)context;
 337
 338     bool valid = PIOS_DSM_Validate(dsm_dev);
 339
 340     PIOS_Assert(valid);
 341
 342     /* process byte(s) and clear receive timer */
 343     for (uint8_t i = 0; i < buf_len; i++) {
 344         PIOS_DSM_UpdateState(dsm_dev, buf[i]);
 345         dsm_dev->state.receive_timer = 0;
 346     }
 347
 348     /* Always signal that we can accept another byte */
 349     if (headroom) {
 350         *headroom = DSM_FRAME_LENGTH;
 351     }
 352
 353     /* We never need a yield */
 354     *need_yield = false;
 355
 356     /* Always indicate that all bytes were consumed */
 357     return buf_len;
 358 }
 359
 360 /**
 361  * Get the value of an input channel
 362  * \param[in] channel Number of the channel desired (zero based)
 363  * \output PIOS_RCVR_INVALID channel not available
 364  * \output PIOS_RCVR_TIMEOUT failsafe condition or missing receiver
 365  * \output >=0 channel value
 366  */
 367 static int32_t PIOS_DSM_Get(uint32_t rcvr_id, uint8_t channel)
 368 {
 369     struct pios_dsm_dev *dsm_dev = (struct pios_dsm_dev *)rcvr_id;
 370
 371     if (!PIOS_DSM_Validate(dsm_dev)) {
 372         return PIOS_RCVR_INVALID;
 373     }
 374
 375     /* return error if channel is not available */
 376     if (channel >= PIOS_DSM_NUM_INPUTS) {
 377         return PIOS_RCVR_INVALID;
 378     }
 379
 380     /* may also be PIOS_RCVR_TIMEOUT set by other function */
 381     return dsm_dev->state.channel_data[channel];
 382 }
 383
 384 /**
 385  * Input data supervisor is called periodically and provides
 386  * two functions: frame syncing and failsafe triggering.
 387  *
 388  * DSM frames come at 11ms or 22ms rate at 115200bps.
 389  * RTC timer is running at 625Hz (1.6ms). So with divider 5 it gives
 390  * 8ms pause between frames which is good for both DSM frame rates.
 391  *
 392  * Data receive function must clear the receive_timer to confirm new
 393  * data reception. If no new data received in 100ms, we must call the
 394  * failsafe function which clears all channels.
 395  */
 396 static void PIOS_DSM_Supervisor(uint32_t dsm_id)
 397 {
 398     struct pios_dsm_dev *dsm_dev = (struct pios_dsm_dev *)dsm_id;
 399
 400     bool valid = PIOS_DSM_Validate(dsm_dev);
 401
 402     PIOS_Assert(valid);
 403
 404     struct pios_dsm_state *state = &(dsm_dev->state);
 405
 406     /* waiting for new frame if no bytes were received in 8ms */
 407     if (++state->receive_timer > 4) {
 408         state->frame_found   = 1;
 409         state->byte_count    = 0;
 410         state->receive_timer = 0;
 411     }
 412
 413     /* activate failsafe if no frames have arrived in 102.4ms */
 414     if (++state->failsafe_timer > 64) {
 415         PIOS_DSM_ResetChannels(dsm_dev);
 416         state->failsafe_timer = 0;
 417     }
 418 }
 419
 420 static uint8_t PIOS_DSM_Quality_Get(uint32_t dsm_id)
 421 {
 422     struct pios_dsm_dev *dsm_dev = (struct pios_dsm_dev *)dsm_id;
 423
 424     bool valid = PIOS_DSM_Validate(dsm_dev);
 425
 426     PIOS_Assert(valid);
 427
 428     struct pios_dsm_state *state = &(dsm_dev->state);
 429
 430     return (uint8_t)(state->quality + 0.5f);
 431 }
 432
 433 #endif /* PIOS_INCLUDE_DSM */
 434
 435 /**
 436  * @}
 437  * @}
 438  */