From 90b2625debed95a1ffde3b04fd30eb159f397bed Mon Sep 17 00:00:00 2001 From: Stacey Sheldon Date: Tue, 2 Aug 2011 01:27:37 -0400 Subject: [PATCH] rcvractivity: Add tracking of Rx channel activity --- flight/CopterControl/Makefile | 1 + flight/Modules/ManualControl/manualcontrol.c | 146 +++++++++++++++++- flight/OpenPilot/UAVObjects.inc | 1 + .../src/plugins/uavobjects/uavobjects.pro | 2 + .../uavobjectdefinition/receiveractivity.xml | 14 ++ 5 files changed, 163 insertions(+), 1 deletion(-) create mode 100644 shared/uavobjectdefinition/receiveractivity.xml diff --git a/flight/CopterControl/Makefile b/flight/CopterControl/Makefile index 3ac59e59b..d437d567d 100644 --- a/flight/CopterControl/Makefile +++ b/flight/CopterControl/Makefile @@ -165,6 +165,7 @@ SRC += $(OPUAVSYNTHDIR)/firmwareiapobj.c SRC += $(OPUAVSYNTHDIR)/attitudesettings.c SRC += $(OPUAVSYNTHDIR)/hwsettings.c SRC += $(OPUAVSYNTHDIR)/gcsreceiver.c +SRC += $(OPUAVSYNTHDIR)/receiveractivity.c #${wildcard ${OBJ}/$(shell echo $(VAR) | tr A-Z a-z)/*.c} #SRC += ${foreach OBJ, ${UAVOBJECTS}, $(UAVOBJECTS)/$(OBJ).c} # Cant use until i can automatically generate list of UAVObjects diff --git a/flight/Modules/ManualControl/manualcontrol.c b/flight/Modules/ManualControl/manualcontrol.c index 7c16ac172..153738dc1 100644 --- a/flight/Modules/ManualControl/manualcontrol.c +++ b/flight/Modules/ManualControl/manualcontrol.c @@ -43,6 +43,7 @@ #include "flighttelemetrystats.h" #include "flightstatus.h" #include "accessorydesired.h" +#include "receiveractivity.h" // Private constants #if defined(PIOS_MANUAL_STACK_SIZE) @@ -87,6 +88,18 @@ static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time) static bool okToArm(void); static bool validInputRange(int16_t min, int16_t max, uint16_t value); +#define RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP 12 +#define RCVR_ACTIVITY_MONITOR_MIN_RANGE 5 +struct rcvr_activity_fsm { + ManualControlSettingsChannelGroupsOptions group; + uint16_t prev[RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP]; + uint8_t sample_count; +}; +static struct rcvr_activity_fsm activity_fsm; + +static void resetRcvrActivity(struct rcvr_activity_fsm * fsm); +static bool updateRcvrActivity(struct rcvr_activity_fsm * fsm); + #define assumptions (assumptions1 && assumptions3 && assumptions5 && assumptions7 && assumptions8 && assumptions_flightmode) /** @@ -138,6 +151,10 @@ static void manualControlTask(void *parameters) flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED; armState = ARM_STATE_DISARMED; + /* Initialize the RcvrActivty FSM */ + portTickType lastActivityTime = xTaskGetTickCount(); + resetRcvrActivity(&activity_fsm); + // Main task loop lastSysTime = xTaskGetTickCount(); while (1) { @@ -150,6 +167,18 @@ static void manualControlTask(void *parameters) // Read settings ManualControlSettingsGet(&settings); + /* Update channel activity monitor */ + if (flightStatus.Armed == ARM_STATE_DISARMED) { + if (updateRcvrActivity(&activity_fsm)) { + /* Reset the aging timer because activity was detected */ + lastActivityTime = lastSysTime; + } + } + if (timeDifferenceMs(lastActivityTime, lastSysTime) > 5000) { + resetRcvrActivity(&activity_fsm); + lastActivityTime = lastSysTime; + } + if (ManualControlCommandReadOnly(&cmd)) { FlightTelemetryStatsData flightTelemStats; FlightTelemetryStatsGet(&flightTelemStats); @@ -266,7 +295,6 @@ static void manualControlTask(void *parameters) ManualControlCommandGet(&cmd); /* Under GCS control */ } - FlightStatusGet(&flightStatus); // Depending on the mode update the Stabilization or Actuator objects @@ -288,6 +316,122 @@ static void manualControlTask(void *parameters) } } +static void resetRcvrActivity(struct rcvr_activity_fsm * fsm) +{ + ReceiverActivityData data; + bool updated = false; + + /* Clear all channel activity flags */ + ReceiverActivityGet(&data); + if (data.ActiveGroup != RECEIVERACTIVITY_ACTIVEGROUP_NONE && + data.ActiveChannel != 255) { + data.ActiveGroup = RECEIVERACTIVITY_ACTIVEGROUP_NONE; + data.ActiveChannel = 255; + updated = true; + } + if (updated) { + ReceiverActivitySet(&data); + } + + /* Reset the FSM state */ + fsm->group = 0; + fsm->sample_count = 0; +} + +static bool updateRcvrActivity(struct rcvr_activity_fsm * fsm) +{ + bool activity_updated = false; + + if (fsm->group >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + /* We're out of range, reset things */ + resetRcvrActivity(fsm); + } + + extern uint32_t pios_rcvr_group_map[]; + if (!pios_rcvr_group_map[fsm->group]) { + /* Unbound group, skip it */ + goto group_completed; + } + + /* Sample the channel */ + for (uint8_t channel = 0; + channel < RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP; + channel++) { + if (fsm->sample_count == 0) { + fsm->prev[channel] = PIOS_RCVR_Read(pios_rcvr_group_map[fsm->group], channel); + } else { + uint16_t delta; + uint16_t prev = fsm->prev[channel]; + uint16_t curr = PIOS_RCVR_Read(pios_rcvr_group_map[fsm->group], channel); + if (curr > prev) { + delta = curr - prev; + } else { + delta = prev - curr; + } + + if (delta > RCVR_ACTIVITY_MONITOR_MIN_RANGE) { + /* Mark this channel as active */ + ReceiverActivityActiveGroupOptions group; + + /* Don't assume manualcontrolsettings and receiveractivity are in the same order. */ + switch (fsm->group) { + case MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM: + group = RECEIVERACTIVITY_ACTIVEGROUP_PWM; + break; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM: + group = RECEIVERACTIVITY_ACTIVEGROUP_PPM; + break; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_SPEKTRUM1: + group = RECEIVERACTIVITY_ACTIVEGROUP_SPEKTRUM1; + break; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_SPEKTRUM2: + group = RECEIVERACTIVITY_ACTIVEGROUP_SPEKTRUM2; + break; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS: + group = RECEIVERACTIVITY_ACTIVEGROUP_SBUS; + break; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS: + group = RECEIVERACTIVITY_ACTIVEGROUP_GCS; + break; + default: + PIOS_Assert(0); + break; + } + + ReceiverActivityActiveGroupSet(&group); + ReceiverActivityActiveChannelSet(&channel); + activity_updated = true; + } + } + } + + if (++fsm->sample_count < 2) { + return (activity_updated); + } + + /* Reset the sample counter */ + fsm->sample_count = 0; + + /* + * Group Completed + */ + +group_completed: + /* Start over at channel zero */ + if (++fsm->group < MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + return (activity_updated); + } + + /* + * All groups completed + */ + + /* Start over at group zero */ + fsm->group = 0; + + return (activity_updated); +} + static void updateActuatorDesired(ManualControlCommandData * cmd) { ActuatorDesiredData actuator; diff --git a/flight/OpenPilot/UAVObjects.inc b/flight/OpenPilot/UAVObjects.inc index a30dcf540..3bb868a7d 100644 --- a/flight/OpenPilot/UAVObjects.inc +++ b/flight/OpenPilot/UAVObjects.inc @@ -70,6 +70,7 @@ UAVOBJSRCFILENAMES += velocitydesired UAVOBJSRCFILENAMES += watchdogstatus UAVOBJSRCFILENAMES += flightstatus UAVOBJSRCFILENAMES += hwsettings +UAVOBJSRCFILENAMES += receiveractivity UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(UAVOBJSYNTHDIR)/$(UAVOBJSRCFILE).c ) UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) ) diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index 3831a4ec3..f5e17d3ca 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -70,6 +70,7 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ $$UAVOBJECT_SYNTHETICS/flightstatus.h \ $$UAVOBJECT_SYNTHETICS/hwsettings.h \ $$UAVOBJECT_SYNTHETICS/gcsreceiver.h \ + $$UAVOBJECT_SYNTHETICS/receiveractivity.h \ $$UAVOBJECT_SYNTHETICS/attitudesettings.h SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ @@ -120,4 +121,5 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ $$UAVOBJECT_SYNTHETICS/flightstatus.cpp \ $$UAVOBJECT_SYNTHETICS/hwsettings.cpp \ $$UAVOBJECT_SYNTHETICS/gcsreceiver.cpp \ + $$UAVOBJECT_SYNTHETICS/receiveractivity.cpp \ $$UAVOBJECT_SYNTHETICS/attitudesettings.cpp diff --git a/shared/uavobjectdefinition/receiveractivity.xml b/shared/uavobjectdefinition/receiveractivity.xml new file mode 100644 index 000000000..2e98c1362 --- /dev/null +++ b/shared/uavobjectdefinition/receiveractivity.xml @@ -0,0 +1,14 @@ + + + Monitors which receiver channels have been active within the last second. + + + + + + + +