diff --git a/src/drivers/dshot/DShot.cpp b/src/drivers/dshot/DShot.cpp index 013faad440..3eee6b88be 100644 --- a/src/drivers/dshot/DShot.cpp +++ b/src/drivers/dshot/DShot.cpp @@ -498,8 +498,6 @@ bool DShot::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], void DShot::Run() { - SmartLock lock_guard(_lock); - if (should_exit()) { ScheduleClear(); _mixing_output.unregister(); @@ -508,6 +506,8 @@ void DShot::Run() return; } + SmartLock lock_guard(_lock); + perf_begin(_cycle_perf); _mixing_output.update(); diff --git a/src/drivers/linux_pwm_out/linux_pwm_out.cpp b/src/drivers/linux_pwm_out/linux_pwm_out.cpp index 63a8b1b5da..e57d7d6369 100644 --- a/src/drivers/linux_pwm_out/linux_pwm_out.cpp +++ b/src/drivers/linux_pwm_out/linux_pwm_out.cpp @@ -126,8 +126,6 @@ bool LinuxPWMOut::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS void LinuxPWMOut::Run() { - SmartLock lock_guard(_lock); - if (should_exit()) { ScheduleClear(); _mixing_output.unregister(); @@ -136,6 +134,8 @@ void LinuxPWMOut::Run() return; } + SmartLock lock_guard(_lock); + perf_begin(_cycle_perf); perf_count(_interval_perf); diff --git a/src/drivers/pca9685_pwm_out/main.cpp b/src/drivers/pca9685_pwm_out/main.cpp index 5e8300a878..7e230fc567 100644 --- a/src/drivers/pca9685_pwm_out/main.cpp +++ b/src/drivers/pca9685_pwm_out/main.cpp @@ -376,8 +376,6 @@ bool PCA9685Wrapper::updateOutputs(bool stop_motors, uint16_t *outputs, unsigned void PCA9685Wrapper::Run() { - SmartLock lock_guard(_lock); - if (should_exit()) { ScheduleClear(); _mixing_output.unregister(); @@ -391,6 +389,8 @@ void PCA9685Wrapper::Run() return; } + SmartLock lock_guard(_lock); + perf_begin(_cycle_perf); switch (_state) { diff --git a/src/drivers/pwm_out/PWMOut.cpp b/src/drivers/pwm_out/PWMOut.cpp index ecf7de378b..25b94c70d5 100644 --- a/src/drivers/pwm_out/PWMOut.cpp +++ b/src/drivers/pwm_out/PWMOut.cpp @@ -439,8 +439,6 @@ bool PWMOut::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], void PWMOut::Run() { - SmartLock lock_guard(_lock); - if (should_exit()) { ScheduleClear(); _mixing_output.unregister(); @@ -449,6 +447,8 @@ void PWMOut::Run() return; } + SmartLock lock_guard(_lock); + perf_begin(_cycle_perf); perf_count(_interval_perf); diff --git a/src/drivers/pwm_out_sim/PWMSim.cpp b/src/drivers/pwm_out_sim/PWMSim.cpp index fd87c08ab6..cc45d54ab4 100644 --- a/src/drivers/pwm_out_sim/PWMSim.cpp +++ b/src/drivers/pwm_out_sim/PWMSim.cpp @@ -58,8 +58,6 @@ PWMSim::PWMSim(bool hil_mode_enabled) : void PWMSim::Run() { - SmartLock lock_guard(_lock); - if (should_exit()) { ScheduleClear(); _mixing_output.unregister(); @@ -68,6 +66,8 @@ PWMSim::Run() return; } + SmartLock lock_guard(_lock); + _mixing_output.update(); // check for parameter updates diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index ed139aca3b..ceb34bc1cd 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -525,8 +525,6 @@ void PX4IO::updateFailsafe() void PX4IO::Run() { - SmartLock lock_guard(_lock); - if (should_exit()) { ScheduleClear(); _mixing_output.unregister(); @@ -535,6 +533,8 @@ void PX4IO::Run() return; } + SmartLock lock_guard(_lock); + perf_begin(_cycle_perf); perf_count(_interval_perf); diff --git a/src/drivers/tap_esc/TAP_ESC.cpp b/src/drivers/tap_esc/TAP_ESC.cpp index 9891108f83..80fd32806f 100644 --- a/src/drivers/tap_esc/TAP_ESC.cpp +++ b/src/drivers/tap_esc/TAP_ESC.cpp @@ -329,8 +329,6 @@ bool TAP_ESC::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], u void TAP_ESC::Run() { - SmartLock lock_guard(_lock); - if (should_exit()) { ScheduleClear(); _mixing_output.unregister(); @@ -339,6 +337,8 @@ void TAP_ESC::Run() return; } + SmartLock lock_guard(_lock); + // push backup schedule ScheduleDelayed(20_ms);