rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-20 10:54:14 +01:00
Code Issues Releases Activity

Added failsafe to GCS Receiver device.

Browse Source
This commit is contained in:
Brian Webb 2012-02-02 19:58:42 -07:00
parent eaf9a4bded
commit a314604f9b
3 changed files with 106 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
flight/CopterControl/System/pios_board.c
View File

@ -548,9 +548,10 @@ void PIOS_Board_Init(void) {
#if defined(PIOS_INCLUDE_GCSRCVR)
GCSReceiverInitialize();
PIOS_GCSRCVR_Init();
uint32_t pios_gcsrcvr_id;
PIOS_GCSRCVR_Init(&pios_gcsrcvr_id);
uint32_t pios_gcsrcvr_rcvr_id;
if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, 0)) {
if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, pios_gcsrcvr_id)) {
PIOS_Assert(0);
}
pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS] = pios_gcsrcvr_rcvr_id;

103
flight/PiOS/Common/pios_gcsrcvr.c
View File

@ -1,3 +1,4 @@
/* -*- Mode: c; c-basic-offset: 2; tab-width: 2; indent-tabs-mode: t -*- */
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
@ -39,22 +40,97 @@ static GCSReceiverData gcsreceiverdata;
/* Provide a RCVR driver */
static int32_t PIOS_GCSRCVR_Get(uint32_t rcvr_id, uint8_t channel);
static void PIOS_gcsrcvr_Supervisor(uint32_t ppm_id);
const struct pios_rcvr_driver pios_gcsrcvr_rcvr_driver = {
.read = PIOS_GCSRCVR_Get,
};
/* Local Variables */
enum pios_gcsrcvr_dev_magic {
PIOS_GCSRCVR_DEV_MAGIC = 0xe9da5c56,
};
struct pios_gcsrcvr_dev {
enum pios_gcsrcvr_dev_magic magic;
uint8_t supv_timer;
bool Fresh;
};
static struct pios_gcsrcvr_dev *global_gcsrcvr_dev;
static bool PIOS_gcsrcvr_validate(struct pios_gcsrcvr_dev *gcsrcvr_dev)
{
return (gcsrcvr_dev->magic == PIOS_GCSRCVR_DEV_MAGIC);
}
#if defined(PIOS_INCLUDE_FREERTOS)
static struct pios_gcsrcvr_dev *PIOS_gcsrcvr_alloc(void)
{
struct pios_gcsrcvr_dev * gcsrcvr_dev;
gcsrcvr_dev = (struct pios_gcsrcvr_dev *)pvPortMalloc(sizeof(*gcsrcvr_dev));
if (!gcsrcvr_dev) return(NULL);
gcsrcvr_dev->magic = PIOS_GCSRCVR_DEV_MAGIC;
gcsrcvr_dev->Fresh = FALSE;
gcsrcvr_dev->supv_timer = 0;
/* The update callback cannot receive the device pointer, so set it in a global */
global_gcsrcvr_dev = gcsrcvr_dev;
return(gcsrcvr_dev);
}
#else
static struct pios_gcsrcvr_dev pios_gcsrcvr_devs[PIOS_GCSRCVR_MAX_DEVS];
static uint8_t pios_gcsrcvr_num_devs;
static struct pios_gcsrcvr_dev *PIOS_gcsrcvr_alloc(void)
{
struct pios_gcsrcvr_dev *gcsrcvr_dev;
if (pios_gcsrcvr_num_devs >= PIOS_GCSRCVR_MAX_DEVS) {
return (NULL);
}
gcsrcvr_dev = &pios_gcsrcvr_devs[pios_gcsrcvr_num_devs++];
gcsrcvr_dev->magic = PIOS_GCSRCVR_DEV_MAGIC;
gcsrcvr_dev->Fresh = FALSE;
gcsrcvr_dev->supv_timer = 0;
global_gcsrcvr_dev = gcsrcvr_dev;
return (gcsrcvr_dev);
}
#endif
static void gcsreceiver_updated(UAVObjEvent * ev)
{
struct pios_gcsrcvr_dev *gcsrcvr_dev = global_gcsrcvr_dev;
if (ev->obj == GCSReceiverHandle()) {
GCSReceiverGet(&gcsreceiverdata);
gcsrcvr_dev->Fresh = TRUE;
}
}
void PIOS_GCSRCVR_Init(void)
extern int32_t PIOS_GCSRCVR_Init(uint32_t *gcsrcvr_id)
{
struct pios_gcsrcvr_dev *gcsrcvr_dev;
/* Allocate the device structure */
gcsrcvr_dev = (struct pios_gcsrcvr_dev *)PIOS_gcsrcvr_alloc();
if (!gcsrcvr_dev)
return -1;
/* Register uavobj callback */
GCSReceiverConnectCallback (gcsreceiver_updated);
/* Register the failsafe timer callback. */
if (!PIOS_RTC_RegisterTickCallback(PIOS_gcsrcvr_Supervisor, (uint32_t)gcsrcvr_dev)) {
PIOS_DEBUG_Assert(0);
}
return 0;
}
static int32_t PIOS_GCSRCVR_Get(uint32_t rcvr_id, uint8_t channel)
@ -67,6 +143,31 @@ static int32_t PIOS_GCSRCVR_Get(uint32_t rcvr_id, uint8_t channel)
return (gcsreceiverdata.Channel[channel]);
}
static void PIOS_gcsrcvr_Supervisor(uint32_t gcsrcvr_id) {
/* Recover our device context */
struct pios_gcsrcvr_dev * gcsrcvr_dev = (struct pios_gcsrcvr_dev *)gcsrcvr_id;
if (!PIOS_gcsrcvr_validate(gcsrcvr_dev)) {
/* Invalid device specified */
return;
}
/*
* RTC runs at 625Hz so divide down the base rate so
* that this loop runs at 25Hz.
*/
if(++(gcsrcvr_dev->supv_timer) < 25) {
return;
}
gcsrcvr_dev->supv_timer = 0;
if (!gcsrcvr_dev->Fresh)
for (int32_t i = 0; i < GCSRECEIVER_CHANNEL_NUMELEM; i++)
gcsreceiverdata.Channel[i] = PIOS_RCVR_TIMEOUT;
gcsrcvr_dev->Fresh = FALSE;
}
#endif /* PIOS_INCLUDE_GCSRCVR */
/**

2
flight/PiOS/inc/pios_gcsrcvr_priv.h
View File

@ -37,7 +37,7 @@
extern const struct pios_rcvr_driver pios_gcsrcvr_rcvr_driver;
extern void PIOS_GCSRCVR_Init(void);
extern int32_t PIOS_GCSRCVR_Init(uint32_t *gcsrcvr_id);
#endif /* PIOS_GCSRCVR_PRIV_H */