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
This commit is contained in:
Oliver Walters 2020-01-23 15:31:44 +11:00 committed by Andrew Tridgell
parent 17932e3d1d
commit 66812c72da
2 changed files with 43 additions and 10 deletions

View File

@ -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)
}
/**
* 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);

View File

@ -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);