Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

AP_ESC_Telem / AP_DroneCAN: add support for extended ESC status message. #27755

Merged
merged 3 commits into from
Aug 6, 2024
Merged

AP_ESC_Telem / AP_DroneCAN: add support for extended ESC status message. #27755

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
c609061
AP_DroneCAN: addition of ESC extended status message
Pradeep-Carbonix Jul 27, 2024
1e42b7f
AP_ESC_Telem: addition of ESC extended status message
Pradeep-Carbonix Jul 27, 2024
965704d
Tools: addition of ESC extended status message
Pradeep-Carbonix Jul 27, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions Tools/scripts/build_options.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -122,6 +122,8 @@ def __init__(self,
Feature('ESC', 'PICCOLOCAN', 'HAL_PICCOLO_CAN_ENABLE', 'Enable PiccoloCAN', 0, None),
Feature('ESC', 'TORQEEDO', 'HAL_TORQEEDO_ENABLED', 'Enable Torqeedo Motors', 0, None),

Feature('ESC', 'ESC_EXTENDED_TELM', 'AP_EXTENDED_ESC_TELEM_ENABLED', 'Enable Extended ESC Telem', 0, 'DroneCAN'),

Feature('AP_Periph', 'LONG_TEXT', 'HAL_PERIPH_SUPPORT_LONG_CAN_PRINTF', 'Enable extended length text strings', 0, None),

Feature('Camera', 'Camera', 'AP_CAMERA_ENABLED', 'Enable Camera Trigger support', 0, None),
Expand Down
2 changes: 2 additions & 0 deletions Tools/scripts/extract_features.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -62,6 +62,8 @@ def __init__(self, filename, nm="arm-none-eabi-nm", strings="strings"):
('HAL_EFI_ENABLED', 'AP_EFI::AP_EFI',),
('AP_EFI_{type}_ENABLED', 'AP_EFI_(?P<type>.*)::update',),

('AP_EXTENDED_ESC_TELEM_ENABLED', r'AP_DroneCAN::handle_esc_ext_status\b',),

('AP_TEMPERATURE_SENSOR_ENABLED', 'AP_TemperatureSensor::AP_TemperatureSensor',),
('AP_TEMPERATURE_SENSOR_{type}_ENABLED', 'AP_TemperatureSensor_(?P<type>.*)::update',),

Expand Down
28 changes: 28 additions & 0 deletions libraries/AP_DroneCAN/AP_DroneCAN.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1457,6 +1457,34 @@ void AP_DroneCAN::handle_ESC_status(const CanardRxTransfer& transfer, const uavc
#endif
}

#if AP_EXTENDED_ESC_TELEM_ENABLED
/*
handle Extended ESC status message
*/
void AP_DroneCAN::handle_esc_ext_status(const CanardRxTransfer& transfer, const uavcan_equipment_esc_StatusExtended& msg)
{
const uint8_t esc_offset = constrain_int16(_esc_offset.get(), 0, DRONECAN_SRV_NUMBER);
const uint8_t esc_index = msg.esc_index + esc_offset;

if (!is_esc_data_index_valid(esc_index)) {
return;
}

TelemetryData telemetryData {
.motor_temp_cdeg = (int16_t)(msg.motor_temperature_degC * 100),
.input_duty = msg.input_pct,
.output_duty = msg.output_pct,
.flags = msg.status_flags,
};

update_telem_data(esc_index, telemetryData,
AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE
| AP_ESC_Telem_Backend::TelemetryType::INPUT_DUTY
| AP_ESC_Telem_Backend::TelemetryType::OUTPUT_DUTY
| AP_ESC_Telem_Backend::TelemetryType::FLAGS);
}
#endif // AP_EXTENDED_ESC_TELEM_ENABLED

bool AP_DroneCAN::is_esc_data_index_valid(const uint8_t index) {
if (index > DRONECAN_SRV_NUMBER) {
// printf("DroneCAN: invalid esc index: %d. max index allowed: %d\n\r", index, DRONECAN_SRV_NUMBER);
Expand Down
8 changes: 8 additions & 0 deletions libraries/AP_DroneCAN/AP_DroneCAN.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -326,6 +326,11 @@ class AP_DroneCAN : public AP_CANDriver, public AP_ESC_Telem_Backend {
Canard::ObjCallback<AP_DroneCAN, uavcan_equipment_esc_Status> esc_status_cb{this, &AP_DroneCAN::handle_ESC_status};
Canard::Subscriber<uavcan_equipment_esc_Status> esc_status_listener{esc_status_cb, _driver_index};

#if AP_EXTENDED_ESC_TELEM_ENABLED
Canard::ObjCallback<AP_DroneCAN, uavcan_equipment_esc_StatusExtended> esc_status_extended_cb{this, &AP_DroneCAN::handle_esc_ext_status};
Canard::Subscriber<uavcan_equipment_esc_StatusExtended> esc_status_extended_listener{esc_status_extended_cb, _driver_index};
#endif

Canard::ObjCallback<AP_DroneCAN, uavcan_protocol_debug_LogMessage> debug_cb{this, &AP_DroneCAN::handle_debug};
Canard::Subscriber<uavcan_protocol_debug_LogMessage> debug_listener{debug_cb, _driver_index};

Expand Down Expand Up @@ -387,6 +392,9 @@ class AP_DroneCAN : public AP_CANDriver, public AP_ESC_Telem_Backend {
void handle_actuator_status(const CanardRxTransfer& transfer, const uavcan_equipment_actuator_Status& msg);
void handle_actuator_status_Volz(const CanardRxTransfer& transfer, const com_volz_servo_ActuatorStatus& msg);
void handle_ESC_status(const CanardRxTransfer& transfer, const uavcan_equipment_esc_Status& msg);
#if AP_EXTENDED_ESC_TELEM_ENABLED
void handle_esc_ext_status(const CanardRxTransfer& transfer, const uavcan_equipment_esc_StatusExtended& msg);
#endif
static bool is_esc_data_index_valid(const uint8_t index);
void handle_debug(const CanardRxTransfer& transfer, const uavcan_protocol_debug_LogMessage& msg);
void handle_param_get_set_response(const CanardRxTransfer& transfer, const uavcan_protocol_param_GetSetResponse& rsp);
Expand Down
49 changes: 47 additions & 2 deletions libraries/AP_ESC_Telem/AP_ESC_Telem.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -492,6 +492,18 @@ void AP_ESC_Telem::update_telem_data(const uint8_t esc_index, const AP_ESC_Telem
telemdata.usage_s = new_data.usage_s;
}

#if AP_EXTENDED_ESC_TELEM_ENABLED
if (data_mask & AP_ESC_Telem_Backend::TelemetryType::INPUT_DUTY) {
_telem_data[esc_index].input_duty = new_data.input_duty;
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Should _telem_data[esc_index] be replaced with telemdata? When we opened the original PR you based this, we did not notice the changes made in #26252

I can open the PR to fix that if you think we should.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@robertlong13 yeah your right. A PR would be great, thanks!

}
if (data_mask & AP_ESC_Telem_Backend::TelemetryType::OUTPUT_DUTY) {
_telem_data[esc_index].output_duty = new_data.output_duty;
}
if (data_mask & AP_ESC_Telem_Backend::TelemetryType::FLAGS) {
_telem_data[esc_index].flags = new_data.flags;
}
#endif //AP_EXTENDED_ESC_TELEM_ENABLED

#if AP_EXTENDED_DSHOT_TELEM_V2_ENABLED
if (data_mask & AP_ESC_Telem_Backend::TelemetryType::EDT2_STATUS) {
telemdata.edt2_status = merge_edt2_status(telemdata.edt2_status, new_data.edt2_status);
Expand Down Expand Up @@ -603,6 +615,10 @@ void AP_ESC_Telem::update()
if (telemdata.last_update_ms != _last_telem_log_ms[i]
|| rpmdata.last_update_us != _last_rpm_log_us[i]) {

// Update last log timestamps
_last_telem_log_ms[i] = telemdata.last_update_ms;
_last_rpm_log_us[i] = rpmdata.last_update_us;

float rpm = AP::logger().quiet_nanf();
get_rpm(i, rpm);
float raw_rpm = AP::logger().quiet_nanf();
Expand Down Expand Up @@ -631,8 +647,37 @@ void AP_ESC_Telem::update()
error_rate : rpmdata.error_rate
};
AP::logger().WriteBlock(&pkt, sizeof(pkt));
_last_telem_log_ms[i] = telemdata.last_update_ms;
_last_rpm_log_us[i] = rpmdata.last_update_us;

#if AP_EXTENDED_ESC_TELEM_ENABLED
// Write ESC extended status messages
// id: starts from 0
// input duty: duty cycle input to the ESC in percent
// output duty: duty cycle output to the motor in percent
// status flags: manufacurer-specific status flags
const bool has_ext_data = telemdata.types &
(AP_ESC_Telem_Backend::TelemetryType::INPUT_DUTY |
AP_ESC_Telem_Backend::TelemetryType::OUTPUT_DUTY |
AP_ESC_Telem_Backend::TelemetryType::FLAGS);
if (has_ext_data) {
// @LoggerMessage: ESCX
// @Description: ESC extended telemetry data
// @Field: TimeUS: Time since system startup
// @Field: Instance: starts from 0
// @Field: inpct: input duty cycle in percent
// @Field: outpct: output duty cycle in percent
// @Field: flags: manufacturer-specific status flags
AP::logger().WriteStreaming("ESCX",
"TimeUS,Instance,inpct,outpct,flags",
"s" "#" "%" "%" "-",
"F" "-" "-" "-" "-",
"Q" "B" "B" "B" "I",
AP_HAL::micros64(),
i,
telemdata.input_duty,
telemdata.output_duty,
telemdata.flags);
}
#endif //AP_EXTENDED_ESC_TELEM_ENABLED
}

#if AP_EXTENDED_DSHOT_TELEM_V2_ENABLED
Expand Down
10 changes: 10 additions & 0 deletions libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,11 @@ class AP_ESC_Telem_Backend {
uint16_t edt2_status; // status reported by Extended DShot Telemetry v2
uint16_t edt2_stress; // stress reported in dedicated messages by Extended DShot Telemetry v2
#endif
#if AP_EXTENDED_ESC_TELEM_ENABLED
uint8_t input_duty; // input duty cycle
uint8_t output_duty; // output duty cycle
uint32_t flags; // Status flags
#endif // AP_EXTENDED_ESC_TELEM_ENABLED

// return true if the data is stale
bool stale(uint32_t now_ms=0) const volatile;
Expand Down Expand Up @@ -50,6 +55,11 @@ class AP_ESC_Telem_Backend {
EDT2_STATUS = 1 << 8,
EDT2_STRESS = 1 << 9,
#endif
#if AP_EXTENDED_ESC_TELEM_ENABLED
INPUT_DUTY = 1 << 10,
OUTPUT_DUTY = 1 << 11,
FLAGS = 1 << 12
#endif // AP_EXTENDED_ESC_TELEM_ENABLED
};


Expand Down
8 changes: 8 additions & 0 deletions libraries/AP_ESC_Telem/AP_ESC_Telem_config.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,3 +6,11 @@
#ifndef HAL_WITH_ESC_TELEM
#define HAL_WITH_ESC_TELEM ((NUM_SERVO_CHANNELS > 0) && ((HAL_SUPPORT_RCOUT_SERIAL || HAL_MAX_CAN_PROTOCOL_DRIVERS)))
#endif

#ifndef AP_EXTENDED_ESC_TELEM_ENABLED
#define AP_EXTENDED_ESC_TELEM_ENABLED HAL_ENABLE_DRONECAN_DRIVERS
#endif

#if AP_EXTENDED_ESC_TELEM_ENABLED && !HAL_WITH_ESC_TELEM
#error "AP_EXTENDED_ESC_TELEM_ENABLED requires HAL_WITH_ESC_TELEM"
#endif
Loading