From feea73ee1a9e50c5a0da8a92f6110835b8eb4d93 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 12 Oct 2018 10:35:04 +1100 Subject: [PATCH] SITL: use WITH_SEMAPHORE() and removed usage of hal.util->new_semaphore() --- libraries/SITL/SIM_FlightAxis.cpp | 19 ++++++------------- libraries/SITL/SIM_FlightAxis.h | 2 +- 2 files changed, 7 insertions(+), 14 deletions(-) diff --git a/libraries/SITL/SIM_FlightAxis.cpp b/libraries/SITL/SIM_FlightAxis.cpp index 7a092edd0d..283effb233 100644 --- a/libraries/SITL/SIM_FlightAxis.cpp +++ b/libraries/SITL/SIM_FlightAxis.cpp @@ -104,9 +104,6 @@ FlightAxis::FlightAxis(const char *home_str, const char *frame_str) : } } - /* Create the thread that will be waiting for data from FlightAxis */ - mutex = hal.util->new_semaphore(); - int ret = pthread_create(&thread, NULL, update_thread, this); if (ret != 0) { AP_HAL::panic("SIM_FlightAxis: failed to create thread"); @@ -139,9 +136,10 @@ void FlightAxis::update_loop(void) { while (true) { struct sitl_input new_input; - mutex->take(HAL_SEMAPHORE_BLOCK_FOREVER); - new_input = last_input; - mutex->give(); + { + WITH_SEMAPHORE(mutex); + new_input = last_input; + } exchange_data(new_input); } } @@ -339,7 +337,7 @@ void FlightAxis::exchange_data(const struct sitl_input &input) scaled_servos[7]); if (reply) { - mutex->take(HAL_SEMAPHORE_BLOCK_FOREVER); + WITH_SEMAPHORE(mutex); double lastt_s = state.m_currentPhysicsTime_SEC; parse_reply(reply); double dt = state.m_currentPhysicsTime_SEC - lastt_s; @@ -350,7 +348,6 @@ void FlightAxis::exchange_data(const struct sitl_input &input) average_frame_time_s = average_frame_time_s * 0.98 + dt * 0.02; } socket_frame_counter++; - mutex->give(); free(reply); } } @@ -361,7 +358,7 @@ void FlightAxis::exchange_data(const struct sitl_input &input) */ void FlightAxis::update(const struct sitl_input &input) { - mutex->take(HAL_SEMAPHORE_BLOCK_FOREVER); + WITH_SEMAPHORE(mutex); last_input = input; @@ -371,7 +368,6 @@ void FlightAxis::update(const struct sitl_input &input) initial_time_s = time_now_us * 1.0e-6f; last_time_s = state.m_currentPhysicsTime_SEC; position_offset.zero(); - mutex->give(); return; } if (dt_seconds < 0.00001f) { @@ -382,14 +378,12 @@ void FlightAxis::update(const struct sitl_input &input) } if (delta_time <= 0) { usleep(1000); - mutex->give(); return; } time_now_us += delta_time * 1.0e6; extrapolate_sensors(delta_time); update_position(); update_mag_field_bf(); - mutex->give(); usleep(delta_time*1.0e6); extrapolated_s += delta_time; report_FPS(); @@ -481,7 +475,6 @@ void FlightAxis::update(const struct sitl_input &input) // update magnetic field update_mag_field_bf(); - mutex->give(); report_FPS(); } diff --git a/libraries/SITL/SIM_FlightAxis.h b/libraries/SITL/SIM_FlightAxis.h index c34fc9d722..7327ec0519 100644 --- a/libraries/SITL/SIM_FlightAxis.h +++ b/libraries/SITL/SIM_FlightAxis.h @@ -181,7 +181,7 @@ private: uint16_t controller_port = 18083; pthread_t thread; - AP_HAL::Semaphore *mutex; + HAL_Semaphore mutex; };