Controlling motors

Hey all,

So I am working on integrating PX4 into a autonomous underwater vehicle. For this we also need to have a backup for manual control. Since this is easier to implement than making it fully autonomous, I have started to do the manual control first so I can understand PX4 better.

I have connected a 35 MHz remote in order to manual control the underwater vehicle.

The 35 MHz receiver is connected to an ESP32, which sends Mavlink messages to the Pixhawk. It is sending RC_CHANNELS_OVERRIDE (70) messages to the Pixhawk. I can see the messages are being received in PX4 when listening to the input_rc uORB topic in a Mavlink shell.
I have tried to control a motor with this signal. So far I have had only one successful way of controlling a motor. However I doubt it is the correct way to do this, so I have hoping someone can help me out.

Currently I have made a custom module which subscribes to the input_rc topic, and when a new message is received it first normalizes the channel value. After which I publish an actuator_test message with the normalized value. This works, however I am pretty sure this is not intended to be used this way. This is fine with just one motor, but eventually there will be 6 motors which need to be controlled. And besides the manual control, there will also be PID’s implemented for the motors. When using the actuator_test topic this way, the code will get confusing quickly I think.

I have looked into how other controllers are implemented. From what I understand they publish to the vehicle_thrust_setpoint and vehicle_attitude_setpoint topics. However I also tried to do this, but for some reason my messages are not published. And I am also not sure if this is the correct way to do this. Most written information on how to implement motor control is on how it has worked with mixers, however since version 1.13 it has been replaced by the Dynamic Control Allocation.

My main question is what is the best way to implement this? I am sure it can work with the actuator_test solution, but I am also sure there must be a better way to handle this.

I have attached the code of my custom module. Any help would be greatly appreciated. :slight_smile:

hpp file:

/****************************************************************************
 *
 *   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.
 *
 ****************************************************************************/

 #include <float.h>

 #include <drivers/drv_hrt.h>
 #include <lib/geo/geo.h>
 #include <lib/mathlib/mathlib.h>
 #include <lib/perf/perf_counter.h>
 #include <matrix/math.hpp>
 #include <px4_platform_common/px4_config.h>
 #include <px4_platform_common/defines.h>
 #include <px4_platform_common/posix.h>
 #include <px4_platform_common/tasks.h>
 #include <px4_platform_common/module.h>
 #include <px4_platform_common/module_params.h>
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionCallback.hpp>
 #include <uORB/Publication.hpp>
#include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/input_rc.h>
#include <uORB/topics/actuator_test.h>
#include <uORB/topics/actuator_motors.h>
#include <uORB/topics/vehicle_thrust_setpoint.h>
#include <uORB/topics/vehicle_torque_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>



 #include <uORB/uORB.h>

 using matrix::Eulerf;
 using matrix::Quatf;
 using matrix::Matrix3f;
 using matrix::Vector3f;
 using matrix::Dcmf;

 using uORB::SubscriptionData;

 using namespace time_literals;

 class RobosubMotorControl: public ModuleBase<RobosubMotorControl>, public ModuleParams, public px4::WorkItem
 {
 public:
 	RobosubMotorControl();
	 ~RobosubMotorControl();

	 /** @see ModuleBase */
	 static int task_spawn(int argc, char *argv[]);

	 static int custom_command(int argc, char *argv[]);

	 /** @see ModuleBase */
	 static int print_usage(const char *reason = nullptr);

	 bool init();

 private:
	 uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};

	 uORB::Subscription _input_rc_sub{ORB_ID(input_rc)};
	 uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)};		/**< vehicle status subscription */


	 uORB::SubscriptionCallbackWorkItem _rs_input_rc_sub{this, ORB_ID(input_rc)};

	 uORB::Publication<vehicle_thrust_setpoint_s>    _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)};
	 uORB::Publication<vehicle_torque_setpoint_s>    _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)};

	 vehicle_thrust_setpoint_s _vehicle_thrust_setpoint{};
	vehicle_torque_setpoint_s _vehicle_torque_setpoint{};
	vehicle_control_mode_s _vcontrol_mode{};

	 input_rc_s _input_rc{};

	 perf_counter_t	_loop_perf;


	 void Run() override;
	 /**
	  * Update our local parameter cache.
	  */
	 void parameters_update(bool force = false);

	 void actuator_test(int function, float value, int timeout_ms, bool release_control);

 };

cpp file:

/****************************************************************************
 *
 *   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.
 *
 ****************************************************************************/

/**
 *
 * This module is a first test for the robosub motor control.
 *
 * @author Daan Smienk <daansmienk10@gmail.com>
 */

 #include "rs_motor_control.hpp"
