From 66812c72da7d574d827ac5c41323909a562748b1 Mon Sep 17 00:00:00 2001 From: Oliver Walters Date: Thu, 23 Jan 2020 15:31:44 +1100 Subject: [PATCH] AP_PiccoloCAN: Specify which ESC channels are used - Uses the CAN_Dx_PC_ESC_BM bitmask to determine which ESC (motor) channels are controlled over PiccoloCAN - Only transmits messages relevent to the selected motor channels --- libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp | 50 ++++++++++++++++++----- libraries/AP_PiccoloCAN/AP_PiccoloCAN.h | 3 ++ 2 files changed, 43 insertions(+), 10 deletions(-) diff --git a/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp b/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp index a927073614..dbdc61ca6c 100644 --- a/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp +++ b/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp @@ -274,12 +274,11 @@ void AP_PiccoloCAN::update() /* Read out the ESC commands from the channel mixer */ for (uint8_t i = 0; i < PICCOLO_CAN_MAX_NUM_ESC; i++) { - // Check each channel to determine if a motor function is assigned - SRV_Channel::Aux_servo_function_t motor_function = SRV_Channels::get_motor_function(i); - - if (SRV_Channels::function_assigned(motor_function)) { + if (is_esc_channel_active(i)) { uint16_t output = 0; + + SRV_Channel::Aux_servo_function_t motor_function = SRV_Channels::get_motor_function(i); if (SRV_Channels::get_output_pwm(motor_function, output)) { @@ -398,7 +397,9 @@ void AP_PiccoloCAN::send_esc_messages(void) // TODO - How to buffer CAN messages properly? // Sending more than 2 messages at each loop instance means that sometimes messages are dropped - if (hal.util->get_soft_armed()) { + // No ESCs are selected? Don't send anything + if (_esc_bm == 0x00) { + } else if (hal.util->get_soft_armed()) { bool send_cmd = false; int16_t cmd[4] {}; @@ -413,6 +414,11 @@ void AP_PiccoloCAN::send_esc_messages(void) idx = (ii * 4) + jj; + // Skip an ESC if the motor channel is not enabled + if (!is_esc_channel_active(idx)) { + continue; + } + /* Check if the ESC is software-inhibited. * If so, send a message to enable it. */ @@ -511,9 +517,32 @@ bool AP_PiccoloCAN::handle_esc_message(AP_HAL::CANFrame &frame) } -bool AP_PiccoloCAN::is_esc_present(uint8_t chan, uint64_t timeout_ms) +/** + * Check if a given ESC channel is "active" (has been configured correctly) + */ +bool AP_PiccoloCAN::is_esc_channel_active(uint8_t chan) { + // First check if the particular ESC channel is enabled in the channel mask + if (((_esc_bm >> chan) & 0x01) == 0x00) { + return false; + } + // Check if a motor function is assigned for this motor channel + SRV_Channel::Aux_servo_function_t motor_function = SRV_Channels::get_motor_function(chan); + + if (SRV_Channels::function_assigned(motor_function)) { + return true; + } + + return false; +} + + +/** + * Determine if an ESC is present on the CAN bus (has telemetry data been received) + */ +bool AP_PiccoloCAN::is_esc_present(uint8_t chan, uint64_t timeout_ms) +{ if (chan >= PICCOLO_CAN_MAX_NUM_ESC) { return false; } @@ -537,6 +566,9 @@ bool AP_PiccoloCAN::is_esc_present(uint8_t chan, uint64_t timeout_ms) } +/** + * Check if a given ESC is enabled (both hardware and software enable flags) + */ bool AP_PiccoloCAN::is_esc_enabled(uint8_t chan) { if (chan >= PICCOLO_CAN_MAX_NUM_ESC) { @@ -565,10 +597,8 @@ bool AP_PiccoloCAN::pre_arm_check(char* reason, uint8_t reason_len) // Check that each required ESC is present on the bus for (uint8_t ii = 0; ii < PICCOLO_CAN_MAX_NUM_ESC; ii++) { - SRV_Channel::Aux_servo_function_t motor_function = SRV_Channels::get_motor_function(ii); - - // There is a motor function assigned to this channel - if (SRV_Channels::function_assigned(motor_function)) { + // Skip any ESC channels where the motor channel is not enabled + if (is_esc_channel_active(ii)) { if (!is_esc_present(ii)) { snprintf(reason, reason_len, "ESC %u not detected", ii + 1); diff --git a/libraries/AP_PiccoloCAN/AP_PiccoloCAN.h b/libraries/AP_PiccoloCAN/AP_PiccoloCAN.h index 49d338a9cb..2a4df6a9f8 100644 --- a/libraries/AP_PiccoloCAN/AP_PiccoloCAN.h +++ b/libraries/AP_PiccoloCAN/AP_PiccoloCAN.h @@ -80,6 +80,9 @@ public: // send ESC telemetry messages over MAVLink void send_esc_telemetry_mavlink(uint8_t mav_chan); + // return true if a particular ESC is 'active' on the Piccolo interface + bool is_esc_channel_active(uint8_t chan); + // return true if a particular ESC has been detected bool is_esc_present(uint8_t chan, uint64_t timeout_ms = 2000);