Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

SITL: use WITH_SEMAPHORE()

Browse Source 
and removed usage of hal.util->new_semaphore()
This commit is contained in:
Andrew Tridgell 2018-10-12 10:35:04 +11:00
parent 1d6b58f9ca
commit feea73ee1a
2 changed files with 7 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
libraries/SITL/SIM_FlightAxis.cpp
View File

@ -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();
}

2
libraries/SITL/SIM_FlightAxis.h
View File

@ -181,7 +181,7 @@ private:
uint16_t controller_port = 18083;
pthread_t thread;
AP_HAL::Semaphore *mutex;
HAL_Semaphore mutex;
};