Run jsbsim

This commit adds a hitl_run script which can be used for various simulations when running HITL simulation

ROMFS/px4fmu_common/init.d/rc.interface

@@ -144,11 +144,6 @@ then
 		set OUTPUT_DEV /dev/uavcan/esc
 	fi
 
-	if [ $OUTPUT_MODE = vesc ]
-	then
-		set OUTPUT_DEV /dev/vesc
-	fi
-
 	if mixer load ${OUTPUT_DEV} ${MIXER_FILE}
 	then
 		echo "INFO  [init] Mixer: ${MIXER_FILE} on ${OUTPUT_DEV}"

ROMFS/px4fmu_common/init.d/rcS

@@ -282,11 +282,6 @@ else
 		fi
 	fi
 
-	if param greater -s VESC_PORT 0
-	then
-		set OUTPUT_MODE vesc
-	fi
-
 	#
 	# Check if PX4IO present and update firmware if needed.
 	# Assumption IOFW set to firmware file and IO_PRESENT = no

Tools/hitl_run.sh

@@ -0,0 +1,122 @@
+#!/bin/bash
+# Run HITL simulation with PX4
+
+function cleanup() {
+	pkill -x px4
+	pkill gzclient
+	pkill gzserver
+}
+
+if [ "$1" == "-h" ] || [ "$1" == "--help" ]
+then
+	echo "Usage: $0 [-s <simulation>] [-m <vehicle_model>] [-w <world>] [-d <device>] [-b <baudrate>]"
+	echo "  This script allows you to run HITL simulation with PX4"
+	exit 1
+fi
+
+while getopts m:w:s:d:b:t:l: option
+do
+	case "${option}"
+	in
+		m) VEHICLE_MODEL=${OPTARG};;
+		w) WORLD=${OPTARG};;
+		s) SIMULATION=${OPTARG};;
+		d) DEVICE=${OPTARG};;
+		b) BAUDRATE=${OPTARG};;
+		l) LABEL=_${OPTARG};;
+	esac
+done
+
+world=${WORLD:=empty}
+target=${TARGET:=px4_sitl_default}
+model=${VEHICLE_MODEL:="iris"}
+simulation=${SIMULATION:="gazebo"}
+device=${DEVICE:="/dev/ttyACM0"}
+baudrate=${BAUDRATE:="921600"}
+export PX4_SIM_MODEL=${model}${LABEL}
+
+echo SIMULATION: ${simulation}
+echo MODEL: ${PX4_SIM_MODEL}
+
+echo ${SCRIPT}
+SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
+src_path="$SCRIPT_DIR/.."
+
+sleep 1
+
+SIM_PID=0
+
+if [ "$simulation" == "jmavsim" ] && [ ! -n "$no_sim" ]; then
+	# Start Java simulator
+	"$src_path"/Tools/jmavsim_run.sh -q -s -d ${device} -b ${baudrate} -r 250 &
+	SIM_PID=$!
+elif [ "$simulation" == "gazebo" ] && [ ! -n "$no_sim" ]; then
+	if [ -x "$(command -v gazebo)" ]; then
+		# Set the plugin path so Gazebo finds our model and sim
+		source "$src_path/Tools/setup_gazebo.bash" "${src_path}" "${build_path}"
+		output_sdf=/tmp/${model}_hitl.sdf
+		python3 ${src_path}/Tools/sitl_gazebo/scripts/jinja_gen.py ${src_path}/Tools/sitl_gazebo/models/${model}/${model}.sdf.jinja ${src_path}/Tools/sitl_gazebo --serial_enabled 1  --hil_mode 1 --serial_device ${device} --serial_baudrate ${baudrate} --mavlink_id 1 --output-file ${output_sdf}
+
+		if [ -z $PX4_SITL_WORLD ]; then
+			#Spawn predefined world
+			if [ "$world" == "none" ]; then
+				if [ -f ${src_path}/Tools/sitl_gazebo/worlds/${model}.world ]; then
+					echo "empty world, default world ${model}.world for model found"
+					world_path="${src_path}/Tools/sitl_gazebo/worlds/${model}.world"
+				else
+					echo "empty world, setting empty.world as default"
+					world_path="${src_path}/Tools/sitl_gazebo/worlds/empty.world"
+				fi
+			else
+				#Spawn empty world if world with model name doesn't exist
+				world_path="${src_path}/Tools/sitl_gazebo/worlds/${world}.world"
+			fi
+		else
+			if [ -f ${src_path}/Tools/sitl_gazebo/worlds/${PX4_SITL_WORLD}.world ]; then
+				# Spawn world by name if exists in the worlds directory from environment variable
+				world_path="${src_path}/Tools/sitl_gazebo/worlds/${PX4_SITL_WORLD}.world"
+			else
+				# Spawn world from environment variable with absolute path
+				world_path="$PX4_SITL_WORLD"
+			fi
+		fi
+		gzserver $verbose $world_path &
+		SIM_PID=$!
+
+		while gz model --verbose --spawn-file="${output_sdf}" --model-name=${model} -x 1.01 -y 0.98 -z 0.83 2>&1 | grep -q "An instance of Gazebo is not running."; do
+			echo "gzserver not ready yet, trying again!"
+			sleep 1
+		done
+
+		if [[ -n "$HEADLESS" ]]; then
+			echo "not running gazebo gui"
+		else
+			# gzserver needs to be running to avoid a race. Since the launch
+			# is putting it into the background we need to avoid it by backing off
+			sleep 3
+			nice -n 20 gzclient --verbose $follow_mode
+			GUI_PID=$!
+		fi
+	else
+		echo "You need to have gazebo simulator installed!"
+		exit 1
+	fi
+elif [ "$simulation" == "jsbsim" ] && [ -z "$no_sim" ]; then
+	source "$src_path/Tools/setup_jsbsim.bash" "${src_path}" "${build_path}" ${model}
+	if [[ -n "$HEADLESS" ]]; then
+		echo "not running flightgear gui"
+	else
+		fgfs --fdm=null \
+			--native-fdm=socket,in,60,,5550,udp \
+			--aircraft=$JSBSIM_AIRCRAFT_MODEL \
+			--airport=${world} \
+			--disable-hud \
+			--disable-ai-models &> /dev/null &
+		FGFS_PID=$!
+	fi
+	"${build_path}/build_jsbsim_bridge/jsbsim_bridge" ${model} -d ${device} -b ${baudrate} -s "${src_path}/Tools/jsbsim_bridge/scene/${world}.xml"
+	SIM_PID=$!
+fi
+
+
+trap "cleanup" SIGINT SIGTERM EXIT

boards/px4/fmu-v4/default.cmake

