diff --git a/Tools/esc_eeprom_decode.py b/Tools/esc_eeprom_decode.py new file mode 100755 index 00000000000..f035eb2ac20 --- /dev/null +++ b/Tools/esc_eeprom_decode.py @@ -0,0 +1,331 @@ +#!/usr/bin/env python3 +""" +Decode the ESC EEPROM dumps logged in the esc_eeprom_read topic of a PX4 ULog file. + +The schema describes the EEPROM layout for one ESC firmware and EEPROM revision. It is published by +the ESC firmware or its vendor, not by PX4, so it has to be passed in with --schema. + +Install: pip install pyulog +Run: ./esc_eeprom_decode.py --schema eeprom.json + ./esc_eeprom_decode.py --schema eeprom.json --esc 1 +""" + +import argparse +import json +import sys + +TOPIC = 'esc_eeprom_read' + +# MAVLink ESC_FIRMWARE enum, as logged in the 'firmware' field +ESC_FIRMWARE = {0: 'UNKNOWN', 1: 'AM32'} + + +def load_dumps(log_path): + """Return every EEPROM dump found in the log, as {'time', 'esc', 'firmware', 'data'} dicts.""" + from pyulog import ULog + + ulog = ULog(log_path, [TOPIC]) + dumps = [] + + for dataset in ulog.data_list: + if dataset.name != TOPIC: + continue + + fields = dataset.data + + for row in range(len(fields['timestamp'])): + length = int(fields['length'][row]) + data = bytes(int(fields[f'data[{i}]'][row]) for i in range(length)) + + dumps.append({ + 'time': int(fields['timestamp'][row]) / 1e6, + 'esc': int(fields['index'][row]) + 1, + 'firmware': int(fields['firmware'][row]), + 'data': data, + }) + + dumps.sort(key=lambda dump: (dump['time'], dump['esc'])) + return dumps + + +def collapse_repeats(dumps): + """Drop dumps identical to the previous one from the same ESC, counting them on the one kept. The + driver republishes every cached dump every 10 seconds so that logs started later still carry them.""" + kept = [] + last = {} + + for dump in dumps: + previous = last.get(dump['esc']) + + if previous and (previous['firmware'], previous['data']) == (dump['firmware'], dump['data']): + previous['repeats'] += 1 + previous['last_time'] = dump['time'] + continue + + dump['repeats'] = 0 + last[dump['esc']] = dump + kept.append(dump) + + return kept + + +def load_schema(schema_path): + """Load the EEPROM layout schema from a local JSON file.""" + with open(schema_path, encoding='utf-8') as schema_file: + return json.load(schema_file) + + +def firmware_key(text): + """Turn '2.18' into the tuple (2, 18), so firmware versions can be compared with <, >=, etc.""" + major, _, minor = text.partition('.') + return (int(major), int(minor or 0)) + + +def applies(item, eeprom_version, firmware_version): + """Whether a field or group exists at these EEPROM layout and firmware versions, bounded the same + way as in the configurator.""" + if item.get('minEepromVersion') is not None and eeprom_version < item['minEepromVersion']: + return False + + if item.get('maxEepromVersion') is not None and eeprom_version > item['maxEepromVersion']: + return False + + if item.get('minFirmwareVersion') and firmware_version < firmware_key(item['minFirmwareVersion']): + return False + + if item.get('maxFirmwareVersion') and firmware_version > firmware_key(item['maxFirmwareVersion']): + return False + + return True + + +def apply_overlay(resolved, overlay): + """Apply a version overlay onto a field definition: objects merge one level deep, everything + else replaces the previous value.""" + for key, value in overlay.items(): + if isinstance(value, dict) and isinstance(resolved.get(key), dict): + resolved[key] = {**resolved[key], **value} + + else: + resolved[key] = value + + +def resolve_field(field, eeprom_version, firmware_version): + """Collapse a field definition and its version overlays into a single definition. + + Overlays are applied in the order the schema defines: 'default' first, then 'eeprom:N+' in + ascending N, then 'firmware:X.Y+' in ascending X.Y, keeping only those the reported versions + satisfy. + """ + resolved = dict(field) + versions = resolved.pop('versions', {}) + + if 'default' in versions: + apply_overlay(resolved, versions['default']) + + eeprom_overlays = [] + firmware_overlays = [] + + for key, overlay in versions.items(): + if key.startswith('eeprom:') and key.endswith('+'): + eeprom_overlays.append((int(key[len('eeprom:'):-1]), overlay)) + + elif key.startswith('firmware:') and key.endswith('+'): + firmware_overlays.append((firmware_key(key[len('firmware:'):-1]), overlay)) + + # The 'N+' / 'X.Y+' thresholds in one field are always unique, so sorting the (threshold, + # overlay) pairs directly sorts by threshold. + eeprom_overlays.sort() + firmware_overlays.sort() + + for minimum, overlay in eeprom_overlays: + if eeprom_version >= minimum: + apply_overlay(resolved, overlay) + + for minimum, overlay in firmware_overlays: + if firmware_version >= minimum: + apply_overlay(resolved, overlay) + + return resolved + + +def read_raw(data, offset, size): + """Read a little-endian unsigned integer, or None if it falls outside the dump.""" + if offset + size > len(data): + return None + + return int.from_bytes(data[offset:offset + size], 'little') + + +def is_disabled(field, raw): + """Whether the raw value switches the setting off: the field's disabled value, or anything outside + its raw range once the field has a disabled value at all.""" + disabled = field.get('disabledValue') + + if not disabled: + return False + + limits = field.get('raw') or {} + return raw == disabled.get('raw') or raw < limits.get('min', raw) or raw > limits.get('max', raw) + + +def format_value(field, raw): + """Render a raw value using the field's enum table or display scaling.""" + field_type = field.get('type') + unit = field.get('unit', '') + + if is_disabled(field, raw): + return f'{field["disabledValue"].get("display", "Disabled")} ({raw})' + + if field_type == 'enum': + for entry in field.get('values', []): + if entry.get('raw') == raw: + return f'{entry.get("name", raw)} ({raw})' + + return f'{raw} (not in schema)' + + if field_type == 'bool': + return f'{"On" if raw else "Off"} ({raw})' + + display = field.get('display') + + if display: + scaled = raw * display.get('factor', 1) + display.get('offset', 0) + text = f'{scaled:.{display.get("decimals", 0)}f}' + return f'{text} {unit}'.strip() + f' (raw {raw})' + + return f'{raw} {unit}'.strip() + + +def eeprom_version_of(data, fields): + """Read the byte the schema calls 'eepromVersion', or 0 if the schema has no such field.""" + field = fields.get('eepromVersion') + + if not field: + return 0 + + return read_raw(data, field['offset'], field.get('size', 1)) or 0 + + +def firmware_version_of(data, fields): + """Read the (major, minor) firmware version the schema reports, or (0, 0) if it can't.""" + major_field = fields.get('firmwareMajor') + minor_field = fields.get('firmwareMinor') + + if not major_field or not minor_field: + return (0, 0) + + major = read_raw(data, major_field['offset'], major_field.get('size', 1)) or 0 + minor = read_raw(data, minor_field['offset'], minor_field.get('size', 1)) or 0 + return (major, minor) + + +def decode(dump, schema): + """Decode one raw dump into {name: (label, text)} for every setting present in it.""" + data = dump['data'] + fields = schema['fields'] + + eeprom_version = eeprom_version_of(data, fields) + firmware_version = firmware_version_of(data, fields) + + settings = {} + + for name, field in fields.items(): + if field.get('type') == 'reserved': + continue + + if not applies(field, eeprom_version, firmware_version): + continue + + resolved = resolve_field(field, eeprom_version, firmware_version) + raw = read_raw(data, resolved['offset'], resolved.get('size', 1)) + + if raw is not None: + settings[name] = (resolved.get('name', name), format_value(resolved, raw)) + + return eeprom_version, firmware_version, settings + + +def print_dump(dump, schema, index, total): + eeprom_version, firmware_version, settings = decode(dump, schema) + layout = schema.get('eepromVersions', {}).get(str(eeprom_version), {}) + + title = f' ESC {dump["esc"]} at {dump["time"]:.1f} s ' + + if dump.get('repeats'): + title += f'(+{dump["repeats"]} unchanged, last {dump["last_time"]:.1f} s) ' + + if total > 1: + title += f'({index}/{total}) ' + + print() + print(title.center(64, '=')) + print(f' firmware type {ESC_FIRMWARE.get(dump["firmware"], dump["firmware"])}') + print(f' firmware {firmware_version[0]}.{firmware_version[1]}') + print(f' EEPROM layout {eeprom_version} ({layout.get("description", "unknown")})') + + groups = schema.get('groups', {}) + order = list(schema.get('groupOrder', [])) + + for group_key in groups: + if group_key not in order: + order.append(group_key) + + placed = set() + + for group_key in order: + group = groups.get(group_key, {}) + names = [name for name in group.get('fields', []) if name in settings] + placed.update(names) + + # A group the ESC's versions rule out hides its fields rather than moving them to 'Other' + if not names or not applies(group, eeprom_version, firmware_version): + continue + + print(f'\n {group.get("name", group_key)}') + + for name in names: + label, text = settings[name] + print(f' {label:<28} {text}') + + leftover = [name for name in settings if name not in placed] + + if leftover: + print('\n Other') + + for name in leftover: + label, text = settings[name] + print(f' {label:<28} {text}') + + +def main(): + parser = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter) + parser.add_argument('log', help='ULog file (.ulg) containing esc_eeprom_read entries') + parser.add_argument('-s', '--schema', required=True, help='EEPROM layout schema JSON, published by the ESC firmware') + parser.add_argument('-e', '--esc', type=int, help='only show this ESC (1-based, as printed by PX4)') + args = parser.parse_args() + + dumps = load_dumps(args.log) + + if args.esc is not None: + dumps = [dump for dump in dumps if dump['esc'] == args.esc] + + dumps = collapse_repeats(dumps) + + if not dumps: + sys.exit(f'no {TOPIC} messages in {args.log}') + + schema = load_schema(args.schema) + + print(f'log: {args.log}') + print(f'schema: {schema.get("title", "untitled")} v{schema.get("version", "?")}') + + for index, dump in enumerate(dumps, start=1): + print_dump(dump, schema, index, len(dumps)) + + +if __name__ == '__main__': + try: + main() + except Exception as error: + sys.exit(f'error: {error}') diff --git a/docs/en/peripherals/dshot.md b/docs/en/peripherals/dshot.md index 9316846eaf5..2507b43c1ae 100644 --- a/docs/en/peripherals/dshot.md +++ b/docs/en/peripherals/dshot.md @@ -130,21 +130,35 @@ When both serial telemetry and BDShot/EDT are enabled, the driver merges data fr ## AM32 ESC Settings (EEPROM) -PX4 can read and write AM32 ESC firmware settings (EEPROM) via a ground station, enabling remote ESC configuration without connecting directly to each ESC. +PX4 logs the AM32 ESC firmware settings (EEPROM), and a ground station can read and write them without connecting to each ESC. ### Requirements - ESCs running [AM32 firmware](https://github.com/am32-firmware/AM32) with serial telemetry connected ([DSHOT_TEL_CFG](../advanced_config/parameter_reference.md#DSHOT_TEL_CFG)) - `DSHOT_ESC_TYPE` set to `1` (AM32) -- Ground station with ESC EEPROM support (QGroundControl feature in development) -- MAVLink development dialect enabled on the flight controller +- For ground station access only: + - Ground station with ESC EEPROM support (QGroundControl feature in development) + - MAVLink development dialect enabled on the flight controller ### How It Works -PX4 automatically reads the full EEPROM from each ESC on boot. +PX4 reads the full EEPROM from each ESC while disarmed: at boot, when the ESC reconnects, after its settings are saved, and when a ground station requests it. The ground station can then display individual settings and allow the user to modify them. Changes are written back to the ESC one byte at a time using the DShot programming protocol. +### Logged Settings + +The last settings read from each ESC are logged in the `esc_eeprom_read` topic when they are read, and again every 10 seconds. +Decode them with `Tools/esc_eeprom_decode.py` and the AM32 EEPROM layout schema, [`schema/eeprom.json`](https://github.com/ARK-Electronics/ARK32/blob/main/schema/eeprom.json) in the ARK32 firmware: + +```sh +pip install pyulog +curl -O https://raw.githubusercontent.com/ARK-Electronics/ARK32/main/schema/eeprom.json +Tools/esc_eeprom_decode.py --schema eeprom.json --esc 1 +``` + +Omit `--esc` to decode every ESC. + ## ESC Serial Passthrough diff --git a/msg/EscEepromRead.msg b/msg/EscEepromRead.msg index c83c3881ce3..ea4d7a7d0a6 100644 --- a/msg/EscEepromRead.msg +++ b/msg/EscEepromRead.msg @@ -1,7 +1,8 @@ uint64 timestamp # [us] Time since system start +uint64 timestamp_sample # [us] Time the dump was read from the ESC. Unchanged when a cached dump is republished uint8 firmware # [-] ESC firmware type (see ESC_FIRMWARE enum in MAVLink) uint8 index # [-] Index of the ESC (0 = ESC1, 1 = ESC2, etc.) uint16 length # [-] Length of valid data uint8[48] data # [-] Raw ESC EEPROM data -uint8 ORB_QUEUE_LENGTH = 8 # To support 8 queued up responses +uint8 ORB_QUEUE_LENGTH = 12 # One dump per ESC, up to esc_status CONNECTED_ESC_MAX diff --git a/src/drivers/dshot/DShot.cpp b/src/drivers/dshot/DShot.cpp index 33e4b93f4b6..00aaa4a2d69 100644 --- a/src/drivers/dshot/DShot.cpp +++ b/src/drivers/dshot/DShot.cpp @@ -97,6 +97,8 @@ void DShot::Run() bool serial_updated = process_serial_telemetry(); bool bdshot_updated = process_bdshot_telemetry(); + _telemetry.publishSettings(); + if (serial_updated || bdshot_updated) { _esc_status.timestamp = hrt_absolute_time(); _esc_status.esc_count = _motor_count; @@ -108,9 +110,8 @@ void DShot::Run() update_params(); } - // Telemetry init hook - if (_request_telemetry_init.load()) { - init_telemetry(_serial_port_path, _telemetry_swap_rxtx); + // Telemetry init hook, retried until the motor mask is known + if (_request_telemetry_init.load() && init_telemetry(_serial_port_path, _telemetry_swap_rxtx)) { _request_telemetry_init.store(false); } @@ -186,12 +187,21 @@ void DShot::select_next_command() PX4_ERR("esc_eeprom_write lost, generation %u -> %u", last_generation, current_generation); } - if (_esc_eeprom_write.index != 255 && _esc_eeprom_write.index >= DSHOT_MAX_MOTORS) { + // A command for a motor without an output is never sent, so it never finishes and blocks every + // later command and the serial telemetry. + uint16_t write_mask = _motor_mask; + + if (_esc_eeprom_write.index != 255) { + write_mask = _esc_eeprom_write.index < DSHOT_MAX_MOTORS ? (_motor_mask & (1u << _esc_eeprom_write.index)) : 0; + } + + if (write_mask == 0) { PX4_ERR("esc_eeprom_write: invalid index %u", _esc_eeprom_write.index); } else { PX4_DEBUG("ESC%u: starting programming mode", _esc_eeprom_write.index + 1); _dshot_programming_active = true; + _telemetry.invalidateSettings(write_mask); } } } @@ -200,13 +210,12 @@ void DShot::select_next_command() // - EDT Request // - Settings Request // - Settings Programming + // Programming, once started, runs to the save without the others: an ESC in programming mode takes the + // next frames of any command as the address and value, and a read before the save returns old settings. // EDT Request mask uint16_t needs_edt_request_mask = _bdshot_telem_online_mask & ~_bdshot_edt_confirmed_mask; - // Settings Request mask - uint16_t needs_settings_request_mask = _serial_telem_online_mask & ~_settings_requested_mask; - bool serial_telem_delay_elapsed = hrt_absolute_time() > _serial_telem_delay_until; _current_command.clear(); @@ -214,7 +223,7 @@ void DShot::select_next_command() // EDT Request: use motor-order masks since needs_edt_request_mask is in motor order uint16_t edt_motors_to_request = _bdshot_motor_mask & needs_edt_request_mask; - if (_bdshot_edt_enabled && edt_motors_to_request != 0) { + if (_bdshot_edt_enabled && !_dshot_programming_active && edt_motors_to_request != 0) { // Find first motor that needs EDT request and has been online long enough hrt_abstime now = hrt_absolute_time(); @@ -250,23 +259,19 @@ void DShot::select_next_command() _bdshot_edt_attempts[motor_index], (double)now / 1000000.); break; } + } - } else if (_esc_type != 0 && _serial_telemetry_enabled && serial_telem_delay_elapsed && (_motor_mask & needs_settings_request_mask)) { - // Settings Request: use motor-order masks since needs_settings_request_mask is in motor order - uint16_t settings_motors_to_request = _motor_mask & needs_settings_request_mask; + // A motor held off or out of EDT attempts leaves no EDT command, and must not keep the settings reads waiting. + if (_current_command.finished() && _esc_type != 0 && _serial_telemetry_enabled && serial_telem_delay_elapsed + && !_dshot_programming_active && !_mixing_output.armed().armed) { + const int motor_index = _telemetry.getSettingsRequest(_motor_mask & _serial_telem_online_mask); - // Find first motor that needs settings request - for (int motor_index = 0; motor_index < DSHOT_MAX_MOTORS; motor_index++) { - if (settings_motors_to_request & (1 << motor_index)) { - auto now = hrt_absolute_time(); - _current_command.num_repetitions = 6; - _current_command.command = DSHOT_CMD_ESC_INFO; - _current_command.motor_mask = (1 << motor_index); - _current_command.expect_response = true; - _settings_requested_mask |= (1 << motor_index); - PX4_DEBUG("ESC%d: requesting Settings at time %.2fs", motor_index + 1, (double)now / 1000000.); - break; - } + if (motor_index >= 0) { + _current_command.num_repetitions = 6; + _current_command.command = DSHOT_CMD_ESC_INFO; + _current_command.motor_mask = (1 << motor_index); + _current_command.expect_response = true; + PX4_DEBUG("ESC%d: requesting settings", motor_index + 1); } } else if (_dshot_programming_active) { @@ -318,8 +323,8 @@ void DShot::select_next_command() // All settings have been written PX4_DEBUG("All settings written at time %.2fs", (double)hrt_absolute_time() / 1000000.); - _dshot_programming_active = false; _current_command.command = DSHOT_CMD_SAVE_SETTINGS; + _current_command.is_save = true; _current_command.num_repetitions = 6; _current_command.motor_mask = programming_motor_mask; _programming_state = ProgrammingState::Idle; @@ -327,10 +332,6 @@ void DShot::select_next_command() // Clear the written mask for this motor for next time _settings_written_mask[0] = 0; _settings_written_mask[1] = 0; - - // Mark as unread so that we read again - _settings_requested_mask &= ~(programming_motor_mask); - _serial_telem_delay_until = hrt_absolute_time() + 500_ms; } } @@ -433,9 +434,18 @@ void DShot::update_motor_commands(int num_outputs) if (command_sent) { --_current_command.num_repetitions; + if (_current_command.finished() && _current_command.is_save) { + // Programming ends with the save: anything sent in its place leaves the ESC on unsaved settings. + _dshot_programming_active = false; + _telemetry.invalidateSettings(_current_command.motor_mask); + + _serial_telem_delay_until = hrt_absolute_time() + 500_ms; + } + // Queue a save command if it has been requested if (_current_command.num_repetitions == 0 && _current_command.save) { _current_command.save = false; + _current_command.is_save = true; _current_command.num_repetitions = 10; _current_command.command = DSHOT_CMD_SAVE_SETTINGS; } @@ -570,6 +580,7 @@ bool DShot::process_serial_telemetry() if (_serial_telem_consecutive_timeouts[motor_index] >= SERIAL_TELEM_SKIP_THRESHOLD) { _serial_telem_skip_mask |= (1 << motor_index); + _telemetry.invalidateSettings(1u << motor_index); PX4_DEBUG("ESC%d serial telemetry lost, skipping", motor_index + 1); } } @@ -836,7 +847,9 @@ void DShot::handle_vehicle_commands() { vehicle_command_s command = {}; - while (_current_command.finished() && _vehicle_command_sub.update(&command)) { + // Wait for an outstanding settings read, which completes within COMMAND_RESPONSE_TIMEOUT: a setting changed + // before it lands would have the read restore the old cache. + while (_current_command.finished() && _telemetry.commandResponseFinished() && _vehicle_command_sub.update(&command)) { switch (command.command) { case vehicle_command_s::VEHICLE_CMD_CONFIGURE_ACTUATOR: @@ -883,7 +896,11 @@ void DShot::handle_configure_actuator(const vehicle_command_s &command) command_ack.target_component = command.source_component; command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED; - if ((motor_index >= 0) && (motor_index < DSHOT_MAX_MOTORS)) { + if (_dshot_programming_active) { + // Programming spans many commands and would take the next frames as an address and value. + command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + + } else if ((motor_index >= 0) && (motor_index < DSHOT_MAX_MOTORS)) { int type = lroundf(command.param1); PX4_DEBUG("motor_index: %i type: %i", motor_index, type); _current_command.clear(); @@ -925,6 +942,10 @@ void DShot::handle_configure_actuator(const vehicle_command_s &command) if (_current_command.command != DSHOT_CMD_MOTOR_STOP) { command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; _current_command.motor_mask = 1 << motor_index; + + if (_current_command.save) { + _telemetry.invalidateSettings(_current_command.motor_mask); + } } } @@ -944,14 +965,7 @@ void DShot::handle_esc_request_eeprom(const vehicle_command_s &command) return; } - if (esc_index == 255) { - PX4_DEBUG("mark all unread"); - _settings_requested_mask = 0; - - } else { - PX4_DEBUG("mark one unread"); - _settings_requested_mask &= ~(1 << esc_index); - } + _telemetry.requestSettings(esc_index == 255 ? _motor_mask : (1u << esc_index)); } int DShot::get_pole_count(int motor_index) const @@ -1131,15 +1145,20 @@ bool DShot::initialize_dshot() return true; } -void DShot::init_telemetry(const char *device, bool swap_rxtx) +bool DShot::init_telemetry(const char *device, bool swap_rxtx) { if (!device) { - return; + return true; + } + + // The settings handlers are created per motor, and the mask is only known once the mixer has run + if (_motor_mask == 0) { + return false; } if (_telemetry.init(device, swap_rxtx) != PX4_OK) { PX4_ERR("telemetry init failed"); - return; + return true; } // Enable serial telemetry now that we've successfully initialized @@ -1148,6 +1167,8 @@ void DShot::init_telemetry(const char *device, bool swap_rxtx) // Initialize ESC settings handlers based on ESC type ESCType esc_type = static_cast(_param_dshot_esc_type.get()); _telemetry.initSettingsHandlers(esc_type, _motor_mask); + + return true; } static void print_spacer() @@ -1235,10 +1256,6 @@ int DShot::print_status() if (_bdshot_output_mask && _bdshot_edt_enabled) { PX4_INFO(" EDT Confirmed Mask (motor order): 0x%02x", _bdshot_edt_confirmed_mask); } - - if (_serial_telemetry_enabled) { - PX4_INFO(" Settings Requested Mask (motor order): 0x%02x", _settings_requested_mask); - } } // Bidirectional DShot hardware status diff --git a/src/drivers/dshot/DShot.h b/src/drivers/dshot/DShot.h index f5d37f0ff6b..ee5da490d05 100644 --- a/src/drivers/dshot/DShot.h +++ b/src/drivers/dshot/DShot.h @@ -106,7 +106,11 @@ private: DShot operator=(const DShot &) = delete; bool initialize_dshot(); - void init_telemetry(const char *device, bool swap_rxtx); + /** + * Initialize serial telemetry and the ESC settings handlers. + * @return false if the request has to be retried later + */ + bool init_telemetry(const char *device, bool swap_rxtx); // Map output channel to motor index [0..DSHOT_MAX_MOTORS-1], or -1 if not a motor int motor_index_from_output(int output_channel) const @@ -182,8 +186,6 @@ private: hrt_abstime _bdshot_edt_last_request[DSHOT_MAX_MOTORS] = {}; bool _armed_prev = false; - uint16_t _settings_requested_mask = 0; - // Array of timestamps indicating when the telemetry came online hrt_abstime _serial_telem_online_timestamps[DSHOT_MAX_MOTORS] = {}; hrt_abstime _bdshot_telem_online_timestamps[DSHOT_MAX_MOTORS] = {}; @@ -235,6 +237,7 @@ private: int num_repetitions{0}; uint16_t motor_mask{(1u << DSHOT_MAXIMUM_CHANNELS) - 1}; bool save{false}; + bool is_save{false}; // programming address and value frames can also carry DSHOT_CMD_SAVE_SETTINGS bool expect_response{false}; bool finished() const { return num_repetitions == 0; } @@ -244,6 +247,7 @@ private: num_repetitions = 0; motor_mask = 0; save = false; + is_save = false; expect_response = false; } }; diff --git a/src/drivers/dshot/DShotTelemetry.cpp b/src/drivers/dshot/DShotTelemetry.cpp index f8cd8f74f56..579547ee411 100644 --- a/src/drivers/dshot/DShotTelemetry.cpp +++ b/src/drivers/dshot/DShotTelemetry.cpp @@ -50,6 +50,16 @@ using namespace time_literals; // still on its way costs a full round-robin pass of telemetry rather than a single frame. static constexpr hrt_abstime TELEMETRY_RESPONSE_TIMEOUT = 30_ms; +// Telemetry stops while a command response is pending, and the arming check reports an ESC offline once its +// telemetry is 400 ms old. AM32 sends the 49-byte dump from its main loop as soon as the sixth frame arrives, +// so a response that has not started within 100 ms is not coming. +static constexpr hrt_abstime COMMAND_RESPONSE_TIMEOUT = 100_ms; + +// A failed read costs a full COMMAND_RESPONSE_TIMEOUT of telemetry, so retries are spaced for all ESCs +// together and an ESC that keeps failing is dropped until something re-requests it. +static constexpr hrt_abstime SETTINGS_RETRY_INTERVAL = 2_s; +static constexpr int SETTINGS_MAX_ATTEMPTS = 3; + DShotTelemetry::~DShotTelemetry() { _uart.close(); @@ -123,12 +133,70 @@ void DShotTelemetry::initSettingsHandlers(ESCType esc_type, uint16_t output_mask if (interface) { _settings_handlers[i] = interface; + _settings_request_mask |= 1u << i; } } _settings_initialized = true; } +void DShotTelemetry::publishSettings() +{ + // Each read is published right away, this only repeats the cached dumps for logs started later and lost copies + if (hrt_elapsed_time(&_settings_last_publish) < 10_s) { + return; + } + + _settings_last_publish = hrt_absolute_time(); + + for (auto handler : _settings_handlers) { + if (handler) { + handler->publish_latest(); + } + } +} + +int DShotTelemetry::getSettingsRequest(uint16_t motor_mask) +{ + if (hrt_absolute_time() < _settings_retry_after) { + return -1; + } + + // Rotate through pending reads so an unresponsive ESC cannot starve the others. + for (int offset = 0; offset < DSHOT_MAX_MOTORS; ++offset) { + const int index = (_next_settings_motor + offset) % DSHOT_MAX_MOTORS; + + if ((motor_mask & _settings_request_mask & (1u << index)) && _settings_handlers[index]) { + _next_settings_motor = (index + 1) % DSHOT_MAX_MOTORS; + return index; + } + } + + return -1; +} + +void DShotTelemetry::requestSettings(uint16_t motor_mask) +{ + for (int i = 0; i < DSHOT_MAX_MOTORS; i++) { + // Only an ESC with a settings handler is ever read, so a request for any other would stay pending + if ((motor_mask & (1u << i)) && _settings_handlers[i]) { + _settings_request_mask |= 1u << i; + _settings_attempts[i] = 0; + } + } +} + +void DShotTelemetry::invalidateSettings(uint16_t motor_mask) +{ + requestSettings(motor_mask); + + for (int i = 0; i < DSHOT_MAX_MOTORS; i++) { + if ((motor_mask & (1 << i)) && _settings_handlers[i]) { + _settings_handlers[i]->invalidate(); + } + } +} + void DShotTelemetry::resetCommandResponse() { _command_response_motor_index = -1; @@ -136,12 +204,27 @@ void DShotTelemetry::resetCommandResponse() _command_response_position = 0; } +void DShotTelemetry::settingsReadFailed() +{ + const int motor_index = _command_response_motor_index; + + if (_command_response_command == DSHOT_CMD_ESC_INFO && motor_index >= 0 && motor_index < DSHOT_MAX_MOTORS) { + _settings_retry_after = hrt_absolute_time() + SETTINGS_RETRY_INTERVAL; + + if (++_settings_attempts[motor_index] >= SETTINGS_MAX_ATTEMPTS) { + PX4_WARN("ESC%d: no settings after %d requests", motor_index + 1, SETTINGS_MAX_ATTEMPTS); + _settings_request_mask &= ~(1u << motor_index); + } + } + + resetCommandResponse(); +} + void DShotTelemetry::parseCommandResponse() { - if (hrt_elapsed_time(&_command_response_start) > 1_s) { - PX4_WARN("Command response timed out: %d bytes received", _command_response_position); - PX4_WARN("At time %.2fs", (double)hrt_absolute_time() / 1000000.); - resetCommandResponse(); + if (hrt_elapsed_time(&_command_response_start) > COMMAND_RESPONSE_TIMEOUT) { + PX4_DEBUG("Command response timed out: %d bytes received", _command_response_position); + settingsReadFailed(); return; } @@ -155,7 +238,7 @@ void DShotTelemetry::parseCommandResponse() // Handle potential overflow, fail out if (_command_response_position + bytes > COMMAND_RESPONSE_MAX_SIZE) { PX4_ERR("command response overflow"); - resetCommandResponse(); + settingsReadFailed(); return; } @@ -178,9 +261,14 @@ void DShotTelemetry::parseCommandResponse() } if (_command_response_position == handler->getExpectedResponseSize()) { - // TODO: handle command failures - handler->decodeInfoResponse(_command_response_buffer, _command_response_position); - resetCommandResponse(); + if (handler->decodeInfoResponse(_command_response_buffer, _command_response_position)) { + _settings_request_mask &= ~(1u << _command_response_motor_index); + _settings_attempts[_command_response_motor_index] = 0; + resetCommandResponse(); + + } else { + settingsReadFailed(); + } } break; @@ -316,4 +404,5 @@ void DShotTelemetry::printStatus() const PX4_INFO("Successful ESC frames: %i", _num_successful_responses); PX4_INFO("Timeouts: %i", _num_timeouts); PX4_INFO("CRC errors: %i", _num_checksum_errors); + PX4_INFO("Pending settings mask (motor order): 0x%02x", _settings_request_mask); } diff --git a/src/drivers/dshot/DShotTelemetry.h b/src/drivers/dshot/DShotTelemetry.h index c9593ac1529..746b8187426 100644 --- a/src/drivers/dshot/DShotTelemetry.h +++ b/src/drivers/dshot/DShotTelemetry.h @@ -52,7 +52,6 @@ public: TelemetryStatus parseTelemetryPacket(EscData *esc_data); - // Attempt to parse a command response. Returns the index of the ESC or -1 on failure. void parseCommandResponse(); bool commandResponseFinished(); bool commandResponseStarted(); @@ -60,11 +59,16 @@ public: void setExpectCommandResponse(int motor_index, uint16_t command); void resetCommandResponse(); void initSettingsHandlers(ESCType esc_type, uint16_t output_mask); + void publishSettings(); + int getSettingsRequest(uint16_t motor_mask); + void requestSettings(uint16_t motor_mask); + void invalidateSettings(uint16_t motor_mask); private: static constexpr int COMMAND_RESPONSE_MAX_SIZE = 49; static constexpr int TELEMETRY_FRAME_SIZE = 10; TelemetryStatus decodeTelemetryResponse(uint8_t *buffer, int length, EscData *esc_data); + void settingsReadFailed(); device::Serial _uart{}; @@ -89,4 +93,9 @@ private: ESCSettingsInterface *_settings_handlers[DSHOT_MAX_MOTORS] = {nullptr}; ESCType _esc_type{ESCType::Unknown}; bool _settings_initialized{false}; + uint16_t _settings_request_mask{0}; + int _next_settings_motor{0}; + uint8_t _settings_attempts[DSHOT_MAX_MOTORS] {}; + hrt_abstime _settings_retry_after{0}; + hrt_abstime _settings_last_publish{0}; }; diff --git a/src/drivers/dshot/esc/AM32Settings.cpp b/src/drivers/dshot/esc/AM32Settings.cpp index 7f67a6ac1ac..6905b539f72 100644 --- a/src/drivers/dshot/esc/AM32Settings.cpp +++ b/src/drivers/dshot/esc/AM32Settings.cpp @@ -37,11 +37,18 @@ static constexpr int RESPONSE_SIZE = EEPROM_SIZE + 1; // 48B data + 1B CRC +// Each periodic publication queues every ESC's dump at once. +static_assert(DSHOT_MAX_MOTORS <= esc_eeprom_read_s::ORB_QUEUE_LENGTH, "esc_eeprom_read must queue one dump per ESC"); + uORB::Publication AM32Settings::_esc_eeprom_read_pub{ORB_ID(esc_eeprom_read)}; AM32Settings::AM32Settings(int index) : _esc_index(index) -{} +{ + // A subscriber that joins after the first publish only sees the newest queued entry, so the + // topic has to exist before the ESCs answer for the logger to capture every ESC + _esc_eeprom_read_pub.advertise(); +} int AM32Settings::getExpectedResponseSize() { @@ -50,8 +57,13 @@ int AM32Settings::getExpectedResponseSize() void AM32Settings::publish_latest() { + if (_timestamp_sample == 0) { + return; + } + esc_eeprom_read_s data = {}; data.timestamp = hrt_absolute_time(); + data.timestamp_sample = _timestamp_sample; data.firmware = 1; // ESC_FIRMWARE_AM32 data.index = _esc_index; memcpy(data.data, &_eeprom_data, sizeof(_eeprom_data)); @@ -59,6 +71,11 @@ void AM32Settings::publish_latest() _esc_eeprom_read_pub.publish(data); } +void AM32Settings::invalidate() +{ + _timestamp_sample = 0; +} + bool AM32Settings::decodeInfoResponse(const uint8_t *buf, int size) { if (size != RESPONSE_SIZE) { @@ -75,10 +92,9 @@ bool AM32Settings::decodeInfoResponse(const uint8_t *buf, int size) PX4_DEBUG("Successfully received AM32 settings from ESC%d", _esc_index + 1); - // Store data for retrieval later if requested memcpy(&_eeprom_data, buf, EEPROM_SIZE); + _timestamp_sample = hrt_absolute_time(); - // Publish data immediately publish_latest(); return true; diff --git a/src/drivers/dshot/esc/AM32Settings.h b/src/drivers/dshot/esc/AM32Settings.h index c3e8f6f3654..68220109925 100644 --- a/src/drivers/dshot/esc/AM32Settings.h +++ b/src/drivers/dshot/esc/AM32Settings.h @@ -34,6 +34,7 @@ #pragma once #include "ESCSettingsInterface.h" +#include #include #include @@ -48,10 +49,12 @@ public: bool decodeInfoResponse(const uint8_t *buf, int size) override; void publish_latest() override; + void invalidate() override; private: int _esc_index{}; uint8_t _eeprom_data[EEPROM_SIZE] {}; + hrt_abstime _timestamp_sample{0}; // 0 while nothing valid is cached static uORB::Publication _esc_eeprom_read_pub; }; diff --git a/src/drivers/dshot/esc/ESCSettingsInterface.h b/src/drivers/dshot/esc/ESCSettingsInterface.h index fe5b6db3717..c617f4e67bc 100644 --- a/src/drivers/dshot/esc/ESCSettingsInterface.h +++ b/src/drivers/dshot/esc/ESCSettingsInterface.h @@ -48,6 +48,7 @@ public: virtual bool decodeInfoResponse(const uint8_t *buf, int size) = 0; virtual int getExpectedResponseSize() = 0; virtual void publish_latest() { /* no-op */}; + virtual void invalidate() { /* no-op */}; // TODO: function to read data // TODO: function to write data diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 7340f3ff426..d99afc81fb5 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -67,6 +67,7 @@ void LoggedTopics::add_default_topics() add_optional_topic("external_ins_attitude"); add_optional_topic("external_ins_global_position"); add_optional_topic("external_ins_local_position"); + add_topic("esc_eeprom_read"); add_topic("esc_status", 100); add_topic("failure_detector_status", 100); add_topic("failsafe_flags"); diff --git a/src/modules/mavlink/streams/ESC_EEPROM.hpp b/src/modules/mavlink/streams/ESC_EEPROM.hpp index 9f078887c7b..30bed221cc7 100644 --- a/src/modules/mavlink/streams/ESC_EEPROM.hpp +++ b/src/modules/mavlink/streams/ESC_EEPROM.hpp @@ -34,7 +34,9 @@ #ifndef ESC_EEPROM_HPP #define ESC_EEPROM_HPP +#include #include +#include class MavlinkStreamEscEeprom : public MavlinkStream { @@ -57,11 +59,24 @@ private: uORB::Subscription _esc_eeprom_read_sub{ORB_ID(esc_eeprom_read)}; - bool emit_message(bool force) + // The driver republishes every cached dump every 10 seconds for the log. Forward each read once per link: a new + // read (boot, reconnect, save, ESC_REQUEST_EEPROM) changes timestamp_sample, and a link that comes up later + // still gets every ESC from the next republish. + hrt_abstime _sent_timestamp_sample[esc_status_s::CONNECTED_ESC_MAX] {}; + + bool send() override { - esc_eeprom_read_s eeprom = {}; + bool sent = false; + esc_eeprom_read_s eeprom; + + while (_esc_eeprom_read_sub.update(&eeprom)) { + if (eeprom.index >= esc_status_s::CONNECTED_ESC_MAX + || eeprom.timestamp_sample == _sent_timestamp_sample[eeprom.index]) { + continue; + } + + _sent_timestamp_sample[eeprom.index] = eeprom.timestamp_sample; - if (_esc_eeprom_read_sub.update(&eeprom) || force) { mavlink_esc_eeprom_t msg = {}; msg.firmware = eeprom.firmware; msg.esc_index = eeprom.index; @@ -72,18 +87,11 @@ private: msg.length = copy_len; mavlink_msg_esc_eeprom_send_struct(_mavlink->get_channel(), &msg); - - return true; + sent = true; } - return false; + return sent; } - - bool send() override - { - return emit_message(false); - } - }; #endif // ESC_EEPROM_HPP