AP_UAVCAN: allow CAN output for function==0 and allow for 32 CAN servos

This commit is contained in:
Andrew Tridgell 2022-05-20 21:33:43 +10:00
parent 5e184e167c
commit cc06327526
2 changed files with 3 additions and 3 deletions

View File

@ -580,7 +580,7 @@ void AP_UAVCAN::SRV_push_servos()
for (uint8_t i = 0; i < UAVCAN_SRV_NUMBER; i++) {
// Check if this channels has any function assigned
if (SRV_Channels::channel_function(i) > SRV_Channel::k_none) {
if (SRV_Channels::channel_function(i) >= SRV_Channel::k_none) {
_SRV_conf[i].pulse = SRV_Channels::srv_channel(i)->get_output_pwm();
_SRV_conf[i].esc_pending = true;
_SRV_conf[i].servo_pending = true;

View File

@ -31,11 +31,11 @@
#include <uavcan/protocol/param/GetSet.hpp>
#include <uavcan/protocol/param/ExecuteOpcode.hpp>
#include <uavcan/helpers/heap_based_pool_allocator.hpp>
#include <SRV_Channel/SRV_Channel.h>
#ifndef UAVCAN_SRV_NUMBER
#define UAVCAN_SRV_NUMBER 18
#define UAVCAN_SRV_NUMBER NUM_SERVO_CHANNELS
#endif
#define AP_UAVCAN_SW_VERS_MAJOR 1