@@ -51,7 +51,6 @@ px4_add_board(
 		telemetry # all available telemetry drivers
 		tone_alarm
 		uavcan
-		vesc
 	MODULES
 		airspeed_selector
 		attitude_estimator_q
@@ -90,7 +89,7 @@ px4_add_board(
 		vtol_att_control
 	SYSTEMCMDS
 		bl_update
-		dmesg
+		#dmesg
 		dumpfile
 		esc_calib
 		gpio

boards/px4/fmu-v4/src/board_config.h

@@ -98,8 +98,6 @@
 #define GPIO_TONE_ALARM_IDLE         (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN15)
 #define GPIO_TONE_ALARM              (GPIO_ALT|GPIO_AF1|GPIO_SPEED_2MHz|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN15)
 
-#define BOARD_ENABLE_CONSOLE_BUFFER
-
 /**
  * PWM:
  *

boards/px4/fmu-v5/default.cmake

@@ -25,7 +25,7 @@ px4_add_board(
 		distance_sensor # all available distance sensor drivers
 		dshot
 		gps
-		#heater
+		heater
 		#imu # all available imu drivers
 		imu/analog_devices/adis16448
 		imu/bosch/bmi055

msg/heater_status.msg

@@ -3,6 +3,7 @@ uint64 timestamp	# time since system start (microseconds)
 uint32 device_id
 
 bool heater_on
+bool temperature_target_met
 
 float32 temperature_sensor
 float32 temperature_target

src/drivers/dshot/DShot.cpp

@@ -43,6 +43,7 @@ DShot::DShot() :
 	_mixing_output.setAllDisarmedValues(DSHOT_DISARM_VALUE);
 	_mixing_output.setAllMinValues(DSHOT_MIN_THROTTLE);
 	_mixing_output.setAllMaxValues(DSHOT_MAX_THROTTLE);
+
 }
 
 DShot::~DShot()

src/drivers/heater/heater.cpp

@@ -77,7 +77,7 @@ Heater::Heater() :
 
 Heater::~Heater()
 {
-	heater_disable();
+	disable_heater();
 }
 
 int Heater::custom_command(int argc, char *argv[])
@@ -91,7 +91,7 @@ int Heater::custom_command(int argc, char *argv[])
 	return print_usage("Unrecognized command.");
 }
 
-void Heater::heater_disable()
+void Heater::disable_heater()
 {
 	// Reset heater to off state.
 #ifdef HEATER_PX4IO
@@ -106,7 +106,7 @@ void Heater::heater_disable()
 #endif
 }
 
-void Heater::heater_initialize()
+void Heater::initialize_heater_io()
 {
 	// Initialize heater to off state.
 #ifdef HEATER_PX4IO
@@ -160,13 +160,15 @@ bool Heater::initialize_topics()
 	for (uint8_t i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
 		uORB::SubscriptionData<sensor_accel_s> sensor_accel_sub{ORB_ID(sensor_accel), i};
 
-		if (sensor_accel_sub.get().timestamp != 0 && sensor_accel_sub.get().device_id != 0
-		    && PX4_ISFINITE(sensor_accel_sub.get().temperature)) {
+		if (sensor_accel_sub.get().timestamp != 0 &&
+		    sensor_accel_sub.get().device_id != 0 &&
+		    PX4_ISFINITE(sensor_accel_sub.get().temperature)) {
 
 			// If the correct ID is found, exit the for-loop with _sensor_accel_sub pointing to the correct instance.
 			if (sensor_accel_sub.get().device_id == (uint32_t)_param_sens_temp_id.get()) {
 				_sensor_accel_sub.ChangeInstance(i);
 				_sensor_device_id = sensor_accel_sub.get().device_id;
+				initialize_heater_io();
 				return true;
 			}
 		}
@@ -190,21 +192,10 @@ void Heater::Run()
 		return;
 	}
 
-	// check for parameter updates
-	if (_parameter_update_sub.updated()) {
-		// clear update
-		parameter_update_s pupdate;
-		_parameter_update_sub.copy(&pupdate);
-
-		// update parameters from storage
-		ModuleParams::updateParams();
-	}
+	update_params();
 
 	if (_sensor_device_id == 0) {
-		if (initialize_topics()) {
-			heater_initialize();
-
-		} else {
+		if (!initialize_topics()) {
 			// if sensor still not found try again in 1 second
 			ScheduleDelayed(1_s);
 			return;
@@ -212,14 +203,15 @@ void Heater::Run()
 	}
 
 	sensor_accel_s sensor_accel;
+	float temperature_delta {0.f};
 
 	if (_heater_on) {
 		// Turn the heater off.
-		heater_off();
 		_heater_on = false;
+		heater_off();
+		ScheduleDelayed(_controller_period_usec - _controller_time_on_usec);
 
 	} else if (_sensor_accel_sub.update(&sensor_accel)) {
-		float temperature_delta {0.f};
 
 		// Update the current IMU sensor temperature if valid.
 		if (PX4_ISFINITE(sensor_accel.temperature)) {
@@ -230,11 +222,6 @@ void Heater::Run()
 		_proportional_value = temperature_delta * _param_sens_imu_temp_p.get();
 		_integrator_value += temperature_delta * _param_sens_imu_temp_i.get();
 
-		if (fabs(_param_sens_imu_temp_i.get()) <= 0.0001) {
-			_integrator_value = 0.f;
-		}
-
-		// Guard against integrator wind up.
 		_integrator_value = math::constrain(_integrator_value, -0.25f, 0.25f);
 
 		_controller_time_on_usec = static_cast<int>((_param_sens_imu_temp_ff.get() + _proportional_value +
@@ -242,36 +229,41 @@ void Heater::Run()
 
 		_controller_time_on_usec = math::constrain(_controller_time_on_usec, 0, _controller_period_usec);
 
+		if (abs(temperature_delta) < TEMPERATURE_TARGET_THRESHOLD) {
+			_temperature_target_met = true;
+
+		} else {
+
+			_temperature_target_met = false;
+		}
+
 		_heater_on = true;
 		heater_on();
-	}
-
-	// Schedule the next cycle.
-	if (_heater_on) {
 		ScheduleDelayed(_controller_time_on_usec);
-
-	} else {
-		ScheduleDelayed(_controller_period_usec - _controller_time_on_usec);
 	}
 
+	publish_status();
+}
 
-	// publish status
+void Heater::publish_status()
+{
 	heater_status_s status{};
-	status.heater_on = _heater_on;
-	status.device_id = _sensor_device_id;
-	status.temperature_sensor = _temperature_last;
-	status.temperature_target = _param_sens_imu_temp.get();
-	status.controller_period_usec = _controller_period_usec;
+	status.device_id               = _sensor_device_id;
+	status.heater_on               = _heater_on;
+	status.temperature_sensor      = _temperature_last;
+	status.temperature_target      = _param_sens_imu_temp.get();
+	status.temperature_target_met  = _temperature_target_met;
+	status.controller_period_usec  = _controller_period_usec;
 	status.controller_time_on_usec = _controller_time_on_usec;
-	status.proportional_value = _proportional_value;
-	status.integrator_value = _integrator_value;
-	status.feed_forward_value = _param_sens_imu_temp_ff.get();
+	status.proportional_value      = _proportional_value;
+	status.integrator_value        = _integrator_value;
+	status.feed_forward_value      = _param_sens_imu_temp_ff.get();
 
 #ifdef HEATER_PX4IO
-	status.mode |= heater_status_s::MODE_PX4IO;
+	status.mode = heater_status_s::MODE_PX4IO;
 #endif
 #ifdef HEATER_GPIO
-	status.mode |= heater_status_s::MODE_GPIO;
+	status.mode = heater_status_s::MODE_GPIO;
 #endif
 
 	status.timestamp = hrt_absolute_time();
@@ -287,6 +279,7 @@ int Heater::start()
 		return PX4_ERROR;
 	}
 
+	update_params(true);
 	ScheduleNow();
 	return PX4_OK;
 }
@@ -307,6 +300,18 @@ int Heater::task_spawn(int argc, char *argv[])
 	return 0;
 }
 
+void Heater::update_params(const bool force)
+{
+	if (_parameter_update_sub.updated() || force) {
+		// clear update
+		parameter_update_s param_update;
+		_parameter_update_sub.copy(&param_update);
+
+		// update parameters from storage
+		ModuleParams::updateParams();
+	}
+}
+
 int Heater::print_usage(const char *reason)
 {
 	if (reason) {

src/drivers/heater/heater.h

@@ -57,7 +57,8 @@
 
 using namespace time_literals;
 
-#define CONTROLLER_PERIOD_DEFAULT 100000
+#define CONTROLLER_PERIOD_DEFAULT    10000
+#define TEMPERATURE_TARGET_THRESHOLD 2.5f
 
 class Heater : public ModuleBase<Heater>, public ModuleParams, public px4::ScheduledWorkItem
 {
@@ -101,9 +102,25 @@ public:
 
 private:
 
+	/** Disables the heater (either by GPIO or PX4IO). */
+	void disable_heater();
+
+	/** Turns the heater on (either by GPIO or PX4IO). */
+	void heater_on();
+
+	/** Turns the heater off (either by GPIO or PX4IO). */
+	void heater_off();
+
+	void initialize();
+
+	/** Enables / configures the heater (either by GPIO or PX4IO). */
+	void initialize_heater_io();
+
 	/** @brief Called once to initialize uORB topics. */
 	bool initialize_topics();
 
+	void publish_status();
+
 	/** @brief Calculates the heater element on/off time and schedules the next cycle. */
 	void Run() override;
 
@@ -113,18 +130,6 @@ private:
 	 */
 	void update_params(const bool force = false);
 
-	/** Enables / configures the heater (either by GPIO or PX4IO). */
-	void heater_initialize();
-
-	/** Disnables the heater (either by GPIO or PX4IO). */
-	void heater_disable();
-
-	/** Turns the heater on (either by GPIO or PX4IO). */
-	void heater_on();
-
-	/** Turns the heater off (either by GPIO or PX4IO). */
-	void heater_off();
-
 	/** Work queue struct for the scheduler. */
 	static struct work_s _work;
 
@@ -133,7 +138,9 @@ private:
 	int _io_fd {-1};
 #endif
 
-	bool _heater_on = false;
+	bool _heater_initialized     = false;
+	bool _heater_on              = false;
+	bool _temperature_target_met = false;
 
 	int _controller_period_usec = CONTROLLER_PERIOD_DEFAULT;
 	int _controller_time_on_usec = 0;
@@ -155,7 +162,7 @@ private:
 		(ParamFloat<px4::params::SENS_IMU_TEMP_FF>) _param_sens_imu_temp_ff,
 		(ParamFloat<px4::params::SENS_IMU_TEMP_I>)  _param_sens_imu_temp_i,
 		(ParamFloat<px4::params::SENS_IMU_TEMP_P>)  _param_sens_imu_temp_p,
-		(ParamInt<px4::params::SENS_TEMP_ID>)       _param_sens_temp_id,
-		(ParamFloat<px4::params::SENS_IMU_TEMP>)    _param_sens_imu_temp
+		(ParamFloat<px4::params::SENS_IMU_TEMP>)    _param_sens_imu_temp,
+		(ParamInt<px4::params::SENS_TEMP_ID>)       _param_sens_temp_id
 	)
 };

