mirror of https://github.com/ArduPilot/ardupilot
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:
parent
17932e3d1d
commit
66812c72da
|
@ -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);
|
||||
|
|
|
@ -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);
|
||||
|
||||
|
|
Loading…
Reference in New Issue