flight/pios/posix/pios_rcvr.c

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