flight/pios/common/pios_gcsrcvr.c

   1 /**
   2  ******************************************************************************
   3  * @addtogroup PIOS PIOS Core hardware abstraction layer
   4  * @{
   5  * @addtogroup   PIOS_GCSRCVR GCS Receiver Input Functions
   6  * @brief               Code to read the channels within the GCS Receiver UAVObject
   7  * @{
   8  *
   9  * @file       pios_gcsrcvr.c
  10  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
  11  * @brief      GCS 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_GCSRCVR
  34
  35 #include "uavobjectmanager.h"
  36
  37 #include "pios_gcsrcvr_priv.h"
  38
  39 static GCSReceiverData gcsreceiverdata;
  40
  41 /* Provide a RCVR driver */
  42 static int32_t PIOS_GCSRCVR_Get(uint32_t rcvr_id, uint8_t channel);
  43 static void PIOS_gcsrcvr_Supervisor(uint32_t ppm_id);
  44
  45 const struct pios_rcvr_driver pios_gcsrcvr_rcvr_driver = {
  46     .read = PIOS_GCSRCVR_Get,
  47 };
  48
  49 /* Local Variables */
  50 enum pios_gcsrcvr_dev_magic {
  51     PIOS_GCSRCVR_DEV_MAGIC = 0xe9da5c56,
  52 };
  53
  54 struct pios_gcsrcvr_dev {
  55     enum pios_gcsrcvr_dev_magic magic;
  56
  57     uint8_t supv_timer;
  58     bool    Fresh;
  59 };
  60
  61 static struct pios_gcsrcvr_dev *global_gcsrcvr_dev;
  62
  63 static bool PIOS_gcsrcvr_validate(struct pios_gcsrcvr_dev *gcsrcvr_dev)
  64 {
  65     return gcsrcvr_dev->magic == PIOS_GCSRCVR_DEV_MAGIC;
  66 }
  67
  68 #if defined(PIOS_INCLUDE_FREERTOS)
  69 static struct pios_gcsrcvr_dev *PIOS_gcsrcvr_alloc(void)
  70 {
  71     struct pios_gcsrcvr_dev *gcsrcvr_dev;
  72
  73     gcsrcvr_dev = (struct pios_gcsrcvr_dev *)pios_malloc(sizeof(*gcsrcvr_dev));
  74     if (!gcsrcvr_dev) {
  75         return NULL;
  76     }
  77
  78     gcsrcvr_dev->magic = PIOS_GCSRCVR_DEV_MAGIC;
  79     gcsrcvr_dev->Fresh = false;
  80     gcsrcvr_dev->supv_timer = 0;
  81
  82     /* The update callback cannot receive the device pointer, so set it in a global */
  83     global_gcsrcvr_dev = gcsrcvr_dev;
  84
  85     return gcsrcvr_dev;
  86 }
  87 #else
  88 static struct pios_gcsrcvr_dev pios_gcsrcvr_devs[PIOS_GCSRCVR_MAX_DEVS];
  89 static uint8_t pios_gcsrcvr_num_devs;
  90 static struct pios_gcsrcvr_dev *PIOS_gcsrcvr_alloc(void)
  91 {
  92     struct pios_gcsrcvr_dev *gcsrcvr_dev;
  93
  94     if (pios_gcsrcvr_num_devs >= PIOS_GCSRCVR_MAX_DEVS) {
  95         return NULL;
  96     }
  97
  98     gcsrcvr_dev = &pios_gcsrcvr_devs[pios_gcsrcvr_num_devs++];
  99     gcsrcvr_dev->magic = PIOS_GCSRCVR_DEV_MAGIC;
 100     gcsrcvr_dev->Fresh = false;
 101     gcsrcvr_dev->supv_timer = 0;
 102
 103     global_gcsrcvr_dev = gcsrcvr_dev;
 104
 105     return gcsrcvr_dev;
 106 }
 107 #endif /* if defined(PIOS_INCLUDE_FREERTOS) */
 108
 109 static void gcsreceiver_updated(UAVObjEvent *ev)
 110 {
 111     struct pios_gcsrcvr_dev *gcsrcvr_dev = global_gcsrcvr_dev;
 112
 113     if (ev->obj == GCSReceiverHandle()) {
 114         GCSReceiverGet(&gcsreceiverdata);
 115         gcsrcvr_dev->Fresh = true;
 116     }
 117 }
 118
 119 extern int32_t PIOS_GCSRCVR_Init(__attribute__((unused)) uint32_t *gcsrcvr_id)
 120 {
 121     struct pios_gcsrcvr_dev *gcsrcvr_dev;
 122
 123     /* Allocate the device structure */
 124     gcsrcvr_dev = (struct pios_gcsrcvr_dev *)PIOS_gcsrcvr_alloc();
 125     if (!gcsrcvr_dev) {
 126         return -1;
 127     }
 128
 129     for (uint8_t i = 0; i < GCSRECEIVER_CHANNEL_NUMELEM; i++) {
 130         /* Flush channels */
 131         gcsreceiverdata.Channel[i] = PIOS_RCVR_TIMEOUT;
 132     }
 133
 134     /* Register uavobj callback */
 135     GCSReceiverConnectCallback(gcsreceiver_updated);
 136
 137     /* Register the failsafe timer callback. */
 138     if (!PIOS_RTC_RegisterTickCallback(PIOS_gcsrcvr_Supervisor, (uint32_t)gcsrcvr_dev)) {
 139         PIOS_DEBUG_Assert(0);
 140     }
 141
 142     return 0;
 143 }
 144
 145 /**
 146  * Get the value of an input channel
 147  * \param[in] channel Number of the channel desired (zero based)
 148  * \output PIOS_RCVR_INVALID channel not available
 149  * \output PIOS_RCVR_TIMEOUT failsafe condition or missing receiver
 150  * \output >=0 channel value
 151  */
 152 static int32_t PIOS_GCSRCVR_Get(__attribute__((unused)) uint32_t rcvr_id, uint8_t channel)
 153 {
 154     if (channel >= GCSRECEIVER_CHANNEL_NUMELEM) {
 155         /* channel is out of range */
 156         return PIOS_RCVR_INVALID;
 157     }
 158
 159     return gcsreceiverdata.Channel[channel];
 160 }
 161
 162 static void PIOS_gcsrcvr_Supervisor(uint32_t gcsrcvr_id)
 163 {
 164     /* Recover our device context */
 165     struct pios_gcsrcvr_dev *gcsrcvr_dev = (struct pios_gcsrcvr_dev *)gcsrcvr_id;
 166
 167     if (!PIOS_gcsrcvr_validate(gcsrcvr_dev)) {
 168         /* Invalid device specified */
 169         return;
 170     }
 171
 172     /*
 173      * RTC runs at 625Hz.
 174      */
 175     if (++(gcsrcvr_dev->supv_timer) < (PIOS_GCSRCVR_TIMEOUT_MS * 1000 / 625)) {
 176         return;
 177     }
 178     gcsrcvr_dev->supv_timer = 0;
 179
 180     if (!gcsrcvr_dev->Fresh) {
 181         for (int32_t i = 0; i < GCSRECEIVER_CHANNEL_NUMELEM; i++) {
 182             gcsreceiverdata.Channel[i] = PIOS_RCVR_TIMEOUT;
 183         }
 184     }
 185
 186     gcsrcvr_dev->Fresh = false;
 187 }
 188
 189 #endif /* PIOS_INCLUDE_GCSRCVR */
 190
 191 /**
 192  * @}
 193  * @}
 194  */