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

SRV_Channel: move UAVCAN code from SRV to UAVCAN files

Browse Source
This commit is contained in:
Tom Pittenger 2018-03-15 20:18:33 -07:00 committed by Andrew Tridgell
parent 66438041ad
commit 1a15a2e749
2 changed files with 4 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/SRV_Channel/SRV_Channel.h
View File

@ -473,6 +473,4 @@ private:
static bool passthrough_disabled(void) {
return disabled_passthrough;
}
static void push_UAVCAN(void);
};

35
libraries/SRV_Channel/SRV_Channels.cpp
View File

@ -24,7 +24,6 @@
#if HAL_WITH_UAVCAN
#include <AP_UAVCAN/AP_UAVCAN.h>
#include <AP_BoardConfig/AP_BoardConfig.h>
#include <AP_BoardConfig/AP_BoardConfig_CAN.h>
#endif
@ -226,9 +225,6 @@ void SRV_Channels::push()
{
hal.rcout->push();
// push outputs to UAVCAN as well
push_UAVCAN();
// give volz library a chance to update
volz_ptr->update();
@ -239,39 +235,16 @@ void SRV_Channels::push()
// give blheli telemetry a chance to update
blheli_ptr->update_telemetry();
#endif
}
void SRV_Channels::push_UAVCAN(void)
{
#if HAL_WITH_UAVCAN
if (AP_BoardConfig_CAN::get_can_num_ifaces() == 0) {
return;
}
for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++) {
// push outputs to UAVCAN
uint8_t can_num_ifaces = AP_BoardConfig_CAN::get_can_num_ifaces();
for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS && i < can_num_ifaces; i++) {
AP_UAVCAN *ap_uavcan = AP_UAVCAN::get_uavcan(i);
if (ap_uavcan == nullptr) {
return;
}
if (!ap_uavcan->SRV_sem_take()) {
continue;
}
for (uint8_t j = 0; j < NUM_SERVO_CHANNELS; j++) {
// Check if this channels has any function assigned
if (channel_function(j)) {
ap_uavcan->SRV_write(channels[j].get_output_pwm(), j);
}
}
if (hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED) {
ap_uavcan->SRV_arm_actuators(true);
} else {
ap_uavcan->SRV_arm_actuators(false);
}
ap_uavcan->SRV_sem_give();
ap_uavcan->SRV_push_servos();
}
#endif // HAL_WITH_UAVCAN
}