aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions 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;
}
uint32_t now = AP_HAL::millis();
if (rc_out_sem_take()) {
if (_rco_armed) {
@ -951,30 +953,35 @@ void AP_UAVCAN::do_cyclic(void)
led_out_sem_give();
}
uint64_t now = AP_HAL::millis64();
if (fix_out_sem_take()) {
if (fix_out_array[_uavcan_i] != nullptr && _broadcast_fix_rate > 0
&& now >= fix_out_next_send_time_ms) {
fix_out_next_send_time_ms = now + (1000 / _broadcast_fix_rate);
if (fix_out_array[_uavcan_i] != nullptr &&
_broadcast_fix_rate != 0 &&
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);
}
if (fix2_out_array[_uavcan_i] != nullptr && _broadcast_fix2_rate > 0
&& now >= fix2_out_next_send_time_ms) {
fix2_out_next_send_time_ms = now + (1000 / _broadcast_fix_rate);
if (fix2_out_array[_uavcan_i] != nullptr &&
_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);
}
fix_out_sem_give();
}
if (att_out_sem_take()) {
if (attitude_out_array[_uavcan_i] != nullptr && _broadcast_att_rate > 0
&& now >= att_out_next_send_time_ms) {
att_out_next_send_time_ms = now + (1000 / _broadcast_fix_rate);
attitude_out_array[_uavcan_i]->broadcast(_att_state);
}
if (att_out_sem_take() &&
attitude_out_array[_uavcan_i] != nullptr &&
_broadcast_att_rate != 0 &&
now - att_out_send_last_ms >= att_out_send_delta_ms) {
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();
}

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];
AP_BattMonitor_Backend* _bi_BM_listeners[AP_UAVCAN_MAX_LISTENERS];
uint64_t fix_out_next_send_time_ms = AP_HAL::millis64();
uint64_t fix2_out_next_send_time_ms = AP_HAL::millis64();
uint64_t att_out_next_send_time_ms = AP_HAL::millis64();
uint32_t fix_out_send_last_ms;
uint32_t fix_out_send_delta_ms;
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 {
uint16_t pulse;