Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 00:04:02 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_HAL_PX4: Remove timer process suspension interface

Browse Source
This commit is contained in:
Michael du Breuil 2018-05-13 15:47:25 -07:00 committed by Andrew Tridgell
parent 4a9fe1745f
commit a833e93708
4 changed files with 51 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

70
libraries/AP_HAL_PX4/AnalogIn.cpp
View File

@ -79,6 +79,7 @@ PX4AnalogSource::PX4AnalogSource(int16_t pin, float initial_value) :
_sum_value(0),
_sum_ratiometric(0)
{
_semaphore = hal.util->new_semaphore();
#ifdef PX4_ANALOG_VCC_5V_PIN
if (_pin == ANALOG_INPUT_BOARD_VCC) {
_pin = PX4_ANALOG_VCC_5V_PIN;
@ -93,16 +94,19 @@ void PX4AnalogSource::set_stop_pin(uint8_t p)
float PX4AnalogSource::read_average()
{
if (_sum_count == 0) {
if (_semaphore->take(1)) {
if (_sum_count == 0) {
_semaphore->give();
return _value;
}
_value = _sum_value / _sum_count;
_value_ratiometric = _sum_ratiometric / _sum_count;
_sum_value = 0;
_sum_ratiometric = 0;
_sum_count = 0;
_semaphore->give();
return _value;
}
hal.scheduler->suspend_timer_procs();
_value = _sum_value / _sum_count;
_value_ratiometric = _sum_ratiometric / _sum_count;
_sum_value = 0;
_sum_ratiometric = 0;
_sum_count = 0;
hal.scheduler->resume_timer_procs();
return _value;
}
@ -158,15 +162,16 @@ void PX4AnalogSource::set_pin(uint8_t pin)
if (_pin == pin) {
return;
}
hal.scheduler->suspend_timer_procs();
_pin = pin;
_sum_value = 0;
_sum_ratiometric = 0;
_sum_count = 0;
_latest_value = 0;
_value = 0;
_value_ratiometric = 0;
hal.scheduler->resume_timer_procs();
if (_semaphore->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
_pin = pin;
_sum_value = 0;
_sum_ratiometric = 0;
_sum_count = 0;
_latest_value = 0;
_value = 0;
_value_ratiometric = 0;
_semaphore->give();
}
}
/*
@ -174,20 +179,23 @@ void PX4AnalogSource::set_pin(uint8_t pin)
*/
void PX4AnalogSource::_add_value(float v, float vcc5V)
{
_latest_value = v;
_sum_value += v;
if (vcc5V < 3.0f) {
_sum_ratiometric += v;
} else {
// this compensates for changes in the 5V rail relative to the
// 3.3V reference used by the ADC.
_sum_ratiometric += v * 5.0f / vcc5V;
}
_sum_count++;
if (_sum_count == 254) {
_sum_value /= 2;
_sum_ratiometric /= 2;
_sum_count /= 2;
if (_semaphore->take(1)) {
_latest_value = v;
_sum_value += v;
if (vcc5V < 3.0f) {
_sum_ratiometric += v;
} else {
// this compensates for changes in the 5V rail relative to the
// 3.3V reference used by the ADC.
_sum_ratiometric += v * 5.0f / vcc5V;
}
_sum_count++;
if (_sum_count == 254) {
_sum_value /= 2;
_sum_ratiometric /= 2;
_sum_count /= 2;
}
_semaphore->give();
}
}

1
libraries/AP_HAL_PX4/AnalogIn.h
View File

@ -47,6 +47,7 @@ private:
float _sum_ratiometric;
void _add_value(float v, float vcc5V);
float _pin_scaler();
AP_HAL::Semaphore *_semaphore;
};
class PX4::PX4AnalogIn : public AP_HAL::AnalogIn {

40
libraries/AP_HAL_PX4/Scheduler.cpp
View File

@ -230,20 +230,6 @@ void PX4Scheduler::register_timer_failsafe(AP_HAL::Proc failsafe, uint32_t perio
_failsafe = failsafe;
}
void PX4Scheduler::suspend_timer_procs()
{
_timer_suspended = true;
}
void PX4Scheduler::resume_timer_procs()
{
_timer_suspended = false;
if (_timer_event_missed == true) {
_run_timers(false);
_timer_event_missed = false;
}
}
void PX4Scheduler::reboot(bool hold_in_bootloader)
{
// disarm motors to ensure they are off during a bootloader upload
@ -256,22 +242,18 @@ void PX4Scheduler::reboot(bool hold_in_bootloader)
px4_systemreset(hold_in_bootloader);
}
void PX4Scheduler::_run_timers(bool called_from_timer_thread)
void PX4Scheduler::_run_timers()
{
if (_in_timer_proc) {
return;
}
_in_timer_proc = true;
if (!_timer_suspended) {
// now call the timer based drivers
for (int i = 0; i < _num_timer_procs; i++) {
if (_timer_proc[i]) {
_timer_proc[i]();
}
// now call the timer based drivers
for (int i = 0; i < _num_timer_procs; i++) {
if (_timer_proc[i]) {
_timer_proc[i]();
}
} else if (called_from_timer_thread) {
_timer_event_missed = true;
}
// and the failsafe, if one is setup
@ -302,7 +284,7 @@ void *PX4Scheduler::_timer_thread(void *arg)
// run registered timers
perf_begin(sched->_perf_timers);
sched->_run_timers(true);
sched->_run_timers();
perf_end(sched->_perf_timers);
// process any pending RC output requests
@ -329,12 +311,10 @@ void PX4Scheduler::_run_io(void)
}
_in_io_proc = true;
if (!_timer_suspended) {
// now call the IO based drivers
for (int i = 0; i < _num_io_procs; i++) {
if (_io_proc[i]) {
_io_proc[i]();
}
// now call the IO based drivers
for (int i = 0; i < _num_io_procs; i++) {
if (_io_proc[i]) {
_io_proc[i]();
}
}

8
libraries/AP_HAL_PX4/Scheduler.h
View File

@ -53,8 +53,6 @@ public:
void register_timer_process(AP_HAL::MemberProc);
void register_io_process(AP_HAL::MemberProc);
void register_timer_failsafe(AP_HAL::Proc, uint32_t period_us);
void suspend_timer_procs();
void resume_timer_procs();
void reboot(bool hold_in_bootloader);
bool in_main_thread() const override;
@ -82,8 +80,6 @@ private:
uint16_t _min_delay_cb_ms;
AP_HAL::Proc _failsafe;
volatile bool _timer_suspended;
AP_HAL::MemberProc _timer_proc[PX4_SCHEDULER_MAX_TIMER_PROCS];
uint8_t _num_timer_procs;
volatile bool _in_timer_proc;
@ -92,8 +88,6 @@ private:
uint8_t _num_io_procs;
volatile bool _in_io_proc;
volatile bool _timer_event_missed;
pid_t _main_task_pid;
pthread_t _timer_thread_ctx;
pthread_t _io_thread_ctx;
@ -112,7 +106,7 @@ private:
static void *_uart_thread(void *arg);
static void *_uavcan_thread(void *arg);
void _run_timers(bool called_from_timer_thread);
void _run_timers();
void _run_io(void);
void delay_microseconds_semaphore(uint16_t us);