aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

ArduCopter, APM_RC: failsafe if PPM encoder fails to update for 2 seconds

Browse Source
This commit is contained in:
rmackay9 2012-11-13 18:18:21 +09:00
parent 30e7b8e494
commit 73ed6c9dbf
5 changed files with 25 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
ArduCopter/radio.pde
View File

@ -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);
}
}
}

9
libraries/APM_RC/APM_RC.cpp Normal file
View File

@ -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;

4
libraries/APM_RC/APM_RC.h
View File

@ -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;
};

2
libraries/APM_RC/APM_RC_APM1.cpp
View File

@ -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 {

4
libraries/APM_RC/APM_RC_APM2.cpp
View File

@ -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)