flight/pios/stm32f10x/pios_ppm_out.c

   1 /**
   2  ******************************************************************************
   3  * @addtogroup PIOS PIOS Core hardware abstraction layer
   4  * @{
   5  * @addtogroup   PIOS_PPM PPM Output Functions
   6  * @brief Code to output a PPM receiver signal
   7  * @{
   8  *
   9  * @file       pios_ppm_out.c
  10  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
  11  * @brief      PPM Input functions (STM32 dependent)
  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_PPM_OUT
  34
  35 #include "pios_ppm_out_priv.h"
  36
  37 #define PIOS_PPM_OUT_MAX_DEVS             1
  38 #define PIOS_PPM_OUT_MAX_CHANNELS         8 + 1  // 8 RC channels + Rssi
  39 #define PIOS_PPM_OUT_FRAME_PERIOD_US      22500  // microseconds
  40 #define PIOS_PPM_OUT_HIGH_PULSE_US        400    // microseconds
  41 #define PIOS_PPM_OUT_MIN_CHANNEL_PULSE_US 1000 // microseconds
  42 #define PIOS_PPM_OUT_MAX_CHANNEL_PULSE_US 2000 // microseconds
  43
  44 enum pios_ppm_out_dev_magic {
  45     PIOS_PPM_OUT_DEV_MAGIC = 0x0e0210e2
  46 };
  47
  48 struct pios_ppm_out_dev {
  49     enum pios_ppm_out_dev_magic   magic;
  50     const struct pios_ppm_out_cfg *cfg;
  51
  52     uint32_t TriggeringPeriod;
  53     uint32_t ChannelSum;
  54     uint8_t  NumChannelCounter;
  55     uint16_t ChannelValue[PIOS_PPM_OUT_MAX_CHANNELS];
  56
  57     uint8_t  SupvTimer;
  58     bool     Fresh;
  59     bool     Tracking;
  60     bool     Enabled;
  61 };
  62
  63 static void PIOS_PPM_Out_Supervisor(uint32_t ppm_id);
  64 static void PIOS_PPM_Out_Enable_Disable(struct pios_ppm_out_dev *ppm_dev, bool enable);
  65
  66 static bool PIOS_PPM_Out_validate(struct pios_ppm_out_dev *ppm_dev)
  67 {
  68     return ppm_dev->magic == PIOS_PPM_OUT_DEV_MAGIC;
  69 }
  70
  71 #if defined(PIOS_INCLUDE_FREERTOS)
  72 static struct pios_ppm_out_dev *PIOS_PPM_OUT_alloc(void)
  73 {
  74     struct pios_ppm_out_dev *ppm_dev;
  75
  76     ppm_dev = (struct pios_ppm_out_dev *)pios_malloc(sizeof(*ppm_dev));
  77     if (!ppm_dev) {
  78         return NULL;
  79     }
  80
  81     ppm_dev->magic = PIOS_PPM_OUT_DEV_MAGIC;
  82     return ppm_dev;
  83 }
  84 #else
  85 static struct pios_ppm_out_dev pios_ppm_out_devs[PIOS_PPM_OUT_MAX_DEVS];
  86 static uint8_t pios_ppm_out_num_devs;
  87 static struct pios_ppm_out_dev *PIOS_PPM_alloc(void)
  88 {
  89     struct pios_ppm_out_dev *ppm_dev;
  90
  91     if (pios_ppm_out_num_devs >= PIOS_PPM_OUT_MAX_DEVS) {
  92         return NULL;
  93     }
  94
  95     ppm_dev = &pios_ppm_out_devs[pios_ppm_out_num_devs++];
  96     ppm_dev->magic = PIOS_PPM_OUT_DEV_MAGIC;
  97
  98     return ppm_dev;
  99 }
 100 #endif /* if defined(PIOS_INCLUDE_FREERTOS) */
 101
 102 static void PIOS_PPM_OUT_tim_edge_cb(uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count);
 103 static const struct pios_tim_callbacks tim_out_callbacks = {
 104     .overflow = NULL,
 105     .edge     = PIOS_PPM_OUT_tim_edge_cb,
 106 };
 107
 108 int32_t PIOS_PPM_Out_Init(uint32_t *ppm_out_id, const struct pios_ppm_out_cfg *cfg)
 109 {
 110     PIOS_DEBUG_Assert(ppm_out_id);
 111     PIOS_DEBUG_Assert(cfg);
 112
 113     // Allocate the device structure
 114     struct pios_ppm_out_dev *ppm_dev = (struct pios_ppm_out_dev *)PIOS_PPM_OUT_alloc();
 115     if (!ppm_dev) {
 116         return -1;
 117     }
 118     ppm_dev->magic = PIOS_PPM_OUT_DEV_MAGIC;
 119     *ppm_out_id    = (uint32_t)ppm_dev;
 120
 121     // Bind the configuration to the device instance
 122     ppm_dev->cfg   = cfg;
 123
 124     // Set up the state variables
 125     ppm_dev->TriggeringPeriod  = PIOS_PPM_OUT_HIGH_PULSE_US;
 126     ppm_dev->ChannelSum = 0;
 127     ppm_dev->NumChannelCounter = 0;
 128
 129     // Flush counter variables
 130     for (uint8_t i = 0; i < PIOS_PPM_OUT_MAX_CHANNELS; ++i) {
 131         ppm_dev->ChannelValue[i] = 1000;
 132     }
 133
 134     uint32_t tim_id;
 135     if (PIOS_TIM_InitChannels(&tim_id, cfg->channel, 1, &tim_out_callbacks, (uint32_t)ppm_dev)) {
 136         return -1;
 137     }
 138
 139     // Configure the channels to be in output compare mode
 140     const struct pios_tim_channel *chan = cfg->channel;
 141
 142     /* Set up for output compare function */
 143     switch (chan->timer_chan) {
 144     case TIM_Channel_1:
 145         TIM_OC1Init(chan->timer, &cfg->tim_oc_init);
 146         TIM_OC1PreloadConfig(chan->timer, TIM_OCPreload_Enable);
 147         break;
 148     case TIM_Channel_2:
 149         TIM_OC2Init(chan->timer, &cfg->tim_oc_init);
 150         TIM_OC2PreloadConfig(chan->timer, TIM_OCPreload_Enable);
 151         break;
 152     case TIM_Channel_3:
 153         TIM_OC3Init(chan->timer, &cfg->tim_oc_init);
 154         TIM_OC3PreloadConfig(chan->timer, TIM_OCPreload_Enable);
 155         break;
 156     case TIM_Channel_4:
 157         TIM_OC4Init(chan->timer, &cfg->tim_oc_init);
 158         TIM_OC4PreloadConfig(chan->timer, TIM_OCPreload_Enable);
 159         break;
 160     }
 161
 162     TIM_ARRPreloadConfig(chan->timer, ENABLE);
 163     TIM_CtrlPWMOutputs(chan->timer, ENABLE);
 164     TIM_Cmd(chan->timer, ENABLE);
 165
 166     TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
 167     TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
 168     TIM_TimeBaseStructure.TIM_CounterMode   = TIM_CounterMode_Up;
 169     TIM_TimeBaseStructure.TIM_Prescaler     = (PIOS_MASTER_CLOCK / 1000000) - 1;
 170     TIM_TimeBaseStructure.TIM_Period = ((1000000 / 100) - 1);
 171     TIM_TimeBaseInit(chan->timer, &TIM_TimeBaseStructure);
 172     PIOS_PPM_Out_Enable_Disable(ppm_dev, false);
 173
 174     // Configure the supervisor
 175     ppm_dev->SupvTimer = 0;
 176     ppm_dev->Fresh     = FALSE;
 177     ppm_dev->Tracking  = FALSE;
 178     ppm_dev->Enabled   = FALSE;
 179     if (!PIOS_RTC_RegisterTickCallback(PIOS_PPM_Out_Supervisor, (uint32_t)ppm_dev)) {
 180         PIOS_DEBUG_Assert(0);
 181     }
 182
 183     return 0;
 184 }
 185
 186 void PIOS_PPM_OUT_Set(uint32_t ppm_out_id, uint8_t servo, uint16_t position)
 187 {
 188     struct pios_ppm_out_dev *ppm_dev = (struct pios_ppm_out_dev *)ppm_out_id;
 189
 190     if (!PIOS_PPM_Out_validate(ppm_dev) || (servo >= PIOS_PPM_OUT_MAX_CHANNELS)) {
 191         return;
 192     }
 193
 194     // Don't allow positions that are out of range.
 195     if (position < PIOS_PPM_OUT_MIN_CHANNEL_PULSE_US) {
 196         position = PIOS_PPM_OUT_MIN_CHANNEL_PULSE_US;
 197     }
 198     if (position > PIOS_PPM_OUT_MAX_CHANNEL_PULSE_US) {
 199         position = PIOS_PPM_OUT_MAX_CHANNEL_PULSE_US;
 200     }
 201
 202     // Update the supervisor tracking variables.
 203     ppm_dev->Fresh = TRUE;
 204
 205     // Reenable the TIM if it's been turned off.
 206     if (!ppm_dev->Tracking) {
 207         ppm_dev->Tracking = TRUE;
 208         PIOS_PPM_Out_Enable_Disable(ppm_dev, true);
 209     }
 210
 211     // Update the position
 212     ppm_dev->ChannelValue[servo] = position;
 213 }
 214
 215 static void PIOS_PPM_OUT_tim_edge_cb(__attribute__((unused)) uint32_t tim_id,
 216                                      uint32_t context,
 217                                      __attribute__((unused)) uint8_t chan_idx,
 218                                      __attribute__((unused)) uint16_t count)
 219 {
 220     struct pios_ppm_out_dev *ppm_dev = (struct pios_ppm_out_dev *)context;
 221
 222     if (!PIOS_PPM_Out_validate(ppm_dev)) {
 223         return;
 224     }
 225
 226     // Just return if the device is disabled.
 227     if (!ppm_dev->Enabled) {
 228         return;
 229     }
 230
 231     // Turn off the PPM stream if we are no longer receiving update
 232     // Note: This must happen between frames.
 233     if ((ppm_dev->NumChannelCounter == 0) && !ppm_dev->Tracking) {
 234         // Flush counter variables
 235         for (uint8_t i = 0; i < PIOS_PPM_OUT_MAX_CHANNELS; ++i) {
 236             ppm_dev->ChannelValue[i] = 1000;
 237         }
 238         PIOS_PPM_Out_Enable_Disable(ppm_dev, false);
 239         return;
 240     }
 241
 242     // Finish out the frame if we reached the last channel.
 243     uint32_t pulse_width;
 244     if ((ppm_dev->NumChannelCounter >= PIOS_PPM_OUT_MAX_CHANNELS)) {
 245         pulse_width = PIOS_PPM_OUT_FRAME_PERIOD_US - ppm_dev->ChannelSum;
 246         ppm_dev->NumChannelCounter = 0;
 247         ppm_dev->ChannelSum = 0;
 248     } else {
 249         ppm_dev->ChannelSum += (pulse_width = ppm_dev->ChannelValue[ppm_dev->NumChannelCounter++]);
 250     }
 251
 252     // Initiate the pulse
 253     TIM_SetAutoreload(ppm_dev->cfg->channel->timer, pulse_width - 1);
 254 }
 255
 256 static void PIOS_PPM_Out_Enable_Disable(struct pios_ppm_out_dev *ppm_dev, bool enable)
 257 {
 258     const struct pios_tim_channel *chan = ppm_dev->cfg->channel;
 259     uint32_t trig = enable ? ppm_dev->TriggeringPeriod : 0;
 260     FunctionalState state = enable ? ENABLE : DISABLE;
 261
 262     ppm_dev->Enabled = enable;
 263     switch (chan->timer_chan) {
 264     case TIM_Channel_1:
 265         TIM_ITConfig(chan->timer, TIM_IT_CC1 | TIM_IT_Update, state);
 266         TIM_SetCompare1(chan->timer, trig);
 267         break;
 268     case TIM_Channel_2:
 269         TIM_ITConfig(chan->timer, TIM_IT_CC2 | TIM_IT_Update, state);
 270         TIM_SetCompare2(chan->timer, trig);
 271         break;
 272     case TIM_Channel_3:
 273         TIM_ITConfig(chan->timer, TIM_IT_CC3 | TIM_IT_Update, state);
 274         TIM_SetCompare3(chan->timer, trig);
 275         break;
 276     case TIM_Channel_4:
 277         TIM_ITConfig(chan->timer, TIM_IT_CC4 | TIM_IT_Update, state);
 278         TIM_SetCompare4(chan->timer, trig);
 279         break;
 280     }
 281 }
 282
 283 static void PIOS_PPM_Out_Supervisor(uint32_t ppm_out_id)
 284 {
 285     struct pios_ppm_out_dev *ppm_dev = (struct pios_ppm_out_dev *)ppm_out_id;
 286
 287     if (!PIOS_PPM_Out_validate(ppm_dev)) {
 288         return;
 289     }
 290
 291     // RTC runs at 625Hz so divide down the base rate so that this loop runs at 12.5Hz.
 292     if (++(ppm_dev->SupvTimer) < 50) {
 293         return;
 294     }
 295     ppm_dev->SupvTimer = 0;
 296
 297     // Go into failsafe the channel values haven't been refreshed since the last time through.
 298     if (!ppm_dev->Fresh) {
 299         ppm_dev->Tracking = FALSE;
 300     }
 301
 302     // Set Fresh to false to test if channel values are being refreshed.
 303     ppm_dev->Fresh = FALSE;
 304 }
 305
 306 #endif /* PIOS_INCLUDE_PPM_OUT */