src/drivers/vesc/CMakeLists.txt

@@ -1,46 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 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.
-#
-############################################################################
-px4_add_module(
-	MODULE drivers__vesc
-	MAIN vesc
-	SRCS
-		vesc_main.cpp
-		VescSerialDevice.hpp
-		VescSerialDevice.cpp
-		VescDriver/VescDriver.hpp
-		VescDriver/VescDriver.cpp
-		VescDriver/VescProtocol.h
-	MODULE_CONFIG
-		module.yaml
-	DEPENDS
-	)

src/drivers/vesc/VescDriver/VescDriver.cpp

@@ -1,285 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 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 VescDriverUart.cpp
- * @author Matthias Grob <maetugr@gmail.com>
- */
-
-#include "VescDriver.hpp"
-#include <memory.h>
-
-void VescDriver::commandDutyCycle(float duty_cycle, const uint8_t forward_can_id)
-{
-	uint8_t command[5] {VescCommand::SET_DUTY};
-	uint16_t index{1};
-	insertInt32(command, index, static_cast<int32_t>(duty_cycle * 100000.f));
-	sendPayload(command, 5, forward_can_id);
-}
-
-void VescDriver::commandCurrent(float current, const uint8_t forward_can_id)
-{
-	uint8_t command[5] {VescCommand::SET_CURRENT};
-	uint16_t index{1};
-	insertInt32(command, index, static_cast<int32_t>(current * 1000.f));
-	sendPayload(command, 5, forward_can_id);
-}
-
-void VescDriver::commandBrakeCurrent(float current, const uint8_t forward_can_id)
-{
-	uint8_t command[5] {VescCommand::SET_CURRENT_BRAKE};
-	uint16_t index{1};
-	insertInt32(command, index, static_cast<int32_t>(current * 1000.f));
-	sendPayload(command, 5, forward_can_id);
-}
-
-void VescDriver::requestFirmwareVersion()
-{
-	uint8_t command{VescCommand::FW_VERSION};
-	sendPayload(&command, 1);
-}
-
-void VescDriver::requestValues()
-{
-	uint8_t command{VescCommand::GET_VALUES};
-	sendPayload(&command, 1);
-}
-
-size_t VescDriver::sendPayload(const uint8_t *payload, const uint16_t payload_length, const uint8_t forward_can_id)
-{
-	if (forward_can_id == 0) {
-		return sendPacket(payload, payload_length);
-
-	} else {
-		uint8_t command[2 + payload_length] {VescCommand::FORWARD_CAN, forward_can_id};
-		uint16_t index{2};
-		memcpy(&command[index], payload, payload_length);
-		index += payload_length;
-		return sendPacket(command, 2 + payload_length);
-	}
-}
-
-size_t VescDriver::sendPacket(const uint8_t *payload, uint16_t payload_length, const uint8_t can_forwarding_id)
-{
-	if (payload_length == 0 || payload_length > MAXIMUM_PAYLOAD_LENGTH) {
-		return 0;
-	}
-
-	uint8_t packet_buffer[payload_length + PACKET_OVERHEAD_LENGTH];
-	uint16_t index{0};
-
-	// Start byte and payload size
-	if (payload_length <= 256) {
-		packet_buffer[index++] = 2;
-		packet_buffer[index++] = payload_length;
-
-	} else {
-		packet_buffer[index++] = 3;
-		packet_buffer[index++] = static_cast<uint8_t>(payload_length >> 8);
-		packet_buffer[index++] = static_cast<uint8_t>(payload_length & 0xFF);
-	}
-
-	// Payload
-	memcpy(&packet_buffer[index], payload, payload_length);
-	index += payload_length;
-
-	// CRC
-	const uint16_t crc = crc16(payload, payload_length);
-	packet_buffer[index++] = static_cast<uint8_t>(crc >> 8);
-	packet_buffer[index++] = static_cast<uint8_t>(crc & 0xFF);
-
-	// Stop byte
-	packet_buffer[index++] = 3;
-
-	// Write bytes out through callback interface
-	return _vesc_writable.writeCallback(packet_buffer, index);
-}
-
-void VescDriver::parseInputByte(uint8_t byte)
-{
-	if (_input_byte_index == 0) {
-		// Start byte
-		if (byte == 2 /*|| byte == 3*/) {
-			_input_start_byte = byte;
-			_input_byte_index++;
-		}
-
-	} else if (_input_byte_index == 1) {
-		// Payload size
-		_input_byte_index++;
-
-		if (_input_start_byte == 2) {
-			// Short packet
-			_input_payload_length = byte;
-
-			if (_input_payload_length < 1) {
-				_input_byte_index = 0;
-			}
-
-		} else {
-			// Long packet high byte
-			_input_payload_length = byte << 8;
-		}
-
-	} else if (_input_byte_index == 2 && _input_start_byte == 3) {
-		// Payload size long packet low byte
-		_input_payload_length |= byte;
-		_input_byte_index++;
-
-		if (_input_payload_length < 255 || _input_payload_length > MAXIMUM_PAYLOAD_LENGTH) {
-			_input_byte_index = 0;
-		}
-
-	} else if (_input_byte_index < _input_start_byte + _input_payload_length) {
-		// Payload
-		_input_payload[_input_byte_index - _input_start_byte] = byte;
-		_input_byte_index++;
-
-	} else if (_input_byte_index == _input_start_byte + _input_payload_length) {
-		// CRC high byte
-		_input_payload_crc = byte << 8;
-		_input_byte_index++;
-
-	} else if (_input_byte_index == _input_start_byte + _input_payload_length + 1u) {
-		// CRC low byte
-		_input_payload_crc |= byte;
-		_input_byte_index++;
-
-		if (_input_payload_crc != crc16(_input_payload, _input_payload_length)) {
-			_input_byte_index = 0;
-		}
-
-	} else if (_input_byte_index == _input_start_byte + _input_payload_length + 2u) {
-		// Stop byte
-		_input_byte_index = 0;
-
-		if (byte == 3) {
-			parsePayload(_input_payload, _input_payload_length);
-		}
-	}
-}
-
-void VescDriver::parsePayload(const uint8_t *payload, const uint16_t payload_length)
-{
-	uint16_t index{1};
-
-	switch (payload[0]) {
-	case VescCommand::FW_VERSION:
-		if (payload_length >= 9u) {
-			_vesc_version.version_major = payload[index++];
-			_vesc_version.version_minor = payload[index++];
-			// strcpy(_vesc_version.hardware_name, reinterpret_cast<const char *>(&payload[index]));
-			// index += strlen(_vesc_version.hardware_name) + 1u;
-			// memcpy(_vesc_version.stm32_uuid_8, &payload[index], sizeof(_vesc_version.stm32_uuid_8));
-			// index += 12;
-			// _vesc_version.pairing_done = payload[index++];
-			// _vesc_version.test_version_number = payload[index++];
-			// _vesc_version.hardware_type = payload[index++];
-			// _vesc_version.custom_configuration = payload[index++];
-		}
-
-		break;
-
-	case VescCommand::GET_VALUES:
-		if (payload_length >= 73u) {
-			_vesc_values.fet_temperature = extractFloat16(payload, index) / 10.f;
-			_vesc_values.motor_temperature = extractFloat16(payload, index) / 10.f;
-			_vesc_values.motor_current = extractFloat32(payload, index) / 100.f;
-			_vesc_values.input_current = extractFloat32(payload, index) / 100.f;
-			_vesc_values.reset_average_id = extractFloat32(payload, index) / 100.f;
-			_vesc_values.reset_average_iq = extractFloat32(payload, index) / 100.f;
-			_vesc_values.duty_cycle = extractFloat16(payload, index) / 1000.f;
-			_vesc_values.rpm = extractInt32(payload, index);
-			_vesc_values.input_voltage = extractFloat16(payload, index) / 10.f;
-			_vesc_values.used_charge_Ah = extractFloat32(payload, index) / 1e4f;
-			_vesc_values.charged_charge_Ah = extractFloat32(payload, index) / 1e4f;
-			_vesc_values.used_energy_Wh = extractFloat32(payload, index) / 1e4f;
-			_vesc_values.charged_energy_wh = extractFloat32(payload, index) / 10.f;
-			_vesc_values.tachometer = extractInt32(payload, index);
-			_vesc_values.tachometer_absolute = extractInt32(payload, index);
-			_vesc_values.fault = payload[index++];
-			_vesc_values.position_pid = extractFloat32(payload, index) / 1e6f;
-			_vesc_values.controller_id = payload[index++];
-			_vesc_values.ntc_temperature_mos1 = extractFloat16(payload, index) / 10.f;
-			_vesc_values.ntc_temperature_mos2 = extractFloat16(payload, index) / 10.f;
-			_vesc_values.ntc_temperature_mos3 = extractFloat16(payload, index) / 10.f;
-			_vesc_values.read_reset_average_vd = extractFloat32(payload, index) / 1000.f;
-			_vesc_values.read_reset_average_vq = extractFloat32(payload, index) / 1000.f;
-		}
-
-		break;
-	}
-}
-
-uint16_t VescDriver::crc16(const uint8_t *buffer, const uint16_t length)
-{
-	uint16_t checksum{0};
-
-	for (size_t i = 0; i < length; i++) {
-		uint8_t table_index = (((checksum >> 8) ^ buffer[i]) & 0xFF);
-		checksum = CRC_TABLE[table_index] ^ (checksum << 8);
-	}
-
-	return checksum;
-}
-
-void VescDriver::insertInt32(uint8_t *buffer, uint16_t &index, int32_t value)
-{
-	buffer[index++] = value >> 24;
-	buffer[index++] = value >> 16;
-	buffer[index++] = value >> 8;
-	buffer[index++] = value;
-}
-
-int16_t VescDriver::extractInt16(const uint8_t *buffer, uint16_t &index)
-{
-	index += 2;
-	return static_cast<int16_t>(buffer[index - 2] << 8 | buffer[index - 1]);
-}
-
-float VescDriver::extractFloat16(const uint8_t *buffer, uint16_t &index)
-{
-	return static_cast<float>(extractInt16(buffer, index));
-}
-
-int32_t VescDriver::extractInt32(const uint8_t *buffer, uint16_t &index)
-{
-	index += 4;
-	return static_cast<int32_t>(
-		       buffer[index - 4] << 24 | buffer[index - 3] << 16 | buffer[index - 2] << 8 | buffer[index - 1]);
-}
-
-float VescDriver::extractFloat32(const uint8_t *buffer, uint16_t &index)
-{
-	return static_cast<float>(extractInt32(buffer, index));
-}

