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

AP_ESC extended status packet addition #27248

Closed
wants to merge 3 commits into from
Closed

AP_ESC extended status packet addition #27248

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
3870bd2
AP_DroneCAN: addition of ESC extended status message
Pradeep-Carbonix Jul 30, 2024
c539d5f
AP_ESC_Telem: addition of ESC extended status message
Pradeep-Carbonix Jul 30, 2024
c67e661
Tools: addition of ESC extended status message
Pradeep-Carbonix Jul 30, 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 @@ -121,6 +121,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', 'AP_Extended_Telem::get_input_duty',),
Copy link
Member

Choose a reason for hiding this comment

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

I suspect this won't work because the get_input_duty function is not used anywhere so the linker will remove it.

Copy link
Contributor Author

Choose a reason for hiding this comment

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

The APD ESC extended status telemetry messages implementation utilizes these functions. Given its specificity to our requirements, we decided to create two separate pull requests.

The first pull request includes the driver implementation (which is this one), as we believe it could be beneficial to a broader audience.
The second pull request will contain the AP_Periph interface with the APD ESC, which we plan to submit at a later time.

Copy link
Member

Choose a reason for hiding this comment

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

AP_Extended_Telem::get_input_duty is not even a function that exists in this PR. As I say, even if it had the correct class name it would not work for extract features because its not used so it won't be in the compiled output which extract features uses.


('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 @@ -1448,6 +1448,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 = (uint32_t)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};
IamPete1 marked this conversation as resolved.
Show resolved Hide resolved
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
84 changes: 84 additions & 0 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 @@ -305,6 +305,47 @@ bool AP_ESC_Telem::get_usage_seconds(uint8_t esc_index, uint32_t& usage_s) const
return true;
}

#if AP_EXTENDED_ESC_TELEM_ENABLED
// get an individual ESC's input duty if available, returns true on success
bool AP_ESC_Telem::get_input_duty(uint8_t esc_index, uint8_t& input_duty) const
{
const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index];
if (esc_index >= ESC_TELEM_MAX_ESCS
|| telemdata.stale()
|| !(_telem_data[esc_index].types & AP_ESC_Telem_Backend::TelemetryType::INPUT_DUTY)) {
return false;
}
input_duty = _telem_data[esc_index].input_duty;
return true;
}

// get an individual ESC's output duty if available, returns true on success
bool AP_ESC_Telem::get_output_duty(uint8_t esc_index, uint8_t& output_duty) const
{
const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index];
if (esc_index >= ESC_TELEM_MAX_ESCS
|| telemdata.stale()
|| !(_telem_data[esc_index].types & AP_ESC_Telem_Backend::TelemetryType::OUTPUT_DUTY)) {
return false;
}
output_duty = _telem_data[esc_index].output_duty;
return true;
}

// get an individual ESC's status flags if available, returns true on success
bool AP_ESC_Telem::get_flags(uint8_t esc_index, uint32_t& flags) const
{
const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index];
if (esc_index >= ESC_TELEM_MAX_ESCS
|| telemdata.stale()
|| !(_telem_data[esc_index].types & AP_ESC_Telem_Backend::TelemetryType::FLAGS)) {
return false;
}
flags = _telem_data[esc_index].flags;
return true;
}
#endif // AP_EXTENDED_ESC_TELEM_ENABLED

// send ESC telemetry messages over MAVLink
void AP_ESC_Telem::send_esc_telemetry_mavlink(uint8_t mav_chan)
{
Expand Down Expand Up @@ -468,6 +509,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;
}
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 @@ -607,6 +660,37 @@ void AP_ESC_Telem::update()
error_rate : rpmdata.error_rate
};
AP::logger().WriteBlock(&pkt, sizeof(pkt));

Pradeep-Carbonix marked this conversation as resolved.
Show resolved Hide resolved
#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
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----",
"QBBBI",
AP_HAL::micros64(),
uint8_t(i),
uint8_t(telemdata.input_duty),
uint8_t(telemdata.output_duty),
uint32_t(telemdata.flags));
}
#endif //AP_EXTENDED_ESC_TELEM_ENABLED
_last_telem_log_ms[i] = telemdata.last_update_ms;
_last_rpm_log_us[i] = rpmdata.last_update_us;
}
Expand Down
11 changes: 11 additions & 0 deletions libraries/AP_ESC_Telem/AP_ESC_Telem.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -67,6 +67,17 @@ class AP_ESC_Telem {
// get an individual ESC's consumption in milli-Ampere.hour if available, returns true on success
bool get_consumption_mah(uint8_t esc_index, float& consumption_mah) const;

#if AP_EXTENDED_ESC_TELEM_ENABLED
// get an individual ESC's input duty if available, returns true on success
bool get_input_duty(uint8_t esc_index, uint8_t& input_duty) const;

// get an individual ESC's output duty if available, returns true on success
bool get_output_duty(uint8_t esc_index, uint8_t& output_duty) const;

// get an individual ESC's status flags if available, returns true on success
bool get_flags(uint8_t esc_index, uint32_t& flags) const;
#endif // AP_EXTENDED_ESC_TELEM_ENABLED
Comment on lines +70 to +79
Copy link
Member

Choose a reason for hiding this comment

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

I don't think these are used anywhere?

Copy link
Contributor Author

Choose a reason for hiding this comment

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

The APD ESC extended status telemetry messages implementation utilizes these functions. Given its specificity to our requirements, we decided to create two separate pull requests.

The first pull request includes the driver implementation (which is this one), as we believe it could be beneficial to a broader audience.
The second pull request will contain the AP_Periph interface with the APD ESC, which we plan to submit at a later time.

Copy link
Member

Choose a reason for hiding this comment

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

If they are unused they should not be included because they cannot have been tested. You should add them in the future PR with the periph interface.


// return the average motor frequency in Hz for dynamic filtering
float get_average_motor_frequency_hz(uint32_t servo_channel_mask) const { return get_average_motor_rpm(servo_channel_mask) * (1.0f / 60.0f); };

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
4 changes: 4 additions & 0 deletions libraries/AP_HAL/AP_HAL_Boards.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -188,6 +188,10 @@
#define AP_EXTENDED_DSHOT_TELEM_V2_ENABLED HAL_REQUIRES_BDSHOT_SUPPORT
#endif

#ifndef AP_EXTENDED_ESC_TELEM_ENABLED
#define AP_EXTENDED_ESC_TELEM_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
Copy link
Collaborator

Choose a reason for hiding this comment

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

Do you not want it enabled on more than sitl? Incidentally, since this is really just to handle dronecan I think the define should be based on drone can being available.

Copy link
Contributor Author

Choose a reason for hiding this comment

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

Enabling this feature for sitl up in the previous discussion. Mainly to help with autotests.
AP_ESC_Telem_config.h is added with -
#ifndef AP_EXTENDED_ESC_TELEM_ENABLED
#define AP_EXTENDED_ESC_TELEM_ENABLED HAL_ENABLE_DRONECAN_DRIVERS
#endif

Copy link
Member

Choose a reason for hiding this comment

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

I think we should just enable for all drone CAN builds.

#endif

// this is used as a general mechanism to make a 'small' build by
// dropping little used features. We use this to allow us to keep
// FMUv2 going for as long as possible
Expand Down
Loading