aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_UAVCAN: 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 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

55
libraries/AP_UAVCAN/AP_UAVCAN.cpp
View File

@ -108,9 +108,6 @@ AP_UAVCAN::AP_UAVCAN() :
_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");
}
@ -289,7 +286,7 @@ void AP_UAVCAN::SRV_send_actuator(void)
uint8_t starting_servo = 0;
bool repeat_send;
SRV_sem->take_blocking();
WITH_SEMAPHORE(SRV_sem);
do {
repeat_send = false;
@ -332,8 +329,6 @@ void AP_UAVCAN::SRV_send_actuator(void)
}
}
} while (repeat_send);
SRV_sem->give();
}
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 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
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);
}
SRV_sem->give();
}
void AP_UAVCAN::SRV_push_servos()
{
SRV_sem->take_blocking();
WITH_SEMAPHORE(SRV_sem);
for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
// 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;
}
@ -410,30 +401,26 @@ void AP_UAVCAN::led_out_send()
return;
}
if (!_led_out_sem->take(1)) {
return;
}
if (_led_conf.devices_count == 0) {
_led_out_sem->give();
return;
}
uavcan::equipment::indication::LightsCommand msg;
uavcan::equipment::indication::SingleLightCommand cmd;
{
WITH_SEMAPHORE(_led_out_sem);
for (uint8_t i = 0; i < _led_conf.devices_count; i++) {
cmd.light_id =_led_conf.devices[i].led_index;
cmd.color.red = _led_conf.devices[i].red >> 3;
cmd.color.green = _led_conf.devices[i].green >> 2;
cmd.color.blue = _led_conf.devices[i].blue >> 3;
if (_led_conf.devices_count == 0) {
return;
}
msg.commands.push_back(cmd);
uavcan::equipment::indication::SingleLightCommand cmd;
for (uint8_t i = 0; i < _led_conf.devices_count; i++) {
cmd.light_id =_led_conf.devices[i].led_index;
cmd.color.red = _led_conf.devices[i].red >> 3;
cmd.color.green = _led_conf.devices[i].green >> 2;
cmd.color.blue = _led_conf.devices[i].blue >> 3;
msg.commands.push_back(cmd);
}
}
_led_out_sem->give();
rgb_led[_driver_index]->broadcast(msg);
_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;
}
if (!_led_out_sem->take(1)) {
return false;
}
WITH_SEMAPHORE(_led_out_sem);
// check if a device instance exists. if so, break so the instance index is remembered
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_out_sem->give();
return true;
}

4
libraries/AP_UAVCAN/AP_UAVCAN.h
View File

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