#include "px4_platform_common/defines.h"
#include "px4_platform_common/log.h"


 /**
  * Robosub motor control app start / stop handling function
  *
  * @ingroup apps
  */
 extern "C" __EXPORT int rs_motor_control_main(int argc, char *argv[]);


 RobosubMotorControl::RobosubMotorControl():
	ModuleParams(nullptr),
	WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
	 /* performance counters */
	_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
 {
 }

 RobosubMotorControl::~RobosubMotorControl()
 {
	 perf_free(_loop_perf);
 }

 bool RobosubMotorControl::init()
 {	// Execute the Run() function everytime an input_rc is publiced
	 if (!_rs_input_rc_sub.registerCallback()) {
		 PX4_ERR("callback registration failed");
		 return false;
	 }
	PX4_DEBUG("RobosubMotorControl::init()");
	return true;
 }

 void RobosubMotorControl::parameters_update(bool force)
 {
	 // check for parameter updates
	 if (_parameter_update_sub.updated() || force) {
		 // clear update
		 parameter_update_s pupdate;
		 _parameter_update_sub.copy(&pupdate);

		 // update parameters from storage
		 updateParams();
	 }
 }

 void RobosubMotorControl::Run()
 {
	PX4_INFO("RobosubMotorControl::Run()");
	 if (should_exit()) {
		//  _vehicle_attitude_sub.unregisterCallback();
		 exit_and_cleanup();
		 return;
	 }

	 perf_begin(_loop_perf);

	 /* check vehicle control mode for changes to publication state */
	//  _vcontrol_mode_sub.update(&_vcontrol_mode);
	_vcontrol_mode_sub.update(&_vcontrol_mode);

	 /* update parameters from storage */
	 parameters_update();

	 PX4_INFO("rc data");
	 // Check if the input_rc topic has been updated
	 // and copy the data to the local variable
	 if (_input_rc_sub.update(&_input_rc)) {
		input_rc_s rc_data {};
		_input_rc_sub.copy(&rc_data);
		// Debug print the rc data
		for (unsigned i = 0; i < rc_data.channel_count; ++i) {
			PX4_INFO("rc_data[%u]: %d", i, rc_data.values[i]);
	 	}

		// Normalize the rc data to a value between -1 and 1
		float normalized = (rc_data.values[1] - 1500) / 400.0f;
		
		// if (_vcontrol_mode.flag_control_manual_enabled ||
		// 	_vcontrol_mode.flag_control_attitude_enabled) {

		// 	_vehicle_thrust_setpoint.timestamp = hrt_absolute_time();
		// 	_vehicle_thrust_setpoint.timestamp_sample = 0.f;
		// 	_vehicle_thrust_setpoint.xyz[0] = normalized;
		// 	_vehicle_thrust_setpoint.xyz[1] = normalized;
		// 	_vehicle_thrust_setpoint.xyz[2] = normalized;

		// 	_vehicle_torque_setpoint.timestamp = hrt_absolute_time();
		// 	_vehicle_torque_setpoint.timestamp_sample = 0.f;
		// 	_vehicle_torque_setpoint.xyz[0] = normalized;
		// 	_vehicle_torque_setpoint.xyz[1] = normalized;
		// 	_vehicle_torque_setpoint.xyz[2] = normalized;

		// 	_vehicle_thrust_setpoint_pub.publish(_vehicle_thrust_setpoint);
		// 	_vehicle_torque_setpoint_pub.publish(_vehicle_torque_setpoint);
		// 	PX4_INFO("Published setpoints");
		// }
		// TODO_RS: Why function 101?
		int function = 101;
		float value = normalized;
		// TODO_RS: Figure out if its possible to do this without a test function
		actuator_test(function, value, 0, false);

	}
	 perf_end(_loop_perf);
 }

 void RobosubMotorControl::actuator_test(int function, float value, int timeout_ms, bool release_control)
{
	actuator_test_s actuator_test{};
	actuator_test.timestamp = hrt_absolute_time();
	actuator_test.function = function;
	actuator_test.value = value;
	actuator_test.action = release_control ? actuator_test_s::ACTION_RELEASE_CONTROL : actuator_test_s::ACTION_DO_CONTROL;
	actuator_test.timeout_ms = timeout_ms;

	uORB::Publication<actuator_test_s> actuator_test_pub{ORB_ID(actuator_test)};
	actuator_test_pub.publish(actuator_test);
}

 int RobosubMotorControl::task_spawn(int argc, char *argv[])
 {
	RobosubMotorControl *instance = new RobosubMotorControl();

	 if (instance) {
		 _object.store(instance);
		 _task_id = task_id_is_work_queue;

		 if (instance->init()) {
			 return PX4_OK;
		 }

	 } else {
		 PX4_ERR("alloc failed");
	 }

	 delete instance;
	 _object.store(nullptr);
	 _task_id = -1;

	 return PX4_ERROR;
 }

 int RobosubMotorControl::custom_command(int argc, char *argv[])
 {
	 return print_usage("unknown command");
 }


 int RobosubMotorControl::print_usage(const char *reason)
 {
	 if (reason) {
		 PX4_WARN("%s\n", reason);
	 }

	 PRINT_MODULE_DESCRIPTION(
		 R"DESCR_STR(
 ### Description
 Controls the attitude of an unmanned underwater vehicle (UUV).

 Publishes `vehicle_thrust_setpont` and `vehicle_torque_setpoint` messages at a constant 250Hz.

 ### Implementation
 Currently, this implementation supports only a few modes:

  * Full manual: Roll, pitch, yaw, and throttle controls are passed directly through to the actuators
  * Auto mission: The uuv runs missions

 ### Examples
 CLI usage example:
 $ rs_motor_control start
 $ rs_motor_control status
 $ rs_motor_control stop

 )DESCR_STR");

	 PRINT_MODULE_USAGE_NAME("_robosub_motor_control", "controller");
	 PRINT_MODULE_USAGE_COMMAND("start")
	 PRINT_MODULE_USAGE_DEFAULT_COMMANDS();

	 return 0;
 }

 int rs_motor_control_main(int argc, char *argv[])
 {
	 return RobosubMotorControl::main(argc, argv);
 }

