APM_RC: Modified get_last_update() to be more interrupt friendly

This commit is contained in:
John Arne Birkeland 2012-11-21 10:34:04 +01:00
parent 704da2a5d9
commit e194c6c740
4 changed files with 18 additions and 9 deletions

View File

@ -5,5 +5,5 @@
*/
#include "APM_RC.h"
volatile uint32_t APM_RC_Class::_last_update;
volatile uint8_t APM_RC_Class::_ppm_count;

View File

@ -50,20 +50,29 @@ public:
virtual void Force_Out2_Out3(void) = 0;
virtual void Force_Out6_Out7(void) = 0;
// get the time of the last radio update (_last_update modified by interrupt, so reading of variable must be interrupt safe)
// get the approximate time of the last radio ppm update
// - resolution of the return time in ms is defined by how often get_last_update() is called
// - _ppm_count is modified by interrupt, but type is volatile uint8_t making it atomic for reading and writting outside the interrupt
virtual uint32_t get_last_update() {
static uint32_t _last_update_time = millis(); // Initialize with current time to prevent false trigger
uint32_t _tmp = _last_update;
while( _tmp != _last_update ) _tmp = _last_update;
// PPM refresh?
if( _ppm_count > 0) {
// Reset _ppm_count
_ppm_count = 0;
return _tmp;
// Approximate time (very much so) of last update
_last_update_time = millis();
}
return _last_update_time;
};
protected:
uint16_t _map_speed(uint16_t speed_hz) {
return 2000000UL / speed_hz;
}
static volatile uint32_t _last_update; // Modified by interrupt
static volatile uint8_t _ppm_count; // Modified by interrupt
};

View File

@ -58,7 +58,7 @@ void APM_RC_APM1::_timer4_capt_cb(void)
// pass through values if at least a minimum number of channels received
if( PPM_Counter >= MIN_CHANNELS ) {
_radio_status = 1;
_last_update = millis();
_ppm_count++;
}
PPM_Counter=0;
}

View File

@ -58,7 +58,7 @@ void APM_RC_APM2::_timer5_capt_cb(void)
// pass through values if at least a minimum number of channels received
if( frame_idx >= MIN_CHANNELS ) {
_radio_status = 1;
_last_update = millis();
_ppm_count++;
}
frame_idx=0;
} else {