diff --git a/flight/pios/common/pios_rcvr.c b/flight/pios/common/pios_rcvr.c index 5e2e856da..db46b1fd7 100644 --- a/flight/pios/common/pios_rcvr.c +++ b/flight/pios/common/pios_rcvr.c @@ -113,6 +113,38 @@ int32_t PIOS_RCVR_Read(uint32_t rcvr_id, uint8_t channel) return rcvr_dev->driver->read(rcvr_dev->lower_id, channel); } +/** + * @brief Get a semaphore that signals when a new sample is available. + * @param[in] rcvr_id driver to read from + * @param[in] channel channel to read + * @returns The semaphore, or NULL if not supported. + */ +xSemaphoreHandle PIOS_RCVR_GetSemaphore(uint32_t rcvr_id, uint8_t channel) +{ + // Publicly facing API uses channel 1 for first channel + if (channel == 0) { + return NULL; + } else { + channel--; + } + + if (rcvr_id == 0) { + return NULL; + } + + struct pios_rcvr_dev *rcvr_dev = (struct pios_rcvr_dev *)rcvr_id; + + if (!PIOS_RCVR_validate(rcvr_dev)) { + /* Undefined RCVR port for this board (see pios_board.c) */ + PIOS_Assert(0); + } + + if (rcvr_dev->driver->get_semaphore) { + return rcvr_dev->driver->get_semaphore(rcvr_dev->lower_id, channel); + } + return NULL; +} + #endif /* PIOS_INCLUDE_RCVR */ /** diff --git a/flight/pios/inc/pios_ppm_priv.h b/flight/pios/inc/pios_ppm_priv.h index 66ccd6eb7..f319b78c0 100644 --- a/flight/pios/inc/pios_ppm_priv.h +++ b/flight/pios/inc/pios_ppm_priv.h @@ -31,9 +31,6 @@ #ifndef PIOS_PPM_PRIV_H #define PIOS_PPM_PRIV_H -#include -#include - struct pios_ppm_cfg { TIM_ICInitTypeDef tim_ic_init; const struct pios_tim_channel *channels; diff --git a/flight/pios/inc/pios_rcvr.h b/flight/pios/inc/pios_rcvr.h index 450e04628..74fac086f 100644 --- a/flight/pios/inc/pios_rcvr.h +++ b/flight/pios/inc/pios_rcvr.h @@ -34,10 +34,12 @@ struct pios_rcvr_driver { void (*init)(uint32_t id); int32_t (*read)(uint32_t id, uint8_t channel); + xSemaphoreHandle (*get_semaphore)(uint32_t id, uint8_t channel); }; /* Public Functions */ extern int32_t PIOS_RCVR_Read(uint32_t rcvr_id, uint8_t channel); +extern xSemaphoreHandle PIOS_RCVR_GetSemaphore(uint32_t rcvr_id, uint8_t channel); /*! Define error codes for PIOS_RCVR_Get */ enum PIOS_RCVR_errors { diff --git a/flight/pios/stm32f10x/pios_ppm.c b/flight/pios/stm32f10x/pios_ppm.c index 39bb2618d..47cf034c0 100644 --- a/flight/pios/stm32f10x/pios_ppm.c +++ b/flight/pios/stm32f10x/pios_ppm.c @@ -32,14 +32,19 @@ #ifdef PIOS_INCLUDE_PPM -#include "pios_ppm_priv.h" +#include +#include "pios_ppm_priv.h" /* Provide a RCVR driver */ static int32_t PIOS_PPM_Get(uint32_t rcvr_id, uint8_t channel); +static xSemaphoreHandle PIOS_PPM_Get_Semaphore(uint32_t rcvr_id, uint8_t channel); const struct pios_rcvr_driver pios_ppm_rcvr_driver = { - .read = PIOS_PPM_Get, + .read = PIOS_PPM_Get, +#if defined(PIOS_INCLUDE_FREERTOS) + .get_semaphore = PIOS_PPM_Get_Semaphore +#endif }; #define PIOS_PPM_IN_MIN_NUM_CHANNELS 4 @@ -76,6 +81,10 @@ struct pios_ppm_dev { uint8_t supv_timer; bool Tracking; bool Fresh; + +#ifdef PIOS_INCLUDE_FREERTOS + xSemaphoreHandle new_sample_semaphores[PIOS_PPM_IN_MIN_NUM_CHANNELS]; +#endif /* PIOS_INCLUDE_FREERTOS */ }; static bool PIOS_PPM_validate(struct pios_ppm_dev *ppm_dev) @@ -93,6 +102,11 @@ static struct pios_ppm_dev *PIOS_PPM_alloc(void) return NULL; } + // Initialize the semaphores to 0. + for (uint8_t i = 0; i < PIOS_PPM_IN_MIN_NUM_CHANNELS; ++i) { + ppm_dev->new_sample_semaphores[i] = 0; + } + ppm_dev->magic = PIOS_PPM_DEV_MAGIC; return ppm_dev; } @@ -199,6 +213,28 @@ out_fail: return -1; } +#if defined(PIOS_INCLUDE_FREERTOS) +static xSemaphoreHandle PIOS_PPM_Get_Semaphore(uint32_t rcvr_id, uint8_t channel) +{ + struct pios_ppm_dev *ppm_dev = (struct pios_ppm_dev *)rcvr_id; + + if (!PIOS_PPM_validate(ppm_dev)) { + /* Invalid device specified */ + return 0; + } + + if (channel >= PIOS_PPM_IN_MAX_NUM_CHANNELS) { + /* Channel out of range */ + return 0; + } + + if (ppm_dev->new_sample_semaphores[channel] == 0) { + vSemaphoreCreateBinary(ppm_dev->new_sample_semaphores[channel]); + } + return ppm_dev->new_sample_semaphores[channel]; +} +#endif /* if defined(PIOS_INCLUDE_FREERTOS) */ + /** * Get the value of an input channel * \param[in] channel Number of the channel desired (zero based) @@ -296,6 +332,15 @@ static void PIOS_PPM_tim_edge_cb(__attribute__((unused)) uint32_t tim_id, i < PIOS_PPM_IN_MAX_NUM_CHANNELS; i++) { ppm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT; } +#if defined(PIOS_INCLUDE_FREERTOS) + /* Signal that a new sample is ready on this channel. */ + if (ppm_dev->new_sample_semaphores[chan_idx] != 0) { + signed portBASE_TYPE pxHigherPriorityTaskWoken = pdFALSE; + if (xSemaphoreGiveFromISR(ppm_dev->new_sample_semaphores[chan_idx], &pxHigherPriorityTaskWoken) == pdTRUE) { + portEND_SWITCHING_ISR(pxHigherPriorityTaskWoken); /* FIXME: is this the right place for this? */ + } + } +#endif /* USE_FREERTOS */ } ppm_dev->Fresh = TRUE; diff --git a/flight/pios/stm32f4xx/pios_ppm.c b/flight/pios/stm32f4xx/pios_ppm.c index 91c5d2ea8..c5f9534f4 100644 --- a/flight/pios/stm32f4xx/pios_ppm.c +++ b/flight/pios/stm32f4xx/pios_ppm.c @@ -36,9 +36,13 @@ /* Provide a RCVR driver */ static int32_t PIOS_PPM_Get(uint32_t rcvr_id, uint8_t channel); +static xSemaphoreHandle PIOS_PPM_Get_Semaphore(uint32_t rcvr_id, uint8_t channel); const struct pios_rcvr_driver pios_ppm_rcvr_driver = { - .read = PIOS_PPM_Get, + .read = PIOS_PPM_Get, +#if defined(PIOS_INCLUDE_FREERTOS) + .get_semaphore = PIOS_PPM_Get_Semaphore +#endif }; #define PIOS_PPM_IN_MIN_NUM_CHANNELS 4 @@ -72,6 +76,10 @@ struct pios_ppm_dev { uint8_t supv_timer; bool Tracking; bool Fresh; + +#ifdef PIOS_INCLUDE_FREERTOS + xSemaphoreHandle new_sample_semaphores[PIOS_PPM_IN_MIN_NUM_CHANNELS]; +#endif /* PIOS_INCLUDE_FREERTOS */ }; static bool PIOS_PPM_validate(struct pios_ppm_dev *ppm_dev) @@ -89,6 +97,11 @@ static struct pios_ppm_dev *PIOS_PPM_alloc(void) return NULL; } + // Initialize the semaphores to 0. + for (uint8_t i = 0; i < PIOS_PPM_IN_MIN_NUM_CHANNELS; ++i) { + ppm_dev->new_sample_semaphores[i] = 0; + } + ppm_dev->magic = PIOS_PPM_DEV_MAGIC; return ppm_dev; } @@ -194,6 +207,28 @@ out_fail: return -1; } +#if defined(PIOS_INCLUDE_FREERTOS) +static xSemaphoreHandle PIOS_PPM_Get_Semaphore(uint32_t rcvr_id, uint8_t channel) +{ + struct pios_ppm_dev *ppm_dev = (struct pios_ppm_dev *)rcvr_id; + + if (!PIOS_PPM_validate(ppm_dev)) { + /* Invalid device specified */ + return 0; + } + + if (channel >= PIOS_PPM_IN_MAX_NUM_CHANNELS) { + /* Channel out of range */ + return 0; + } + + if (ppm_dev->new_sample_semaphores[channel] == 0) { + vSemaphoreCreateBinary(ppm_dev->new_sample_semaphores[channel]); + } + return ppm_dev->new_sample_semaphores[channel]; +} +#endif /* if defined(PIOS_INCLUDE_FREERTOS) */ + /** * Get the value of an input channel * \param[in] channel Number of the channel desired (zero based) @@ -286,6 +321,15 @@ static void PIOS_PPM_tim_edge_cb(__attribute__((unused)) uint32_t tim_id, uint32 i < PIOS_PPM_IN_MAX_NUM_CHANNELS; i++) { ppm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT; } +#if defined(PIOS_INCLUDE_FREERTOS) + /* Signal that a new sample is ready on this channel. */ + if (ppm_dev->new_sample_semaphores[chan_idx] != 0) { + signed portBASE_TYPE pxHigherPriorityTaskWoken = pdFALSE; + if (xSemaphoreGiveFromISR(ppm_dev->new_sample_semaphores[chan_idx], &pxHigherPriorityTaskWoken) == pdTRUE) { + portEND_SWITCHING_ISR(pxHigherPriorityTaskWoken); /* FIXME: is this the right place for his? */ + } + } +#endif /* USE_FREERTOS */ } ppm_dev->Fresh = true;