Add ESC Flasher module files, msg files.

Enable ESC Flasher module in fmu-v6x. It still needs changes to DShot before it works.

boards/px4/fmu-v6x/default.px4board

@@ -56,6 +56,7 @@ CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_ESC_BATTERY=y
+CONFIG_MODULES_ESC_FLASHER=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y

msg/CMakeLists.txt

@@ -68,6 +68,9 @@ set(msg_files
 	DistanceSensorModeChangeRequest.msg
 	DronecanNodeStatus.msg
 	Ekf2Timestamps.msg
+	EscFlasherRequest.msg
+	EscFlasherRequestAck.msg
+	EscFlasherStatus.msg
 	EscReport.msg
 	EscStatus.msg
 	EstimatorAidSource1d.msg

msg/EscFlasherRequest.msg

@@ -0,0 +1,16 @@
+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

msg/EscFlasherRequestAck.msg

@@ -0,0 +1,31 @@
+# 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

msg/EscFlasherStatus.msg

@@ -0,0 +1,7 @@
+uint64 timestamp # time since system start (microseconds)
+
+bool esc_flashing_in_progress
+
+uint8 version_major
+
+uint8 version_minor

src/modules/esc_flasher/AM32Firmware.h

@@ -0,0 +1,14 @@
+#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];

src/modules/esc_flasher/CMakeLists.txt

@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (c) 2020 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.
+#
+############################################################################
+
+px4_add_module(
+	MODULE modules__esc_flasher
+	MAIN esc_flasher
+	COMPILE_FLAGS
+	SRCS
+		EscFlasher.cpp
+		EscFlasher.hpp
+		AM32Firmware.c
+	)

src/modules/esc_flasher/EscFlasher.hpp

@@ -0,0 +1,229 @@
+#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 <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/vehicle_command.h>
+
+// Firefly drones have 4 ESCs
+#define ESC_COUNT (4)
+
+class ESC_Flasher : public ModuleBase<ESC_Flasher>, public ModuleParams
+{
+public:
+
+	ESC_Flasher();
+	~ESC_Flasher() override;
+
+	ESC_Flasher(const ESC_Flasher &) = delete;
+	ESC_Flasher operator=(const ESC_Flasher &) = delete;
+
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static ESC_Flasher *instantiate(int argc, char *argv[]);
+
+	/** @see ModuleBase::run() */
+	void run() override;
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	/** @see ModuleBase::print_status() */
+	int print_status() override;
+
+private:
+
+	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
+	};
+	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};
+
+	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 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;
+
+	// 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;
+
+	const uint16_t max_chunk_length = 256;
+
+	// Variables to save responses from DShot
+	typedef struct {
+		uint8_t major;
+		uint8_t minor;
+	} ESC_FW_VERSIONS;
+	ESC_FW_VERSIONS esc_versions[ESC_COUNT];
+
+	uint32_t esc_gpios[ESC_COUNT];
+	uint32_t gpio_high;
+	hrt_abstime time_gpio_set;
+
+	// 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
+	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
+	)
+
+};

src/modules/esc_flasher/Kconfig

@@ -0,0 +1,5 @@
+menuconfig MODULES_ESC_FLASHER
+	bool "esc_flasher"
+	default n
+	---help---
+		Enable support for ESC Flasher module

src/modules/esc_flasher/esc_params.c

@@ -0,0 +1,73 @@
+/****************************************************************************
+ *
+ *   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);