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. 
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);
}