src/drivers/vesc/VescDriver/VescDriver.hpp

@@ -1,94 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 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 VescDriverUart.hpp
- * @brief Driver to communicate with VESC motor contollers
- * @details More about VESC BLDC (brushless direct current) electric motor controllers by Benjamin Vedder on https://vesc-project.com/
- * @author Matthias Grob <maetugr@gmail.com>
- */
-
-#pragma once
-
-#include "VescProtocol.h"
-#include "VescWritableInterface.hpp"
-#include <stdint.h>
-#include <stdio.h>
-
-class VescDriver
-{
-public:
-	VescDriver(VescWritableInterface &vesc_writable) : _vesc_writable(vesc_writable) {};
-	~VescDriver() = default;
-
-	void commandDutyCycle(float duty_cycle, const uint8_t forward_can_id = 0);
-	void commandCurrent(float current, const uint8_t forward_can_id = 0);
-	void commandBrakeCurrent(float current, const uint8_t forward_can_id = 0);
-
-	void requestFirmwareVersion();
-	void requestValues();
-	float getRpm() { return _vesc_values.rpm; };
-	float getInputVoltage() { return _vesc_values.input_voltage; };
-	uint8_t getVersionMajor() { return _vesc_version.version_major; }
-	uint8_t getVersionMinor() { return _vesc_version.version_minor; }
-
-	void parseInputByte(uint8_t byte); ///< Call when data is ready to read
-
-private:
-	// De-/serialize packets
-	size_t sendPayload(const uint8_t *payload, const uint16_t payload_length, const uint8_t forward_can_id = 0);
-	size_t sendPacket(const uint8_t *payload, uint16_t payload_length, const uint8_t can_forwarding_id = 0);
-	void parsePayload(const uint8_t *payload, const uint16_t payload_length);
-	uint16_t crc16(const uint8_t *buffer, const uint16_t length);
-
-	// Big-endian helpers
-	void insertInt32(uint8_t *buffer, uint16_t &index, int32_t value);
-	int16_t extractInt16(const uint8_t *buffer, uint16_t &index);
-	float extractFloat16(const uint8_t *buffer, uint16_t &index);
-	int32_t extractInt32(const uint8_t *buffer, uint16_t &index);
-	float extractFloat32(const uint8_t *buffer, uint16_t &index);
-
-	// Write access to device through callback
-	VescWritableInterface &_vesc_writable;
-
-	// Input packet parsing
-	size_t _input_byte_index{0}; ///< keeps track of the input packets parsing state
-	uint8_t _input_start_byte{0};
-	uint16_t _input_payload_length{0};
-	uint8_t _input_payload[MAXIMUM_PAYLOAD_LENGTH];
-	uint16_t _input_payload_crc{0};
-
-	// Information storage for getters
-	VescVersion _vesc_version{};
-	VescValues _vesc_values{};
-};

src/drivers/vesc/VescDriver/VescProtocol.h

