Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-24 16:53:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_UAVCAN: changed att and fix send timers to be 32bit

Browse Source
This commit is contained in:
Tom Pittenger 2018-03-02 15:38:04 -08:00 committed by Tom Pittenger
parent 1add05d9a1
commit 1ace5ac534
2 changed files with 26 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
libraries/AP_UAVCAN/AP_UAVCAN.cpp
View File

@ -922,6 +922,8 @@ void AP_UAVCAN::do_cyclic(void)
return; return;
} }
uint32_t now = AP_HAL::millis();
if (rc_out_sem_take()) { if (rc_out_sem_take()) {
if (_rco_armed) { if (_rco_armed) {
@ -951,30 +953,35 @@ void AP_UAVCAN::do_cyclic(void)
led_out_sem_give(); led_out_sem_give();
} }
uint64_t now = AP_HAL::millis64();
if (fix_out_sem_take()) { if (fix_out_sem_take()) {
if (fix_out_array[_uavcan_i] != nullptr && _broadcast_fix_rate > 0 if (fix_out_array[_uavcan_i] != nullptr &&
&& now >= fix_out_next_send_time_ms) { _broadcast_fix_rate != 0 &&
fix_out_next_send_time_ms = now + (1000 / _broadcast_fix_rate); now - fix_out_send_last_ms >= fix_out_send_delta_ms) {
fix_out_send_last_ms = now;
fix_out_send_delta_ms = 1000U / (uint16_t)_broadcast_fix_rate;
fix_out_array[_uavcan_i]->broadcast(_fix_state); fix_out_array[_uavcan_i]->broadcast(_fix_state);
} }
if (fix2_out_array[_uavcan_i] != nullptr && _broadcast_fix2_rate > 0
&& now >= fix2_out_next_send_time_ms) { if (fix2_out_array[_uavcan_i] != nullptr &&
fix2_out_next_send_time_ms = now + (1000 / _broadcast_fix_rate); _broadcast_fix2_rate != 0 &&
now - fix2_out_send_last_ms >= fix2_out_send_delta_ms) {
fix2_out_send_last_ms = now;
fix2_out_send_delta_ms = 1000U / (uint16_t)_broadcast_fix2_rate;
fix2_out_array[_uavcan_i]->broadcast(_fix2_state); fix2_out_array[_uavcan_i]->broadcast(_fix2_state);
} }
fix_out_sem_give(); fix_out_sem_give();
} }
if (att_out_sem_take()) { if (att_out_sem_take() &&
if (attitude_out_array[_uavcan_i] != nullptr && _broadcast_att_rate > 0 attitude_out_array[_uavcan_i] != nullptr &&
&& now >= att_out_next_send_time_ms) { _broadcast_att_rate != 0 &&
att_out_next_send_time_ms = now + (1000 / _broadcast_fix_rate); now - att_out_send_last_ms >= att_out_send_delta_ms) {
attitude_out_array[_uavcan_i]->broadcast(_att_state); att_out_send_last_ms = now;
} att_out_send_delta_ms = 1000U / (uint16_t)_broadcast_att_rate;
attitude_out_array[_uavcan_i]->broadcast(_att_state);
att_out_sem_give(); att_out_sem_give();
} }

9
libraries/AP_UAVCAN/AP_UAVCAN.h
View File

@ -183,9 +183,12 @@ private:
uint16_t _bi_BM_listener_to_id[AP_UAVCAN_MAX_LISTENERS]; uint16_t _bi_BM_listener_to_id[AP_UAVCAN_MAX_LISTENERS];
AP_BattMonitor_Backend* _bi_BM_listeners[AP_UAVCAN_MAX_LISTENERS]; AP_BattMonitor_Backend* _bi_BM_listeners[AP_UAVCAN_MAX_LISTENERS];
uint64_t fix_out_next_send_time_ms = AP_HAL::millis64(); uint32_t fix_out_send_last_ms;
uint64_t fix2_out_next_send_time_ms = AP_HAL::millis64(); uint32_t fix_out_send_delta_ms;
uint64_t att_out_next_send_time_ms = AP_HAL::millis64(); uint32_t fix2_out_send_last_ms;
uint32_t fix2_out_send_delta_ms;
uint32_t att_out_send_last_ms;
uint32_t att_out_send_delta_ms;
struct { struct {
uint16_t pulse; uint16_t pulse;