diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp index 5906103329..dcd2a8892d 100644 --- a/Tools/AP_Periph/can.cpp +++ b/Tools/AP_Periph/can.cpp @@ -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, diff --git a/libraries/AP_BLHeli/AP_BLHeli.cpp b/libraries/AP_BLHeli/AP_BLHeli.cpp index 966d4ca9dc..2c6c3fa076 100644 --- a/libraries/AP_BLHeli/AP_BLHeli.cpp +++ b/libraries/AP_BLHeli/AP_BLHeli.cpp @@ -1449,31 +1449,29 @@ void AP_BLHeli::read_telemetry_packet(void) hal.rcout->set_active_escs_mask(1<= 2) { uint16_t trpm = new_rpm; if (has_bidir_dshot(last_telem_esc)) { @@ -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 diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp index ec8689246d..12619d1746 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp @@ -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; } @@ -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 @@ -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; } diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.h b/libraries/AP_ESC_Telem/AP_ESC_Telem.h index b900acfc7c..e1c0b94a43 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.h +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.h @@ -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); }; @@ -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; @@ -159,4 +153,3 @@ namespace AP { }; #endif // HAL_WITH_ESC_TELEM - diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.cpp index 66f51a2cf5..ca5df84796 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.cpp +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.cpp @@ -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 \ No newline at end of file +#endif diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h b/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h index 1238da2e3d..20b651d5e5 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h @@ -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 { @@ -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(); @@ -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(); diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.cpp b/libraries/AP_UAVCAN/AP_UAVCAN.cpp index a3b099a84f..bb9b901167 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.cpp +++ b/libraries/AP_UAVCAN/AP_UAVCAN.cpp @@ -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 }