@@ -1,206 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 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 VescProtocol.h
- * @brief VESC protocol definitions
- * @author Matthias Grob <maetugr@gmail.com>
- */
-
-#pragma once
-
-#include <stdint.h>
-
-static constexpr int MAXIMUM_PAYLOAD_LENGTH{512};
-static constexpr int PACKET_OVERHEAD_LENGTH{6}; // Bytes: 1 start, 2 size, 2 CRC, 1 stop
-static constexpr int MAXIMUM_PACKET_LENGTH{MAXIMUM_PAYLOAD_LENGTH + PACKET_OVERHEAD_LENGTH};
-
-enum VescCommand : uint8_t {
-	FW_VERSION = 0,
-	JUMP_TO_BOOTLOADER,
-	ERASE_NEW_APP,
-	WRITE_NEW_APP_DATA,
-	GET_VALUES,
-	SET_DUTY,
-	SET_CURRENT,
-	SET_CURRENT_BRAKE,
-	SET_RPM,
-	SET_POS,
-	SET_HANDBRAKE,
-	SET_DETECT,
-	SET_SERVO_POS,
-	SET_MCCONF,
-	GET_MCCONF,
-	GET_MCCONF_DEFAULT,
-	SET_APPCONF,
-	GET_APPCONF,
-	GET_APPCONF_DEFAULT,
-	SAMPLE_PRINT,
-	TERMINAL_CMD,
-	PRINT,
-	ROTOR_POSITION,
-	EXPERIMENT_SAMPLE,
-	DETECT_MOTOR_PARAM,
-	DETECT_MOTOR_R_L,
-	DETECT_MOTOR_FLUX_LINKAGE,
-	DETECT_ENCODER,
-	DETECT_HALL_FOC,
-	REBOOT,
-	ALIVE,
-	GET_DECODED_PPM,
-	GET_DECODED_ADC,
-	GET_DECODED_CHUK,
-	FORWARD_CAN,
-	SET_CHUCK_DATA,
-	CUSTOM_APP_DATA,
-	NRF_START_PAIRING,
-	GPD_SET_FSW,
-	GPD_BUFFER_NOTIFY,
-	GPD_BUFFER_SIZE_LEFT,
-	GPD_FILL_BUFFER,
-	GPD_OUTPUT_SAMPLE,
-	GPD_SET_MODE,
-	GPD_FILL_BUFFER_INT8,
-	GPD_FILL_BUFFER_INT16,
-	GPD_SET_BUFFER_INT_SCALE,
-	GET_VALUES_SETUP,
-	SET_MCCONF_TEMP,
-	SET_MCCONF_TEMP_SETUP,
-	GET_VALUES_SELECTIVE,
-	GET_VALUES_SETUP_SELECTIVE,
-	EXT_NRF_PRESENT,
-	EXT_NRF_ESB_SET_CH_ADDR,
-	EXT_NRF_ESB_SEND_DATA,
-	EXT_NRF_ESB_RX_DATA,
-	EXT_NRF_SET_ENABLED,
-	DETECT_MOTOR_FLUX_LINKAGE_OPENLOOP,
-	DETECT_APPLY_ALL_FOC,
-	JUMP_TO_BOOTLOADER_ALL_CAN,
-	ERASE_NEW_APP_ALL_CAN,
-	WRITE_NEW_APP_DATA_ALL_CAN,
-	PING_CAN,
-	APP_DISABLE_OUTPUT,
-	TERMINAL_CMD_SYNC,
-	GET_IMU_DATA,
-	BM_CONNECT,
-	BM_ERASE_FLASH_ALL,
-	BM_WRITE_FLASH,
-	BM_REBOOT,
-	BM_DISCONNECT,
-	BM_MAP_PINS_DEFAULT,
-	BM_MAP_PINS_NRF5X,
-	ERASE_BOOTLOADER,
-	ERASE_BOOTLOADER_ALL_CAN,
-	PLOT_INIT,
-	PLOT_DATA,
-	PLOT_ADD_GRAPH,
-	PLOT_SET_GRAPH,
-	GET_DECODED_BALANCE,
-	BM_MEM_READ,
-	WRITE_NEW_APP_DATA_LZO,
-	WRITE_NEW_APP_DATA_ALL_CAN_LZO,
-	BM_WRITE_FLASH_LZO,
-	SET_CURRENT_REL,
-	CAN_FWD_FRAME,
-	SET_BATTERY_CUT,
-	SET_BLE_NAME,
-	SET_BLE_PIN,
-	SET_CAN_MODE,
-	GET_IMU_CALIBRATION,
-	GET_MCCONF_TEMP,
-
-	// Custom configuration for hardware
-	GET_CUSTOM_CONFIG_XML,
-	GET_CUSTOM_CONFIG,
-	GET_CUSTOM_CONFIG_DEFAULT,
-	SET_CUSTOM_CONFIG,
-
-	// BMS commands
-	BMS_GET_VALUES,
-	BMS_SET_CHARGE_ALLOWED,
-	BMS_SET_BALANCE_OVERRIDE,
-	BMS_RESET_COUNTERS,
-	BMS_FORCE_BALANCE,
-	BMS_ZERO_CURRENT_OFFSET,
-
-	// Firmware update commands for different hardware types
-	JUMP_TO_BOOTLOADER_HW,
-	ERASE_NEW_APP_HW,
-	WRITE_NEW_APP_DATA_HW,
-	ERASE_BOOTLOADER_HW,
-	JUMP_TO_BOOTLOADER_ALL_CAN_HW,
-	ERASE_NEW_APP_ALL_CAN_HW,
-	WRITE_NEW_APP_DATA_ALL_CAN_HW,
-	ERASE_BOOTLOADER_ALL_CAN_HW,
-
-	SET_ODOMETER,
-};
-
-static constexpr uint16_t CRC_TABLE[256] = {0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50a5, 0x60c6, 0x70e7, 0x8108, 0x9129, 0xa14a, 0xb16b, 0xc18c, 0xd1ad, 0xe1ce, 0xf1ef, 0x1231, 0x0210, 0x3273, 0x2252, 0x52b5, 0x4294, 0x72f7, 0x62d6, 0x9339, 0x8318, 0xb37b, 0xa35a, 0xd3bd, 0xc39c, 0xf3ff, 0xe3de, 0x2462, 0x3443, 0x0420, 0x1401, 0x64e6, 0x74c7, 0x44a4, 0x5485, 0xa56a, 0xb54b, 0x8528, 0x9509, 0xe5ee, 0xf5cf, 0xc5ac, 0xd58d, 0x3653, 0x2672, 0x1611, 0x0630, 0x76d7, 0x66f6, 0x5695, 0x46b4, 0xb75b, 0xa77a, 0x9719, 0x8738, 0xf7df, 0xe7fe, 0xd79d, 0xc7bc, 0x48c4, 0x58e5, 0x6886, 0x78a7, 0x0840, 0x1861, 0x2802, 0x3823, 0xc9cc, 0xd9ed, 0xe98e, 0xf9af, 0x8948, 0x9969, 0xa90a, 0xb92b, 0x5af5, 0x4ad4, 0x7ab7, 0x6a96, 0x1a71, 0x0a50, 0x3a33, 0x2a12, 0xdbfd, 0xcbdc, 0xfbbf, 0xeb9e, 0x9b79, 0x8b58, 0xbb3b, 0xab1a, 0x6ca6, 0x7c87, 0x4ce4, 0x5cc5, 0x2c22, 0x3c03, 0x0c60, 0x1c41, 0xedae, 0xfd8f, 0xcdec, 0xddcd, 0xad2a, 0xbd0b, 0x8d68, 0x9d49, 0x7e97, 0x6eb6, 0x5ed5, 0x4ef4, 0x3e13, 0x2e32, 0x1e51, 0x0e70, 0xff9f, 0xefbe, 0xdfdd, 0xcffc, 0xbf1b, 0xaf3a, 0x9f59, 0x8f78, 0x9188, 0x81a9, 0xb1ca, 0xa1eb, 0xd10c, 0xc12d, 0xf14e, 0xe16f, 0x1080, 0x00a1, 0x30c2, 0x20e3, 0x5004, 0x4025, 0x7046, 0x6067, 0x83b9, 0x9398, 0xa3fb, 0xb3da, 0xc33d, 0xd31c, 0xe37f, 0xf35e, 0x02b1, 0x1290, 0x22f3, 0x32d2, 0x4235, 0x5214, 0x6277, 0x7256, 0xb5ea, 0xa5cb, 0x95a8, 0x8589, 0xf56e, 0xe54f, 0xd52c, 0xc50d, 0x34e2, 0x24c3, 0x14a0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405, 0xa7db, 0xb7fa, 0x8799, 0x97b8, 0xe75f, 0xf77e, 0xc71d, 0xd73c, 0x26d3, 0x36f2, 0x0691, 0x16b0, 0x6657, 0x7676, 0x4615, 0x5634, 0xd94c, 0xc96d, 0xf90e, 0xe92f, 0x99c8, 0x89e9, 0xb98a, 0xa9ab, 0x5844, 0x4865, 0x7806, 0x6827, 0x18c0, 0x08e1, 0x3882, 0x28a3, 0xcb7d, 0xdb5c, 0xeb3f, 0xfb1e, 0x8bf9, 0x9bd8, 0xabbb, 0xbb9a, 0x4a75, 0x5a54, 0x6a37, 0x7a16, 0x0af1, 0x1ad0, 0x2ab3, 0x3a92, 0xfd2e, 0xed0f, 0xdd6c, 0xcd4d, 0xbdaa, 0xad8b, 0x9de8, 0x8dc9, 0x7c26, 0x6c07, 0x5c64, 0x4c45, 0x3ca2, 0x2c83, 0x1ce0, 0x0cc1, 0xef1f, 0xff3e, 0xcf5d, 0xdf7c, 0xaf9b, 0xbfba, 0x8fd9, 0x9ff8, 0x6e17, 0x7e36, 0x4e55, 0x5e74, 0x2e93, 0x3eb2, 0x0ed1, 0x1ef0};
-
-struct VescVersion {
-	uint8_t version_major;
-	uint8_t version_minor;
-	// char hardware_name[50];
-	// uint8_t stm32_uuid_8[12];
-	// bool pairing_done;
-	// uint8_t test_version_number;
-	// uint8_t hardware_type;
-	// uint8_t custom_configuration;
-};
-
-struct VescValues {
-	float fet_temperature;
-	float motor_temperature;
-	float motor_current;
-	float input_current;
-	float reset_average_id;
-	float reset_average_iq;
-	float duty_cycle;
-	int32_t rpm;
-	float input_voltage;
-	float used_charge_Ah;
-	float charged_charge_Ah;
-	float used_energy_Wh;
-	float charged_energy_wh;
-	int32_t tachometer;
-	int32_t tachometer_absolute;
-	int8_t fault;
-	float position_pid;
-	int8_t controller_id;
-	float ntc_temperature_mos1;
-	float ntc_temperature_mos2;
-	float ntc_temperature_mos3;
-	float read_reset_average_vd;
-	float read_reset_average_vq;
-};

src/drivers/vesc/VescDriver/VescWritableInterface.hpp

@@ -1,50 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 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 VescWritableInterface.hpp
- * @brief Interface calls defining the write callback for VESC communication
- * @details To keep the driver cross-platform compatible the device owner class
- * needs to implement this interface and pass itself to the driver via constructor
- * such that writeCallback() can be used by the driver to send out data.
- * @author Matthias Grob <maetugr@gmail.com>
- */
-
-#include <stdint.h>
-#include <stdio.h>
-
-class VescWritableInterface
-{
-public:
-	virtual size_t writeCallback(const uint8_t *buffer, const uint16_t length) = 0;
-};

src/drivers/vesc/VescSerialDevice.cpp

