Skip to content

Commit

Permalink
AP_ESC_Telem: consolidate state structures
Browse files Browse the repository at this point in the history
  • Loading branch information
@robertlong13
robertlong13 committed May 23, 2024
1 parent 1464549 commit cb1a967
Show file tree
Hide file tree
Showing 7 changed files with 73 additions and 125 deletions.
40 changes: 6 additions & 34 deletions Tools/AP_Periph/can.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1660,42 +1660,14 @@ void AP_Periph_FW::esc_telem_update()
CANARD_TRANSFER_PRIORITY_LOW,
&buffer[0],
total_size);

if (esc_telem.is_extended_telem_enabled()) {
uavcan_equipment_esc_StatusExtended pkt2{};

if (channel == nullptr) {
pkt2.esc_index = i;
}
else {
const int8_t motor_num = channel->get_motor_num();
pkt2.esc_index = motor_num == -1 ? i : motor_num;
}

uint8_t input_duty;
if (esc_telem.get_input_duty(i, input_duty)) {
pkt2.input_pct = input_duty;
}
else {
pkt2.input_pct = 0;
}

uint8_t output_duty;
if (esc_telem.get_output_duty(i, output_duty)) {
pkt2.output_pct = output_duty;
}
else {
pkt2.output_pct = 0;
}

uint8_t flags;
if (esc_telem.get_flags(i, flags)) {
pkt2.status_flags = flags;
}
else {
pkt2.status_flags = 0;
}
uavcan_equipment_esc_StatusExtended pkt2{};
pkt2.esc_index = pkt.esc_index;
bool has_ext_data = esc_telem.get_input_duty(i, pkt2.input_pct)
|| esc_telem.get_output_duty(i, pkt2.output_pct)
|| esc_telem.get_flags(i, pkt2.status_flags);

if (has_ext_data) {
uint8_t buffer_ext[UAVCAN_EQUIPMENT_ESC_STATUSEXTENDED_MAX_SIZE] {};
uint16_t total_size_ext = uavcan_equipment_esc_StatusExtended_encode(&pkt2, buffer_ext, !periph.canfdout());
canard_broadcast(UAVCAN_EQUIPMENT_ESC_STATUSEXTENDED_SIGNATURE,
Expand Down
32 changes: 15 additions & 17 deletions libraries/AP_BLHeli/AP_BLHeli.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1449,31 +1449,29 @@ void AP_BLHeli::read_telemetry_packet(void)
hal.rcout->set_active_escs_mask(1<<motor_idx);
update_rpm(motor_idx - chan_offset, new_rpm);

TelemetryData t {
TelemetryData telemetryData {
.temperature_cdeg = int16_t(buf[0] * 100),
.voltage = float(uint16_t((buf[1]<<8) | buf[2])) * 0.01,
.current = float(uint16_t((buf[3]<<8) | buf[4])) * 0.01,
.consumption_mah = float(uint16_t((buf[5]<<8) | buf[6])),
};

update_telem_data(motor_idx - chan_offset, t,
AP_ESC_Telem_Backend::TelemetryType::CURRENT
uint16_t types = AP_ESC_Telem_Backend::TelemetryType::CURRENT
| AP_ESC_Telem_Backend::TelemetryType::VOLTAGE
| AP_ESC_Telem_Backend::TelemetryType::CONSUMPTION
| AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE);
| AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE;

if (AP_ESC_Telem_Backend::is_extended_telem_enabled()) {
ExtendedTelemData e{
.input_duty = buf[9],
.output_duty = buf[10],
.flags = buf[11],
};
update_ext_telem_data(motor_idx - chan_offset, e,
AP_ESC_Telem_Backend::ExtTelemetryType::INPUT_DUTY
| AP_ESC_Telem_Backend::ExtTelemetryType::OUTPUT_DUTY
| AP_ESC_Telem_Backend::ExtTelemetryType::FLAGS);
telemetryData.input_duty = buf[9];
telemetryData.output_duty = buf[10];
telemetryData.flags = buf[11];
types |= AP_ESC_Telem_Backend::TelemetryType::INPUT_DUTY
| AP_ESC_Telem_Backend::TelemetryType::OUTPUT_DUTY
| AP_ESC_Telem_Backend::TelemetryType::FLAGS;
}

update_telem_data(motor_idx - chan_offset, telemetryData, types);

if (debug_level >= 2) {
uint16_t trpm = new_rpm;
if (has_bidir_dshot(last_telem_esc)) {
Expand All @@ -1484,10 +1482,10 @@ void AP_BLHeli::read_telemetry_packet(void)
}
DEV_PRINTF("ESC[%u] T=%u V=%f C=%f con=%f RPM=%u e=%.1f t=%u\n",
last_telem_esc,
t.temperature_cdeg,
t.voltage,
t.current,
t.consumption_mah,
telemetryData.temperature_cdeg,
telemetryData.voltage,
telemetryData.current,
telemetryData.consumption_mah,
trpm, hal.rcout->get_erpm_error_rate(motor_idx), (unsigned)AP_HAL::millis());
}
#endif // HAL_WITH_ESC_TELEM
Expand Down
77 changes: 37 additions & 40 deletions libraries/AP_ESC_Telem/AP_ESC_Telem.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -308,33 +308,36 @@ bool AP_ESC_Telem::get_usage_seconds(uint8_t esc_index, uint32_t& usage_s) const
// 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
{
if (esc_index >= ESC_TELEM_MAX_ESCS) {
// || AP_HAL::millis() - _ext_telem_data[esc_index].last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS)) {
if (esc_index >= ESC_TELEM_MAX_ESCS
|| AP_HAL::millis() - _telem_data[esc_index].last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS
|| !(_telem_data[esc_index].types & AP_ESC_Telem_Backend::TelemetryType::INPUT_DUTY)) {
return false;
}
input_duty = _ext_telem_data[esc_index].input_duty;
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
{
if (esc_index >= ESC_TELEM_MAX_ESCS) {
// || AP_HAL::millis() - _ext_telem_data[esc_index].last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS)) {
if (esc_index >= ESC_TELEM_MAX_ESCS
|| AP_HAL::millis() - _telem_data[esc_index].last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS
|| !(_telem_data[esc_index].types & AP_ESC_Telem_Backend::TelemetryType::OUTPUT_DUTY)) {
return false;
}
output_duty = _ext_telem_data[esc_index].output_duty;
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, uint8_t& flags) const
bool AP_ESC_Telem::get_flags(uint8_t esc_index, uint32_t& flags) const
{
if (esc_index >= ESC_TELEM_MAX_ESCS) {
// || AP_HAL::millis() - _ext_telem_data[esc_index].last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS)) {
if (esc_index >= ESC_TELEM_MAX_ESCS
|| AP_HAL::millis() - _telem_data[esc_index].last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS
|| !(_telem_data[esc_index].types & AP_ESC_Telem_Backend::TelemetryType::FLAGS)) {
return false;
}
flags = _ext_telem_data[esc_index].flags;
flags = _telem_data[esc_index].flags;
return true;
}

Expand Down Expand Up @@ -483,30 +486,19 @@ void AP_ESC_Telem::update_telem_data(const uint8_t esc_index, const AP_ESC_Telem
if (data_mask & AP_ESC_Telem_Backend::TelemetryType::USAGE) {
_telem_data[esc_index].usage_s = new_data.usage_s;
}

_telem_data[esc_index].count++;
_telem_data[esc_index].types |= data_mask;
_telem_data[esc_index].last_update_ms = AP_HAL::millis();
}

// record an update to the extended telemetry data together with timestamp
// this should be called by backends when new extended telemetry values are available
void AP_ESC_Telem::update_ext_telem_data(const uint8_t esc_index, const AP_ESC_Telem_Backend::ExtendedTelemData& new_data, const uint16_t data_mask)
{
if (esc_index >= ESC_TELEM_MAX_ESCS) {
return;
}
if (data_mask & AP_ESC_Telem_Backend::ExtTelemetryType::INPUT_DUTY) {
_ext_telem_data[esc_index].input_duty = new_data.input_duty;
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::ExtTelemetryType::OUTPUT_DUTY) {
_ext_telem_data[esc_index].output_duty = new_data.output_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::ExtTelemetryType::FLAGS) {
_ext_telem_data[esc_index].flags = new_data.flags;
if (data_mask & AP_ESC_Telem_Backend::TelemetryType::FLAGS) {
_telem_data[esc_index].flags = new_data.flags;
}

_ext_telem_data[esc_index].last_update_ms = AP_HAL::millis();
_telem_data[esc_index].count++;
_telem_data[esc_index].types |= data_mask;
_telem_data[esc_index].last_update_ms = AP_HAL::millis();
}

// record an update to the RPM together with timestamp, this allows the notch values to be slewed
Expand Down Expand Up @@ -585,17 +577,22 @@ void AP_ESC_Telem::update()
// - bit-1 : signal loss flag [200mS since last throttle packet]
// - bit-2 : over voltage flag
// - bit-3 : reboot flag [After every reboot or power cycle this bit will stay high for 30 seconds for cube to capture it]
const struct log_ExtEsc pkt2{
LOG_PACKET_HEADER_INIT(uint8_t(LOG_EXTESC_MSG)),
time_us : AP_HAL::micros64(),
instance : i,
input_duty : _ext_telem_data[i].input_duty,
output_duty : _ext_telem_data[i].output_duty,
flags : _ext_telem_data[i].flags,
};
AP::logger().WriteBlock(&pkt2, sizeof(pkt2));
bool has_ext_data = _telem_data[i].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) {
const struct log_ExtEsc pkt2{
LOG_PACKET_HEADER_INIT(uint8_t(LOG_EXTESC_MSG)),
time_us : AP_HAL::micros64(),
instance : i,
input_duty : _telem_data[i].input_duty,
output_duty : _telem_data[i].output_duty,
flags : (uint8_t)_telem_data[i].flags,
};
AP::logger().WriteBlock(&pkt2, sizeof(pkt2));
}

_last_ext_telem_log_ms[i] = _ext_telem_data[i].last_update_ms;
_last_telem_log_ms[i] = _telem_data[i].last_update_ms;
_last_rpm_log_us[i] = _rpm_data[i].last_update_us;
}
Expand Down
9 changes: 1 addition & 8 deletions libraries/AP_ESC_Telem/AP_ESC_Telem.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -73,7 +73,7 @@ class AP_ESC_Telem {
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, uint8_t& flags) const;
bool get_flags(uint8_t esc_index, uint32_t& flags) const;

// 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 Expand Up @@ -125,17 +125,11 @@ class AP_ESC_Telem {
// callback to update the data in the frontend, should be called by the driver when new data is available
void update_telem_data(const uint8_t esc_index, const AP_ESC_Telem_Backend::TelemetryData& new_data, const uint16_t data_mask);

// callback to update the data in the frontend, should be called by the driver when new data is available
void update_ext_telem_data(const uint8_t esc_index, const AP_ESC_Telem_Backend::ExtendedTelemData& new_data, const uint16_t data_mask);

// rpm data
volatile AP_ESC_Telem_Backend::RpmData _rpm_data[ESC_TELEM_MAX_ESCS];
// telemetry data
volatile AP_ESC_Telem_Backend::TelemetryData _telem_data[ESC_TELEM_MAX_ESCS];
// Extended telemetry data
volatile AP_ESC_Telem_Backend::ExtendedTelemData _ext_telem_data[ESC_TELEM_MAX_ESCS];

uint32_t _last_ext_telem_log_ms[ESC_TELEM_MAX_ESCS];
uint32_t _last_telem_log_ms[ESC_TELEM_MAX_ESCS];
uint32_t _last_rpm_log_us[ESC_TELEM_MAX_ESCS];
uint8_t next_idx;
Expand All @@ -159,4 +153,3 @@ namespace AP {
};

#endif // HAL_WITH_ESC_TELEM

7 changes: 1 addition & 6 deletions libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,14 +40,9 @@ void AP_ESC_Telem_Backend::update_telem_data(const uint8_t esc_index, const Tele
_frontend->update_telem_data(esc_index, new_data, data_present_mask);
}

// callback to update the data in the frontend, should be called by the driver when new data is available
void AP_ESC_Telem_Backend::update_ext_telem_data(const uint8_t esc_index, const ExtendedTelemData& new_ext_data, const uint16_t data_mask) {
_frontend->update_ext_telem_data(esc_index, new_ext_data, data_mask);
}

// check if ESC extended telemetry is enabled
bool AP_ESC_Telem_Backend::is_extended_telem_enabled() {
return _frontend->is_extended_telem_enabled();
}

#endif
#endif
23 changes: 8 additions & 15 deletions libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,13 +25,9 @@ class AP_ESC_Telem_Backend {
uint32_t last_update_ms; // last update time in milliseconds, determines whether active
uint16_t types; // telemetry types present
uint16_t count; // number of times updated
};

struct ExtendedTelemData {
uint8_t input_duty; // input duty cycle
uint8_t output_duty; // output duty cycle
uint8_t flags; // Status flags
uint32_t last_update_ms; // last update time in milliseconds, determines whether active
uint32_t flags; // Status flags
};

struct RpmData {
Expand All @@ -48,13 +44,13 @@ class AP_ESC_Telem_Backend {
VOLTAGE = 1 << 2,
CURRENT = 1 << 3,
CONSUMPTION = 1 << 4,
USAGE = 1 << 5
};

enum ExtTelemetryType {
INPUT_DUTY = 1 << 0,
OUTPUT_DUTY = 1 << 1,
FLAGS = 1 << 2
USAGE = 1 << 5,
// 6 reserved for temperature external
// 7 reserved for motor temperature external
// 8 and 9 are reserved for recent dshot extensions
INPUT_DUTY = 1 << 10,
OUTPUT_DUTY = 1 << 11,
FLAGS = 1 << 12
};

AP_ESC_Telem_Backend();
Expand All @@ -70,9 +66,6 @@ class AP_ESC_Telem_Backend {
// callback to update the data in the frontend, should be called by the driver when new data is available
void update_telem_data(const uint8_t esc_index, const TelemetryData& new_data, const uint16_t data_present_mask);

// callback to update the data in the frontend, should be called by the driver when new data is available
void update_ext_telem_data(const uint8_t esc_index, const ExtendedTelemData& new_ext_data, const uint16_t data_present_mask);

// check if ESC extended telemetry is enabled
bool is_extended_telem_enabled();

Expand Down
10 changes: 5 additions & 5 deletions libraries/AP_UAVCAN/AP_UAVCAN.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1096,16 +1096,16 @@ void AP_UAVCAN::handle_ExtESC_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, cons
return;
}

ExtendedTelemData e {
TelemetryData telemetryData {
.input_duty = cb.msg->input_pct,
.output_duty = cb.msg->output_pct,
.flags = (uint8_t)cb.msg->status_flags,
};

ap_uavcan->update_ext_telem_data(esc_index, e,
AP_ESC_Telem_Backend::ExtTelemetryType::INPUT_DUTY
|AP_ESC_Telem_Backend::ExtTelemetryType::OUTPUT_DUTY
|AP_ESC_Telem_Backend::ExtTelemetryType::FLAGS);
ap_uavcan->update_telem_data(esc_index, telemetryData,
AP_ESC_Telem_Backend::TelemetryType::INPUT_DUTY
|AP_ESC_Telem_Backend::TelemetryType::OUTPUT_DUTY
|AP_ESC_Telem_Backend::TelemetryType::FLAGS);
#endif
}

Expand Down

0 comments on commit cb1a967

Please sign in to comment.