|
|
|
@ -789,6 +789,7 @@ void AP_UAVCAN::handle_actuator_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, co
@@ -789,6 +789,7 @@ void AP_UAVCAN::handle_actuator_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, co
|
|
|
|
|
*/ |
|
|
|
|
void AP_UAVCAN::handle_ESC_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ESCStatusCb &cb) |
|
|
|
|
{ |
|
|
|
|
#if HAL_WITH_ESC_TELEM |
|
|
|
|
const uint8_t esc_index = cb.msg->esc_index; |
|
|
|
|
|
|
|
|
|
if (!is_esc_data_index_valid(esc_index)) { |
|
|
|
@ -806,6 +807,7 @@ void AP_UAVCAN::handle_ESC_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const E
@@ -806,6 +807,7 @@ void AP_UAVCAN::handle_ESC_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const E
|
|
|
|
|
AP_ESC_Telem_Backend::TelemetryType::CURRENT |
|
|
|
|
| AP_ESC_Telem_Backend::TelemetryType::VOLTAGE |
|
|
|
|
| AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE); |
|
|
|
|
#endif |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
bool AP_UAVCAN::is_esc_data_index_valid(const uint8_t index) { |
|
|
|
|