@@ -1,303 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 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 VescSerialDevice.cpp
- * @author Matthias Grob <maetugr@gmail.com>
- */
-
-#include "VescSerialDevice.hpp"
-#include <stdint.h>
-#include <termios.h>
-#include <lib/drivers/device/Device.hpp>
-
-using namespace time_literals;
-
-VescDevice::VescDevice(const char *port) :
-	CDev("/dev/vesc"),
-	OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default),
-	_vesc_driver(*this)
-{
-	// Store port name
-	strncpy(_port, port, sizeof(_port) - 1);
-
-	// Enforce null termination
-	_port[sizeof(_port) - 1] = '\0';
-
-	_mixing_output.setAllDisarmedValues(DISARM_VALUE);
-	_mixing_output.setAllMinValues(MIN_THROTTLE);
-	_mixing_output.setAllMaxValues(MAX_THROTTLE);
-}
-
-VescDevice::~VescDevice()
-{
-	perf_free(_cycle_perf);
-}
-
-int VescDevice::init()
-{
-	// Do regular cdev init
-	int ret = CDev::init();
-
-	if (ret != OK) {
-		return ret;
-	}
-
-	// Set Baudrate and schedule for the first time
-	ret = setBaudrate(115200);
-
-	if (ret != OK) {
-		return ret;
-	}
-
-	ScheduleNow();
-	return OK;
-}
-
-void VescDevice::printStatus()
-{
-	printf("Using port '%s'\n", _port);
-	printf("VESC version: %d.%d\n", _vesc_driver.getVersionMajor(), _vesc_driver.getVersionMinor());
-	_mixing_output.printStatus();
-	perf_print_counter(_cycle_perf);
-}
-
-bool VescDevice::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
-			       unsigned num_control_groups_updated)
-{
-	// for (unsigned i = 0; i < num_outputs; i++) {
-	// 	if (!stop_motors && outputs[i] != DISARM_VALUE) {
-	// 		_vesc_driver.commandDutyCycle(static_cast<float>(outputs[i]) / 1e3f);
-	// 	}
-	// }
-
-	if (!stop_motors) {
-		_vesc_driver.commandDutyCycle(static_cast<float>(outputs[0]) / 1e3f);
-		_vesc_driver.commandDutyCycle(static_cast<float>(outputs[1]) / 1e3f, 11);
-		_vesc_driver.commandDutyCycle(static_cast<float>(outputs[2]) / 1e3f, 7);
-		_vesc_driver.commandDutyCycle(static_cast<float>(outputs[3]) / 1e3f, 107);
-
-	} else {
-		_vesc_driver.commandDutyCycle(0.f);
-		_vesc_driver.commandDutyCycle(0.f, 11);
-		_vesc_driver.commandDutyCycle(0.f, 7);
-		_vesc_driver.commandDutyCycle(0.f, 107);
-	}
-
-	return true;
-}
-
-void VescDevice::Run()
-{
-	perf_begin(_cycle_perf);
-
-	if (_serial_fd < 0) {
-		// Reopen fd from the work queue context
-		_serial_fd = ::open(_port, O_RDWR | O_NOCTTY);
-		_vesc_driver.requestFirmwareVersion();
-	}
-
-	_mixing_output.update();
-
-	// Check the number of bytes available in the buffer
-	int bytes_available{0};
-	int ret = ::ioctl(_serial_fd, FIONREAD, (unsigned long)&bytes_available);
-
-	while (ret >= 0 && bytes_available > 0) {
-		printf("read: ");
-		uint8_t readbuf[READ_BUFFER_SIZE] {};
-
-		// Read from the uart buffer
-		ret = ::read(_serial_fd, readbuf, READ_BUFFER_SIZE);
-
-		for (int i = 0; i < ret; i++) {
-			printf("%02X ", readbuf[i]);
-		}
-
-		printf("\n");
-
-		for (int i = 0; i < ret; i++) {
-			_vesc_driver.parseInputByte(readbuf[i]);
-		}
-
-		bytes_available -= ret;
-	}
-
-	_mixing_output.updateSubscriptions(true);
-
-	perf_end(_cycle_perf);
-}
-
-int VescDevice::setBaudrate(const unsigned baudrate)
-{
-	// Open fd in main task context
-	_serial_fd = ::open(_port, O_RDWR | O_NOCTTY);
-
-	// Translate baud rate to define
-	int speed;
-
-	switch (baudrate) {
-	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;
-
-#ifndef B460800
-#define B460800 460800
-#endif
-
-	case 460800: speed = B460800; break;
-
-	default:
-		PX4_ERR("ERR: unknown baudrate: %d", baudrate);
-		return -EINVAL;
-	}
-
-	// Fill the struct for the new configuration
-	struct termios uart_config;
-	tcgetattr(_serial_fd, &uart_config);
-
-	// Configure the terminal (see https://en.wikibooks.org/wiki/Serial_Programming/termios)
-
-	//
-	// 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);
-
-	// Apply configuration and baud rate
-	int termios_state;
-
-	if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
-		PX4_ERR("ERR: %d (cfsetispeed)", termios_state);
-		return PX4_ERROR;
-	}
-
-	if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
-		PX4_ERR("ERR: %d (cfsetospeed)", termios_state);
-		return PX4_ERROR;
-	}
-
-	if ((termios_state = tcsetattr(_serial_fd, TCSANOW, &uart_config)) < 0) {
-		PX4_ERR("ERR: %d (tcsetattr)", termios_state);
-		return PX4_ERROR;
-	}
-
-	// Close fd in main task context
-	::close(_serial_fd);
-	_serial_fd = -1;
-
-	return PX4_OK;
-}
-
-size_t VescDevice::writeCallback(const uint8_t *buffer, const uint16_t length)
-{
-	printf("write: ");
-
-	for (int i = 0; i < length; i++) {
-		printf("%02X ", buffer[i]);
-	}
-
-	printf("\n");
-	int ret = ::write(_serial_fd, buffer, length);
-	printf("write result: %d %d\n", ret, _serial_fd);
-	return ret;
-}
-
-int VescDevice::ioctl(device::file_t *filp, int cmd, unsigned long arg)
-{
-	int ret = OK;
-
-	PX4_DEBUG("ioctl cmd: %d, arg: %ld", cmd, arg);
-
-	lock();
-
-	switch (cmd) {
-	case MIXERIOCRESET:
-		_mixing_output.resetMixerThreadSafe();
-		break;
-
-	case MIXERIOCLOADBUF: {
-			const char *buf = (const char *)arg;
-			unsigned buflen = strlen(buf);
-			ret = _mixing_output.loadMixerThreadSafe(buf, buflen);
-			break;
-		}
-
-	default:
-		ret = -ENOTTY;
-		break;
-	}
-
-	unlock();
-
-	if (ret == -ENOTTY) {
-		ret = CDev::ioctl(filp, cmd, arg);
-	}
-
-	return ret;
-}

src/drivers/vesc/VescSerialDevice.hpp

@@ -1,73 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 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 VescSerialDevice.hpp
- * @brief PX4 serial motor output device using VescDriver for core protocol
- * @author Matthias Grob <maetugr@gmail.com>
- */
-
-#include "VescDriver/VescDriver.hpp"
-#include <drivers/device/device.h>
-#include <drivers/drv_mixer.h>
-#include <lib/cdev/CDev.hpp>
-#include <lib/mixer_module/mixer_module.hpp>
-#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
-
-class VescDevice : public VescWritableInterface, public cdev::CDev, public OutputModuleInterface
-{
-public:
-	VescDevice(const char *port);
-	~VescDevice();
-	int init();
-	void printStatus();
-	int ioctl(device::file_t *filp, int cmd, unsigned long arg);
-
-private:
-	void Run();
-	size_t writeCallback(const uint8_t *buffer, const uint16_t length) override;
-	int setBaudrate(const unsigned baudrate);
-	bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
-			   unsigned num_control_groups_updated) override;
-
-	static constexpr size_t READ_BUFFER_SIZE{256};
-	static constexpr int DISARM_VALUE = 0;
-	static constexpr int MIN_THROTTLE = 100;
-	static constexpr int MAX_THROTTLE = 1000;
-
-	char _port[20] {};
-	int _serial_fd{-1};
-	VescDriver _vesc_driver;
-	MixingOutput _mixing_output{4, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
-	perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
-};

src/drivers/vesc/module.yaml

@@ -1,6 +0,0 @@
-module_name: VESC motor contoller
-serial_config:
-    - command: vesc start -d ${SERIAL_DEV}
-      port_config_param:
-        name: VESC_PORT
-        group: VESC

src/drivers/vesc/vesc_main.cpp

