diff --git a/ArduCopter/radio.pde b/ArduCopter/radio.pde index a0205b7f85..c1095b9da4 100644 --- a/ArduCopter/radio.pde +++ b/ArduCopter/radio.pde @@ -124,6 +124,8 @@ void output_min() motors.enable(); motors.output_min(); } + +#define RADIO_FS_TIMEOUT_MS 2000 // 2 seconds static void read_radio() { if (APM_RC.GetState() == 1) { @@ -143,6 +145,11 @@ static void read_radio() // limit our input to 800 so we can still pitch and roll g.rc_3.control_in = min(g.rc_3.control_in, MAXIMUM_THROTTLE); #endif + }else{ + // turn on throttle failsafe if no update from ppm encoder for 2 seconds + if ((millis() - APM_RC.get_last_update() >= RADIO_FS_TIMEOUT_MS) && g.throttle_fs_enabled && motors.armed() && !ap.failsafe) { + set_failsafe(true); + } } } diff --git a/libraries/APM_RC/APM_RC.cpp b/libraries/APM_RC/APM_RC.cpp new file mode 100644 index 0000000000..ac016dfa4e --- /dev/null +++ b/libraries/APM_RC/APM_RC.cpp @@ -0,0 +1,9 @@ +/* + * APM_RC_.cpp - Radio Control Library for Ardupilot Mega. Arduino + * Code by Jordi Muņoz and Jose Julio. DIYDrones.com + * + */ +#include "APM_RC.h" + +uint32_t APM_RC_Class::_last_update; + diff --git a/libraries/APM_RC/APM_RC.h b/libraries/APM_RC/APM_RC.h index 5f370e8d35..d74d5b487d 100644 --- a/libraries/APM_RC/APM_RC.h +++ b/libraries/APM_RC/APM_RC.h @@ -49,10 +49,14 @@ public: virtual void Force_Out2_Out3(void) = 0; virtual void Force_Out6_Out7(void) = 0; + // get the time of the last radio update + virtual uint32_t get_last_update() { return _last_update; }; + protected: uint16_t _map_speed(uint16_t speed_hz) { return 2000000UL / speed_hz; } + static uint32_t _last_update; }; diff --git a/libraries/APM_RC/APM_RC_APM1.cpp b/libraries/APM_RC/APM_RC_APM1.cpp index 0b86be58f9..73c4626d56 100644 --- a/libraries/APM_RC/APM_RC_APM1.cpp +++ b/libraries/APM_RC/APM_RC_APM1.cpp @@ -63,6 +63,7 @@ void APM_RC_APM1::_timer4_capt_cb(void) if (PPM_Counter >= NUM_CHANNELS) { _radio_status = 1; + _last_update = millis(); } } } @@ -316,6 +317,7 @@ bool APM_RC_APM1::setHIL(int16_t v[NUM_CHANNELS]) } } _radio_status = 1; + _last_update = millis(); if (sum == 0) { return 0; } else { diff --git a/libraries/APM_RC/APM_RC_APM2.cpp b/libraries/APM_RC/APM_RC_APM2.cpp index 3ad1abff5e..389d10d07e 100644 --- a/libraries/APM_RC/APM_RC_APM2.cpp +++ b/libraries/APM_RC/APM_RC_APM2.cpp @@ -63,6 +63,7 @@ void APM_RC_APM2::_timer5_capt_cb(void) // If this is the last pulse in a frame, set _radio_status. if (frame_idx >= NUM_CHANNELS) { _radio_status = 1; + _last_update = millis(); } } } @@ -308,12 +309,13 @@ bool APM_RC_APM2::setHIL(int16_t v[NUM_CHANNELS]) sum++; } } + _radio_status = 1; + _last_update = millis(); if (sum == 0) { return 0; } else { return 1; } - _radio_status = 1; } void APM_RC_APM2::clearOverride(void)