flight/pios/common/pios_rcvr.c

   1 #include "pios.h"
   2
   3 #ifdef PIOS_INCLUDE_RCVR
   4
   5 #include <pios_rcvr_priv.h>
   6
   7 enum pios_rcvr_dev_magic {
   8     PIOS_RCVR_DEV_MAGIC = 0x99aabbcc,
   9 };
  10
  11 struct pios_rcvr_dev {
  12     enum pios_rcvr_dev_magic magic;
  13     uint32_t lower_id;
  14     const struct pios_rcvr_driver *driver;
  15 };
  16
  17 static bool PIOS_RCVR_validate(struct pios_rcvr_dev *rcvr_dev)
  18 {
  19     return rcvr_dev->magic == PIOS_RCVR_DEV_MAGIC;
  20 }
  21
  22 #if defined(PIOS_INCLUDE_FREERTOS)
  23 static struct pios_rcvr_dev *PIOS_RCVR_alloc(void)
  24 {
  25     struct pios_rcvr_dev *rcvr_dev;
  26
  27     rcvr_dev = (struct pios_rcvr_dev *)pios_malloc(sizeof(*rcvr_dev));
  28     if (!rcvr_dev) {
  29         return NULL;
  30     }
  31
  32     rcvr_dev->magic = PIOS_RCVR_DEV_MAGIC;
  33     return rcvr_dev;
  34 }
  35 #else
  36 static struct pios_rcvr_dev pios_rcvr_devs[PIOS_RCVR_MAX_DEVS];
  37 static uint8_t pios_rcvr_num_devs;
  38 static struct pios_rcvr_dev *PIOS_RCVR_alloc(void)
  39 {
  40     struct pios_rcvr_dev *rcvr_dev;
  41
  42     if (pios_rcvr_num_devs >= PIOS_RCVR_MAX_DEVS) {
  43         return NULL;
  44     }
  45
  46     rcvr_dev = &pios_rcvr_devs[pios_rcvr_num_devs++];
  47     rcvr_dev->magic = PIOS_RCVR_DEV_MAGIC;
  48
  49     return rcvr_dev;
  50 }
  51 #endif /* if defined(PIOS_INCLUDE_FREERTOS) */
  52
  53 /**
  54  * Initialises RCVR layer
  55  * \param[out] handle
  56  * \param[in] driver
  57  * \param[in] id
  58  * \return < 0 if initialisation failed
  59  */
  60 int32_t PIOS_RCVR_Init(uint32_t *rcvr_id, const struct pios_rcvr_driver *driver, uint32_t lower_id)
  61 {
  62     PIOS_DEBUG_Assert(rcvr_id);
  63     PIOS_DEBUG_Assert(driver);
  64
  65     struct pios_rcvr_dev *rcvr_dev;
  66
  67     rcvr_dev = (struct pios_rcvr_dev *)PIOS_RCVR_alloc();
  68     if (!rcvr_dev) {
  69         goto out_fail;
  70     }
  71
  72     rcvr_dev->driver   = driver;
  73     rcvr_dev->lower_id = lower_id;
  74
  75     *rcvr_id = (uint32_t)rcvr_dev;
  76     return 0;
  77
  78 out_fail:
  79     return -1;
  80 }
  81
  82 /**
  83  * @brief Reads an input channel from the appropriate driver
  84  * @param[in] rcvr_id driver to read from
  85  * @param[in] channel channel to read
  86  * @returns Unitless input value
  87  *  @retval PIOS_RCVR_TIMEOUT indicates a failsafe or timeout from that channel
  88  *  @retval PIOS_RCVR_INVALID invalid channel for this driver (usually out of range supported)
  89  *  @retval PIOS_RCVR_NODRIVER driver was not initialized
  90  */
  91 int32_t PIOS_RCVR_Read(uint32_t rcvr_id, uint8_t channel)
  92 {
  93     // Publicly facing API uses channel 1 for first channel
  94     if (channel == 0) {
  95         return PIOS_RCVR_INVALID;
  96     } else {
  97         channel--;
  98     }
  99
 100     if (rcvr_id == 0) {
 101         return PIOS_RCVR_NODRIVER;
 102     }
 103
 104     struct pios_rcvr_dev *rcvr_dev = (struct pios_rcvr_dev *)rcvr_id;
 105
 106     if (!PIOS_RCVR_validate(rcvr_dev)) {
 107         /* Undefined RCVR port for this board (see pios_board.c) */
 108         PIOS_Assert(0);
 109     }
 110
 111     PIOS_DEBUG_Assert(rcvr_dev->driver->read);
 112
 113     return rcvr_dev->driver->read(rcvr_dev->lower_id, channel);
 114 }
 115
 116 /**
 117  * @brief Get a semaphore that signals when a new sample is available.
 118  * @param[in] rcvr_id driver to read from
 119  * @param[in] channel channel to read
 120  * @returns The semaphore, or NULL if not supported.
 121  */
 122 xSemaphoreHandle PIOS_RCVR_GetSemaphore(uint32_t rcvr_id, uint8_t channel)
 123 {
 124     // Publicly facing API uses channel 1 for first channel
 125     if (channel == 0) {
 126         return NULL;
 127     } else {
 128         channel--;
 129     }
 130
 131     if (rcvr_id == 0) {
 132         return NULL;
 133     }
 134
 135     struct pios_rcvr_dev *rcvr_dev = (struct pios_rcvr_dev *)rcvr_id;
 136
 137     if (!PIOS_RCVR_validate(rcvr_dev)) {
 138         /* Undefined RCVR port for this board (see pios_board.c) */
 139         PIOS_Assert(0);
 140     }
 141
 142     if (rcvr_dev->driver->get_semaphore) {
 143         return rcvr_dev->driver->get_semaphore(rcvr_dev->lower_id, channel);
 144     }
 145     return NULL;
 146 }
 147
 148 #endif /* PIOS_INCLUDE_RCVR */
 149
 150 /**
 151  * @}
 152  * @}
 153  */