@@ -1,180 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2017-2019 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.
- *
- ****************************************************************************/
-
-#include "VescSerialDevice.hpp"
-#include <px4_platform_common/defines.h>
-#include <px4_platform_common/getopt.h>
-#include <px4_platform_common/log.h>
-#include <px4_platform_common/module.h>
-
-// Local functions in support of the shell command
-namespace vesc
-{
-
-VescDevice *g_dev{nullptr};
-
-int start(const char *port);
-int status();
-int stop();
-int usage();
-
-int
-start(const char *port)
-{
-	if (g_dev != nullptr) {
-		PX4_ERR("already started");
-		return PX4_OK;
-	}
-
-	// Instantiate the driver
-	g_dev = new VescDevice(port);
-
-	if (g_dev == nullptr) {
-		PX4_ERR("driver start failed");
-		return PX4_ERROR;
-	}
-
-	if (OK != g_dev->init()) {
-		PX4_ERR("driver start failed");
-		delete g_dev;
-		g_dev = nullptr;
-		return PX4_ERROR;
-	}
-
-	return PX4_OK;
-}
-
-int
-status()
-{
-	if (g_dev == nullptr) {
-		PX4_ERR("driver not running");
-		return 1;
-	}
-
-	printf("state @ %p\n", g_dev);
-	g_dev->printStatus();
-
-	return 0;
-}
-
-int stop()
-{
-	if (g_dev != nullptr) {
-		PX4_INFO("stopping driver");
-		delete g_dev;
-		g_dev = nullptr;
-		PX4_INFO("driver stopped");
-
-	} else {
-		PX4_ERR("driver not running");
-		return 1;
-	}
-
-	return PX4_OK;
-}
-
-int
-usage()
-{
-	PRINT_MODULE_DESCRIPTION(
-		R"DESCR_STR(
-### Description
-
-Serial driver for VESC motor contollers
-
-Most boards are configured to enable/start the driver on a specified UART using the VESC_PORT parameter.
-
-Setup/usage information to be added.
-
-### Examples
-
-Attempt to start driver on a specified serial device.
-$ vesc start -d /dev/ttyS1
-Stop driver
-$ vesc stop
-)DESCR_STR");
-
-	PRINT_MODULE_USAGE_NAME("vesc", "driver");
-	PRINT_MODULE_USAGE_COMMAND_DESCR("start","Start driver");
-	PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, nullptr, "Serial device", false);
-	PRINT_MODULE_USAGE_COMMAND_DESCR("status","Driver status");
-	PRINT_MODULE_USAGE_COMMAND_DESCR("stop","Stop driver");
-	PRINT_MODULE_USAGE_COMMAND_DESCR("status","Print driver status");
-	return PX4_OK;
-}
-
-} // namespace
-
-extern "C" __EXPORT int vesc_main(int argc, char *argv[])
-{
-	int ch{0};
-	const char *device_path{nullptr};
-	int myoptind{1};
-	const char *myoptarg{nullptr};
-
-	while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) {
-		switch (ch) {
-		case 'd':
-			device_path = myoptarg;
-			break;
-
-		default:
-			PX4_WARN("Unknown option");
-			return PX4_ERROR;
-		}
-	}
-
-	if (myoptind >= argc) {
-		PX4_ERR("Unrecognized command");
-		return vesc::usage();
-	}
-
-	if (!strcmp(argv[myoptind], "start")) {
-		if (strcmp(device_path, "") != 0) {
-			return vesc::start(device_path);
-
-		} else {
-			PX4_WARN("Please specify device path");
-			return vesc::usage();
-		}
-
-	} else if (!strcmp(argv[myoptind], "stop")) {
-		return vesc::stop();
-
-	} else if (!strcmp(argv[myoptind], "status")) {
-		return vesc::status();
-	}
-
-	return vesc::usage();
-}

src/lib/mixer_module/mixer_module.cpp

@@ -439,7 +439,8 @@ bool MixingOutput::update()
 	return true;
 }
 
