mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-13 15:23:33 +08:00
claude first attempt at converting https://github.com/PX4/PX4-Autopilot/pull/25503
This commit is contained in:
Jacob Dahl
@@ -0,0 +1,280 @@
|
||||
AM32 ESC Flasher Module - Implementation Plan
|
||||
|
||||
Context
|
||||
|
||||
PX4 has no upstream support for AM32 ESC firmware updates. Matt's branch (jake/matt_firefly_am32_flasher) proves the concept works but has architectural issues: firmware baked into the build as a C
|
||||
array, ~100-state machine with poor error recovery, tight coupling to DShot via a 4-message uORB handshake, no GCS progress reporting, and ~826 lines of flashing logic injected into the DShot driver.
|
||||
|
||||
Goal: Clean, upstream-quality esc_flasher module that reads firmware from SD card, is triggered via MAVLink command from QGC, reports per-ESC progress via a custom MAVLink message, and requires a
|
||||
reboot to resume normal DShot operation. Structured to allow future USB passthrough (4-way interface) extension.
|
||||
|
||||
Architecture Overview
|
||||
|
||||
QGC PX4
|
||||
│ │
|
||||
├─COMMAND_LONG──────────────►│ vehicle_command uORB
|
||||
│ MAV_CMD_ESC_FW_UPDATE │ │
|
||||
│ │ esc_flasher module
|
||||
│ │ (dormant at boot, wakes on cmd)
|
||||
│ │ │
|
||||
│ │ 1. Validate (disarmed, files exist)
|
||||
│ │ 2. ACK command
|
||||
│ │ 3. Stop DShot module
|
||||
│ │ 4. Take over motor GPIOs
|
||||
│ │ 5. For each ESC:
|
||||
│ │ - Bootloader handshake
|
||||
│ │ - Read device info → pick FW file
|
||||
│◄─ESC_FW_UPDATE_STATUS──────│ - Flash 256-byte chunks
|
||||
│ (per-ESC progress) │ - Send RUN_APP
|
||||
│ │ 6. Publish REBOOT_REQUIRED
|
||||
│◄─COMMAND_ACK (ACCEPTED)────│ 7. Go idle (reboot needed)
|
||||
|
||||
|
||||
New Files
|
||||
|
||||
1. src/modules/esc_flasher/EscFlasher.hpp
|
||||
|
||||
Module class inheriting ModuleBase<EscFlasher> + ModuleParams + ScheduledWorkItem.
|
||||
|
||||
Key members:
|
||||
- _vehicle_command_sub — polls for MAV_CMD trigger
|
||||
- _esc_fw_update_status_pub — publishes per-ESC progress
|
||||
- _command_ack_pub — ACKs the initial command
|
||||
- State enum: Idle, StoppingDShot, InitGPIOs, ConnectingESC, ReadingInfo, Flashing, RunApp, Complete, Failed
|
||||
- Per-ESC state: _current_esc_index, _esc_count, _bytes_written, _firmware_size
|
||||
- GPIO pin array: uint32_t _gpio_pins[MAX_MOTORS]
|
||||
- File handle for current firmware
|
||||
|
||||
2. src/modules/esc_flasher/EscFlasher.cpp
|
||||
|
||||
Main module implementation (~600-800 lines, much simpler than Matt's ~1,900).
|
||||
|
||||
Module lifecycle:
|
||||
- Auto-started at boot on supported boards (Kconfig opt-in)
|
||||
- Runs at 1Hz when idle (vehicle_command poll only — negligible overhead)
|
||||
- Transitions to 100Hz active polling during flashing
|
||||
- Returns to idle after completion (reboot required to restore DShot)
|
||||
|
||||
DShot shutdown:
|
||||
- Calls system("dshot stop") via NuttX shell — DShot destructor calls up_dshot_arm(false)
|
||||
- Waits up to 2s for DShot to exit, then proceeds
|
||||
- Simple, no coupling — DShot module has no knowledge of the flasher
|
||||
|
||||
GPIO takeover:
|
||||
- After DShot stops, configures motor pins as GPIO outputs via px4_arch_configgpio(io_timer_channel_get_gpio_output(ch))
|
||||
- Drives all pins HIGH for 600ms to force ESCs into bootloader mode on reboot
|
||||
- ESC power cycle not required — AM32 bootloader detects signal-high at startup
|
||||
|
||||
Flashing loop (per ESC):
|
||||
1. Select ESC (drive target pin, keep others HIGH)
|
||||
2. Send bootloader handshake → get 9-byte device info
|
||||
3. Read firmware tag (CMD_READ_FLASH at tag address) → extract hardware target string
|
||||
4. Open /fs/microsd/am32/<target>.bin (e.g., AM32_G071_48K.bin)
|
||||
5. For each 256-byte chunk:
|
||||
- CMD_SET_ADDRESS
|
||||
- CMD_SET_BUFFER + data + CRC
|
||||
- CMD_PROG_FLASH → wait ACK
|
||||
- Publish progress (chunk_index / total_chunks * 100)
|
||||
6. Write firmware tag section
|
||||
7. CMD_RUN → ESC reboots to application
|
||||
8. Move to next ESC
|
||||
|
||||
3. src/modules/esc_flasher/am32_protocol.hpp / .cpp
|
||||
|
||||
Extracted and cleaned AM32 bootloader protocol implementation.
|
||||
|
||||
Reused from Matt's code (protocol is correct):
|
||||
- crc16_ccitt() — CRC-16 with polynomial 0xA001
|
||||
- bitbang_send_packet() — bit-banged UART TX/RX at 19200 baud
|
||||
- 52µs bit time, 26µs half-bit for sampling
|
||||
- px4_enter_critical_section() for timing accuracy
|
||||
- hrt_absolute_time() busy-wait loops
|
||||
- Bootloader handshake packet: {0x00 * 8, 0x0D, 'B','L','H','e','l','i', CRC}
|
||||
- Command encoding: SET_ADDRESS(0xFF), SET_BUFFER(0xFE), PROG_FLASH(0x01), RUN(0x00), READ_FLASH(0x03)
|
||||
|
||||
Cleaned up vs Matt's code:
|
||||
- Function-based API instead of inline state machine code
|
||||
- Proper error return codes instead of magic numbers
|
||||
- Configurable timeouts instead of hardcoded values
|
||||
- Separated from module state machine logic
|
||||
|
||||
Public API:
|
||||
namespace am32 {
|
||||
int handshake(uint32_t gpio, uint8_t device_info[9]);
|
||||
int set_address(uint32_t gpio, uint16_t address);
|
||||
int write_chunk(uint32_t gpio, const uint8_t *data, uint16_t len);
|
||||
int read_flash(uint32_t gpio, uint16_t address, uint8_t *buf, uint16_t len);
|
||||
int run_app(uint32_t gpio);
|
||||
uint16_t crc16(const uint8_t *buf, uint16_t len);
|
||||
}
|
||||
|
||||
4. src/modules/esc_flasher/CMakeLists.txt
|
||||
|
||||
px4_add_module(
|
||||
MODULE modules__esc_flasher
|
||||
MAIN esc_flasher
|
||||
SRCS
|
||||
EscFlasher.cpp
|
||||
am32_protocol.cpp
|
||||
DEPENDS
|
||||
px4_work_queue
|
||||
)
|
||||
|
||||
5. src/modules/esc_flasher/Kconfig
|
||||
|
||||
menuconfig MODULES_ESC_FLASHER
|
||||
bool "esc_flasher"
|
||||
default n
|
||||
---help---
|
||||
AM32 ESC firmware update module
|
||||
|
||||
|
||||
6. msg/EscFirmwareUpdateStatus.msg
|
||||
|
||||
uint64 timestamp
|
||||
|
||||
uint8 esc_index # Current ESC being flashed (0-based)
|
||||
uint8 esc_count # Total ESCs being flashed
|
||||
uint8 progress_pct # 0-100 for current ESC
|
||||
uint8 overall_progress_pct # 0-100 for entire operation
|
||||
|
||||
uint8 status
|
||||
uint8 STATUS_IDLE = 0
|
||||
uint8 STATUS_CONNECTING = 1
|
||||
uint8 STATUS_READING_INFO = 2
|
||||
uint8 STATUS_ERASING = 3
|
||||
uint8 STATUS_WRITING = 4
|
||||
uint8 STATUS_COMPLETE = 5
|
||||
uint8 STATUS_FAILED = 6
|
||||
uint8 STATUS_REBOOT_REQUIRED = 7
|
||||
|
||||
uint8 error
|
||||
uint8 ERROR_NONE = 0
|
||||
uint8 ERROR_NOT_DISARMED = 1
|
||||
uint8 ERROR_NO_FIRMWARE_FILE = 2
|
||||
uint8 ERROR_COMMS_TIMEOUT = 3
|
||||
uint8 ERROR_CRC_MISMATCH = 4
|
||||
uint8 ERROR_VERIFICATION_FAILED = 5
|
||||
uint8 ERROR_UNSUPPORTED_HARDWARE = 6
|
||||
uint8 ERROR_DSHOT_STOP_FAILED = 7
|
||||
|
||||
uint32 current_firmware_version # Currently installed version
|
||||
uint32 new_firmware_version # Version being written
|
||||
|
||||
|
||||
Modified Files
|
||||
|
||||
7. msg/versioned/VehicleCommand.msg
|
||||
|
||||
Add command constant in PX4-internal range (for development; upstream MAVLink later):
|
||||
|
||||
uint32 VEHICLE_CMD_ESC_FIRMWARE_UPDATE = 100002
|
||||
|
||||
|
||||
- param1: ESC index (0-based, 255 = all ESCs)
|
||||
- param2: ESC type (0 = AM32)
|
||||
|
||||
8. MAVLink message definition (development.xml in mavlink submodule)
|
||||
|
||||
Add ESC_FIRMWARE_UPDATE_STATUS message definition. This is in the MAVLink submodule — we'll fork/PR it separately. For initial development, stream the uORB message as a generic STATUSTEXT fallback.
|
||||
|
||||
9. src/modules/mavlink/streams/ESC_FIRMWARE_UPDATE_STATUS.hpp
|
||||
|
||||
New MAVLink stream class that subscribes to esc_firmware_update_status uORB and sends the MAVLink message. Follow the pattern of existing streams like ESC_STATUS.hpp.
|
||||
|
||||
10. src/modules/mavlink/mavlink_messages.cpp
|
||||
|
||||
Register the new stream.
|
||||
|
||||
11. Board configs
|
||||
|
||||
Enable MODULES_ESC_FLASHER on boards that use AM32 ESCs (e.g., ARK boards). Add esc_flasher start to the board's startup script (rc.board_defaults or similar).
|
||||
|
||||
Key Design Decisions
|
||||
|
||||
Why stop DShot entirely (not just pause)?
|
||||
- DShot uses DMA + timer hardware that conflicts with GPIO bit-bang
|
||||
- Clean shutdown via destructor guarantees all DMA released, timers freed
|
||||
- Reboot to restore is simple and safe — avoids complex reinit edge cases
|
||||
- ESCs need power cycle anyway after reflash
|
||||
|
||||
Why system("dshot stop") instead of uORB handshake?
|
||||
- Zero coupling — DShot module doesn't know flasher exists
|
||||
- Works with any output driver (PWM, OneShot, future drivers)
|
||||
- DShot's existing ModuleBase::stop() + destructor already handles cleanup
|
||||
- Matt's 4-message uORB handshake added 826 lines to DShot for this
|
||||
|
||||
Why auto-start + idle instead of on-demand spawn?
|
||||
- Module is already loaded and subscribed to vehicle_command — instant response
|
||||
- 1Hz idle poll is negligible overhead
|
||||
- Avoids complexity of commander spawning external modules
|
||||
- Kconfig opt-in means boards that don't need it pay zero cost
|
||||
|
||||
Why separate am32_protocol from module?
|
||||
- Clean separation of protocol logic from module orchestration
|
||||
- Protocol code can be reused by future 4-way passthrough implementation
|
||||
- Easier to test protocol functions in isolation
|
||||
|
||||
Firmware File Convention
|
||||
|
||||
Files in /fs/microsd/am32/:
|
||||
- Named by AM32 hardware target: AM32_G071_48K.bin, AM32_F051_32K.bin, etc.
|
||||
- The flasher connects to each ESC bootloader, reads the firmware tag area to determine hardware target
|
||||
- Maps tag → filename, opens file, flashes
|
||||
- If no matching file found, reports ERROR_NO_FIRMWARE_FILE for that ESC and moves on
|
||||
|
||||
State Machine (simplified)
|
||||
|
||||
Idle ──(MAV_CMD)──► StoppingDShot ──(dshot stopped)──► InitGPIOs
|
||||
│
|
||||
(pins HIGH, 600ms)
|
||||
│
|
||||
┌──► ConnectESC
|
||||
│ │
|
||||
│ (handshake OK)
|
||||
│ │
|
||||
│ ReadInfo ──► LoadFirmware ──► Flash
|
||||
│ │
|
||||
│ (all chunks done)
|
||||
│ │
|
||||
│ RunApp ◄──────────────────────┘
|
||||
│ │
|
||||
│ (next ESC?)
|
||||
│ │
|
||||
└──YES─┘
|
||||
│ NO
|
||||
▼
|
||||
Complete
|
||||
(REBOOT_REQUIRED)
|
||||
|
||||
|
||||
Error from any flashing state → Failed (with error code), skip to next ESC or abort.
|
||||
|
||||
Implementation Order
|
||||
|
||||
1. am32_protocol.cpp/hpp — Extract and clean up Matt's protocol code (CRC, bitbang, commands)
|
||||
2. EscFirmwareUpdateStatus.msg — uORB message definition
|
||||
3. VehicleCommand.msg — Add command constant
|
||||
4. EscFlasher.hpp/cpp — Module skeleton with state machine
|
||||
5. CMakeLists.txt + Kconfig — Build integration
|
||||
6. MAVLink stream — ESC_FIRMWARE_UPDATE_STATUS.hpp + register in mavlink_messages.cpp
|
||||
7. Board config — Enable on a test board
|
||||
8. Test — SITL smoke test (state machine logic), then hardware test
|
||||
|
||||
Verification
|
||||
|
||||
1. Build: make ark_fmu-v6x_default (or target board) — verify clean compile
|
||||
2. SITL smoke test: Module starts, responds to vehicle command, reports error (no GPIOs in SITL)
|
||||
3. Hardware test:
|
||||
- Place AM32 firmware on SD card
|
||||
- Send MAV_CMD_ESC_FIRMWARE_UPDATE via commander shell or MAVLink
|
||||
- Verify DShot stops, ESCs enter bootloader, firmware flashes, progress reported
|
||||
- Reboot, verify ESCs running new firmware
|
||||
4. QGC integration: Verify COMMAND_ACK and status messages appear in MAVLink inspector
|
||||
|
||||
Future Extensions (not in this PR)
|
||||
|
||||
- 4-way interface passthrough: Add USB serial passthrough mode using the same am32_protocol infrastructure. The flasher module would proxy between USB serial (4-way protocol) and GPIO bit-bang.
|
||||
- Runtime DShot restart: Instead of requiring reboot, add dshot start after flashing. Needs careful timer/DMA reinit validation.
|
||||
- ESC settings read/write: Extend am32_protocol with EEPROM read/write commands.
|
||||
- Orchestrator module: If hot-switching between DShot and passthrough becomes needed, add a motor_output_mgr similar to CDC ACM.
|
||||
+1
-4
@@ -68,10 +68,7 @@ set(msg_files
|
||||
DistanceSensorModeChangeRequest.msg
|
||||
DronecanNodeStatus.msg
|
||||
Ekf2Timestamps.msg
|
||||
EscFlasherRequest.msg
|
||||
EscFlasherRequestAck.msg
|
||||
EscFlasherStatus.msg
|
||||
EscFlasherVersions.msg
|
||||
EscFirmwareUpdateStatus.msg
|
||||
EscReport.msg
|
||||
EscStatus.msg
|
||||
EstimatorAidSource1d.msg
|
||||
|
||||
@@ -0,0 +1,29 @@
|
||||
uint64 timestamp # time since system start (microseconds)
|
||||
|
||||
uint8 esc_index # Current ESC being flashed (0-based)
|
||||
uint8 esc_count # Total ESCs being flashed
|
||||
uint8 progress_pct # 0-100 for current ESC
|
||||
uint8 overall_progress_pct # 0-100 for entire operation
|
||||
|
||||
uint8 status
|
||||
uint8 STATUS_IDLE = 0
|
||||
uint8 STATUS_CONNECTING = 1
|
||||
uint8 STATUS_READING_INFO = 2
|
||||
uint8 STATUS_ERASING = 3
|
||||
uint8 STATUS_WRITING = 4
|
||||
uint8 STATUS_COMPLETE = 5
|
||||
uint8 STATUS_FAILED = 6
|
||||
uint8 STATUS_REBOOT_REQUIRED = 7
|
||||
|
||||
uint8 error
|
||||
uint8 ERROR_NONE = 0
|
||||
uint8 ERROR_NOT_DISARMED = 1
|
||||
uint8 ERROR_NO_FIRMWARE_FILE = 2
|
||||
uint8 ERROR_COMMS_TIMEOUT = 3
|
||||
uint8 ERROR_CRC_MISMATCH = 4
|
||||
uint8 ERROR_VERIFICATION_FAILED = 5
|
||||
uint8 ERROR_UNSUPPORTED_HARDWARE = 6
|
||||
uint8 ERROR_DSHOT_STOP_FAILED = 7
|
||||
|
||||
uint32 current_firmware_version # Currently installed version
|
||||
uint32 new_firmware_version # Version being written
|
||||
@@ -1,16 +0,0 @@
|
||||
uint64 timestamp # time since system start (microseconds)
|
||||
|
||||
uint32 msg_id # Give a unique message identifier so we can compare when getting an ACK
|
||||
|
||||
uint8 request # Request type
|
||||
# use these items as enum values, they can never change
|
||||
uint8 REQUEST_ESC_INFO = 0 # Request ESC_INFO from one or more ESCs
|
||||
uint8 REQUEST_FLASHING = 1 # Request flashing for one or more ESCs
|
||||
uint8 REQUEST_CANCEL = 2 # Cancel any pending request and return control to DShot
|
||||
uint8 REQUEST_FLASHING_COMPLETE = 3 # Flashing complete, return control to DShot
|
||||
|
||||
uint8 motor_flags # Flags for which motors are being requested
|
||||
# motor_flags bit 0 : Set to 1 if ESC0 is being flashed
|
||||
# motor_flags bit 1 : Set to 1 if ESC1 is being flashed
|
||||
# motor_flags bit 2 : Set to 1 if ESC2 is being flashed
|
||||
# motor_flags bit 3 : Set to 1 if ESC3 is being flashed
|
||||
@@ -1,31 +0,0 @@
|
||||
# ESC Flasher Request Acknowledgement uORB message.
|
||||
# Used for acknowledging the ESC Flasher command being received.
|
||||
# Follows the MAVLink COMMAND_ACK message definition
|
||||
|
||||
uint64 timestamp # time since system start (microseconds)
|
||||
|
||||
uint32 msg_id # Message identifier that we are responding to
|
||||
|
||||
uint8 request # Request type
|
||||
# use these items as enum values, they can never change
|
||||
uint8 REQUEST_ESC_INFO = 0 # Request ESC_INFO from one or more ESCs
|
||||
uint8 REQUEST_FLASHING = 1 # Request flashing for one or more ESCs
|
||||
uint8 REQUEST_CANCEL = 2 # Cancel any pending request and return control to DShot
|
||||
uint8 REQUEST_FLASHING_COMPLETE = 3 # Flashing complete, return control to DShot
|
||||
|
||||
uint8 result # Command acknowledge result
|
||||
# Result cases. This follows the MAVLink MAV_RESULT enum definition
|
||||
uint8 ESC_FLASHER_CMD_RESULT_ACCEPTED = 0 # Command ACCEPTED and EXECUTED |
|
||||
uint8 ESC_FLASHER_CMD_RESULT_TEMPORARILY_REJECTED = 1 # Command TEMPORARY REJECTED/DENIED |
|
||||
uint8 ESC_FLASHER_CMD_RESULT_DENIED = 2 # Command PERMANENTLY DENIED |
|
||||
uint8 ESC_FLASHER_CMD_RESULT_UNSUPPORTED = 3 # Command UNKNOWN/UNSUPPORTED |
|
||||
uint8 ESC_FLASHER_CMD_RESULT_FAILED = 4 # Command executed, but failed |
|
||||
uint8 ESC_FLASHER_CMD_RESULT_IN_PROGRESS = 5 # Command being executed |
|
||||
uint8 ESC_FLASHER_CMD_RESULT_CANCELLED = 6 # Command Canceled
|
||||
|
||||
uint8[16] fw_major # Firmware versions major
|
||||
uint8[16] fw_minor # Firmware versions minor
|
||||
uint16 fw_flags # Bit flags for valid fw versions
|
||||
|
||||
uint32[16] gpio_pins # GPIO pin definitions
|
||||
uint16 gpio_flags # Bit flags for valid gpio pins
|
||||
@@ -1,7 +0,0 @@
|
||||
uint64 timestamp # time since system start (microseconds)
|
||||
|
||||
bool esc_flashing_in_progress
|
||||
|
||||
uint8 version_major
|
||||
|
||||
uint8 version_minor
|
||||
@@ -1,25 +0,0 @@
|
||||
# ESC Flasher Versions uOrb message
|
||||
#
|
||||
uint64 timestamp # time since system start (microseconds)
|
||||
|
||||
uint8 result
|
||||
# use these items as enum values, they can never change
|
||||
uint8 RESULT_UNKNOWN = 0 #
|
||||
uint8 RESULT_SUCCESS = 1 #
|
||||
uint8 RESULT_NO_DATA = 2 #
|
||||
uint8 RESULT_MISMATCH = 3 #
|
||||
uint8 RESULT_NEEDS_UPDATE = 4 #
|
||||
uint8 RESULT_BROKEN_ESC = 5 #
|
||||
|
||||
uint16 versions_valid # Bit-flags to identify valid version information
|
||||
|
||||
uint8[16] esc_type
|
||||
# use these items as enum values, they can never change
|
||||
uint8 ESC_TYPE_UNKNOWN = 0 # Unknown
|
||||
uint8 ESC_TYPE_AM32 = 1 # AM32 2.18+
|
||||
uint8 ESC_TYPE_BLHELI = 2 # BLHeli
|
||||
uint8 ESC_TYPE_AM32_OLD = 3 # AM32 2.16
|
||||
uint8 ESC_TYPE_BLUEJAY = 4 # BlueJay
|
||||
|
||||
uint8[16] versions_major # Firmware versions major
|
||||
uint8[16] versions_minor # Firmware versions minor
|
||||
@@ -111,6 +111,7 @@ uint16 VEHICLE_CMD_EXTERNAL_WIND_ESTIMATE = 43004
|
||||
uint32 VEHICLE_CMD_PX4_INTERNAL_START = 65537 # Start of PX4 internal only vehicle commands (> UINT16_MAX).
|
||||
uint32 VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN = 100000 # Sets the GPS coordinates of the vehicle local origin (0,0,0) position. |Unused|Unused|Unused|Unused|Latitude (WGS-84)|Longitude (WGS-84)|[m] Altitude (AMSL from GNSS, positive above ground)|
|
||||
uint32 VEHICLE_CMD_SET_NAV_STATE = 100001 # Change mode by specifying nav_state directly. |nav_state|Unused|Unused|Unused|Unused|Unused|Unused|
|
||||
uint32 VEHICLE_CMD_ESC_FIRMWARE_UPDATE = 100002 # Trigger AM32 ESC firmware update from SD card. |ESC index (0-based, 255=all)|ESC type (0=AM32)|Unused|Unused|Unused|Unused|Unused|
|
||||
|
||||
uint8 VEHICLE_MOUNT_MODE_RETRACT = 0 # Load and keep safe position (Roll,Pitch,Yaw) from permanent memory and stop stabilization.
|
||||
uint8 VEHICLE_MOUNT_MODE_NEUTRAL = 1 # Load and keep neutral position (Roll,Pitch,Yaw) from permanent memory.
|
||||
|
||||
+98
-740
File diff suppressed because it is too large
Load Diff
@@ -42,11 +42,6 @@
|
||||
|
||||
#include "DShotTelemetry.h"
|
||||
|
||||
#include <uORB/topics/esc_flasher_request.h>
|
||||
#include <uORB/topics/esc_flasher_request_ack.h>
|
||||
#include <uORB/topics/esc_flasher_status.h>
|
||||
#include <uORB/topics/esc_flasher_versions.h>
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
#if !defined(DIRECT_PWM_OUTPUT_CHANNELS)
|
||||
@@ -81,8 +76,6 @@ public:
|
||||
/** @see ModuleBase */
|
||||
static int print_usage(const char *reason = nullptr);
|
||||
|
||||
void retrieve_and_print_esc_info_thread_safe(const int motor_index);
|
||||
|
||||
/**
|
||||
* Send a dshot command to one or all motors
|
||||
* This is expected to be called from another thread.
|
||||
@@ -100,9 +93,6 @@ public:
|
||||
bool updateOutputs(uint16_t outputs[MAX_ACTUATORS],
|
||||
unsigned num_outputs, unsigned num_control_groups_updated) override;
|
||||
|
||||
bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
|
||||
unsigned num_outputs, unsigned num_control_groups_updated);// override;
|
||||
|
||||
private:
|
||||
|
||||
/** Disallow copy construction and move assignment. */
|
||||
@@ -131,7 +121,7 @@ private:
|
||||
|
||||
void enable_dshot_outputs(const bool enabled);
|
||||
|
||||
void init_telemetry(const char *device);
|
||||
void init_telemetry(const char *device, bool swap_rxtx);
|
||||
|
||||
int handle_new_telemetry_data(const int telemetry_index, const DShotTelemetry::EscData &data, bool ignore_rpm);
|
||||
|
||||
@@ -139,8 +129,6 @@ private:
|
||||
|
||||
int handle_new_bdshot_erpm(void);
|
||||
|
||||
int request_esc_info();
|
||||
|
||||
void Run() override;
|
||||
|
||||
void update_params();
|
||||
@@ -149,13 +137,7 @@ private:
|
||||
|
||||
void handle_vehicle_commands();
|
||||
|
||||
void handle_esc_flasher_requests();
|
||||
|
||||
void run_get_esc_info();
|
||||
|
||||
int retrieve_and_print_esc_info_non_blocking(const int motor_index);
|
||||
|
||||
int retrieve_and_print_esc_info_check_result(uint8_t* fw_ver_major, uint8_t* fw_ver_minor);
|
||||
uint16_t convert_output_to_3d_scaling(uint16_t output);
|
||||
|
||||
MixingOutput _mixing_output{PARAM_PREFIX, DIRECT_PWM_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
|
||||
uint32_t _reversible_outputs{};
|
||||
@@ -165,78 +147,31 @@ private:
|
||||
uORB::PublicationMultiData<esc_status_s> esc_status_pub{ORB_ID(esc_status)};
|
||||
|
||||
static char _telemetry_device[20];
|
||||
static bool _telemetry_swap_rxtx;
|
||||
static px4::atomic_bool _request_telemetry_init;
|
||||
|
||||
px4::atomic<Command *> _new_command{nullptr};
|
||||
|
||||
px4::atomic<DShotTelemetry::OutputBuffer *> _request_esc_info{nullptr};
|
||||
|
||||
bool _outputs_initialized{false};
|
||||
bool _outputs_on{false};
|
||||
bool _waiting_for_esc_info{false};
|
||||
bool _bidirectional_dshot_enabled{false};
|
||||
|
||||
const char *esc_type_unknown = "Unknown";
|
||||
const char *esc_type_am32 = "AM32";
|
||||
const char *esc_type_blheli32 = "BLHeli32";
|
||||
const char *esc_type_am32_old = "AM32_Old";
|
||||
const char *esc_type_bluejay = "BlueJay";
|
||||
|
||||
const char* esc_types_strings[5] = {esc_type_unknown, esc_type_am32, esc_type_blheli32, esc_type_am32_old, esc_type_bluejay};
|
||||
|
||||
enum class ESCType {
|
||||
Unknown = 0,
|
||||
AM32 = 1,
|
||||
BLHELI32 = 2,
|
||||
AM32_Old = 3,
|
||||
BlueJay = 4
|
||||
};
|
||||
ESCType _esc_type{ESCType::Unknown};
|
||||
ESCType _esc_type_temp{ESCType::Unknown};
|
||||
|
||||
esc_flasher_request_s _esc_flasher_request{0};
|
||||
esc_flasher_request_ack_s _esc_flasher_response{0};
|
||||
uint32_t esc_flasher_esc_info_state{0};
|
||||
uint32_t esc_flasher_esc_info_motor_index;
|
||||
uint32_t esc_flasher_flashing_state{0};
|
||||
DShotTelemetry::OutputBuffer esc_flasher_output_buffer{};
|
||||
|
||||
static constexpr unsigned _num_outputs{DIRECT_PWM_OUTPUT_CHANNELS};
|
||||
uint32_t _output_mask{0};
|
||||
|
||||
int _num_motors{0};
|
||||
uint32_t _broken_esc{0};
|
||||
bool _broken_esc_flag{0};
|
||||
|
||||
// run_get_esc_info variables
|
||||
struct EscInfoSave {
|
||||
ESCType type;
|
||||
uint8_t fw_major;
|
||||
uint8_t fw_minor;
|
||||
};
|
||||
|
||||
EscInfoSave _esc_info_save[esc_status_s::CONNECTED_ESC_MAX];
|
||||
|
||||
uint64_t get_esc_info_time{0};
|
||||
uint32_t get_esc_info_state{0};
|
||||
uint32_t get_esc_info_motor_index{0};
|
||||
uint32_t get_esc_info_start{0};
|
||||
uint32_t get_esc_info_boot{0};
|
||||
uint32_t get_esc_info_tries{0};
|
||||
//uint32_t get_esc_info_debug{0};
|
||||
|
||||
perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
|
||||
perf_counter_t _bdshot_rpm_perf{perf_alloc(PC_COUNT, MODULE_NAME": bdshot rpm")};
|
||||
perf_counter_t _dshot_telem_perf{perf_alloc(PC_COUNT, MODULE_NAME": dshot telem")};
|
||||
|
||||
Command _current_command{};
|
||||
|
||||
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
|
||||
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
|
||||
uORB::Publication<vehicle_command_ack_s> _command_ack_pub{ORB_ID(vehicle_command_ack)};
|
||||
uORB::Subscription _esc_flasher_request_sub{ORB_ID(esc_flasher_request)};
|
||||
uORB::Subscription _esc_flasher_status_sub{ORB_ID(esc_flasher_status)};
|
||||
uORB::Publication<esc_flasher_request_ack_s> _esc_flasher_request_ack_pub{ORB_ID(esc_flasher_request_ack)};
|
||||
uint16_t _esc_status_counter{0};
|
||||
uORB::Publication<esc_flasher_versions_s> _esc_flasher_versions_pub{ORB_ID(esc_flasher_versions)};
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamFloat<px4::params::DSHOT_MIN>) _param_dshot_min,
|
||||
@@ -244,7 +179,6 @@ private:
|
||||
(ParamInt<px4::params::DSHOT_3D_DEAD_H>) _param_dshot_3d_dead_h,
|
||||
(ParamInt<px4::params::DSHOT_3D_DEAD_L>) _param_dshot_3d_dead_l,
|
||||
(ParamInt<px4::params::MOT_POLE_COUNT>) _param_mot_pole_count,
|
||||
(ParamBool<px4::params::DSHOT_BIDIR_EN>) _param_bidirectional_enable,
|
||||
(ParamInt<px4::params::ESC_TYPE>) _param_esc_type
|
||||
(ParamBool<px4::params::DSHOT_BIDIR_EN>) _param_bidirectional_enable
|
||||
)
|
||||
};
|
||||
|
||||
@@ -46,53 +46,38 @@ using namespace time_literals;
|
||||
|
||||
DShotTelemetry::~DShotTelemetry()
|
||||
{
|
||||
deinit();
|
||||
_uart.close();
|
||||
}
|
||||
|
||||
int DShotTelemetry::init(const char *uart_device)
|
||||
int DShotTelemetry::init(const char *port, bool swap_rxtx)
|
||||
{
|
||||
deinit();
|
||||
_uart_fd = ::open(uart_device, O_RDONLY | O_NOCTTY);
|
||||
|
||||
if (_uart_fd < 0) {
|
||||
PX4_ERR("failed to open serial port: %s err: %d", uart_device, errno);
|
||||
return -errno;
|
||||
if (!_uart.setPort(port)) {
|
||||
PX4_ERR("Error configuring port %s", port);
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
_num_timeouts = 0;
|
||||
_num_successful_responses = 0;
|
||||
_current_motor_index_request = -1;
|
||||
return setBaudrate(DSHOT_TELEMETRY_UART_BAUDRATE);
|
||||
}
|
||||
|
||||
void DShotTelemetry::deinit()
|
||||
{
|
||||
if (_uart_fd >= 0) {
|
||||
close(_uart_fd);
|
||||
_uart_fd = -1;
|
||||
}
|
||||
}
|
||||
|
||||
int DShotTelemetry::redirectOutput(OutputBuffer &buffer)
|
||||
{
|
||||
if (expectingData()) {
|
||||
// Error: cannot override while we already expect data
|
||||
return -EBUSY;
|
||||
if (!_uart.setBaudrate(DSHOT_TELEMETRY_UART_BAUDRATE)) {
|
||||
PX4_ERR("Error setting baudrate on %s", port);
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
_current_motor_index_request = buffer.motor_index;
|
||||
_current_request_start = hrt_absolute_time();
|
||||
_redirect_output = &buffer;
|
||||
_redirect_output->buf_pos = 0;
|
||||
return 0;
|
||||
if (swap_rxtx) {
|
||||
if (!_uart.setSwapRxTxMode()) {
|
||||
PX4_ERR("Error swapping TX/RX");
|
||||
return PX4_ERROR;
|
||||
}
|
||||
}
|
||||
|
||||
if (! _uart.open()) {
|
||||
PX4_ERR("Error opening %s", port);
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
int DShotTelemetry::update(int num_motors)
|
||||
{
|
||||
if (_uart_fd < 0) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (_current_motor_index_request == -1) {
|
||||
// nothing in progress, start a request
|
||||
_current_motor_index_request = 0;
|
||||
@@ -102,10 +87,9 @@ int DShotTelemetry::update(int num_motors)
|
||||
}
|
||||
|
||||
// read from the uart. This must be non-blocking, so check first if there is data available
|
||||
int bytes_available = 0;
|
||||
int ret = ioctl(_uart_fd, FIONREAD, (unsigned long)&bytes_available);
|
||||
int bytes_available = _uart.bytesAvailable();
|
||||
|
||||
if (ret != 0 || bytes_available <= 0) {
|
||||
if (bytes_available <= 0) {
|
||||
// no data available. Check for a timeout
|
||||
const hrt_abstime now = hrt_absolute_time();
|
||||
|
||||
@@ -127,13 +111,12 @@ int DShotTelemetry::update(int num_motors)
|
||||
return -1;
|
||||
}
|
||||
|
||||
const int buf_length = ESC_FRAME_SIZE;
|
||||
uint8_t buf[buf_length];
|
||||
uint8_t buf[ESC_FRAME_SIZE];
|
||||
int bytes = _uart.read(buf, sizeof(buf));
|
||||
|
||||
int num_read = read(_uart_fd, buf, buf_length);
|
||||
ret = -1;
|
||||
int ret = -1;
|
||||
|
||||
for (int i = 0; i < num_read && ret == -1; ++i) {
|
||||
for (int i = 0; i < bytes && ret == -1; ++i) {
|
||||
if (_redirect_output) {
|
||||
_redirect_output->buffer[_redirect_output->buf_pos++] = buf[i];
|
||||
|
||||
@@ -183,6 +166,9 @@ bool DShotTelemetry::decodeByte(uint8_t byte, bool &successful_decoding)
|
||||
_latest_data.erpm);
|
||||
++_num_successful_responses;
|
||||
successful_decoding = true;
|
||||
|
||||
} else {
|
||||
++_num_checksum_errors;
|
||||
}
|
||||
|
||||
return true;
|
||||
@@ -195,32 +181,30 @@ void DShotTelemetry::printStatus() const
|
||||
{
|
||||
PX4_INFO("Number of successful ESC frames: %i", _num_successful_responses);
|
||||
PX4_INFO("Number of timeouts: %i", _num_timeouts);
|
||||
PX4_INFO("Number of CRC errors: %i", _num_checksum_errors);
|
||||
}
|
||||
|
||||
uint8_t DShotTelemetry::updateCrc8(uint8_t crc, uint8_t crc_seed)
|
||||
{
|
||||
uint8_t crc_u = crc ^ crc_seed;
|
||||
|
||||
for (int i = 0; i < 8; ++i) {
|
||||
crc_u = (crc_u & 0x80) ? 0x7 ^ (crc_u << 1) : (crc_u << 1);
|
||||
}
|
||||
|
||||
return crc_u;
|
||||
}
|
||||
|
||||
|
||||
uint8_t DShotTelemetry::crc8(const uint8_t *buf, uint8_t len)
|
||||
{
|
||||
auto update_crc8 = [](uint8_t crc, uint8_t crc_seed) {
|
||||
uint8_t crc_u = crc ^ crc_seed;
|
||||
|
||||
for (int i = 0; i < 8; ++i) {
|
||||
crc_u = (crc_u & 0x80) ? 0x7 ^ (crc_u << 1) : (crc_u << 1);
|
||||
}
|
||||
|
||||
return crc_u;
|
||||
};
|
||||
|
||||
uint8_t crc = 0;
|
||||
|
||||
for (int i = 0; i < len; ++i) {
|
||||
crc = updateCrc8(buf[i], crc);
|
||||
crc = update_crc8(buf[i], crc);
|
||||
}
|
||||
|
||||
return crc;
|
||||
}
|
||||
|
||||
|
||||
void DShotTelemetry::requestNextMotor(int num_motors)
|
||||
{
|
||||
_current_motor_index_request = (_current_motor_index_request + 1) % num_motors;
|
||||
@@ -253,7 +237,6 @@ void DShotTelemetry::decodeAndPrintEscInfoPacket(const OutputBuffer &buffer)
|
||||
BLHELI32,
|
||||
KissV1,
|
||||
KissV2,
|
||||
AM32
|
||||
};
|
||||
ESCVersionInfo version;
|
||||
int packet_length;
|
||||
@@ -267,21 +250,8 @@ void DShotTelemetry::decodeAndPrintEscInfoPacket(const OutputBuffer &buffer)
|
||||
packet_length = esc_info_size_kiss_v2;
|
||||
|
||||
} else {
|
||||
if (buffer.buf_pos > esc_info_size_kiss_v1) {
|
||||
version = ESCVersionInfo::AM32;
|
||||
packet_length = esc_info_size_am32;
|
||||
}
|
||||
else {
|
||||
version = ESCVersionInfo::KissV1;
|
||||
packet_length = esc_info_size_kiss_v1;
|
||||
}
|
||||
}
|
||||
|
||||
// Check for AM32 but with bad data
|
||||
if (version != ESCVersionInfo::AM32 && buffer.buf_pos == esc_info_size_am32) {
|
||||
version = ESCVersionInfo::AM32;
|
||||
packet_length = esc_info_size_am32;
|
||||
PX4_WARN("Packet length mismatch (%i != %i), reverting to AM32 decoding", buffer.buf_pos, packet_length);
|
||||
version = ESCVersionInfo::KissV1;
|
||||
packet_length = esc_info_size_kiss_v1;
|
||||
}
|
||||
|
||||
if (buffer.buf_pos != packet_length) {
|
||||
@@ -294,337 +264,116 @@ void DShotTelemetry::decodeAndPrintEscInfoPacket(const OutputBuffer &buffer)
|
||||
return;
|
||||
}
|
||||
|
||||
if (version == ESCVersionInfo::AM32) {
|
||||
// AM32 2.18+ ESC_INFO packet sends bytes 0-47 of its EEPROM struct
|
||||
if (buffer.buf_pos < packet_length) {
|
||||
PX4_ERR("Not enough data received");
|
||||
return;
|
||||
}
|
||||
uint8_t esc_firmware_version = 0;
|
||||
uint8_t esc_firmware_subversion = 0;
|
||||
uint8_t esc_type = 0;
|
||||
|
||||
uint8_t eeprom_version = data[1];
|
||||
if (eeprom_version != 2) {
|
||||
PX4_ERR("Unsupported EEPROM version from ESC");
|
||||
return;
|
||||
}
|
||||
switch (version) {
|
||||
case ESCVersionInfo::KissV1:
|
||||
esc_firmware_version = data[12];
|
||||
esc_firmware_subversion = (data[13] & 0x1f) + 97;
|
||||
esc_type = (data[13] & 0xe0) >> 5;
|
||||
break;
|
||||
|
||||
AM32_EEprom_st* esc_info = (AM32_EEprom_st*)buffer.buffer;
|
||||
|
||||
char firmware_name[sizeof(esc_info->firmware_name) + 1];
|
||||
memcpy(firmware_name, esc_info->firmware_name, sizeof(firmware_name));
|
||||
firmware_name[sizeof(esc_info->firmware_name)] = 0;
|
||||
|
||||
PX4_INFO("Received ESC_INFO packet from motor %d:", buffer.motor_index + 1);
|
||||
|
||||
PX4_INFO(" ESC firmware version: %d.%d - ESC firmware name: %s", esc_info->version.major, esc_info->version.minor, firmware_name);
|
||||
|
||||
if (esc_info->stuck_rotor_protection) PX4_INFO(" Stuck rotor protection is ENABLED");
|
||||
else PX4_INFO(" Stuck rotor protection is DISABLED");
|
||||
|
||||
if (esc_info->telemetry_on_interval) PX4_INFO(" Auto-Telemetry interval is ENABLED");
|
||||
else PX4_INFO(" Auto-Telemetry interval is DISABLED");
|
||||
|
||||
if (esc_info->bi_direction) PX4_INFO(" Bi-Directional DShot is ENABLED");
|
||||
else PX4_INFO(" Bi-Directional DShot is DISABLED");
|
||||
case ESCVersionInfo::KissV2:
|
||||
case ESCVersionInfo::BLHELI32:
|
||||
esc_firmware_version = data[13];
|
||||
esc_firmware_subversion = data[14];
|
||||
esc_type = data[15];
|
||||
break;
|
||||
}
|
||||
else {
|
||||
uint8_t esc_firmware_version = 0;
|
||||
uint8_t esc_firmware_subversion = 0;
|
||||
uint8_t esc_type = 0;
|
||||
|
||||
switch (version) {
|
||||
case ESCVersionInfo::KissV1:
|
||||
esc_firmware_version = data[12];
|
||||
esc_firmware_subversion = (data[13] & 0x1f) + 97;
|
||||
esc_type = (data[13] & 0xe0) >> 5;
|
||||
const char *esc_type_str = "";
|
||||
|
||||
switch (version) {
|
||||
case ESCVersionInfo::KissV1:
|
||||
case ESCVersionInfo::KissV2:
|
||||
switch (esc_type) {
|
||||
case 1: esc_type_str = "KISS8A";
|
||||
break;
|
||||
|
||||
case ESCVersionInfo::KissV2:
|
||||
case ESCVersionInfo::BLHELI32:
|
||||
esc_firmware_version = data[13];
|
||||
esc_firmware_subversion = data[14];
|
||||
esc_type = data[15];
|
||||
case 2: esc_type_str = "KISS16A";
|
||||
break;
|
||||
|
||||
case 3: esc_type_str = "KISS24A";
|
||||
break;
|
||||
|
||||
case 5: esc_type_str = "KISS Ultralite";
|
||||
break;
|
||||
|
||||
default: esc_type_str = "KISS (unknown)";
|
||||
break;
|
||||
case ESCVersionInfo::AM32:
|
||||
// AM32 is handled above, should not get to this case
|
||||
return;
|
||||
}
|
||||
|
||||
const char *esc_type_str = "";
|
||||
break;
|
||||
|
||||
switch (version) {
|
||||
case ESCVersionInfo::KissV1:
|
||||
case ESCVersionInfo::KissV2:
|
||||
switch (esc_type) {
|
||||
case 1: esc_type_str = "KISS8A";
|
||||
break;
|
||||
case ESCVersionInfo::BLHELI32: {
|
||||
char *esc_type_mutable = (char *)(data + 31);
|
||||
esc_type_mutable[32] = 0;
|
||||
esc_type_str = esc_type_mutable;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case 2: esc_type_str = "KISS16A";
|
||||
break;
|
||||
PX4_INFO("ESC Type: %s", esc_type_str);
|
||||
|
||||
case 3: esc_type_str = "KISS24A";
|
||||
break;
|
||||
PX4_INFO("MCU Serial Number: %02x%02x%02x-%02x%02x%02x-%02x%02x%02x-%02x%02x%02x",
|
||||
data[0], data[1], data[2], data[3], data[4], data[5], data[6], data[7], data[8],
|
||||
data[9], data[10], data[11]);
|
||||
|
||||
case 5: esc_type_str = "KISS Ultralite";
|
||||
break;
|
||||
switch (version) {
|
||||
case ESCVersionInfo::KissV1:
|
||||
case ESCVersionInfo::KissV2:
|
||||
PX4_INFO("Firmware version: %d.%d%c", esc_firmware_version / 100, esc_firmware_version % 100,
|
||||
(char)esc_firmware_subversion);
|
||||
break;
|
||||
|
||||
default: esc_type_str = "KISS (unknown)";
|
||||
break;
|
||||
}
|
||||
case ESCVersionInfo::BLHELI32:
|
||||
PX4_INFO("Firmware version: %d.%d", esc_firmware_version, esc_firmware_subversion);
|
||||
break;
|
||||
}
|
||||
|
||||
if (version == ESCVersionInfo::KissV2 || version == ESCVersionInfo::BLHELI32) {
|
||||
PX4_INFO("Rotation Direction: %s", data[16] ? "reversed" : "normal");
|
||||
PX4_INFO("3D Mode: %s", data[17] ? "on" : "off");
|
||||
}
|
||||
|
||||
if (version == ESCVersionInfo::BLHELI32) {
|
||||
uint8_t setting = data[18];
|
||||
|
||||
switch (setting) {
|
||||
case 0:
|
||||
PX4_INFO("Low voltage Limit: off");
|
||||
break;
|
||||
|
||||
case ESCVersionInfo::BLHELI32: {
|
||||
char *esc_type_mutable = (char *)(data + 31);
|
||||
esc_type_mutable[32] = 0;
|
||||
esc_type_str = esc_type_mutable;
|
||||
}
|
||||
case 255:
|
||||
PX4_INFO("Low voltage Limit: unsupported");
|
||||
break;
|
||||
|
||||
default:
|
||||
PX4_INFO("Low voltage Limit: %d.%01d V", setting / 10, setting % 10);
|
||||
break;
|
||||
case ESCVersionInfo::AM32:
|
||||
// AM32 is handled above, should not get to this case
|
||||
return;
|
||||
}
|
||||
|
||||
PX4_INFO("ESC Type: %s", esc_type_str);
|
||||
setting = data[19];
|
||||
|
||||
PX4_INFO("MCU Serial Number: %02x%02x%02x-%02x%02x%02x-%02x%02x%02x-%02x%02x%02x",
|
||||
data[0], data[1], data[2], data[3], data[4], data[5], data[6], data[7], data[8],
|
||||
data[9], data[10], data[11]);
|
||||
|
||||
switch (version) {
|
||||
case ESCVersionInfo::KissV1:
|
||||
case ESCVersionInfo::KissV2:
|
||||
PX4_INFO("Firmware version: %d.%d%c", esc_firmware_version / 100, esc_firmware_version % 100,
|
||||
(char)esc_firmware_subversion);
|
||||
switch (setting) {
|
||||
case 0:
|
||||
PX4_INFO("Current Limit: off");
|
||||
break;
|
||||
|
||||
case ESCVersionInfo::BLHELI32:
|
||||
PX4_INFO("Firmware version: %d.%d", esc_firmware_version, esc_firmware_subversion);
|
||||
case 255:
|
||||
PX4_INFO("Current Limit: unsupported");
|
||||
break;
|
||||
|
||||
default:
|
||||
PX4_INFO("Current Limit: %d A", setting);
|
||||
break;
|
||||
case ESCVersionInfo::AM32:
|
||||
// AM32 is handled above, should not get to this case
|
||||
return;
|
||||
}
|
||||
|
||||
if (version == ESCVersionInfo::KissV2 || version == ESCVersionInfo::BLHELI32) {
|
||||
PX4_INFO("Rotation Direction: %s", data[16] ? "reversed" : "normal");
|
||||
PX4_INFO("3D Mode: %s", data[17] ? "on" : "off");
|
||||
}
|
||||
|
||||
if (version == ESCVersionInfo::BLHELI32) {
|
||||
uint8_t setting = data[18];
|
||||
|
||||
switch (setting) {
|
||||
case 0:
|
||||
PX4_INFO("Low voltage Limit: off");
|
||||
break;
|
||||
|
||||
case 255:
|
||||
PX4_INFO("Low voltage Limit: unsupported");
|
||||
break;
|
||||
|
||||
default:
|
||||
PX4_INFO("Low voltage Limit: %d.%01d V", setting / 10, setting % 10);
|
||||
break;
|
||||
}
|
||||
|
||||
setting = data[19];
|
||||
|
||||
switch (setting) {
|
||||
case 0:
|
||||
PX4_INFO("Current Limit: off");
|
||||
break;
|
||||
|
||||
case 255:
|
||||
PX4_INFO("Current Limit: unsupported");
|
||||
break;
|
||||
|
||||
default:
|
||||
PX4_INFO("Current Limit: %d A", setting);
|
||||
break;
|
||||
}
|
||||
|
||||
for (int i = 0; i < 4; ++i) {
|
||||
setting = data[i + 20];
|
||||
PX4_INFO("LED %d: %s", i, setting ? (setting == 255 ? "unsupported" : "on") : "off");
|
||||
}
|
||||
for (int i = 0; i < 4; ++i) {
|
||||
setting = data[i + 20];
|
||||
PX4_INFO("LED %d: %s", i, setting ? (setting == 255 ? "unsupported" : "on") : "off");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int DShotTelemetry::decodeEscInfoPacketFwVersion(const OutputBuffer &buffer, uint8_t* fw_version_major, uint8_t* fw_version_minor)
|
||||
{
|
||||
// Return 0 for AM32 success
|
||||
// Return -1 for not enough data (unknown)
|
||||
// Return -2 for BLHeli success
|
||||
|
||||
static constexpr int version_position = 12;
|
||||
const uint8_t *data = buffer.buffer;
|
||||
|
||||
if (buffer.buf_pos < version_position) {
|
||||
PX4_ERR("Not enough data received");
|
||||
return -1;
|
||||
}
|
||||
|
||||
enum class ESCVersionInfo {
|
||||
BLHELI32,
|
||||
KissV1,
|
||||
KissV2,
|
||||
AM32
|
||||
};
|
||||
ESCVersionInfo version;
|
||||
int packet_length;
|
||||
|
||||
if (data[version_position] == 254) {
|
||||
version = ESCVersionInfo::BLHELI32;
|
||||
packet_length = esc_info_size_blheli32;
|
||||
|
||||
} else if (data[version_position] == 255) {
|
||||
version = ESCVersionInfo::KissV2;
|
||||
packet_length = esc_info_size_kiss_v2;
|
||||
|
||||
} else {
|
||||
if (buffer.buf_pos > esc_info_size_kiss_v1) {
|
||||
version = ESCVersionInfo::AM32;
|
||||
packet_length = esc_info_size_am32;
|
||||
}
|
||||
else {
|
||||
version = ESCVersionInfo::KissV1;
|
||||
packet_length = esc_info_size_kiss_v1;
|
||||
}
|
||||
}
|
||||
|
||||
// Check for AM32 but with bad data
|
||||
if (version != ESCVersionInfo::AM32 && buffer.buf_pos == esc_info_size_am32) {
|
||||
version = ESCVersionInfo::AM32;
|
||||
packet_length = esc_info_size_am32;
|
||||
PX4_WARN("Packet length mismatch (%i != %i), reverting to AM32 decoding", buffer.buf_pos, packet_length);
|
||||
}
|
||||
|
||||
if (buffer.buf_pos != packet_length) {
|
||||
PX4_ERR("Packet length mismatch (%i != %i)", buffer.buf_pos, packet_length);
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (DShotTelemetry::crc8(data, packet_length - 1) != data[packet_length - 1]) {
|
||||
PX4_ERR("Checksum mismatch");
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (version == ESCVersionInfo::AM32) {
|
||||
// AM32 2.18+ ESC_INFO packet sends bytes 0-47 of its EEPROM struct
|
||||
if (buffer.buf_pos < packet_length) {
|
||||
PX4_ERR("Not enough data received");
|
||||
return -1;
|
||||
}
|
||||
|
||||
uint8_t eeprom_version = data[1];
|
||||
if (eeprom_version != 2) {
|
||||
PX4_ERR("Unsupported EEPROM version from ESC");
|
||||
return -1;
|
||||
}
|
||||
|
||||
AM32_EEprom_st* esc_info = (AM32_EEprom_st*)buffer.buffer;
|
||||
|
||||
*fw_version_major = esc_info->version.major;
|
||||
*fw_version_minor = esc_info->version.minor;
|
||||
return 0;
|
||||
}
|
||||
else {
|
||||
//PX4_ERR("Unsupported ESC type, this function only supports AM32");
|
||||
|
||||
switch (version) {
|
||||
case ESCVersionInfo::KissV1:
|
||||
*fw_version_major = data[12];
|
||||
*fw_version_minor = (data[13] & 0x1f) + 97;
|
||||
break;
|
||||
case ESCVersionInfo::KissV2:
|
||||
case ESCVersionInfo::BLHELI32:
|
||||
*fw_version_major = data[13];
|
||||
*fw_version_minor = data[14];
|
||||
break;
|
||||
case ESCVersionInfo::AM32:
|
||||
// AM32 is handled above, should not get to this case
|
||||
return -1;
|
||||
}
|
||||
|
||||
return -2;
|
||||
}
|
||||
}
|
||||
|
||||
int DShotTelemetry::setBaudrate(unsigned baud)
|
||||
{
|
||||
int speed;
|
||||
|
||||
switch (baud) {
|
||||
case 9600: speed = B9600; break;
|
||||
|
||||
case 19200: speed = B19200; break;
|
||||
|
||||
case 38400: speed = B38400; break;
|
||||
|
||||
case 57600: speed = B57600; break;
|
||||
|
||||
case 115200: speed = B115200; break;
|
||||
|
||||
case 230400: speed = B230400; break;
|
||||
|
||||
default:
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
struct termios uart_config;
|
||||
|
||||
int termios_state;
|
||||
|
||||
/* fill the struct for the new configuration */
|
||||
tcgetattr(_uart_fd, &uart_config);
|
||||
|
||||
//
|
||||
// Input flags - Turn off input processing
|
||||
//
|
||||
// convert break to null byte, no CR to NL translation,
|
||||
// no NL to CR translation, don't mark parity errors or breaks
|
||||
// no input parity check, don't strip high bit off,
|
||||
// no XON/XOFF software flow control
|
||||
//
|
||||
uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL |
|
||||
INLCR | PARMRK | INPCK | ISTRIP | IXON);
|
||||
//
|
||||
// Output flags - Turn off output processing
|
||||
//
|
||||
// no CR to NL translation, no NL to CR-NL translation,
|
||||
// no NL to CR translation, no column 0 CR suppression,
|
||||
// no Ctrl-D suppression, no fill characters, no case mapping,
|
||||
// no local output processing
|
||||
//
|
||||
// config.c_oflag &= ~(OCRNL | ONLCR | ONLRET |
|
||||
// ONOCR | ONOEOT| OFILL | OLCUC | OPOST);
|
||||
uart_config.c_oflag = 0;
|
||||
|
||||
//
|
||||
// No line processing
|
||||
//
|
||||
// echo off, echo newline off, canonical mode off,
|
||||
// extended input processing off, signal chars off
|
||||
//
|
||||
uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
|
||||
|
||||
/* no parity, one stop bit, disable flow control */
|
||||
uart_config.c_cflag &= ~(CSTOPB | PARENB | CRTSCTS);
|
||||
|
||||
/* set baud rate */
|
||||
if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
|
||||
return -errno;
|
||||
}
|
||||
|
||||
if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
|
||||
return -errno;
|
||||
}
|
||||
|
||||
if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &uart_config)) < 0) {
|
||||
return -errno;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
@@ -33,55 +33,9 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <px4_platform_common/Serial.hpp>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
typedef struct {
|
||||
uint8_t reserved_0; //0
|
||||
uint8_t eeprom_version; //1
|
||||
uint8_t reserved_1; //2
|
||||
struct {
|
||||
uint8_t major; //3
|
||||
uint8_t minor; //4
|
||||
} version;
|
||||
char firmware_name[12]; //5-16
|
||||
uint8_t dir_reversed; // 17
|
||||
uint8_t bi_direction; // 18
|
||||
uint8_t use_sine_start; // 19
|
||||
uint8_t comp_pwm; // 20
|
||||
uint8_t variable_pwm; // 21
|
||||
uint8_t stuck_rotor_protection; // 22
|
||||
uint8_t advance_level; // 23
|
||||
uint8_t pwm_frequency; // 24
|
||||
uint8_t startup_power; // 25
|
||||
uint8_t motor_kv; // 26
|
||||
uint8_t motor_poles; // 27
|
||||
uint8_t brake_on_stop; // 28
|
||||
uint8_t stall_protection; // 29
|
||||
uint8_t beep_volume; // 30
|
||||
uint8_t telemetry_on_interval; // 31
|
||||
struct {
|
||||
uint8_t low_threshold; // 32
|
||||
uint8_t high_threshold; // 33
|
||||
uint8_t neutral; // 34
|
||||
uint8_t dead_band; // 35
|
||||
} servo;
|
||||
uint8_t low_voltage_cut_off; // 36
|
||||
uint8_t low_cell_volt_cutoff; // 37
|
||||
uint8_t rc_car_reverse; // 38
|
||||
uint8_t use_hall_sensors; // 39
|
||||
uint8_t sine_mode_changeover_thottle_level; // 40
|
||||
uint8_t drag_brake_strength; // 41
|
||||
uint8_t driving_brake_strength; // 42
|
||||
struct {
|
||||
uint8_t temperature; // 43
|
||||
uint8_t current; // 44
|
||||
} limits;
|
||||
uint8_t sine_mode_power; // 45
|
||||
uint8_t input_type; // 46
|
||||
uint8_t auto_advance; // 47
|
||||
uint8_t crc; // 48
|
||||
} AM32_EEprom_st;
|
||||
|
||||
class DShotTelemetry
|
||||
{
|
||||
public:
|
||||
@@ -97,7 +51,6 @@ public:
|
||||
static constexpr int esc_info_size_blheli32 = 64;
|
||||
static constexpr int esc_info_size_kiss_v1 = 15;
|
||||
static constexpr int esc_info_size_kiss_v2 = 21;
|
||||
static constexpr int esc_info_size_am32 = 49;
|
||||
static constexpr int max_esc_info_size = esc_info_size_blheli32;
|
||||
|
||||
struct OutputBuffer {
|
||||
@@ -108,9 +61,7 @@ public:
|
||||
|
||||
~DShotTelemetry();
|
||||
|
||||
int init(const char *uart_device);
|
||||
|
||||
void deinit();
|
||||
int init(const char *uart_device, bool swap_rxtx);
|
||||
|
||||
/**
|
||||
* Read telemetry from the UART (non-blocking) and handle timeouts.
|
||||
@@ -119,16 +70,6 @@ public:
|
||||
*/
|
||||
int update(int num_motors);
|
||||
|
||||
/**
|
||||
* Redirect everything that is read into a different buffer.
|
||||
* Future calls to @update will write to that instead of an internal buffer, until @update returns
|
||||
* a value different from -1. No decoding is done.
|
||||
* The caller must ensure the buffer exists until that point.
|
||||
* @param buffer
|
||||
* @return 0 on success <0 on error
|
||||
*/
|
||||
int redirectOutput(OutputBuffer &buffer);
|
||||
|
||||
bool redirectActive() const { return _redirect_output != nullptr; }
|
||||
|
||||
/**
|
||||
@@ -148,18 +89,9 @@ public:
|
||||
|
||||
static void decodeAndPrintEscInfoPacket(const OutputBuffer &buffer);
|
||||
|
||||
static int decodeEscInfoPacketFwVersion(const OutputBuffer &buffer, uint8_t* fw_version_major, uint8_t* fw_version_minor);
|
||||
|
||||
private:
|
||||
static constexpr int ESC_FRAME_SIZE = 10;
|
||||
|
||||
/**
|
||||
* set the Baudrate
|
||||
* @param baud
|
||||
* @return 0 on success, <0 on error
|
||||
*/
|
||||
int setBaudrate(unsigned baud);
|
||||
|
||||
void requestNextMotor(int num_motors);
|
||||
|
||||
/**
|
||||
@@ -170,10 +102,10 @@ private:
|
||||
*/
|
||||
bool decodeByte(uint8_t byte, bool &successful_decoding);
|
||||
|
||||
static inline uint8_t updateCrc8(uint8_t crc, uint8_t crc_seed);
|
||||
static uint8_t crc8(const uint8_t *buf, uint8_t len);
|
||||
|
||||
int _uart_fd{-1};
|
||||
device::Serial _uart {};
|
||||
|
||||
uint8_t _frame_buffer[ESC_FRAME_SIZE];
|
||||
int _frame_position{0};
|
||||
|
||||
@@ -187,4 +119,5 @@ private:
|
||||
// statistics
|
||||
int _num_timeouts{0};
|
||||
int _num_successful_responses{0};
|
||||
int _num_checksum_errors{0};
|
||||
};
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,14 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <px4_platform_common/defines.h>
|
||||
|
||||
#define INCLUDE_AM32_FIRMWARE
|
||||
|
||||
#define AM32_FIRMWARE_ADDR (0x08001000)
|
||||
#define AM32_FIRMWARE_TAG_ADDR (0x08007BE0)
|
||||
|
||||
__EXPORT extern const uint8_t am32_fw_version_major;
|
||||
__EXPORT extern const uint8_t am32_fw_version_minor;
|
||||
|
||||
__EXPORT extern const uint8_t am32_firmware[20752];
|
||||
__EXPORT extern const uint8_t am32_firmware_tag[16];
|
||||
@@ -1,6 +1,6 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2020 PX4 Development Team. All rights reserved.
|
||||
# Copyright (c) 2024 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
@@ -34,9 +34,9 @@
|
||||
px4_add_module(
|
||||
MODULE modules__esc_flasher
|
||||
MAIN esc_flasher
|
||||
COMPILE_FLAGS
|
||||
SRCS
|
||||
EscFlasher.cpp
|
||||
EscFlasher.hpp
|
||||
AM32Firmware.c
|
||||
)
|
||||
am32_protocol.cpp
|
||||
DEPENDS
|
||||
px4_work_queue
|
||||
)
|
||||
|
||||
+551
-1412
File diff suppressed because it is too large
Load Diff
@@ -1,47 +1,78 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2024 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file EscFlasher.hpp
|
||||
*
|
||||
* AM32 ESC firmware update module.
|
||||
*
|
||||
* Triggered by MAV_CMD_ESC_FIRMWARE_UPDATE via vehicle_command.
|
||||
* Reads firmware from SD card, stops DShot, takes over motor GPIOs,
|
||||
* and flashes each ESC via bit-banged AM32 bootloader protocol.
|
||||
* Requires reboot to restore normal DShot operation.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <px4_platform_common/getopt.h>
|
||||
#include <px4_platform_common/cli.h>
|
||||
#include <px4_platform_common/posix.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <parameters/param.h>
|
||||
#include <px4_platform_common/posix.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <systemlib/mavlink_log.h>
|
||||
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/esc_flasher_request.h>
|
||||
#include <uORB/topics/esc_flasher_request_ack.h>
|
||||
#include <uORB/topics/esc_flasher_status.h>
|
||||
#include <uORB/topics/led_control.h>
|
||||
#include <uORB/topics/sensor_gps.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/tune_control.h>
|
||||
#include <uORB/topics/esc_firmware_update_status.h>
|
||||
#include <uORB/topics/vehicle_command.h>
|
||||
#include <uORB/topics/vehicle_command_ack.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
|
||||
// Firefly drones have 4 ESCs
|
||||
#define ESC_COUNT (4)
|
||||
#include "am32_protocol.hpp"
|
||||
|
||||
class ESC_Flasher : public ModuleBase<ESC_Flasher>, public ModuleParams
|
||||
class EscFlasher : public ModuleBase<EscFlasher>, public ModuleParams
|
||||
{
|
||||
public:
|
||||
EscFlasher();
|
||||
~EscFlasher() override;
|
||||
|
||||
ESC_Flasher();
|
||||
~ESC_Flasher() override;
|
||||
|
||||
ESC_Flasher(const ESC_Flasher &) = delete;
|
||||
ESC_Flasher operator=(const ESC_Flasher &) = delete;
|
||||
EscFlasher(const EscFlasher &) = delete;
|
||||
EscFlasher &operator=(const EscFlasher &) = delete;
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int task_spawn(int argc, char *argv[]);
|
||||
|
||||
/** @see ModuleBase */
|
||||
static ESC_Flasher *instantiate(int argc, char *argv[]);
|
||||
static EscFlasher *instantiate(int argc, char *argv[]);
|
||||
|
||||
/** @see ModuleBase::run() */
|
||||
void run() override;
|
||||
@@ -56,174 +87,74 @@ public:
|
||||
int print_status() override;
|
||||
|
||||
private:
|
||||
static constexpr uint8_t MAX_ESCS = 8;
|
||||
static constexpr const char *FIRMWARE_PATH = "/fs/microsd/am32/firmware.bin";
|
||||
static constexpr uint32_t DSHOT_STOP_TIMEOUT_US = 2000000; // 2 s
|
||||
static constexpr uint32_t GPIO_SETTLE_TIME_US = 600000; // 600 ms
|
||||
static constexpr uint32_t IDLE_INTERVAL_US = 1000000; // 1 s
|
||||
|
||||
void RunLoop();
|
||||
|
||||
void subscribe_orb_messages(void);
|
||||
void unsubscribe_orb_messages(void);
|
||||
|
||||
void update_params(void);
|
||||
uint8_t get_current_arming_state(void);
|
||||
|
||||
void set_esc_type(bool print);
|
||||
|
||||
void load_firmware_source(void);
|
||||
|
||||
int bitbang_send_packet(uint32_t gpio, uint8_t* packet, uint16_t length, uint8_t* response, uint8_t response_length);
|
||||
|
||||
uint16_t make_crc(uint8_t* buffer, uint16_t length);
|
||||
|
||||
void play_tune(int tune);
|
||||
void stop_tune(void);
|
||||
|
||||
void set_physical_led_outputs(uint8_t color);
|
||||
|
||||
void send_vehicle_command_leds(void);
|
||||
|
||||
perf_counter_t _loop_perf; /**< loop performance counter */
|
||||
|
||||
uint32_t run_delay{10000};
|
||||
const uint32_t run_delay_min = 5000; //wait 0.005 sec between running the work loop (200 Hz)
|
||||
const uint32_t run_delay_max = 1000000; //wait 1 sec between running the work loop (1 Hz)
|
||||
volatile bool wake{false};
|
||||
uint32_t delay_loops;
|
||||
|
||||
hrt_abstime time_now;
|
||||
hrt_abstime time_1s;
|
||||
hrt_abstime time_250ms;
|
||||
hrt_abstime time_flash_start;
|
||||
hrt_abstime time_delayed_start;
|
||||
|
||||
uint32_t run_calls;
|
||||
|
||||
bool print_flag; // Turn this on to enable continuous status printing
|
||||
|
||||
bool flashing_in_progress{false};
|
||||
bool flashing_in_progress_last{false};
|
||||
|
||||
uint8_t vehicle_armed; // Indicates if the vehicle is armed
|
||||
|
||||
uint8_t last_color{0};
|
||||
|
||||
uint8_t _am32_fw_ver_major;
|
||||
uint8_t _am32_fw_ver_minor;
|
||||
enum class AM32_FIRMWARE_SOURCE {
|
||||
MEM_ARRAY,
|
||||
SD_CARD,
|
||||
STREAM_HEAP
|
||||
enum class State : uint8_t {
|
||||
Idle,
|
||||
StoppingDShot,
|
||||
InitGPIOs,
|
||||
ConnectESC,
|
||||
ReadInfo,
|
||||
LoadFirmware,
|
||||
Flashing,
|
||||
RunApp,
|
||||
Complete,
|
||||
Failed
|
||||
};
|
||||
AM32_FIRMWARE_SOURCE _am32_fw_source{AM32_FIRMWARE_SOURCE::MEM_ARRAY};
|
||||
|
||||
enum class ESCType {
|
||||
Unknown = 0,
|
||||
AM32 = 1,
|
||||
BLHELI32 = 2,
|
||||
AM32_Old = 3,
|
||||
BlueJay = 4
|
||||
};
|
||||
ESCType _esc_type{ESCType::Unknown};
|
||||
// --- state machine helpers ---
|
||||
void handle_vehicle_command();
|
||||
void ack_vehicle_command(const vehicle_command_s &cmd, uint8_t result);
|
||||
void begin_update(uint8_t esc_index);
|
||||
void advance_to_next_esc();
|
||||
void transition(State s);
|
||||
void fail(uint8_t error_code);
|
||||
void cleanup();
|
||||
void publish_status();
|
||||
|
||||
const char *esc_type_unknown = "Unknown";
|
||||
const char *esc_type_am32 = "AM32";
|
||||
const char *esc_type_blheli32 = "BLHeli32";
|
||||
const char *esc_type_am32_old = "AM32_Old";
|
||||
const char *esc_type_bluejay = "BlueJay";
|
||||
// --- state handlers (called from run loop) ---
|
||||
void state_stopping_dshot();
|
||||
void state_init_gpios();
|
||||
void state_connect_esc();
|
||||
void state_read_info();
|
||||
void state_load_firmware();
|
||||
void state_flash_chunk();
|
||||
void state_run_app();
|
||||
void state_complete();
|
||||
void state_failed();
|
||||
|
||||
const char* esc_types_strings[5] = {esc_type_unknown, esc_type_am32, esc_type_blheli32, esc_type_am32_old, esc_type_bluejay};
|
||||
// --- current state ---
|
||||
State _state{State::Idle};
|
||||
hrt_abstime _state_entered{0};
|
||||
hrt_abstime _flash_started{0};
|
||||
|
||||
enum class ESCUpdate {
|
||||
Disabled,
|
||||
Update,
|
||||
ForceUpdate,
|
||||
Recover,
|
||||
Recover2,
|
||||
Cancel
|
||||
};
|
||||
ESCUpdate _esc_update{ESCUpdate::Disabled};
|
||||
uint32_t esc_update_state{0};
|
||||
uint32_t prev_esc_update_state{0};
|
||||
uint8_t esc_update_motors[ESC_COUNT];
|
||||
uint32_t esc_update_motor_index;
|
||||
uint32_t flashing_result;
|
||||
// --- ESC tracking ---
|
||||
uint8_t _target_esc{255}; // from vehicle_command param1
|
||||
uint8_t _current_esc{0}; // index into _gpio_pins[]
|
||||
uint8_t _esc_count{0};
|
||||
uint32_t _gpio_pins[MAX_ESCS]{};
|
||||
uint8_t _device_info[am32::DEVICE_INFO_SIZE]{};
|
||||
|
||||
// Struct to track flashing progress for a single ESC
|
||||
typedef struct {
|
||||
uint8_t current_motor;
|
||||
uint32_t current_gpio;
|
||||
uint32_t fw_length;
|
||||
uint32_t fw_bytes_sent;
|
||||
uint32_t fw_bytes_left;
|
||||
uint16_t base_address;
|
||||
uint16_t next_address;
|
||||
uint8_t fw_part; // We have 2 parts to write for AM32 firmware, am32_firmware and am32_firmware_tag
|
||||
uint16_t crc;
|
||||
uint32_t tx_length;
|
||||
uint8_t tx_buffer[258];
|
||||
} ESC_FLASHING_STATE;
|
||||
ESC_FLASHING_STATE esc_flashing_state;
|
||||
// --- firmware file ---
|
||||
int _fw_fd{-1};
|
||||
uint32_t _fw_size{0};
|
||||
uint32_t _fw_written{0};
|
||||
|
||||
const uint16_t max_chunk_length = 256;
|
||||
// --- published status ---
|
||||
esc_firmware_update_status_s _status{};
|
||||
|
||||
// Variables to save responses from DShot
|
||||
typedef struct {
|
||||
uint8_t major;
|
||||
uint8_t minor;
|
||||
} ESC_FW_VERSIONS;
|
||||
ESC_FW_VERSIONS esc_versions[ESC_COUNT];
|
||||
// --- publications ---
|
||||
uORB::Publication<esc_firmware_update_status_s> _status_pub{ORB_ID(esc_firmware_update_status)};
|
||||
uORB::Publication<vehicle_command_ack_s> _cmd_ack_pub{ORB_ID(vehicle_command_ack)};
|
||||
|
||||
uint32_t esc_gpios[ESC_COUNT];
|
||||
uint32_t gpio_high;
|
||||
hrt_abstime time_gpio_set;
|
||||
// --- subscriptions ---
|
||||
uORB::Subscription _vehicle_cmd_sub{ORB_ID(vehicle_command)};
|
||||
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
|
||||
|
||||
// AM32 Bootloader commands
|
||||
uint8_t response_data[16];
|
||||
const uint8_t am32_boot_init[17] = {0, 0, 0, 0, 0, 0, 0, 0, 0x0D, 'B', 'L', 'H', 'e', 'l', 'i', 0xF4, 0x7D};
|
||||
const uint8_t am32_boot_init_resp_len = 9;
|
||||
uint8_t am32_set_addr[6] = {0xFF, 0, 0x10, 0x00, 0, 0};
|
||||
const uint8_t am32_set_addr_resp_len = 1;
|
||||
uint8_t am32_set_buff_size[6] = {0xFE, 0, 1, 0, 0, 0};
|
||||
const uint8_t am32_set_buff_size_resp_len = 0;
|
||||
const uint8_t am32_send_buff_resp_len = 1;
|
||||
uint8_t am32_write[4] = {0x01, 0, 0, 0};
|
||||
const uint8_t am32_write_resp_len = 1;
|
||||
const uint8_t am32_run_app[4] = {0x00, 0, 0, 0};
|
||||
const uint8_t am32_run_app_resp_len = 0;
|
||||
|
||||
// These variables are useful for starting a flash from command-line
|
||||
volatile uint32_t start_flash;
|
||||
volatile uint16_t motor_flags;
|
||||
volatile uint32_t cancel_flash;
|
||||
|
||||
// Request/Ack variables
|
||||
uint32_t msg_id;
|
||||
uint32_t esc_info_timeout;
|
||||
|
||||
// Tune control
|
||||
uint32_t esc_flasher_tune{0}; // Play some different tunes during flashing
|
||||
uint32_t esc_flasher_stop_tune{0}; // Stop tune 1 second after starting
|
||||
|
||||
// Publication variables
|
||||
uORB::Publication<esc_flasher_request_s> _esc_flasher_request_pub{ORB_ID(esc_flasher_request)};
|
||||
uORB::Publication<esc_flasher_status_s> _esc_flasher_status_pub{ORB_ID(esc_flasher_status)};
|
||||
uORB::Publication<tune_control_s> _tune_control_pub{ORB_ID(tune_control)};
|
||||
uORB::PublicationData<led_control_s> _led_control_pub{ORB_ID(led_control)};
|
||||
uORB::Publication<vehicle_command_s> _vehicle_command_pub{ORB_ID(vehicle_command)};
|
||||
|
||||
// Subscription variables
|
||||
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
|
||||
uORB::Subscription _esc_flasher_request_ack_sub{ORB_ID(esc_flasher_request_ack)};
|
||||
uORB::Subscription _sensor_gps_sub {ORB_ID(sensor_gps)};
|
||||
uORB::Subscription _vehicle_status_sub {ORB_ID(vehicle_status)};
|
||||
|
||||
// MAVLink
|
||||
// --- mavlink log ---
|
||||
orb_advert_t _mavlink_log_pub{nullptr};
|
||||
|
||||
sensor_gps_s _sensor_gps {}; /**< gps sensor */
|
||||
vehicle_status_s _vehicle_status {}; /**< vehicle status */
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamInt<px4::params::ESC_TYPE>) _param_esc_type,
|
||||
(ParamInt<px4::params::ESC_UPDATE>) _param_esc_update
|
||||
)
|
||||
|
||||
};
|
||||
|
||||
@@ -0,0 +1,304 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2024 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file am32_protocol.cpp
|
||||
*
|
||||
* AM32 ESC bootloader protocol — bit-banged UART implementation.
|
||||
*/
|
||||
|
||||
#include "am32_protocol.hpp"
|
||||
|
||||
#include <string.h>
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <px4_platform_common/micro_hal.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
namespace am32 {
|
||||
|
||||
// Bootloader handshake packet (8 zero bytes + 0x0D + "BLHeli" + CRC)
|
||||
static const uint8_t boot_init[] = {
|
||||
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
||||
0x0D, 'B', 'L', 'H', 'e', 'l', 'i', 0xF4, 0x7D
|
||||
};
|
||||
|
||||
uint16_t crc16(const uint8_t *buf, uint16_t len)
|
||||
{
|
||||
uint16_t crc = 0;
|
||||
|
||||
for (uint16_t i = 0; i < len; i++) {
|
||||
uint8_t xb = buf[i];
|
||||
|
||||
for (uint8_t j = 0; j < 8; j++) {
|
||||
if (((xb & 0x01) ^ (crc & 0x0001)) != 0) {
|
||||
crc = (crc >> 1) ^ 0xA001;
|
||||
|
||||
} else {
|
||||
crc >>= 1;
|
||||
}
|
||||
|
||||
xb >>= 1;
|
||||
}
|
||||
}
|
||||
|
||||
return crc;
|
||||
}
|
||||
|
||||
int send_packet(uint32_t gpio, const uint8_t *tx_data, uint16_t tx_len,
|
||||
uint8_t *rx_data, uint8_t rx_len)
|
||||
{
|
||||
// Build GPIO configurations from the port+pin value
|
||||
uint32_t gpio_out = (gpio & (GPIO_PORT_MASK | GPIO_PIN_MASK))
|
||||
| GPIO_OUTPUT | GPIO_OUTPUT_SET | GPIO_PULLUP;
|
||||
uint32_t gpio_in = (gpio & (GPIO_PORT_MASK | GPIO_PIN_MASK))
|
||||
| GPIO_INPUT | GPIO_PULLUP;
|
||||
|
||||
// Start as output HIGH
|
||||
px4_arch_configgpio(gpio_out);
|
||||
|
||||
// --- critical section: interrupts off for timing accuracy ---
|
||||
irqstate_t irq_state = px4_enter_critical_section();
|
||||
|
||||
// Transmit bytes (19200 8N1, LSB-first)
|
||||
for (uint16_t k = 0; k < tx_len; k++) {
|
||||
uint8_t byte = tx_data[k];
|
||||
|
||||
// Start bit (LOW)
|
||||
px4_arch_gpiowrite(gpio_out, 0);
|
||||
hrt_abstime t = hrt_absolute_time();
|
||||
|
||||
while (hrt_absolute_time() - t < BIT_TIME_US) {}
|
||||
|
||||
// 8 data bits, LSB first
|
||||
for (uint8_t i = 0; i < 8; i++) {
|
||||
px4_arch_gpiowrite(gpio_out, (byte >> i) & 1);
|
||||
t = hrt_absolute_time();
|
||||
|
||||
while (hrt_absolute_time() - t < BIT_TIME_US) {}
|
||||
}
|
||||
|
||||
// Stop bit (HIGH)
|
||||
px4_arch_gpiowrite(gpio_out, 1);
|
||||
t = hrt_absolute_time();
|
||||
|
||||
while (hrt_absolute_time() - t < BIT_TIME_US) {}
|
||||
}
|
||||
|
||||
// Switch to input for receiving
|
||||
px4_arch_configgpio(gpio_in);
|
||||
|
||||
// Receive response bytes
|
||||
uint8_t read_count = 0;
|
||||
|
||||
if (rx_len > 0 && rx_data != nullptr) {
|
||||
for (uint8_t i = 0; i < rx_len; i++) {
|
||||
|
||||
// Wait for start bit (line goes LOW)
|
||||
hrt_abstime t = hrt_absolute_time();
|
||||
|
||||
while (px4_arch_gpioread(gpio_in)) {
|
||||
if (hrt_absolute_time() - t > RX_TIMEOUT_US) {
|
||||
// Timeout — restore output HIGH and bail
|
||||
px4_arch_configgpio(gpio_out);
|
||||
px4_leave_critical_section(irq_state);
|
||||
return ERR_TIMEOUT;
|
||||
}
|
||||
}
|
||||
|
||||
// Half-bit delay to centre sampling window
|
||||
t = hrt_absolute_time();
|
||||
|
||||
while (hrt_absolute_time() - t < HALF_BIT_TIME_US) {}
|
||||
|
||||
// Read 8 data bits
|
||||
uint8_t byte = 0;
|
||||
|
||||
for (uint8_t bit = 0; bit < 8; bit++) {
|
||||
t = hrt_absolute_time();
|
||||
|
||||
while (hrt_absolute_time() - t < BIT_TIME_US) {}
|
||||
|
||||
if (px4_arch_gpioread(gpio_in)) {
|
||||
byte |= (1 << bit);
|
||||
}
|
||||
}
|
||||
|
||||
rx_data[read_count++] = byte;
|
||||
|
||||
// Wait through stop bit
|
||||
t = hrt_absolute_time();
|
||||
|
||||
while (hrt_absolute_time() - t < BIT_TIME_US) {}
|
||||
}
|
||||
}
|
||||
|
||||
// Restore pin to output HIGH
|
||||
px4_arch_configgpio(gpio_out);
|
||||
|
||||
// Inter-packet gap (8 bit-times)
|
||||
hrt_abstime t = hrt_absolute_time();
|
||||
|
||||
while (hrt_absolute_time() - t < BIT_TIME_US * 8) {}
|
||||
|
||||
px4_leave_critical_section(irq_state);
|
||||
|
||||
return read_count;
|
||||
}
|
||||
|
||||
int handshake(uint32_t gpio, uint8_t device_info[DEVICE_INFO_SIZE])
|
||||
{
|
||||
int ret = send_packet(gpio, boot_init, sizeof(boot_init),
|
||||
device_info, DEVICE_INFO_SIZE);
|
||||
|
||||
if (ret == DEVICE_INFO_SIZE) {
|
||||
return OK;
|
||||
}
|
||||
|
||||
return (ret < 0) ? ret : ERR_NACK;
|
||||
}
|
||||
|
||||
int set_address(uint32_t gpio, uint16_t address)
|
||||
{
|
||||
uint8_t cmd[6];
|
||||
cmd[0] = CMD_SET_ADDRESS;
|
||||
cmd[1] = 0x00;
|
||||
cmd[2] = (address >> 8) & 0xFF; // high byte
|
||||
cmd[3] = address & 0xFF; // low byte
|
||||
|
||||
uint16_t crc = crc16(cmd, 4);
|
||||
cmd[4] = crc & 0xFF;
|
||||
cmd[5] = (crc >> 8) & 0xFF;
|
||||
|
||||
uint8_t response;
|
||||
int ret = send_packet(gpio, cmd, sizeof(cmd), &response, 1);
|
||||
|
||||
if (ret == 1 && response == ACK) {
|
||||
return OK;
|
||||
}
|
||||
|
||||
return (ret < 0) ? ret : ERR_NACK;
|
||||
}
|
||||
|
||||
int write_chunk(uint32_t gpio, const uint8_t *data, uint16_t len)
|
||||
{
|
||||
if (len == 0 || len > MAX_CHUNK_SIZE || data == nullptr) {
|
||||
return ERR_INVALID;
|
||||
}
|
||||
|
||||
// 1) CMD_SET_BUFFER — tell bootloader how many bytes follow
|
||||
uint8_t size_cmd[6];
|
||||
size_cmd[0] = CMD_SET_BUFFER;
|
||||
size_cmd[1] = 0x00;
|
||||
|
||||
if (len == MAX_CHUNK_SIZE) {
|
||||
size_cmd[2] = 1; // 256 encoded as (1, 0)
|
||||
size_cmd[3] = 0;
|
||||
|
||||
} else {
|
||||
size_cmd[2] = 0;
|
||||
size_cmd[3] = (uint8_t)len;
|
||||
}
|
||||
|
||||
uint16_t crc = crc16(size_cmd, 4);
|
||||
size_cmd[4] = crc & 0xFF;
|
||||
size_cmd[5] = (crc >> 8) & 0xFF;
|
||||
|
||||
// SET_BUFFER has no response
|
||||
send_packet(gpio, size_cmd, sizeof(size_cmd), nullptr, 0);
|
||||
|
||||
// 2) Send data + CRC — bootloader ACKs after receiving
|
||||
uint8_t buf[MAX_CHUNK_SIZE + 2];
|
||||
memcpy(buf, data, len);
|
||||
crc = crc16(data, len);
|
||||
buf[len] = crc & 0xFF;
|
||||
buf[len + 1] = (crc >> 8) & 0xFF;
|
||||
|
||||
uint8_t response;
|
||||
int ret = send_packet(gpio, buf, len + 2, &response, 1);
|
||||
|
||||
if (ret != 1 || response != ACK) {
|
||||
return (ret < 0) ? ret : ERR_NACK;
|
||||
}
|
||||
|
||||
// 3) CMD_PROG_FLASH — commit buffer to flash
|
||||
uint8_t prog_cmd[4];
|
||||
prog_cmd[0] = CMD_PROG_FLASH;
|
||||
prog_cmd[1] = 0x00;
|
||||
crc = crc16(prog_cmd, 2);
|
||||
prog_cmd[2] = crc & 0xFF;
|
||||
prog_cmd[3] = (crc >> 8) & 0xFF;
|
||||
|
||||
ret = send_packet(gpio, prog_cmd, sizeof(prog_cmd), &response, 1);
|
||||
|
||||
if (ret == 1 && response == ACK) {
|
||||
return OK;
|
||||
}
|
||||
|
||||
return (ret < 0) ? ret : ERR_NACK;
|
||||
}
|
||||
|
||||
int read_flash(uint32_t gpio, uint8_t *buf, uint16_t len)
|
||||
{
|
||||
if (len == 0 || buf == nullptr) {
|
||||
return ERR_INVALID;
|
||||
}
|
||||
|
||||
// Address must already be set via set_address()
|
||||
uint8_t cmd[4];
|
||||
cmd[0] = CMD_READ_FLASH;
|
||||
cmd[1] = (uint8_t)len;
|
||||
|
||||
uint16_t crc = crc16(cmd, 2);
|
||||
cmd[2] = crc & 0xFF;
|
||||
cmd[3] = (crc >> 8) & 0xFF;
|
||||
|
||||
int ret = send_packet(gpio, cmd, sizeof(cmd), buf, (uint8_t)len);
|
||||
|
||||
if (ret == (int)len) {
|
||||
return OK;
|
||||
}
|
||||
|
||||
return (ret < 0) ? ret : ERR_NACK;
|
||||
}
|
||||
|
||||
int run_app(uint32_t gpio)
|
||||
{
|
||||
static const uint8_t cmd[] = {0x00, 0x00, 0x00, 0x00};
|
||||
|
||||
// No response expected
|
||||
send_packet(gpio, cmd, sizeof(cmd), nullptr, 0);
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
} // namespace am32
|
||||
@@ -0,0 +1,156 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2024 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file am32_protocol.hpp
|
||||
*
|
||||
* AM32 ESC bootloader protocol — bit-banged UART at 19200 baud.
|
||||
*
|
||||
* Extracted from Matt's proof-of-concept and cleaned up into a
|
||||
* function-based API for reuse by the esc_flasher module.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
namespace am32 {
|
||||
|
||||
// AM32 bootloader command bytes
|
||||
static constexpr uint8_t CMD_RUN = 0x00;
|
||||
static constexpr uint8_t CMD_PROG_FLASH = 0x01;
|
||||
static constexpr uint8_t CMD_READ_FLASH = 0x03;
|
||||
static constexpr uint8_t CMD_SET_BUFFER = 0xFE;
|
||||
static constexpr uint8_t CMD_SET_ADDRESS = 0xFF;
|
||||
|
||||
// Bootloader ACK byte
|
||||
static constexpr uint8_t ACK = 0x30;
|
||||
|
||||
// AM32 firmware memory layout (STM32 flash addresses)
|
||||
static constexpr uint32_t FIRMWARE_ADDR = 0x08001000;
|
||||
static constexpr uint32_t FIRMWARE_TAG_ADDR = 0x08007BE0;
|
||||
static constexpr uint16_t FIRMWARE_TAG_SIZE = 16;
|
||||
|
||||
// Protocol constants
|
||||
static constexpr uint16_t MAX_CHUNK_SIZE = 256;
|
||||
static constexpr uint32_t BIT_TIME_US = 52; // ~19200 baud
|
||||
static constexpr uint32_t HALF_BIT_TIME_US = 26;
|
||||
static constexpr uint32_t RX_TIMEOUT_US = 100000; // 100 ms
|
||||
|
||||
// Device info from handshake response
|
||||
static constexpr uint8_t DEVICE_INFO_SIZE = 9;
|
||||
|
||||
// Return codes
|
||||
static constexpr int OK = 0;
|
||||
static constexpr int ERR_TIMEOUT = -1;
|
||||
static constexpr int ERR_NACK = -2;
|
||||
static constexpr int ERR_CRC = -3;
|
||||
static constexpr int ERR_INVALID = -4;
|
||||
|
||||
/**
|
||||
* CRC-16 with polynomial 0xA001 (AM32 bootloader).
|
||||
*/
|
||||
uint16_t crc16(const uint8_t *buf, uint16_t len);
|
||||
|
||||
/**
|
||||
* Bit-banged UART send/receive on a GPIO pin.
|
||||
*
|
||||
* Enters a critical section (interrupts disabled) for the
|
||||
* duration of the transfer to maintain 19200-baud timing.
|
||||
*
|
||||
* @param gpio GPIO configuration value (port + pin bits)
|
||||
* @param tx_data Bytes to transmit
|
||||
* @param tx_len Number of bytes to transmit
|
||||
* @param rx_data Buffer for response (may be nullptr if rx_len == 0)
|
||||
* @param rx_len Expected number of response bytes
|
||||
* @return Number of bytes received, or negative error code
|
||||
*/
|
||||
int send_packet(uint32_t gpio, const uint8_t *tx_data, uint16_t tx_len,
|
||||
uint8_t *rx_data, uint8_t rx_len);
|
||||
|
||||
/**
|
||||
* Perform the bootloader handshake.
|
||||
*
|
||||
* Sends the AM32 boot-init sequence and reads back 9 bytes
|
||||
* of device information.
|
||||
*
|
||||
* @param gpio Target ESC GPIO
|
||||
* @param device_info Output buffer for 9-byte device info
|
||||
* @return OK or negative error code
|
||||
*/
|
||||
int handshake(uint32_t gpio, uint8_t device_info[DEVICE_INFO_SIZE]);
|
||||
|
||||
/**
|
||||
* Set the flash write/read address.
|
||||
*
|
||||
* @param gpio Target ESC GPIO
|
||||
* @param address 16-bit flash address
|
||||
* @return OK or negative error code
|
||||
*/
|
||||
int set_address(uint32_t gpio, uint16_t address);
|
||||
|
||||
/**
|
||||
* Write a chunk to flash.
|
||||
*
|
||||
* Sends CMD_SET_BUFFER + size, then data + CRC, then
|
||||
* CMD_PROG_FLASH and waits for ACK. Caller must call
|
||||
* set_address() first.
|
||||
*
|
||||
* @param gpio Target ESC GPIO
|
||||
* @param data Data to write (max MAX_CHUNK_SIZE bytes)
|
||||
* @param len Byte count
|
||||
* @return OK or negative error code
|
||||
*/
|
||||
int write_chunk(uint32_t gpio, const uint8_t *data, uint16_t len);
|
||||
|
||||
/**
|
||||
* Read flash memory.
|
||||
*
|
||||
* Caller must call set_address() first.
|
||||
*
|
||||
* @param gpio Target ESC GPIO
|
||||
* @param buf Output buffer
|
||||
* @param len Number of bytes to read
|
||||
* @return OK or negative error code
|
||||
*/
|
||||
int read_flash(uint32_t gpio, uint8_t *buf, uint16_t len);
|
||||
|
||||
/**
|
||||
* Send the RUN_APP command so the ESC boots its application.
|
||||
*
|
||||
* @param gpio Target ESC GPIO
|
||||
* @return OK (always; no response expected)
|
||||
*/
|
||||
int run_app(uint32_t gpio);
|
||||
|
||||
} // namespace am32
|
||||
@@ -1,73 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-2021 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file esc_params.c
|
||||
*
|
||||
* Parameters defined for ESCs.
|
||||
*
|
||||
*/
|
||||
|
||||
/**
|
||||
* ESC type
|
||||
*
|
||||
* @reboot_required true
|
||||
*
|
||||
* @group ESC Flasher
|
||||
* @value 0 - Default
|
||||
* @value 1 - AM32
|
||||
* @value 2 - BLHeli32
|
||||
* @value 3 - AM32_Old
|
||||
* @value 4 - BlueJay
|
||||
*
|
||||
* @min 0
|
||||
* @max 4
|
||||
*/
|
||||
PARAM_DEFINE_INT32(ESC_TYPE, 0);
|
||||
|
||||
/**
|
||||
* ESC update
|
||||
*
|
||||
* @reboot_required false
|
||||
*
|
||||
* @group ESC Flasher
|
||||
* @value 0 - Disabled
|
||||
* @value 1 - Update if version is outdated
|
||||
* @value 2 - Force update all ESCs
|
||||
* @value 3 - Start recovery process
|
||||
* @value 4 - Continue recovery process
|
||||
*
|
||||
* @min 0
|
||||
* @max 4
|
||||
*/
|
||||
PARAM_DEFINE_INT32(ESC_UPDATE, 0);
|
||||
Reference in New Issue
Block a user