Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_UAVCAN: use WITH_SEMAPHORE() 

and removed usage of hal.util->new_semaphore()
This commit is contained in:
Andrew Tridgell 2018-10-12 10:35:04 +11:00
parent 046bcfa0a9
commit f662cf55e5
2 changed files with 21 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
libraries/AP_UAVCAN/AP_UAVCAN.cpp
View File

@ -108,9 +108,6 @@ AP_UAVCAN::AP_UAVCAN() :
_SRV_conf[i].servo_pending = false; _SRV_conf[i].servo_pending = false;
} }
SRV_sem = hal.util->new_semaphore();
_led_out_sem = hal.util->new_semaphore();
debug_uavcan(2, "AP_UAVCAN constructed\n\r"); debug_uavcan(2, "AP_UAVCAN constructed\n\r");
} }
@ -289,7 +286,7 @@ void AP_UAVCAN::SRV_send_actuator(void)
uint8_t starting_servo = 0; uint8_t starting_servo = 0;
bool repeat_send; bool repeat_send;
SRV_sem->take_blocking(); WITH_SEMAPHORE(SRV_sem);
do { do {
repeat_send = false; repeat_send = false;
@ -332,8 +329,6 @@ void AP_UAVCAN::SRV_send_actuator(void)
} }
} }
} while (repeat_send); } while (repeat_send);
SRV_sem->give();
} }
void AP_UAVCAN::SRV_send_esc(void) void AP_UAVCAN::SRV_send_esc(void)
@ -344,7 +339,7 @@ void AP_UAVCAN::SRV_send_esc(void)
uint8_t active_esc_num = 0, max_esc_num = 0; uint8_t active_esc_num = 0, max_esc_num = 0;
uint8_t k = 0; uint8_t k = 0;
SRV_sem->take_blocking(); WITH_SEMAPHORE(SRV_sem);
// find out how many esc we have enabled and if they are active at all // find out how many esc we have enabled and if they are active at all
for (uint8_t i = 0; i < UAVCAN_SRV_NUMBER; i++) { for (uint8_t i = 0; i < UAVCAN_SRV_NUMBER; i++) {
@ -377,13 +372,11 @@ void AP_UAVCAN::SRV_send_esc(void)
esc_raw[_driver_index]->broadcast(esc_msg); esc_raw[_driver_index]->broadcast(esc_msg);
} }
SRV_sem->give();
} }
void AP_UAVCAN::SRV_push_servos() void AP_UAVCAN::SRV_push_servos()
{ {
SRV_sem->take_blocking(); WITH_SEMAPHORE(SRV_sem);
for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) { for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
// Check if this channels has any function assigned // Check if this channels has any function assigned
@ -394,8 +387,6 @@ void AP_UAVCAN::SRV_push_servos()
} }
} }
SRV_sem->give();
_SRV_armed = hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED; _SRV_armed = hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED;
} }
@ -410,17 +401,14 @@ void AP_UAVCAN::led_out_send()
return; return;
} }
if (!_led_out_sem->take(1)) { uavcan::equipment::indication::LightsCommand msg;
return; {
} WITH_SEMAPHORE(_led_out_sem);
if (_led_conf.devices_count == 0) { if (_led_conf.devices_count == 0) {
_led_out_sem->give();
return; return;
} }
uavcan::equipment::indication::LightsCommand msg;
uavcan::equipment::indication::SingleLightCommand cmd; uavcan::equipment::indication::SingleLightCommand cmd;
for (uint8_t i = 0; i < _led_conf.devices_count; i++) { for (uint8_t i = 0; i < _led_conf.devices_count; i++) {
@ -431,8 +419,7 @@ void AP_UAVCAN::led_out_send()
msg.commands.push_back(cmd); msg.commands.push_back(cmd);
} }
}
_led_out_sem->give();
rgb_led[_driver_index]->broadcast(msg); rgb_led[_driver_index]->broadcast(msg);
_led_conf.last_update = now; _led_conf.last_update = now;
@ -444,9 +431,7 @@ bool AP_UAVCAN::led_write(uint8_t led_index, uint8_t red, uint8_t green, uint8_t
return false; return false;
} }
if (!_led_out_sem->take(1)) { WITH_SEMAPHORE(_led_out_sem);
return false;
}
// check if a device instance exists. if so, break so the instance index is remembered // check if a device instance exists. if so, break so the instance index is remembered
uint8_t instance = 0; uint8_t instance = 0;
@ -470,8 +455,6 @@ bool AP_UAVCAN::led_write(uint8_t led_index, uint8_t red, uint8_t green, uint8_t
_led_conf.devices_count++; _led_conf.devices_count++;
} }
_led_out_sem->give();
return true; return true;
} }

4
libraries/AP_UAVCAN/AP_UAVCAN.h
View File

@ -169,7 +169,7 @@ private:
uint8_t _SRV_armed; uint8_t _SRV_armed;
uint32_t _SRV_last_send_us; uint32_t _SRV_last_send_us;
AP_HAL::Semaphore *SRV_sem; HAL_Semaphore SRV_sem;
///// LED ///// ///// LED /////
struct led_device { struct led_device {
@ -185,7 +185,7 @@ private:
uint64_t last_update; uint64_t last_update;
} _led_conf; } _led_conf;
AP_HAL::Semaphore *_led_out_sem; HAL_Semaphore _led_out_sem;
}; };
#endif /* AP_UAVCAN_H_ */ #endif /* AP_UAVCAN_H_ */