Skip to content

Commit

Permalink
AP_ESC_Telem : addition of ESC extended status message
Browse files Browse the repository at this point in the history 
 - Conditional compilation definition : AP_EXTENDED_ESC_TELEM_ENABLE
 - ESCX log structure
 - Update functionalities for ESCX status message 
 - ESCX DroneCAN callback
  • Loading branch information
@Pradeep-Carbonix
Pradeep-Carbonix committed Jun 25, 2024
1 parent b9104c4 commit 9da04c7
Show file tree
Hide file tree
Showing 6 changed files with 36 additions and 12 deletions.
3 changes: 1 addition & 2 deletions libraries/AP_DroneCAN/AP_DroneCAN.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1453,7 +1453,7 @@ void AP_DroneCAN::handle_ESC_status(const CanardRxTransfer& transfer, const uavc
*/
void AP_DroneCAN::handle_esc_ext_status(const CanardRxTransfer& transfer, const uavcan_equipment_esc_StatusExtended& msg)
{
#if HAL_WITH_ESC_TELEM
#if HAL_WITH_ESC_TELEM && AP_EXTENDED_ESC_TELEM_ENABLE
const uint8_t esc_offset = constrain_int16(_esc_offset.get(), 0, DRONECAN_SRV_NUMBER);
const uint8_t esc_index = msg.esc_index + esc_offset;

Expand All @@ -1476,7 +1476,6 @@ void AP_DroneCAN::handle_esc_ext_status(const CanardRxTransfer& transfer, const
#endif
}


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
29 changes: 22 additions & 7 deletions libraries/AP_ESC_Telem/AP_ESC_Telem.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -305,6 +305,7 @@ bool AP_ESC_Telem::get_usage_seconds(uint8_t esc_index, uint32_t& usage_s) const
return true;
}

#if AP_EXTENDED_ESC_TELEM_ENABLE
// 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
{
Expand Down Expand Up @@ -340,6 +341,7 @@ bool AP_ESC_Telem::get_flags(uint8_t esc_index, uint32_t& flags) const
flags = _telem_data[esc_index].flags;
return true;
}
#endif // AP_EXTENDED_ESC_TELEM_ENABLE

// send ESC telemetry messages over MAVLink
void AP_ESC_Telem::send_esc_telemetry_mavlink(uint8_t mav_chan)
Expand Down Expand Up @@ -513,6 +515,18 @@ void AP_ESC_Telem::update_telem_data(const uint8_t esc_index, const AP_ESC_Telem
_telem_data[esc_index].flags = new_data.flags;
}

#if AP_EXTENDED_ESC_TELEM_ENABLE
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_ENABLE

#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 @@ -653,12 +667,13 @@ void AP_ESC_Telem::update()
};
AP::logger().WriteBlock(&pkt, sizeof(pkt));

#if AP_EXTENDED_ESC_TELEM_ENABLE
// 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 = _telem_data[i].types &
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);
Expand All @@ -667,15 +682,15 @@ void AP_ESC_Telem::update()
LOG_PACKET_HEADER_INIT(uint8_t(LOG_ESCX_MSG)),
time_us : AP_HAL::micros64(),
instance : i,
input_duty : _telem_data[i].input_duty,
output_duty : _telem_data[i].output_duty,
flags : _telem_data[i].flags,
input_duty : telemdata.input_duty,
output_duty : telemdata.output_duty,
flags : telemdata.flags,
};
AP::logger().WriteBlock(&pkt2, sizeof(pkt2));
}

_last_telem_log_ms[i] = _telem_data[i].last_update_ms;
_last_rpm_log_us[i] = _rpm_data[i].last_update_us;
#endif //AP_EXTENDED_ESC_TELEM_ENABLE
_last_telem_log_ms[i] = telemdata.last_update_ms;
_last_rpm_log_us[i] = rpmdata.last_update_us;
}

#if AP_EXTENDED_DSHOT_TELEM_V2_ENABLED
Expand Down
2 changes: 2 additions & 0 deletions libraries/AP_ESC_Telem/AP_ESC_Telem.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -67,6 +67,7 @@ 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_ENABLE
// 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;

Expand All @@ -75,6 +76,7 @@ class AP_ESC_Telem {

// 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_ENABLE

// 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
4 changes: 4 additions & 0 deletions libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,9 +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_ENABLE
uint8_t input_duty; // input duty cycle
uint8_t output_duty; // output duty cycle
uint32_t flags; // Status flags
#endif // AP_EXTENDED_ESC_TELEM_ENABLE

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


Expand Down
6 changes: 3 additions & 3 deletions libraries/AP_ESC_Telem/LogStructure.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -78,8 +78,8 @@ struct PACKED log_EscX {
#define LOG_STRUCTURE_FROM_ESC_TELEM \
{ LOG_ESC_MSG, sizeof(log_Esc), \
"ESC", "QBeeffcfcf", "TimeUS,Instance,RPM,RawRPM,Volt,Curr,Temp,CTot,MotTemp,Err", "s#qqvAOaO%", "F-BB--BCB-" , true }, \
{ LOG_ESCX_MSG, sizeof(log_EscX), \
"ESCX", "QBBBI", "TimeUS,Instance,inpct,outpct,flags", "s#%%-", "F----" , true },\
{ LOG_EDT2_MSG, sizeof(log_Edt2), \
"EDT2", "QBBBB", "TimeUS,Instance,Stress,MaxStress,Status", "s#---", "F----" , true },
"EDT2", "QBBBB", "TimeUS,Instance,Stress,MaxStress,Status", "s#---", "F----" , true }, \
{ LOG_ESCX_MSG, sizeof(log_EscX), \
"ESCX", "QBBBI", "TimeUS,Instance,inpct,outpct,flags", "s#%%-", "F----" , true },

4 changes: 4 additions & 0 deletions libraries/AP_HAL/AP_HAL_Boards.h
View file
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_ENABLE
#define AP_EXTENDED_ESC_TELEM_ENABLE 0
#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

0 comments on commit 9da04c7

Please sign in to comment.