SITL: use WITH_SEMAPHORE()

and removed usage of hal.util->new_semaphore()

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);
+        {
+            WITH_SEMAPHORE(mutex);
             new_input = last_input;
-        mutex->give();
+        }
         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();
 }

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;
 };