mirror of https://github.com/ArduPilot/ardupilot
Revert "APM_RC: Modified get_last_update() to be more interrupt friendly"
This reverts commit 50ba0ce4fa41c6d4101dff8da17c267a65750522. It didn't build, and I want to do a release based on tested code
This commit is contained in:
parent
24adc1aaa3
commit
a71c7b336d
|
@ -5,5 +5,5 @@
|
||||||
*/
|
*/
|
||||||
#include "APM_RC.h"
|
#include "APM_RC.h"
|
||||||
|
|
||||||
volatile uint8_t APM_RC_Class::_ppm_count;
|
volatile uint32_t APM_RC_Class::_last_update;
|
||||||
|
|
||||||
|
|
|
@ -50,29 +50,20 @@ public:
|
||||||
virtual void Force_Out2_Out3(void) = 0;
|
virtual void Force_Out2_Out3(void) = 0;
|
||||||
virtual void Force_Out6_Out7(void) = 0;
|
virtual void Force_Out6_Out7(void) = 0;
|
||||||
|
|
||||||
// get the approximate time of the last radio ppm update
|
// get the time of the last radio update (_last_update modified by interrupt, so reading of variable must be interrupt safe)
|
||||||
// - 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() {
|
virtual uint32_t get_last_update() {
|
||||||
static uint32_t _last_update_time = millis(); // Initialize with current time to prevent false trigger
|
|
||||||
|
|
||||||
// PPM refresh?
|
uint32_t _tmp = _last_update;
|
||||||
if( _ppm_count > 0) {
|
while( _tmp != _last_update ) _tmp = _last_update;
|
||||||
// Reset _ppm_count
|
|
||||||
_ppm_count = 0;
|
|
||||||
|
|
||||||
// Approximate time (very much so) of last update
|
return _tmp;
|
||||||
_last_update_time = millis();
|
|
||||||
}
|
|
||||||
|
|
||||||
return _last_update_time;
|
|
||||||
};
|
};
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
uint16_t _map_speed(uint16_t speed_hz) {
|
uint16_t _map_speed(uint16_t speed_hz) {
|
||||||
return 2000000UL / speed_hz;
|
return 2000000UL / speed_hz;
|
||||||
}
|
}
|
||||||
static volatile uint8_t _ppm_count; // Modified by interrupt
|
static volatile uint32_t _last_update; // Modified by interrupt
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
|
@ -58,7 +58,7 @@ void APM_RC_APM1::_timer4_capt_cb(void)
|
||||||
// pass through values if at least a minimum number of channels received
|
// pass through values if at least a minimum number of channels received
|
||||||
if( PPM_Counter >= MIN_CHANNELS ) {
|
if( PPM_Counter >= MIN_CHANNELS ) {
|
||||||
_radio_status = 1;
|
_radio_status = 1;
|
||||||
_ppm_count++;
|
_last_update = millis();
|
||||||
}
|
}
|
||||||
PPM_Counter=0;
|
PPM_Counter=0;
|
||||||
}
|
}
|
||||||
|
|
|
@ -58,7 +58,7 @@ void APM_RC_APM2::_timer5_capt_cb(void)
|
||||||
// pass through values if at least a minimum number of channels received
|
// pass through values if at least a minimum number of channels received
|
||||||
if( frame_idx >= MIN_CHANNELS ) {
|
if( frame_idx >= MIN_CHANNELS ) {
|
||||||
_radio_status = 1;
|
_radio_status = 1;
|
||||||
_ppm_count++;
|
_last_update = millis();
|
||||||
}
|
}
|
||||||
frame_idx=0;
|
frame_idx=0;
|
||||||
} else {
|
} else {
|
||||||
|
|
Loading…
Reference in New Issue