-void MixingOutput::setAndPublishActuatorOutputs(unsigned num_outputs, actuator_outputs_s &actuator_outputs)
+void
+MixingOutput::setAndPublishActuatorOutputs(unsigned num_outputs, actuator_outputs_s &actuator_outputs)
 {
 	actuator_outputs.noutputs = num_outputs;
 
@@ -451,7 +452,8 @@ void MixingOutput::setAndPublishActuatorOutputs(unsigned num_outputs, actuator_o
 	_outputs_pub.publish(actuator_outputs);
 }
 
-void MixingOutput::publishMixerStatus(const actuator_outputs_s &actuator_outputs)
+void
+MixingOutput::publishMixerStatus(const actuator_outputs_s &actuator_outputs)
 {
 	MultirotorMixer::saturation_status saturation_status;
 	saturation_status.value = _mixers->get_saturation_status();
@@ -465,7 +467,8 @@ void MixingOutput::publishMixerStatus(const actuator_outputs_s &actuator_outputs
 	}
 }
 
-void MixingOutput::updateLatencyPerfCounter(const actuator_outputs_s &actuator_outputs)
+void
+MixingOutput::updateLatencyPerfCounter(const actuator_outputs_s &actuator_outputs)
 {
 	// use first valid timestamp_sample for latency tracking
 	for (int i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
@@ -479,27 +482,37 @@ void MixingOutput::updateLatencyPerfCounter(const actuator_outputs_s &actuator_o
 	}
 }
 
-void MixingOutput::reorderOutputs(uint16_t values[MAX_ACTUATORS])
+void
+MixingOutput::reorderOutputs(uint16_t values[MAX_ACTUATORS])
 {
 	if (MAX_ACTUATORS < 4) {
 		return;
 	}
 
-	const uint16_t value_tmp[4] = {values[0], values[1], values[2], values[3]};
-	values[reorderedMotorIndex(0)] = value_tmp[0];
-	values[reorderedMotorIndex(1)] = value_tmp[1];
-	values[reorderedMotorIndex(2)] = value_tmp[2];
-	values[reorderedMotorIndex(3)] = value_tmp[3];
+	if ((MotorOrdering)_param_mot_ordering.get() == MotorOrdering::Betaflight) {
+		/*
+		 * Betaflight default motor ordering:
+		 * 4     2
+		 *    ^
+		 * 3     1
+		 */
+		const uint16_t value_tmp[4] = {values[0], values[1], values[2], values[3] };
+		values[0] = value_tmp[3];
+		values[1] = value_tmp[0];
+		values[2] = value_tmp[1];
+		values[3] = value_tmp[2];
+	}
+
+	/* else: PX4, no need to reorder
+	 * 3     1
+	 *    ^
+	 * 2     4
+	 */
 }
 
 int MixingOutput::reorderedMotorIndex(int index) const
 {
 	if ((MotorOrdering)_param_mot_ordering.get() == MotorOrdering::Betaflight) {
-		/* Betaflight default motor ordering:
-		 * 4     2
-		 *    ^
-		 * 3     1
-		 */
 		switch (index) {
 		case 0: return 1;
 
@@ -511,29 +524,6 @@ int MixingOutput::reorderedMotorIndex(int index) const
 		}
 	}
 
-	if ((MotorOrdering)_param_mot_ordering.get() == MotorOrdering::Clockwise) {
-		/* Clockwise motor ordering:
-		 * 4     1
-		 *    ^
-		 * 3     2
-		 */
-		switch (index) {
-		case 0: return 0;
-
-		case 1: return 2;
-
-		case 2: return 3;
-
-		case 3: return 1;
-		}
-	}
-
-	/* else: PX4, no need to reorder
-	 * 3     1
-	 *    ^
-	 * 2     4
-	 */
-
 	return index;
 }
 

src/lib/mixer_module/mixer_module.hpp

@@ -203,8 +203,7 @@ private:
 
 	enum class MotorOrdering : int32_t {
 		PX4 = 0,
-		Betaflight = 1,
-		Clockwise = 2
+		Betaflight = 1
 	};
 
 	struct Command {
@@ -282,5 +281,6 @@ private:
 		(ParamFloat<px4::params::MOT_SLEW_MAX>) _param_mot_slew_max,
 		(ParamFloat<px4::params::THR_MDL_FAC>) _param_thr_mdl_fac, ///< thrust to motor control signal modelling factor
 		(ParamInt<px4::params::MOT_ORDERING>) _param_mot_ordering
+
 	)
 };

src/lib/mixer_module/params.c

@@ -29,7 +29,6 @@ PARAM_DEFINE_INT32(MC_AIRMODE, 0);
  *
  * @value 0 PX4
  * @value 1 Betaflight / Cleanflight
- * @value 2 Clockwise
  *
  * @group Mixer Output
  */

src/lib/rc/dsm.cpp

@@ -81,6 +81,7 @@ static unsigned dsm_partial_frame_count;	/**< Count of bytes received for curren
 static unsigned dsm_channel_shift = 0;			/**< Channel resolution, 0=unknown, 10=10 bit (1024), 11=11 bit (2048) */
 static unsigned dsm_frame_drops = 0;			/**< Count of incomplete DSM frames */
 static uint16_t dsm_chan_count = 0;         /**< DSM channel count */
+static uint16_t dsm_chan_count_prev = 0;    /**< last valid DSM channel count */
 
 /**
  * Attempt to decode a single channel raw channel datum
@@ -135,7 +136,7 @@ static bool dsm_decode_channel(uint16_t raw, unsigned shift, uint8_t &channel, u
 
 		// Spektrum range is 903μs to 2097μs (Specification for Spektrum Remote Receiver Interfacing Rev G 9.1)
 		//  ±100% travel is 1102µs to 1898 µs
-		if (value < 990 || value > 2010) {
+		if (value < 903 || value > 2097) {
 			// if the value is unrealistic, fail the parsing entirely
 			PX4_DEBUG("channel %d invalid range %d", channel, value);
 			return false;
@@ -190,12 +191,14 @@ static bool dsm_decode_channel(uint16_t raw, unsigned shift, uint8_t &channel, u
 
 		// Spektrum range is 903μs to 2097μs (Specification for Spektrum Remote Receiver Interfacing Rev G 9.1)
 		//  ±100% travel is 1102µs to 1898 µs
-		if (value < 990 || value > 2010) {
+		if (value < 903 || value > 2097) {
 			// if the value is unrealistic, fail the parsing entirely
 			PX4_DEBUG("channel %d invalid range %d", channel, value);
 			return false;
 		}
 
+		PX4_DEBUG(stderr, "CH%d=%d(0x%02x), ", channel, value, raw);
+
 		return true;
 	}
 
@@ -215,6 +218,7 @@ static bool dsm_guess_format(bool reset)
 
 	/* reset the 10/11 bit sniffed channel masks */
 	if (reset) {
+		PX4_DEBUG("dsm_guess_format reset");
 		cs10 = 0;
 		cs11 = 0;
 		samples = 0;
@@ -283,8 +287,8 @@ static bool dsm_guess_format(bool reset)
 	printf("dsm guess format: samples: %d %s\n", samples, (reset) ? "RESET" : "");
 #endif
 
-	/* wait until we have seen plenty of frames - 5 should normally be enough */
-	if (samples < 5) {
+	/* wait until we have seen plenty of frames */
+	if (samples < 10) {
 		return false;
 	}
 
@@ -597,6 +601,7 @@ bool dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values,
 		// abort if channel already found, no duplicate channels per DSM frame
 		if (channels_found[channel]) {
 			PX4_DEBUG("duplicate channel %d\n\n", channel);
+			dsm_guess_format(true);
 			return false;
 
 		} else {
@@ -605,6 +610,7 @@ bool dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values,
 
 		/* reset bit guessing state machine if the channel index is out of bounds */
 		if (channel > DSM_MAX_CHANNEL_COUNT) {
+			PX4_DEBUG("channel %d > %d (DSM_MAX_CHANNEL_COUNT)", channel, DSM_MAX_CHANNEL_COUNT);
 			dsm_guess_format(true);
 			return false;
 		}
@@ -645,16 +651,6 @@ bool dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values,
 		values[channel] = value;
 	}
 
-	/*
-	 * Spektrum likes to send junk in higher channel numbers to fill
-	 * their packets. We don't know about a 13 channel model in their TX
-	 * lines, so if we get a channel count of 13, we'll return 12 (the last
-	 * data index that is stable).
-	 */
-	if (*num_values == 13) {
-		*num_values = 12;
-	}
-
 	/* Set the 11-bit data indicator */
 	*dsm_11_bit = (dsm_channel_shift == 11);
 
@@ -845,11 +841,18 @@ bool dsm_parse(const uint64_t now, const uint8_t *frame, const unsigned len, uin
 	}
 
 	if (decode_ret) {
-		*num_values = dsm_chan_count;
+		// require stable channel count (dsm_chan_count == dsm_chan_count_prev) before considering the decode valid
+		if ((dsm_chan_count > 0) && (dsm_chan_count <= DSM_MAX_CHANNEL_COUNT) && (dsm_chan_count == dsm_chan_count_prev)) {
+			*num_values = dsm_chan_count;
+			memcpy(&values[0], &dsm_chan_buf[0], dsm_chan_count * sizeof(dsm_chan_buf[0]));
+
+		} else {
+			decode_ret = false;
+		}
+
+		dsm_chan_count_prev = dsm_chan_count;
 
-		memcpy(&values[0], &dsm_chan_buf[0], dsm_chan_count * sizeof(dsm_chan_buf[0]));
 #ifdef DSM_DEBUG
-
 		printf("PACKET ---------\n");
 		printf("frame drops: %u, chan #: %u\n", dsm_frame_drops, dsm_chan_count);
 

src/lib/rc/rc_tests/RCTest.cpp

@@ -231,17 +231,17 @@ bool RCTest::ghstTest()
 
 bool RCTest::dsmTest10Ch()
 {
-	return dsmTest(TEST_DATA_PATH "dsm_x_data.txt", 10, 6, 1500);
+	return dsmTest(TEST_DATA_PATH "dsm_x_data.txt", 10, 17, 1500);
 }
 
 bool RCTest::dsmTest16Ch()
 {
-	return dsmTest(TEST_DATA_PATH "dsm_x_dx9_data.txt", 16, 3, 1500);
+	return dsmTest(TEST_DATA_PATH "dsm_x_dx9_data.txt", 16, 6, 1500);
 }
 
 bool RCTest::dsmTest22msDSMX16Ch()
 {
-	return dsmTest(TEST_DATA_PATH "dsm_x_dx9_px4_binding_data.txt", 16, 6, 1499);
+	return dsmTest(TEST_DATA_PATH "dsm_x_dx9_px4_binding_data.txt", 16, 11, 1499);
 }
 
 bool RCTest::dsmTest(const char *filepath, unsigned expected_chancount, unsigned expected_dropcount, unsigned chan0)
@@ -290,7 +290,7 @@ bool RCTest::dsmTest(const char *filepath, unsigned expected_chancount, unsigned
 					&dsm_11_bit, &dsm_frame_drops, nullptr, max_channels);
 
 		if (result) {
-			if (count > (16 * 10)) { // need to process enough data to have full channel count
+			if (count > (16 * 20)) { // need to process enough data to have full channel count
 				ut_compare("num_values == expected_chancount", num_values, expected_chancount);
 			}
 
@@ -313,6 +313,8 @@ bool RCTest::dsmTest(const char *filepath, unsigned expected_chancount, unsigned
 
 	fclose(fp);
 
+	ut_compare("num_values == expected_chancount", num_values, expected_chancount);
+
 	ut_test(ret == EOF);
 	//PX4_INFO("drop: %d", (int)last_drop);
 	ut_compare("last_drop == expected_dropcount", last_drop, expected_dropcount);

src/modules/commander/Commander.cpp

@@ -3533,7 +3533,7 @@ void Commander::data_link_check()
 			_status.data_link_lost = true;
 			_status.data_link_lost_counter++;
 
-			mavlink_log_critical(&_mavlink_log_pub, "Connection to ground station lost");
+			mavlink_log_info(&_mavlink_log_pub, "Connection to ground station lost");
 
 			_status_changed = true;
 		}

src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp

@@ -96,7 +96,17 @@ static int parse_options(int argc, char *argv[])
 
 		case 'w': _options.sleep_ms        = strtoul(myoptarg, nullptr, 10);    break;
 
-		case 'b': _options.baudrate        = strtoul(myoptarg, nullptr, 10);    break;
+		case 'b': {
+				int baudrate = 0;
+
+				if (px4_get_parameter_value(myoptarg, baudrate) != 0) {
+					PX4_ERR("baudrate parsing failed");
+				}
+
+				_options.baudrate = baudrate;
+
+				break;
+			}
 
 		case 'r': _options.recv_port       = strtoul(myoptarg, nullptr, 10);    break;
 

src/modules/rc_update/rc_update.cpp

@@ -424,35 +424,39 @@ void RCUpdate::Run()
 		/* publish rc_channels topic even if signal is invalid, for debug */
 		_rc_channels_pub.publish(_rc);
 
-		/* only publish manual control if the signal is present */
-		if (input_source_stable && channel_count_stable && !signal_lost
-		    && (input_rc.timestamp_last_signal > _last_timestamp_signal)) {
+		// only publish manual control if the signal is present and regularly updating
+		if (input_source_stable && channel_count_stable && !signal_lost) {
 
-			_last_timestamp_signal = input_rc.timestamp_last_signal;
-			perf_count(_valid_data_interval_perf);
+			if ((input_rc.timestamp_last_signal > _last_timestamp_signal)
+			    && (input_rc.timestamp_last_signal - _last_timestamp_signal < 1_s)) {
 
-			// check if channels actually updated
-			bool rc_updated = false;
+				perf_count(_valid_data_interval_perf);
 
-			for (unsigned i = 0; i < channel_count_limited; i++) {
-				if (_rc_values_previous[i] != input_rc.values[i]) {
-					rc_updated = true;
-					break;
+				// check if channels actually updated
+				bool rc_updated = false;
+
+				for (unsigned i = 0; i < channel_count_limited; i++) {
+					if (_rc_values_previous[i] != input_rc.values[i]) {
+						rc_updated = true;
+						break;
+					}
+				}
+
+				// limit processing if there's no update
+				if (rc_updated || (hrt_elapsed_time(&_last_manual_control_setpoint_publish) > 300_ms)) {
+					UpdateManualSetpoint(input_rc.timestamp_last_signal);
+				}
+
+				UpdateManualSwitches(input_rc.timestamp_last_signal);
+
+				/* Update parameters from RC Channels (tuning with RC) if activated */
+				if (hrt_elapsed_time(&_last_rc_to_param_map_time) > 1_s) {
+					set_params_from_rc();
+					_last_rc_to_param_map_time = hrt_absolute_time();
 				}
 			}
 
-			// limit processing if there's no update
-			if (rc_updated || (hrt_elapsed_time(&_last_manual_control_setpoint_publish) > 300_ms)) {
-				UpdateManualSetpoint(input_rc.timestamp_last_signal);
-			}
-
-			UpdateManualSwitches(input_rc.timestamp_last_signal);
-
-			/* Update parameters from RC Channels (tuning with RC) if activated */
-			if (hrt_elapsed_time(&_last_rc_to_param_map_time) > 1_s) {
-				set_params_from_rc();
-				_last_rc_to_param_map_time = hrt_absolute_time();
-			}
+			_last_timestamp_signal = input_rc.timestamp_last_signal;
 		}
 
 		memcpy(_rc_values_previous, input_rc.values, sizeof(input_rc.values[0]) * channel_count_limited);