From f7f408b569e8817f24a412cb0a512a9eb2169822 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 10 Jan 2013 17:22:57 -0800 Subject: [PATCH 1/5] Fix compile errors --- .../Projects/arduino-usbserial/Descriptors.c | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/Tools/ArduPPM/ATMega32U2/Projects/arduino-usbserial/Descriptors.c b/Tools/ArduPPM/ATMega32U2/Projects/arduino-usbserial/Descriptors.c index c26f81d61d..4ad4b52be4 100644 --- a/Tools/ArduPPM/ATMega32U2/Projects/arduino-usbserial/Descriptors.c +++ b/Tools/ArduPPM/ATMega32U2/Projects/arduino-usbserial/Descriptors.c @@ -54,7 +54,7 @@ * number of device configurations. The descriptor is read out by the USB host when the enumeration * process begins. */ -USB_Descriptor_Device_t PROGMEM DeviceDescriptor = +const USB_Descriptor_Device_t PROGMEM DeviceDescriptor = { .Header = {.Size = sizeof(USB_Descriptor_Device_t), .Type = DTYPE_Device}, @@ -91,7 +91,7 @@ USB_Descriptor_Device_t PROGMEM DeviceDescriptor = * and endpoints. The descriptor is read out by the USB host during the enumeration process when selecting * a configuration so that the host may correctly communicate with the USB device. */ -USB_Descriptor_Configuration_t PROGMEM ConfigurationDescriptor = +const USB_Descriptor_Configuration_t PROGMEM ConfigurationDescriptor = { .Config = { @@ -199,7 +199,7 @@ USB_Descriptor_Configuration_t PROGMEM ConfigurationDescriptor = * the string descriptor with index 0 (the first index). It is actually an array of 16-bit integers, which indicate * via the language ID table available at USB.org what languages the device supports for its string descriptors. */ -USB_Descriptor_String_t PROGMEM LanguageString = +const USB_Descriptor_String_t PROGMEM LanguageString = { .Header = {.Size = USB_STRING_LEN(1), .Type = DTYPE_String}, @@ -210,7 +210,7 @@ USB_Descriptor_String_t PROGMEM LanguageString = * form, and is read out upon request by the host when the appropriate string ID is requested, listed in the Device * Descriptor. */ -USB_Descriptor_String_t PROGMEM ManufacturerString = +const USB_Descriptor_String_t PROGMEM ManufacturerString = { .Header = {.Size = USB_STRING_LEN(24), .Type = DTYPE_String}, @@ -221,7 +221,7 @@ USB_Descriptor_String_t PROGMEM ManufacturerString = * and is read out upon request by the host when the appropriate string ID is requested, listed in the Device * Descriptor. */ -USB_Descriptor_String_t PROGMEM ProductString = +const USB_Descriptor_String_t PROGMEM ProductString = { #if (ARDUINO_MODEL_PID == ARDUINO_UNO_PID) .Header = {.Size = USB_STRING_LEN(11), .Type = DTYPE_String}, From 71b2c3e13da8b9416259a16cdc24d640348dec76 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 10 Jan 2013 17:45:26 -0800 Subject: [PATCH 2/5] ArduPPM: throttle failsafe is now triggered on single channel loss but recovery from the throttle low is possible --- Tools/ArduPPM/ATMega328p/Encoder-PPM.c | 12 ++++--- Tools/ArduPPM/Libraries/PPM_Encoder.h | 45 ++++++++++++++++++++++++-- 2 files changed, 49 insertions(+), 8 deletions(-) diff --git a/Tools/ArduPPM/ATMega328p/Encoder-PPM.c b/Tools/ArduPPM/ATMega328p/Encoder-PPM.c index 3b7eb5f81b..32752d880e 100644 --- a/Tools/ArduPPM/ATMega328p/Encoder-PPM.c +++ b/Tools/ArduPPM/ATMega328p/Encoder-PPM.c @@ -35,11 +35,12 @@ // 0.9.85 : Added brownout reset detection flag // 0.9.86 : Added a #define to disable Radio Passthrough mode (hardware failsafe for Arduplane) // 0.9.87 : #define correction for radio passthrough (was screwed up). +// 0.9.88 : LED fail-safe indication is on whenever throttle is low // ------------------------------------------------------------------------------------------------------------------------------------------------------------ // PREPROCESSOR DIRECTIVES // ------------------------------------------------------------------------------------------------------------------------------------------------------------ -#include "..\Libraries\PPM_Encoder.h" +#include "../Libraries/PPM_Encoder.h" #include @@ -47,7 +48,7 @@ #define ERROR_DETECTION_WINDOW 3000 * LOOP_TIMER_10MS // Detection window for error detection (default to 30s) #define ERROR_CONDITION_DELAY 500 * LOOP_TIMER_10MS // Servo error condition LED delay (LED blinking duration) -#define PASSTHROUGH_MODE_ENABLED // Comment this line to remove CH8 radio passthrough mode support (hardware failsafe for Arduplane) +//#define PASSTHROUGH_MODE_ENABLED // Comment this line to remove CH8 radio passthrough mode support (hardware failsafe for Arduplane) #define PASSTHROUGH_CHANNEL 8 * 2 // Channel for passthrough mode selection #define PASSTHROUGH_CHANNEL_OFF_US ONE_US * 1600 - PPM_PRE_PULSE // Passthrough off threshold #define PASSTHROUGH_CHANNEL_ON_US ONE_US * 1800 - PPM_PRE_PULSE // Passthrough on threshold @@ -283,7 +284,7 @@ int main(void) // ------------------------------------------------------------------------------ while( 1 ) { - if ( servo_error_condition || servo_input_missing ) // We have an error + if ( throttle_failsafe_force ) // We have an error { blink_led ( 6 * LOOP_TIMER_10MS ); // Status LED very fast blink if invalid servo input or missing signal } @@ -368,7 +369,8 @@ int main(void) // ------------------------------------------------------------------------------ // Status LED control // ------------------------------------------------------------------------------ - if ( servo_error_condition || servo_input_missing ) // We have an error + + if ( throttle_failsafe_force ) // We have an error { blink_led ( 6 * LOOP_TIMER_10MS ); // Status LED very fast blink if invalid servo input or missing signal } @@ -454,7 +456,7 @@ int main(void) // Status LED control // ------------------------------------------------------------------------------ - if ( servo_input_missing ) // We have an error + if ( throttle_failsafe_force ) // We have an error { blink_led ( 6 * LOOP_TIMER_10MS ); // Status LED very fast blink if invalid servo input or missing signal } diff --git a/Tools/ArduPPM/Libraries/PPM_Encoder.h b/Tools/ArduPPM/Libraries/PPM_Encoder.h index 5e1dbd313e..f15c8537b6 100644 --- a/Tools/ArduPPM/Libraries/PPM_Encoder.h +++ b/Tools/ArduPPM/Libraries/PPM_Encoder.h @@ -1,5 +1,5 @@ // ------------------------------------------------------------- -// ArduPPM (PPM Encoder) V2.3.13 +// ArduPPM (PPM Encoder) V2.3.14pre // ------------------------------------------------------------- // Improved servo to ppm for ArduPilot MEGA v1.x (ATmega328p), // PhoneDrone and APM2.x (ATmega32u2) @@ -127,6 +127,12 @@ // V2.3.13 - Official release // - Fail-safe vales changed back to normal fail-safe values. Use until APM code knows how to handle lost channel signal (800us) +// 16-12-2012 +// V2.3.14pre - Internal test release +// - If one or more or all channel(s) are disconnected, throttle is set to fail-safe low (RTL) +// - If the misssing channel(s) are regained, throttle control is regained + + // ------------------------------------------------------------- #ifndef _PPM_ENCODER_H_ @@ -168,7 +174,7 @@ #endif // Version stamp for firmware hex file ( decode hex file using and look for "ArduPPM" string ) -const char ver[15] = "ArduPPMv2.3.13"; +const char ver[15] = "ArduPPMv2.3.14pre"; // ------------------------------------------------------------- // INPUT MODE @@ -322,6 +328,9 @@ volatile uint8_t ppm_timeout[ PPM_ARRAY_MAX ]; #define SERVO_INPUT_CONNECTED_VALUE 100 volatile uint8_t servo_input_connected[ PPM_ARRAY_MAX ]; +// count the channels which have been once connected but then got disconnected +volatile uint8_t disconnected_channels; + // AVR parameters for PhoneDrone and APM2 boards using ATmega32u2 #if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__) @@ -413,6 +422,8 @@ volatile bool ppm_generator_active = false; // Used to indicate a brownout restart volatile bool brownout_reset = false; +// Used to force throttle fail-safe mode (RTL) +volatile bool throttle_failsafe_force = false; // ------------------------------------------------------------------------------ // PPM GENERATOR START - TOGGLE ON COMPARE INTERRUPT ENABLE @@ -655,8 +666,18 @@ CHECK_PINS_LOOP: // Input servo pin check loop servo_input_connected[ ppm_channel ]++; } - //Reset ppm single channel fail-safe timeout + //Reset ppm single channel fail-safe timeout ppm_timeout[ ppm_channel ] = 0; + + // Check for forced throttle fail-safe + if( throttle_failsafe_force ) + { + if( ppm_channel == 5 ) + { + // Force throttle fail-safe + servo_width = PPM_THROTTLE_FAILSAFE; + } + } #ifdef _AVERAGE_FILTER_ // Average filter to smooth input jitter @@ -754,6 +775,12 @@ ISR( PPM_INT_VECTOR, ISR_NOBLOCK ) PPM_COMPARE += failsafe_ppm[ ppm_out_channel ]; sei(); + // Count the channel that we have lost + if( servo_input_connected[ ppm_out_channel ] ) + { + disconnected_channels++; + } + #if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__) // Turn on RX LED to indicate a fail-safe condition PORTD &= ~(1 << PD4); @@ -777,6 +804,18 @@ ISR( PPM_INT_VECTOR, ISR_NOBLOCK ) { ppm_out_channel = 0; + // Did we lose one or more active servo input channel? If so force throttle fail-safe (RTL) + if( disconnected_channels > 0 ) + { + throttle_failsafe_force = true; + } + else + { + throttle_failsafe_force = false; + } + + disconnected_channels = 0; + #if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__) // Blink TX LED when PPM generator has finished a pulse train PIND |= ( 1<< PD5 ); From 70ce94ee88064f9194e32de35f7278d7ba494952 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 11 Jan 2013 13:19:36 -0800 Subject: [PATCH 3/5] ArduPPM: added a define for throttle low fail-safe indication --- Tools/ArduPPM/Libraries/PPM_Encoder.h | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/Tools/ArduPPM/Libraries/PPM_Encoder.h b/Tools/ArduPPM/Libraries/PPM_Encoder.h index f15c8537b6..6077c08e88 100644 --- a/Tools/ArduPPM/Libraries/PPM_Encoder.h +++ b/Tools/ArduPPM/Libraries/PPM_Encoder.h @@ -127,11 +127,14 @@ // V2.3.13 - Official release // - Fail-safe vales changed back to normal fail-safe values. Use until APM code knows how to handle lost channel signal (800us) -// 16-12-2012 +// 10-01-2013 // V2.3.14pre - Internal test release // - If one or more or all channel(s) are disconnected, throttle is set to fail-safe low (RTL) // - If the misssing channel(s) are regained, throttle control is regained +// 11-01-2013 +// V2.3.14 - temporary release for ArduCopter 2.9 +// - fail-safe throttle low can be set with a define // ------------------------------------------------------------- @@ -174,7 +177,7 @@ #endif // Version stamp for firmware hex file ( decode hex file using and look for "ArduPPM" string ) -const char ver[15] = "ArduPPMv2.3.14pre"; +const char ver[15] = "ArduPPMv2.3.14"; // ------------------------------------------------------------- // INPUT MODE @@ -195,6 +198,8 @@ volatile uint8_t servo_input_mode = JUMPER_SELECT_MODE; //#define _APM_FAILSAFE_ // Used to spesify APM 800us channel loss fail safe values, remove to use normal fail safe values (stand alone encoder board) +#define _THROTTLE_LOW_FAILSAFE_INDICATION //if set, throttle is set to low in an error condition + // ------------------------------------------------------------- // SERVO LIMIT VALUES // ------------------------------------------------------------- @@ -669,6 +674,7 @@ CHECK_PINS_LOOP: // Input servo pin check loop //Reset ppm single channel fail-safe timeout ppm_timeout[ ppm_channel ] = 0; + #ifdef _THROTTLE_LOW_FAILSAFE_INDICATION // Check for forced throttle fail-safe if( throttle_failsafe_force ) { @@ -678,6 +684,7 @@ CHECK_PINS_LOOP: // Input servo pin check loop servo_width = PPM_THROTTLE_FAILSAFE; } } + #endif #ifdef _AVERAGE_FILTER_ // Average filter to smooth input jitter From ef6268f62a1e86e7a27241676d796afcf3926b95 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 11 Jan 2013 15:08:13 -0800 Subject: [PATCH 4/5] ArduPPM: changes for throttle low indication and also recovery after a throttle low indication are now set with defines --- Tools/ArduPPM/ATMega328p/Encoder-PPM.c | 16 ++++++++++++---- Tools/ArduPPM/Libraries/PPM_Encoder.h | 20 ++++++++++++++++++-- 2 files changed, 30 insertions(+), 6 deletions(-) diff --git a/Tools/ArduPPM/ATMega328p/Encoder-PPM.c b/Tools/ArduPPM/ATMega328p/Encoder-PPM.c index 32752d880e..7991ac7057 100644 --- a/Tools/ArduPPM/ATMega328p/Encoder-PPM.c +++ b/Tools/ArduPPM/ATMega328p/Encoder-PPM.c @@ -1,5 +1,5 @@ // ------------------------------------------------------------------------------------------------------------------------------------------------------------ -// ArduPPM Version v0.9.87 +// ArduPPM Version v0.9.89 // ------------------------------------------------------------------------------------------------------------------------------------------------------------ // ARDUCOPTER 2 : PPM ENCODER for AT Mega 328p and APM v1.4 Boards // By:John Arne Birkeland - 2011 @@ -36,6 +36,7 @@ // 0.9.86 : Added a #define to disable Radio Passthrough mode (hardware failsafe for Arduplane) // 0.9.87 : #define correction for radio passthrough (was screwed up). // 0.9.88 : LED fail-safe indication is on whenever throttle is low +// 0.9.89 : LED fail-safe change can be reverted with a define // ------------------------------------------------------------------------------------------------------------------------------------------------------------ // PREPROCESSOR DIRECTIVES // ------------------------------------------------------------------------------------------------------------------------------------------------------------ @@ -48,7 +49,7 @@ #define ERROR_DETECTION_WINDOW 3000 * LOOP_TIMER_10MS // Detection window for error detection (default to 30s) #define ERROR_CONDITION_DELAY 500 * LOOP_TIMER_10MS // Servo error condition LED delay (LED blinking duration) -//#define PASSTHROUGH_MODE_ENABLED // Comment this line to remove CH8 radio passthrough mode support (hardware failsafe for Arduplane) +#define PASSTHROUGH_MODE_ENABLED // Comment this line to remove CH8 radio passthrough mode support (hardware failsafe for Arduplane) #define PASSTHROUGH_CHANNEL 8 * 2 // Channel for passthrough mode selection #define PASSTHROUGH_CHANNEL_OFF_US ONE_US * 1600 - PPM_PRE_PULSE // Passthrough off threshold #define PASSTHROUGH_CHANNEL_ON_US ONE_US * 1800 - PPM_PRE_PULSE // Passthrough on threshold @@ -284,7 +285,11 @@ int main(void) // ------------------------------------------------------------------------------ while( 1 ) { + #if defined _THROTTLE_LOW_RECOVERY_POSSIBLE if ( throttle_failsafe_force ) // We have an error + #else + if ( servo_error_condition || servo_input_missing ) // We have an error + #endif { blink_led ( 6 * LOOP_TIMER_10MS ); // Status LED very fast blink if invalid servo input or missing signal } @@ -369,8 +374,11 @@ int main(void) // ------------------------------------------------------------------------------ // Status LED control // ------------------------------------------------------------------------------ - + #ifdef _THROTTLE_LOW_FAILSAFE_INDICATION if ( throttle_failsafe_force ) // We have an error + #else + if ( servo_error_condition || servo_input_missing ) // We have an error + #endif { blink_led ( 6 * LOOP_TIMER_10MS ); // Status LED very fast blink if invalid servo input or missing signal } @@ -456,7 +464,7 @@ int main(void) // Status LED control // ------------------------------------------------------------------------------ - if ( throttle_failsafe_force ) // We have an error + if ( servo_input_missing ) // We have an error { blink_led ( 6 * LOOP_TIMER_10MS ); // Status LED very fast blink if invalid servo input or missing signal } diff --git a/Tools/ArduPPM/Libraries/PPM_Encoder.h b/Tools/ArduPPM/Libraries/PPM_Encoder.h index 6077c08e88..9d2f4cbfdd 100644 --- a/Tools/ArduPPM/Libraries/PPM_Encoder.h +++ b/Tools/ArduPPM/Libraries/PPM_Encoder.h @@ -1,5 +1,5 @@ // ------------------------------------------------------------- -// ArduPPM (PPM Encoder) V2.3.14pre +// ArduPPM (PPM Encoder) V2.3.14 // ------------------------------------------------------------- // Improved servo to ppm for ArduPilot MEGA v1.x (ATmega328p), // PhoneDrone and APM2.x (ATmega32u2) @@ -135,6 +135,7 @@ // 11-01-2013 // V2.3.14 - temporary release for ArduCopter 2.9 // - fail-safe throttle low can be set with a define +// - recovery from error condition can also be set with a define // ------------------------------------------------------------- @@ -199,6 +200,11 @@ volatile uint8_t servo_input_mode = JUMPER_SELECT_MODE; //#define _APM_FAILSAFE_ // Used to spesify APM 800us channel loss fail safe values, remove to use normal fail safe values (stand alone encoder board) #define _THROTTLE_LOW_FAILSAFE_INDICATION //if set, throttle is set to low in an error condition +#define _THROTTLE_LOW_RECOVERY_POSSIBLE //if set, a channel can be regained when the error disappears, only makes sense together with _THROTTLE_LOW_FAILSAFE_INDICATION + +#if defined _THROTTLE_LOW_RECOVERY_POSSIBLE && !defined _THROTTLE_LOW_FAILSAFE_INDICATION +#error failsafe recovery is only possible with throttle_low_failsafe_indication defined as well +#endif // ------------------------------------------------------------- // SERVO LIMIT VALUES @@ -333,8 +339,10 @@ volatile uint8_t ppm_timeout[ PPM_ARRAY_MAX ]; #define SERVO_INPUT_CONNECTED_VALUE 100 volatile uint8_t servo_input_connected[ PPM_ARRAY_MAX ]; +#ifdef _THROTTLE_LOW_RECOVERY_POSSIBLE // count the channels which have been once connected but then got disconnected volatile uint8_t disconnected_channels; +#endif // AVR parameters for PhoneDrone and APM2 boards using ATmega32u2 #if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__) @@ -427,8 +435,10 @@ volatile bool ppm_generator_active = false; // Used to indicate a brownout restart volatile bool brownout_reset = false; +#ifdef _THROTTLE_LOW_FAILSAFE_INDICATION // Used to force throttle fail-safe mode (RTL) volatile bool throttle_failsafe_force = false; +#endif // ------------------------------------------------------------------------------ // PPM GENERATOR START - TOGGLE ON COMPARE INTERRUPT ENABLE @@ -782,11 +792,15 @@ ISR( PPM_INT_VECTOR, ISR_NOBLOCK ) PPM_COMPARE += failsafe_ppm[ ppm_out_channel ]; sei(); + #if defined _THROTTLE_LOW_RECOVERY_POSSIBLE && defined _THROTTLE_LOW_FAILSAFE_INDICATION // Count the channel that we have lost if( servo_input_connected[ ppm_out_channel ] ) { disconnected_channels++; } + #elif defined _THROTTLE_LOW_FAILSAFE_INDICATION + throttle_failsafe_force = true; + #endif #if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__) // Turn on RX LED to indicate a fail-safe condition @@ -811,7 +825,8 @@ ISR( PPM_INT_VECTOR, ISR_NOBLOCK ) { ppm_out_channel = 0; - // Did we lose one or more active servo input channel? If so force throttle fail-safe (RTL) + #ifdef _THROTTLE_LOW_RECOVERY_POSSIBLE + // Did we lose one or more active servo input channel? If so force throttle fail-safe (RTL) if( disconnected_channels > 0 ) { throttle_failsafe_force = true; @@ -822,6 +837,7 @@ ISR( PPM_INT_VECTOR, ISR_NOBLOCK ) } disconnected_channels = 0; + #endif #if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__) // Blink TX LED when PPM generator has finished a pulse train From 5f84b5a5ddadc30aeb902ddbceb1f3744447f862 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 11 Jan 2013 15:37:03 -0800 Subject: [PATCH 5/5] ArduPPM: latest changes are now disabled by default, throttle low triggering by single channel errors and also recovering from a throttle low event can be enabled with defines --- Tools/ArduPPM/Libraries/PPM_Encoder.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Tools/ArduPPM/Libraries/PPM_Encoder.h b/Tools/ArduPPM/Libraries/PPM_Encoder.h index 9d2f4cbfdd..c582302983 100644 --- a/Tools/ArduPPM/Libraries/PPM_Encoder.h +++ b/Tools/ArduPPM/Libraries/PPM_Encoder.h @@ -199,8 +199,8 @@ volatile uint8_t servo_input_mode = JUMPER_SELECT_MODE; //#define _APM_FAILSAFE_ // Used to spesify APM 800us channel loss fail safe values, remove to use normal fail safe values (stand alone encoder board) -#define _THROTTLE_LOW_FAILSAFE_INDICATION //if set, throttle is set to low in an error condition -#define _THROTTLE_LOW_RECOVERY_POSSIBLE //if set, a channel can be regained when the error disappears, only makes sense together with _THROTTLE_LOW_FAILSAFE_INDICATION +//#define _THROTTLE_LOW_FAILSAFE_INDICATION //if set, throttle is set to low when a single channel is lost +//#define _THROTTLE_LOW_RECOVERY_POSSIBLE //if set, throttle low recovers from being low when the single channel comes back, only makes sense together with _THROTTLE_LOW_FAILSAFE_INDICATION #if defined _THROTTLE_LOW_RECOVERY_POSSIBLE && !defined _THROTTLE_LOW_FAILSAFE_INDICATION #error failsafe recovery is only possible with throttle_low_failsafe_indication defined as well