From 697dbf4f5f1bb860c693d5519bb12f7b364a36b5 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sat, 3 Sep 2011 23:47:56 -0500 Subject: [PATCH 1/8] OP-568 PIOS_RCVR: Standardize the values that are returned from the PIOS_RCVR and make them symbolic constants. - A timeout is 0 - A missing driver is 65534 - An invalid channel is 65535 ManualControl: Make it deal with the values explicitly. A timed out value should not be treated like a minimum duration signal. Instead it does not updated the scaled value but marks the data window as invalid to trigger the failsafe. --- flight/Modules/ManualControl/manualcontrol.c | 33 +++++++++++++++---- flight/PiOS/Common/pios_rcvr.c | 3 ++ flight/PiOS/STM32F10x/pios_ppm.c | 9 +++-- flight/PiOS/STM32F10x/pios_spektrum.c | 4 +-- flight/PiOS/inc/pios_rcvr.h | 4 +++ .../OpenPilotOSX.xcodeproj/project.pbxproj | 12 +++++++ 6 files changed, 52 insertions(+), 13 deletions(-) diff --git a/flight/Modules/ManualControl/manualcontrol.c b/flight/Modules/ManualControl/manualcontrol.c index 9165f06f8..3e2662bce 100644 --- a/flight/Modules/ManualControl/manualcontrol.c +++ b/flight/Modules/ManualControl/manualcontrol.c @@ -202,6 +202,8 @@ static void manualControlTask(void *parameters) if (!ManualControlCommandReadOnly(&cmd)) { + bool valid_input_detected = true; + // Read channel values in us for (uint8_t n = 0; n < MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM && n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; @@ -209,14 +211,20 @@ static void manualControlTask(void *parameters) extern uint32_t pios_rcvr_group_map[]; if (settings.ChannelGroups[n] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { - cmd.Channel[n] = -1; - } else if (!pios_rcvr_group_map[settings.ChannelGroups[n]]) { - cmd.Channel[n] = -2; + cmd.Channel[n] = PIOS_RCVR_INVALID; } else { cmd.Channel[n] = PIOS_RCVR_Read(pios_rcvr_group_map[settings.ChannelGroups[n]], settings.ChannelNumber[n]); } - scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax[n], settings.ChannelMin[n], settings.ChannelNeutral[n]); + + // If a channel has timed out this is not valid data and we shouldn't update anything + // until we decide to go to failsafe + if(cmd.Channel[n] == PIOS_RCVR_TIMEOUT) + valid_input_detected = false; + else if (cmd.Channel[n] == PIOS_RCVR_INVALID) + scaledChannel[n] = 0; + else + scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax[n], settings.ChannelMin[n], settings.ChannelNeutral[n]); } // Check settings, if error raise alarm @@ -224,7 +232,20 @@ static void manualControlTask(void *parameters) settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE || settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE || settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE || - settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE || + // Check all channel mappings are valid + cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == PIOS_RCVR_INVALID || + cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == PIOS_RCVR_INVALID || + cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == PIOS_RCVR_INVALID || + cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == PIOS_RCVR_INVALID || + cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == PIOS_RCVR_INVALID || + // Check the driver is exists + cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == PIOS_RCVR_NODRIVER || + cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == PIOS_RCVR_NODRIVER || + cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == PIOS_RCVR_NODRIVER || + cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == PIOS_RCVR_NODRIVER || + cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == PIOS_RCVR_NODRIVER) { + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; ManualControlCommandSet(&cmd); @@ -232,7 +253,7 @@ static void manualControlTask(void *parameters) } // decide if we have valid manual input or not - bool valid_input_detected = validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE]) && + valid_input_detected &= validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE]) && validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL]) && validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW], settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW]) && validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH], settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]); diff --git a/flight/PiOS/Common/pios_rcvr.c b/flight/PiOS/Common/pios_rcvr.c index 29acb2594..b76e04a44 100644 --- a/flight/PiOS/Common/pios_rcvr.c +++ b/flight/PiOS/Common/pios_rcvr.c @@ -78,6 +78,9 @@ out_fail: int32_t PIOS_RCVR_Read(uint32_t rcvr_id, uint8_t channel) { + if (rcvr_id == 0) + return PIOS_RCVR_NODRIVER; + struct pios_rcvr_dev * rcvr_dev = (struct pios_rcvr_dev *)rcvr_id; if (!PIOS_RCVR_validate(rcvr_dev)) { diff --git a/flight/PiOS/STM32F10x/pios_ppm.c b/flight/PiOS/STM32F10x/pios_ppm.c index 0b7a1c5cd..bee955861 100644 --- a/flight/PiOS/STM32F10x/pios_ppm.c +++ b/flight/PiOS/STM32F10x/pios_ppm.c @@ -47,7 +47,6 @@ const struct pios_rcvr_driver pios_ppm_rcvr_driver = { #define PIOS_PPM_IN_MIN_SYNC_PULSE_US 3800 // microseconds #define PIOS_PPM_IN_MIN_CHANNEL_PULSE_US 750 // microseconds #define PIOS_PPM_IN_MAX_CHANNEL_PULSE_US 2250 // microseconds -#define PIOS_PPM_INPUT_INVALID 0 /* Local Variables */ static TIM_ICInitTypeDef TIM_ICInitStructure; @@ -290,7 +289,7 @@ static void PIOS_PPM_tim_edge_cb (uint32_t tim_id, uint32_t context, uint8_t cha } for (uint32_t i = ppm_dev->NumChannels; i < PIOS_PPM_IN_MAX_NUM_CHANNELS; i++) { - ppm_dev->CaptureValue[i] = PIOS_PPM_INPUT_INVALID; + ppm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT; } } @@ -314,7 +313,7 @@ static void PIOS_PPM_tim_edge_cb (uint32_t tim_id, uint32_t context, uint8_t cha /* Not a valid pulse duration */ ppm_dev->Tracking = FALSE; for (uint32_t i = 0; i < PIOS_PPM_IN_MAX_NUM_CHANNELS ; i++) { - ppm_dev->CaptureValueNewFrame[i] = PIOS_PPM_INPUT_INVALID; + ppm_dev->CaptureValueNewFrame[i] = PIOS_RCVR_TIMEOUT; } } } @@ -342,8 +341,8 @@ static void PIOS_PPM_Supervisor(uint32_t ppm_id) { ppm_dev->Tracking = FALSE; for (int32_t i = 0; i < PIOS_PPM_IN_MAX_NUM_CHANNELS ; i++) { - ppm_dev->CaptureValue[i] = PIOS_PPM_INPUT_INVALID; - ppm_dev->CaptureValueNewFrame[i] = PIOS_PPM_INPUT_INVALID; + ppm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT; + ppm_dev->CaptureValueNewFrame[i] = PIOS_RCVR_TIMEOUT; } } diff --git a/flight/PiOS/STM32F10x/pios_spektrum.c b/flight/PiOS/STM32F10x/pios_spektrum.c index dab724ef8..53142069a 100644 --- a/flight/PiOS/STM32F10x/pios_spektrum.c +++ b/flight/PiOS/STM32F10x/pios_spektrum.c @@ -338,8 +338,8 @@ static void PIOS_SPEKTRUM_Supervisor(uint32_t spektrum_id) /* signal lost */ fsm->sync_of = 0; for (int i = 0; i < PIOS_SPEKTRUM_NUM_INPUTS; i++) { - fsm->CaptureValue[i] = 0; - fsm->CaptureValueTemp[i] = 0; + fsm->CaptureValue[i] = PIOS_RCVR_TIMEOUT; + fsm->CaptureValueTemp[i] = PIOS_RCVR_TIMEOUT; } } spektrum_dev->supv_timer = 0; diff --git a/flight/PiOS/inc/pios_rcvr.h b/flight/PiOS/inc/pios_rcvr.h index 0f4e6a973..ce28d37af 100644 --- a/flight/PiOS/inc/pios_rcvr.h +++ b/flight/PiOS/inc/pios_rcvr.h @@ -39,6 +39,10 @@ struct pios_rcvr_driver { /* Public Functions */ extern int32_t PIOS_RCVR_Read(uint32_t rcvr_id, uint8_t channel); +#define PIOS_RCVR_TIMEOUT 0 +#define PIOS_RCVR_NODRIVER 65534 +#define PIOS_RCVR_INVALID 65535 + #endif /* PIOS_RCVR_H */ /** diff --git a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj index ceca7eafb..da0793b51 100644 --- a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj +++ b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj @@ -91,6 +91,12 @@ 65632DF51251650300469B77 /* pios_board.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_board.h; sourceTree = ""; }; 65632DF61251650300469B77 /* STM32103CB_AHRS.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = STM32103CB_AHRS.h; sourceTree = ""; }; 65632DF71251650300469B77 /* STM3210E_OP.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = STM3210E_OP.h; sourceTree = ""; }; + 65643CAB1413322000A32F59 /* pios_rcvr_priv.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_rcvr_priv.h; sourceTree = ""; }; + 65643CAC1413322000A32F59 /* pios_rcvr.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_rcvr.h; sourceTree = ""; }; + 65643CAD1413322000A32F59 /* pios_rtc_priv.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_rtc_priv.h; sourceTree = ""; }; + 65643CAE1413322000A32F59 /* pios_sbus_priv.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_sbus_priv.h; sourceTree = ""; }; + 65643CAF1413322000A32F59 /* pios_sbus.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_sbus.h; sourceTree = ""; }; + 65643CB01413322000A32F59 /* pios_spektrum_priv.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_spektrum_priv.h; sourceTree = ""; }; 6572CB1613D0F2B200FC2972 /* link_STM32103CB_CC_Rev1_memory.ld */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; name = link_STM32103CB_CC_Rev1_memory.ld; path = ../../PiOS/STM32F10x/link_STM32103CB_CC_Rev1_memory.ld; sourceTree = SOURCE_ROOT; }; 6572CB1713D0F2B200FC2972 /* link_STM32103CB_CC_Rev1_sections.ld */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; name = link_STM32103CB_CC_Rev1_sections.ld; path = ../../PiOS/STM32F10x/link_STM32103CB_CC_Rev1_sections.ld; sourceTree = SOURCE_ROOT; }; 657CEEAD121DB6C8007A1FBE /* homelocation.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = homelocation.xml; sourceTree = ""; }; @@ -7724,11 +7730,17 @@ 65E8F04811EFF25C00BBF654 /* pios_ppm.h */, 65E8F04911EFF25C00BBF654 /* pios_pwm.h */, 657FF86A12EA8BFB00801617 /* pios_pwm_priv.h */, + 65643CAC1413322000A32F59 /* pios_rcvr.h */, + 65643CAB1413322000A32F59 /* pios_rcvr_priv.h */, 6589A9E2131DF1C7006BD67C /* pios_rtc.h */, + 65643CAD1413322000A32F59 /* pios_rtc_priv.h */, + 65643CAE1413322000A32F59 /* pios_sbus_priv.h */, + 65643CAF1413322000A32F59 /* pios_sbus.h */, 65E8F04A11EFF25C00BBF654 /* pios_sdcard.h */, 65E8F04B11EFF25C00BBF654 /* pios_servo.h */, 65FBE14412E7C98100176B5A /* pios_servo_priv.h */, 65E8F04C11EFF25C00BBF654 /* pios_spektrum.h */, + 65643CB01413322000A32F59 /* pios_spektrum_priv.h */, 65E8F04D11EFF25C00BBF654 /* pios_spi.h */, 65E8F04E11EFF25C00BBF654 /* pios_spi_priv.h */, 65E8F04F11EFF25C00BBF654 /* pios_stm32.h */, From d3de8ff0effe2616e837adc1ca4586ed91f2d085 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sat, 3 Sep 2011 23:57:51 -0500 Subject: [PATCH 2/8] OP-559: Process the arm status when disconnect and allow it to timeout and disarm --- flight/Modules/ManualControl/manualcontrol.c | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/flight/Modules/ManualControl/manualcontrol.c b/flight/Modules/ManualControl/manualcontrol.c index 3e2662bce..db9e75dc4 100644 --- a/flight/Modules/ManualControl/manualcontrol.c +++ b/flight/Modules/ManualControl/manualcontrol.c @@ -278,7 +278,6 @@ static void manualControlTask(void *parameters) // Important: Throttle < 0 will reset Stabilization coefficients among other things. Either change this, // or leave throttle at IDLE speed or above when going into AUTO-failsafe. AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); - ManualControlCommandSet(&cmd); } else { AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL); @@ -314,13 +313,15 @@ static void manualControlTask(void *parameters) processFlightMode(&settings, flightMode); - processArm(&cmd, &settings); - - // Update cmd object - ManualControlCommandSet(&cmd); } + // Process arming outside conditional so system will disarm when disconnected + processArm(&cmd, &settings); + + // Update cmd object + ManualControlCommandSet(&cmd); + } else { ManualControlCommandGet(&cmd); /* Under GCS control */ } @@ -645,7 +646,7 @@ static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData } else { // Not really needed since this function not called when disconnected if (cmd->Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) - return; + lowThrottle = true; // The throttle is not low, in case we where arming or disarming, abort if (!lowThrottle) { From 20de0462924eb6c4c8ef22091a31ed663c816df4 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sun, 4 Sep 2011 00:07:34 -0500 Subject: [PATCH 3/8] Force system to be disarmed when a bad configuration is present --- flight/Modules/ManualControl/manualcontrol.c | 6 ++++++ .../OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj | 2 ++ 2 files changed, 8 insertions(+) diff --git a/flight/Modules/ManualControl/manualcontrol.c b/flight/Modules/ManualControl/manualcontrol.c index db9e75dc4..de8937f92 100644 --- a/flight/Modules/ManualControl/manualcontrol.c +++ b/flight/Modules/ManualControl/manualcontrol.c @@ -81,6 +81,7 @@ static void updateActuatorDesired(ManualControlCommandData * cmd); static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings); static void processFlightMode(ManualControlSettingsData * settings, float flightMode); static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings); +static void setArmedIfChanged(uint8_t val); static void manualControlTask(void *parameters); static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral); @@ -249,6 +250,11 @@ static void manualControlTask(void *parameters) AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; ManualControlCommandSet(&cmd); + + // Need to do this here since we don't process armed status. Since this shouldn't happen in flight (changed config) + // immediately disarm + setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); + continue; } diff --git a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj index da0793b51..0a0834048 100644 --- a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj +++ b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj @@ -97,6 +97,7 @@ 65643CAE1413322000A32F59 /* pios_sbus_priv.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_sbus_priv.h; sourceTree = ""; }; 65643CAF1413322000A32F59 /* pios_sbus.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_sbus.h; sourceTree = ""; }; 65643CB01413322000A32F59 /* pios_spektrum_priv.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_spektrum_priv.h; sourceTree = ""; }; + 65643CB91413456D00A32F59 /* pios_tim.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_tim.c; sourceTree = ""; }; 6572CB1613D0F2B200FC2972 /* link_STM32103CB_CC_Rev1_memory.ld */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; name = link_STM32103CB_CC_Rev1_memory.ld; path = ../../PiOS/STM32F10x/link_STM32103CB_CC_Rev1_memory.ld; sourceTree = SOURCE_ROOT; }; 6572CB1713D0F2B200FC2972 /* link_STM32103CB_CC_Rev1_sections.ld */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; name = link_STM32103CB_CC_Rev1_sections.ld; path = ../../PiOS/STM32F10x/link_STM32103CB_CC_Rev1_sections.ld; sourceTree = SOURCE_ROOT; }; 657CEEAD121DB6C8007A1FBE /* homelocation.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = homelocation.xml; sourceTree = ""; }; @@ -7782,6 +7783,7 @@ 65E8F0E711EFF25C00BBF654 /* pios_spektrum.c */, 65E8F0E811EFF25C00BBF654 /* pios_spi.c */, 65E8F0E911EFF25C00BBF654 /* pios_sys.c */, + 65643CB91413456D00A32F59 /* pios_tim.c */, 65E8F0EA11EFF25C00BBF654 /* pios_usart.c */, 65E8F0ED11EFF25C00BBF654 /* pios_usb_hid.c */, 651CF9E5120B5D8300EEFD70 /* pios_usb_hid_desc.c */, From 51967ae63f3c768f3afc56f5037d4cada88c5430 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sun, 4 Sep 2011 01:14:54 -0500 Subject: [PATCH 4/8] OP-571 PIOS_PWM: Add back the PWM supervisor --- flight/PiOS/STM32F10x/pios_pwm.c | 24 +++++++++++++++++++++++- 1 file changed, 23 insertions(+), 1 deletion(-) diff --git a/flight/PiOS/STM32F10x/pios_pwm.c b/flight/PiOS/STM32F10x/pios_pwm.c index 9717f66fc..392ab7f32 100644 --- a/flight/PiOS/STM32F10x/pios_pwm.c +++ b/flight/PiOS/STM32F10x/pios_pwm.c @@ -42,6 +42,8 @@ const struct pios_rcvr_driver pios_pwm_rcvr_driver = { }; /* Local Variables */ +/* 100 ms timeout without updates on channels */ +const static uint32_t PWM_SUPERVISOR_TIMEOUT = 100000; enum pios_pwm_dev_magic { PIOS_PWM_DEV_MAGIC = 0xab30293c, @@ -56,6 +58,7 @@ struct pios_pwm_dev { uint16_t FallValue[PIOS_PWM_NUM_INPUTS]; uint32_t CaptureValue[PIOS_PWM_NUM_INPUTS]; uint32_t CapCounter[PIOS_PWM_NUM_INPUTS]; + uint32_t us_since_update[PIOS_PWM_NUM_INPUTS]; }; static bool PIOS_PWM_validate(struct pios_pwm_dev * pwm_dev) @@ -120,7 +123,7 @@ int32_t PIOS_PWM_Init(uint32_t * pwm_id, const struct pios_pwm_cfg * cfg) pwm_dev->CaptureState[i] = 0; pwm_dev->RiseValue[i] = 0; pwm_dev->FallValue[i] = 0; - pwm_dev->CaptureValue[i] = 0; + pwm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT; } uint32_t tim_id; @@ -152,6 +155,10 @@ int32_t PIOS_PWM_Init(uint32_t * pwm_id, const struct pios_pwm_cfg * cfg) TIM_ITConfig(chan->timer, TIM_IT_CC4, ENABLE); break; } + + // Need the update event for that timer to detect timeouts + TIM_ITConfig(chan->timer, TIM_IT_Update, ENABLE); + } *pwm_id = (uint32_t) pwm_dev; @@ -193,6 +200,20 @@ static void PIOS_PWM_tim_overflow_cb (uint32_t tim_id, uint32_t context, uint8_t return; } + if (channel >= pwm_dev->cfg->num_channels) { + /* Channel out of range */ + return; + } + + pwm_dev->us_since_update[channel] += count; + if(pwm_dev->us_since_update[channel] >= PWM_SUPERVISOR_TIMEOUT) { + pwm_dev->CaptureState[channel] = 0; + pwm_dev->RiseValue[channel] = 0; + pwm_dev->FallValue[channel] = 0; + pwm_dev->CaptureValue[channel] = PIOS_RCVR_TIMEOUT; + pwm_dev->us_since_update[channel] = 0; + } + return; } @@ -215,6 +236,7 @@ static void PIOS_PWM_tim_edge_cb (uint32_t tim_id, uint32_t context, uint8_t cha if (pwm_dev->CaptureState[chan_idx] == 0) { pwm_dev->RiseValue[chan_idx] = count; + pwm_dev->us_since_update[chan_idx] = 0; } else { pwm_dev->FallValue[chan_idx] = count; } From 89e640ae7fcb5fb9ef22f51cde2accbecc7c8f83 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sun, 4 Sep 2011 01:23:59 -0500 Subject: [PATCH 5/8] Make sure all receiver drivers return correct constants for invalid channels. --- flight/Modules/ManualControl/manualcontrol.c | 22 +++++++++---------- flight/PiOS/STM32F10x/pios_ppm.c | 4 ++-- flight/PiOS/STM32F10x/pios_pwm.c | 4 ++-- flight/PiOS/STM32F10x/pios_sbus.c | 2 +- flight/PiOS/STM32F10x/pios_spektrum.c | 6 ++--- flight/PiOS/inc/pios_rcvr.h | 4 ++-- .../OpenPilotOSX.xcodeproj/project.pbxproj | 2 ++ 7 files changed, 22 insertions(+), 22 deletions(-) diff --git a/flight/Modules/ManualControl/manualcontrol.c b/flight/Modules/ManualControl/manualcontrol.c index de8937f92..ef0719510 100644 --- a/flight/Modules/ManualControl/manualcontrol.c +++ b/flight/Modules/ManualControl/manualcontrol.c @@ -222,8 +222,6 @@ static void manualControlTask(void *parameters) // until we decide to go to failsafe if(cmd.Channel[n] == PIOS_RCVR_TIMEOUT) valid_input_detected = false; - else if (cmd.Channel[n] == PIOS_RCVR_INVALID) - scaledChannel[n] = 0; else scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax[n], settings.ChannelMin[n], settings.ChannelNeutral[n]); } @@ -235,17 +233,17 @@ static void manualControlTask(void *parameters) settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE || settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE || // Check all channel mappings are valid - cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == PIOS_RCVR_INVALID || - cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == PIOS_RCVR_INVALID || - cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == PIOS_RCVR_INVALID || - cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == PIOS_RCVR_INVALID || - cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == PIOS_RCVR_INVALID || + cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t) PIOS_RCVR_INVALID || + cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t) PIOS_RCVR_INVALID || + cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t) PIOS_RCVR_INVALID || + cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t) PIOS_RCVR_INVALID || + cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t) PIOS_RCVR_INVALID || // Check the driver is exists - cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == PIOS_RCVR_NODRIVER || - cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == PIOS_RCVR_NODRIVER || - cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == PIOS_RCVR_NODRIVER || - cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == PIOS_RCVR_NODRIVER || - cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == PIOS_RCVR_NODRIVER) { + cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t) PIOS_RCVR_NODRIVER || + cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t) PIOS_RCVR_NODRIVER || + cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t) PIOS_RCVR_NODRIVER || + cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t) PIOS_RCVR_NODRIVER || + cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t) PIOS_RCVR_NODRIVER) { AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; diff --git a/flight/PiOS/STM32F10x/pios_ppm.c b/flight/PiOS/STM32F10x/pios_ppm.c index bee955861..0dd9aabba 100644 --- a/flight/PiOS/STM32F10x/pios_ppm.c +++ b/flight/PiOS/STM32F10x/pios_ppm.c @@ -211,12 +211,12 @@ static int32_t PIOS_PPM_Get(uint32_t rcvr_id, uint8_t channel) if (!PIOS_PPM_validate(ppm_dev)) { /* Invalid device specified */ - return -1; + return PIOS_RCVR_INVALID; } if (channel >= PIOS_PPM_IN_MAX_NUM_CHANNELS) { /* Channel out of range */ - return -1; + return PIOS_RCVR_INVALID; } return ppm_dev->CaptureValue[channel]; } diff --git a/flight/PiOS/STM32F10x/pios_pwm.c b/flight/PiOS/STM32F10x/pios_pwm.c index 392ab7f32..b49ce1abf 100644 --- a/flight/PiOS/STM32F10x/pios_pwm.c +++ b/flight/PiOS/STM32F10x/pios_pwm.c @@ -181,12 +181,12 @@ static int32_t PIOS_PWM_Get(uint32_t rcvr_id, uint8_t channel) if (!PIOS_PWM_validate(pwm_dev)) { /* Invalid device specified */ - return -1; + return PIOS_RCVR_INVALID; } if (channel >= PIOS_PWM_NUM_INPUTS) { /* Channel out of range */ - return -1; + return PIOS_RCVR_INVALID; } return pwm_dev->CaptureValue[channel]; } diff --git a/flight/PiOS/STM32F10x/pios_sbus.c b/flight/PiOS/STM32F10x/pios_sbus.c index a80fe8fe3..4c1c90618 100644 --- a/flight/PiOS/STM32F10x/pios_sbus.c +++ b/flight/PiOS/STM32F10x/pios_sbus.c @@ -183,7 +183,7 @@ static int32_t PIOS_SBUS_Get(uint32_t rcvr_id, uint8_t channel) { /* return error if channel is not available */ if (channel >= SBUS_NUMBER_OF_CHANNELS) { - return -1; + return PIOS_RCVR_INVALID; } return channel_data[channel]; } diff --git a/flight/PiOS/STM32F10x/pios_spektrum.c b/flight/PiOS/STM32F10x/pios_spektrum.c index 53142069a..24c034e88 100644 --- a/flight/PiOS/STM32F10x/pios_spektrum.c +++ b/flight/PiOS/STM32F10x/pios_spektrum.c @@ -264,12 +264,12 @@ static int32_t PIOS_SPEKTRUM_Get(uint32_t rcvr_id, uint8_t channel) { struct pios_spektrum_dev * spektrum_dev = (struct pios_spektrum_dev *)rcvr_id; - bool valid = PIOS_SPEKTRUM_validate(spektrum_dev); - PIOS_Assert(valid); + if(!PIOS_SPEKTRUM_validate(spektrum_dev)) + return PIOS_RCVR_INVALID; /* Return error if channel not available */ if (channel >= PIOS_SPEKTRUM_NUM_INPUTS) { - return -1; + return PIOS_RCVR_INVALID; } return spektrum_dev->fsm.CaptureValue[channel]; } diff --git a/flight/PiOS/inc/pios_rcvr.h b/flight/PiOS/inc/pios_rcvr.h index ce28d37af..a61798d8c 100644 --- a/flight/PiOS/inc/pios_rcvr.h +++ b/flight/PiOS/inc/pios_rcvr.h @@ -40,8 +40,8 @@ struct pios_rcvr_driver { extern int32_t PIOS_RCVR_Read(uint32_t rcvr_id, uint8_t channel); #define PIOS_RCVR_TIMEOUT 0 -#define PIOS_RCVR_NODRIVER 65534 -#define PIOS_RCVR_INVALID 65535 +#define PIOS_RCVR_NODRIVER -2 +#define PIOS_RCVR_INVALID -1 #endif /* PIOS_RCVR_H */ diff --git a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj index 0a0834048..a51d5a08d 100644 --- a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj +++ b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj @@ -98,6 +98,7 @@ 65643CAF1413322000A32F59 /* pios_sbus.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_sbus.h; sourceTree = ""; }; 65643CB01413322000A32F59 /* pios_spektrum_priv.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_spektrum_priv.h; sourceTree = ""; }; 65643CB91413456D00A32F59 /* pios_tim.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_tim.c; sourceTree = ""; }; + 65643CBA141350C200A32F59 /* pios_sbus.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_sbus.c; sourceTree = ""; }; 6572CB1613D0F2B200FC2972 /* link_STM32103CB_CC_Rev1_memory.ld */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; name = link_STM32103CB_CC_Rev1_memory.ld; path = ../../PiOS/STM32F10x/link_STM32103CB_CC_Rev1_memory.ld; sourceTree = SOURCE_ROOT; }; 6572CB1713D0F2B200FC2972 /* link_STM32103CB_CC_Rev1_sections.ld */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; name = link_STM32103CB_CC_Rev1_sections.ld; path = ../../PiOS/STM32F10x/link_STM32103CB_CC_Rev1_sections.ld; sourceTree = SOURCE_ROOT; }; 657CEEAD121DB6C8007A1FBE /* homelocation.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = homelocation.xml; sourceTree = ""; }; @@ -7779,6 +7780,7 @@ 65E8F0E411EFF25C00BBF654 /* pios_ppm.c */, 65E8F0E511EFF25C00BBF654 /* pios_pwm.c */, 6589A9DB131DEE76006BD67C /* pios_rtc.c */, + 65643CBA141350C200A32F59 /* pios_sbus.c */, 65E8F0E611EFF25C00BBF654 /* pios_servo.c */, 65E8F0E711EFF25C00BBF654 /* pios_spektrum.c */, 65E8F0E811EFF25C00BBF654 /* pios_spi.c */, From 510a1760caf0f348a79974b66176ba05133113de Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sun, 4 Sep 2011 03:45:36 -0500 Subject: [PATCH 6/8] When disconnected set any accessory channels to neutral. Otherwise the actuator module could keep the pitch at high. However the "right" thing to do seems very specific. For example negative pitch is probably preferable. --- flight/Modules/ManualControl/manualcontrol.c | 24 ++++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/flight/Modules/ManualControl/manualcontrol.c b/flight/Modules/ManualControl/manualcontrol.c index ef0719510..eab3605be 100644 --- a/flight/Modules/ManualControl/manualcontrol.c +++ b/flight/Modules/ManualControl/manualcontrol.c @@ -282,6 +282,30 @@ static void manualControlTask(void *parameters) // Important: Throttle < 0 will reset Stabilization coefficients among other things. Either change this, // or leave throttle at IDLE speed or above when going into AUTO-failsafe. AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + + AccessoryDesiredData accessory; + // Set Accessory 0 + if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0] != + MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + accessory.AccessoryVal = 0; + if(AccessoryDesiredInstSet(0, &accessory) != 0) + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + } + // Set Accessory 1 + if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] != + MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + accessory.AccessoryVal = 0; + if(AccessoryDesiredInstSet(1, &accessory) != 0) + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + } + // Set Accessory 2 + if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] != + MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + accessory.AccessoryVal = 0; + if(AccessoryDesiredInstSet(2, &accessory) != 0) + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + } + } else { AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL); From 533ae9bb41600eff87396abaaff4baa6f30cd785 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sun, 4 Sep 2011 12:15:34 -0500 Subject: [PATCH 7/8] SBUS: Missed handling the S.Bus failsafe. Now returns PIOS_RVCR_TIMEOUT for either SBus failsafe mode or when no data for 100 ms. --- flight/PiOS/STM32F10x/pios_sbus.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/PiOS/STM32F10x/pios_sbus.c b/flight/PiOS/STM32F10x/pios_sbus.c index 4c1c90618..543da364b 100644 --- a/flight/PiOS/STM32F10x/pios_sbus.c +++ b/flight/PiOS/STM32F10x/pios_sbus.c @@ -59,7 +59,7 @@ static void PIOS_SBUS_Supervisor(uint32_t sbus_id); static void reset_channels(void) { for (int i = 0; i < SBUS_NUMBER_OF_CHANNELS; i++) { - channel_data[i] = 0; + channel_data[i] = PIOS_RCVR_TIMEOUT; } } From 82c5f9f0f4f3af0a0709b9022f48e0e760f60c15 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sun, 4 Sep 2011 12:37:39 -0500 Subject: [PATCH 8/8] PIOS_RCVR: Document return values better and use enum for them --- flight/PiOS/Common/pios_rcvr.c | 9 +++++++++ flight/PiOS/inc/pios_rcvr.h | 12 +++++++++--- 2 files changed, 18 insertions(+), 3 deletions(-) diff --git a/flight/PiOS/Common/pios_rcvr.c b/flight/PiOS/Common/pios_rcvr.c index b76e04a44..bd8cce3cd 100644 --- a/flight/PiOS/Common/pios_rcvr.c +++ b/flight/PiOS/Common/pios_rcvr.c @@ -76,6 +76,15 @@ out_fail: return(-1); } +/** + * @brief Reads an input channel from the appropriate driver + * @param[in] rcvr_id driver to read from + * @param[in] channel channel to read + * @returns Unitless input value + * @retval PIOS_RCVR_TIMEOUT indicates a failsafe or timeout from that channel + * @retval PIOS_RCVR_INVALID invalid channel for this driver (usually out of range supported) + * @retval PIOS_RCVR_NODRIVER driver was not initialized + */ int32_t PIOS_RCVR_Read(uint32_t rcvr_id, uint8_t channel) { if (rcvr_id == 0) diff --git a/flight/PiOS/inc/pios_rcvr.h b/flight/PiOS/inc/pios_rcvr.h index a61798d8c..ab493cd35 100644 --- a/flight/PiOS/inc/pios_rcvr.h +++ b/flight/PiOS/inc/pios_rcvr.h @@ -39,9 +39,15 @@ struct pios_rcvr_driver { /* Public Functions */ extern int32_t PIOS_RCVR_Read(uint32_t rcvr_id, uint8_t channel); -#define PIOS_RCVR_TIMEOUT 0 -#define PIOS_RCVR_NODRIVER -2 -#define PIOS_RCVR_INVALID -1 +/*! Define error codes for PIOS_RCVR_Get */ +enum PIOS_RCVR_errors { + /*! Indicates that a failsafe condition or missing receiver detected for that channel */ + PIOS_RCVR_TIMEOUT = 0, + /*! Channel is invalid for this driver (usually out of range supported) */ + PIOS_RCVR_INVALID = -1, + /*! Indicates that the driver for this channel has not been initialized */ + PIOS_RCVR_NODRIVER = -2 +}; #endif /* PIOS_RCVR_H */