mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-11-22 14:19:57 +08:00
feat(dshot): log the ESC EEPROM dumps (#28800)
* fix(dshot): retry telemetry init until the motor mask is known rc.serial issues `dshot telemetry` right after `dshot start`, and the first Run() usually handles it before the mixer has reported the outputs. initSettingsHandlers() then latches with an empty motor mask, so no ESC settings handler is ever created and the EEPROM is never read. Co-authored-by: danielbuleandra <daniel.buleandra@auterion.com> Assisted-by: Claude:claude-fable-5-1 Signed-off-by: Jacob Dahl <dahl.jakejacob@gmail.com> * feat(logger): log the ESC EEPROM dumps The raw dump is logged through the existing queued esc_eeprom_read topic. The layout depends on the ESC firmware and its EEPROM revision, so decoding happens offline in Tools/esc_eeprom_decode.py against the schema the ESC vendor publishes. A subscriber that joins after the first publish only sees the newest queued entry. The logger subscribes on a sweep that can lag a new topic by half a second while all ESCs answer within milliseconds of each other, so the topic is advertised when the settings handlers are created, well before the first request goes out at ESC_INIT_TELEM_DELAY. Co-authored-by: danielbuleandra <daniel.buleandra@auterion.com> Assisted-by: Claude:claude-fable-5-1 Signed-off-by: Jacob Dahl <dahl.jakejacob@gmail.com> * feat(dshot): publish and refresh cached ESC settings Logs can start after boot or during flight, so cached settings must be published independently of arming. The queue must retain a complete set of ESC dumps for the logger and MAVLink stream. Keep programming uninterrupted and cached data valid across saves and reconnects. Failed reads remain pending with spaced retries so one unresponsive ESC cannot starve the rest. Signed-off-by: Jacob Dahl <dahl.jakejacob@gmail.com> Assisted-by: Codex * fix(tools): decode repeated ESC dumps across firmware layouts Logs contain repeated dumps and may mix EEPROM layouts. Collapse unchanged samples, apply the schema's version bounds and disabled values, and keep overlays immutable so one ESC cannot change another's decoding. Signed-off-by: Jacob Dahl <dahl.jakejacob@gmail.com> Assisted-by: Codex * fix(dshot): bound ESC settings read retries A pending settings read blocks the telemetry round-robin until it completes or times out, and the retry spacing was kept per ESC. With several ESCs that never answer, reads ran back to back, telemetry went stale and the arming check reported every ESC offline. Space retries across all ESCs, give up on one after three failed reads until it reconnects or is asked for again, and time a read out at 100 ms: AM32 answers within milliseconds of the sixth frame, and anything under the 400 ms arming-check window keeps telemetry fresh. Assisted-by: Claude:claude-fable-5-1 Signed-off-by: Jacob Dahl <dahl.jakejacob@gmail.com> * fix(dshot): keep programming active until the save is sent The flag was cleared when the save command was queued, so it no longer guarded the save itself. Only the finished() gate in the vehicle command loop kept a CONFIGURE_ACTUATOR from replacing it. Clear the flag when the last save frame goes out so the guard covers the whole sequence. Assisted-by: Claude:claude-fable-5-1 Signed-off-by: Jacob Dahl <dahl.jakejacob@gmail.com> * fix(dshot): send each ESC EEPROM read once per MAVLink link The once-a-second republish that keeps the dumps in every log also went out on every link with ESC_EEPROM configured, low-bandwidth ones included, one message per ESC per second. timestamp_sample records when a dump was read, so the stream forwards each read once per link and still reaches a link that connects later. A motor waiting on an EDT retry or out of EDT attempts took the command slot without sending anything, so settings reads never ran while any ESC left EDT unconfirmed. Vehicle commands now wait for an outstanding settings read, which ends within 100 ms, instead of CONFIGURE_ACTUATOR being rejected. Assisted-by: Claude:claude-opus-5-5 * fix(msg): queue one ESC EEPROM dump per esc_status ESC The republish queues every cached dump at once, so the queue needs one entry per ESC and no more. DSHOT_MAX_MOTORS is capped at CONNECTED_ESC_MAX, and the static_assert in AM32Settings.cpp keeps the two in step. Assisted-by: Claude:claude-opus-5-5 * fix(dshot): mark the settings save explicitly Programming ended when any finished command carried the value 12, which the address and value frames do whenever they write offset 12 (active brake power) or a value of 12. The write then stopped part way, with the ESC still in programming mode and the next write resuming mid-sequence. Assisted-by: Claude:claude-opus-5-5 * fix(dshot): keep settings requests and writes to configured motors An all-ESC save requested settings for every motor slot, and the single-ESC request and write paths took any index below the motor limit. Reads only go to motors with a settings handler, so the other bits stayed pending in dshot status forever. A write to an index with no output was never sent, so the command never finished and blocked every later command and the serial telemetry until reboot. Assisted-by: Claude:claude-opus-5-5 * docs(dshot): describe the logged ESC settings and their decoding The page said the settings are only read at boot and only reach a ground station. Assisted-by: Claude:claude-opus-5-5 * feat(dshot): set the ciclicity to 10s for eeprom logging --------- Signed-off-by: Jacob Dahl <dahl.jakejacob@gmail.com> Co-authored-by: danielbuleandra <daniel.buleandra@auterion.com>
This commit is contained in:
1 parent
a408b6584a
commit
ae7977f868
12 files changed
+571
-77
No files matched your search
Executable
+331
@@ -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 <log.ulg> --schema eeprom.json
|
||||
./esc_eeprom_decode.py <log.ulg> --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}')
|
||||
@@ -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 <log.ulg> --schema eeprom.json --esc 1
|
||||
```
|
||||
|
||||
Omit `--esc` to decode every ESC.
|
||||
|
||||
## ESC Serial Passthrough
|
||||
|
||||
<Badge type="tip" text="main (PX4 v2.0)" />
|
||||
|
||||
@@ -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
|
||||
+62
-45
@@ -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<ESCType>(_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
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
};
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
@@ -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};
|
||||
};
|
||||
@@ -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<esc_eeprom_read_s> 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;
|
||||
|
||||
@@ -34,6 +34,7 @@
|
||||
#pragma once
|
||||
|
||||
#include "ESCSettingsInterface.h"
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/topics/esc_eeprom_read.h>
|
||||
|
||||
@@ -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_s> _esc_eeprom_read_pub;
|
||||
};
|
||||
@@ -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
|
||||
|
||||
@@ -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");
|
||||
|
||||
@@ -34,7 +34,9 @@
|
||||
#ifndef ESC_EEPROM_HPP
|
||||
#define ESC_EEPROM_HPP
|
||||
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/topics/esc_eeprom_read.h>
|
||||
#include <uORB/topics/esc_status.h>
|
||||
|
||||
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
|
||||
Reference in new issue
Block a user