|
|
@ -94,7 +94,7 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys |
|
|
|
std::abort(); |
|
|
|
std::abort(); |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
res = sem_init(&_server_command_sem, 0 , 0); |
|
|
|
res = px4_sem_init(&_server_command_sem, 0 , 0); |
|
|
|
|
|
|
|
|
|
|
|
if (res < 0) { |
|
|
|
if (res < 0) { |
|
|
|
std::abort(); |
|
|
|
std::abort(); |
|
|
@ -158,7 +158,7 @@ UavcanNode::~UavcanNode() |
|
|
|
perf_free(_perfcnt_esc_mixer_output_elapsed); |
|
|
|
perf_free(_perfcnt_esc_mixer_output_elapsed); |
|
|
|
perf_free(_perfcnt_esc_mixer_total_elapsed); |
|
|
|
perf_free(_perfcnt_esc_mixer_total_elapsed); |
|
|
|
pthread_mutex_destroy(&_node_mutex); |
|
|
|
pthread_mutex_destroy(&_node_mutex); |
|
|
|
sem_destroy(&_server_command_sem); |
|
|
|
px4_sem_destroy(&_server_command_sem); |
|
|
|
|
|
|
|
|
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
@ -208,7 +208,7 @@ int UavcanNode::start_fw_server() |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
_fw_server_action = None; |
|
|
|
_fw_server_action = None; |
|
|
|
sem_post(&_server_command_sem); |
|
|
|
px4_sem_post(&_server_command_sem); |
|
|
|
return rv; |
|
|
|
return rv; |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
@ -224,7 +224,7 @@ int UavcanNode::request_fw_check() |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
_fw_server_action = None; |
|
|
|
_fw_server_action = None; |
|
|
|
sem_post(&_server_command_sem); |
|
|
|
px4_sem_post(&_server_command_sem); |
|
|
|
return rv; |
|
|
|
return rv; |
|
|
|
|
|
|
|
|
|
|
|
} |
|
|
|
} |
|
|
@ -248,7 +248,7 @@ int UavcanNode::stop_fw_server() |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
_fw_server_action = None; |
|
|
|
_fw_server_action = None; |
|
|
|
sem_post(&_server_command_sem); |
|
|
|
px4_sem_post(&_server_command_sem); |
|
|
|
return rv; |
|
|
|
return rv; |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
@ -263,7 +263,7 @@ int UavcanNode::fw_server(eServerAction action) |
|
|
|
case CheckFW: |
|
|
|
case CheckFW: |
|
|
|
if (_fw_server_action == None) { |
|
|
|
if (_fw_server_action == None) { |
|
|
|
_fw_server_action = action; |
|
|
|
_fw_server_action = action; |
|
|
|
sem_wait(&_server_command_sem); |
|
|
|
px4_sem_wait(&_server_command_sem); |
|
|
|
rv = _fw_server_status; |
|
|
|
rv = _fw_server_status; |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
@ -678,7 +678,7 @@ UavcanNode::control_callback(uintptr_t handle, uint8_t control_group, uint8_t co |
|
|
|
int |
|
|
|
int |
|
|
|
UavcanNode::teardown() |
|
|
|
UavcanNode::teardown() |
|
|
|
{ |
|
|
|
{ |
|
|
|
sem_post(&_server_command_sem); |
|
|
|
px4_sem_post(&_server_command_sem); |
|
|
|
|
|
|
|
|
|
|
|
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { |
|
|
|
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { |
|
|
|
if (_control_subs[i] > 0) { |
|
|
|
if (_control_subs[i] > 0) { |
|
|
|