Hi @MightyPiggie ,

I can confirm that you shouldn’t use the actuator test command as this freezes all the other channels.
The correct way is to send those torque and thrust setpoints as you have. However, you also need to make sure no other module is sending them. Also, the vehicle needs to be armed to actuate motors. For the servos, they can be moved when disarmed if you have COM_PREARM_MODE set to “disabled”.
You can check in the mavlink shell (in QGC) if the thrust/torque setpoints are correctly sent by sending the command: listener vehicle_torque_setpoint and listener vehicle_thrust_setpoint.

Finally, you need to set the timestamp_sample to hrt_absolute_time(), not 0, otherwise the control allocator might think the samples are outdated.

Hey @bresch

First of thank you for helping me understand PX4 better!

As you can see in the cpp file, I have a piece of commented code which does send vehicle_torque_setpoint and vehicle_thrust_setpoint message’s.

if (_vcontrol_mode.flag_control_manual_enabled ||
	_vcontrol_mode.flag_control_attitude_enabled) {

	_vehicle_thrust_setpoint.timestamp = hrt_absolute_time();
	_vehicle_thrust_setpoint.timestamp_sample = 0.f;
	_vehicle_thrust_setpoint.xyz[0] = normalized;
	_vehicle_thrust_setpoint.xyz[1] = normalized;
	_vehicle_thrust_setpoint.xyz[2] = normalized;

	_vehicle_torque_setpoint.timestamp = hrt_absolute_time();
	_vehicle_torque_setpoint.timestamp_sample = 0.f;
	_vehicle_torque_setpoint.xyz[0] = normalized;
	_vehicle_torque_setpoint.xyz[1] = normalized;
	_vehicle_torque_setpoint.xyz[2] = normalized;

	_vehicle_thrust_setpoint_pub.publish(_vehicle_thrust_setpoint);
	_vehicle_torque_setpoint_pub.publish(_vehicle_torque_setpoint);
	PX4_INFO("Published setpoints");
	 }

If I uncomment this code and run it I get the following output in dmesg:

INFO  [rs_motor_control] RobosubMotorControl::Run()
INFO  [rs_motor_control] rc data
INFO  [rs_motor_control] rc_data[0]: 1487
INFO  [rs_motor_control] rc_data[1]: 1802
INFO  [rs_motor_control] rc_data[2]: 1160
INFO  [rs_motor_control] rc_data[3]: 1467
INFO  [rs_motor_control] rc_data[4]: 1102
INFO  [rs_motor_control] rc_data[5]: 1592
INFO  [rs_motor_control] rc_data[6]: 1500
INFO  [rs_motor_control] rc_data[7]: 0
INFO  [rs_motor_control] Published setpoints

So they are being published. So if I listen to the topic vehicle_torque_setpoint and vehicle_thrust_setpoint I should be able to see the setpoints right? So I run the following command:

Insh> listener vehicle_torque_setpoint

TOPIC: vehicle_torque_setpoint
 vehicle_torque_setpoint
    timestamp: 692052509 (0.006206 seconds ago)
    timestamp_sample: 0
    xyz: [0.00453, -0.05322, 0.24525]
nsh> listener vehicle_thrust_setpoint

TOPIC: vehicle_thrust_setpoint
 vehicle_thrust_setpoint
    timestamp: 723989368 (0.005338 seconds ago)
    timestamp_sample: 0
    xyz: [0.00000, 0.00000, 0.00000]

But as you said no other modules can send messages to these topics so I checked if there are no modules and noticed that uuv_att_control is active and it is sending messages to these topics. So I disabled the uuv_att_control module and ran the code again. This time I get the following output in dmesg:

nsh> uuv_att_control status
INFO  [uuv_att_control] running
nsh> uuv_att_control stop
nsh> uuv_att_control status
INFO  [uuv_att_control] not running
nsh> listener vehicle_thrust_setpoint
never published
nsh> listener vehicle_torque_setpoint
never published

So for some reason even though I am calling the publish function the messages are not being published. I am not sure what is going wrong here?

Lastly the reason I used 0 as the timestamp_sample is because I followed what uuv_att_control does. uuv_att_control.cpp Is this also wrong?