diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index 8158ab8b03..d3cae81179 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -79,3 +79,5 @@ mc_pos_control start # Start Multicopter Land Detector. # land_detector start multicopter + +offboard_switch start diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index 51f3e092f1..87193eb977 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -52,6 +52,7 @@ CONFIG_MODULES_UUV_ATT_CONTROL=y CONFIG_MODULES_UUV_POS_CONTROL=y CONFIG_MODULES_UXRCE_DDS_CLIENT=y CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_MODULES_OFFBOARD_SWITCH=y CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y CONFIG_SYSTEMCMDS_BSONDUMP=y CONFIG_SYSTEMCMDS_DYN=y diff --git a/msg/TrajectorySetpoint.msg b/msg/TrajectorySetpoint.msg index 4a88c86769..e122655050 100644 --- a/msg/TrajectorySetpoint.msg +++ b/msg/TrajectorySetpoint.msg @@ -13,3 +13,5 @@ float32[3] jerk # in meters/second^3 (for logging only) float32 yaw # euler angle of desired attitude in radians -PI..+PI float32 yawspeed # angular velocity around NED frame z-axis in radians/second + +# TOPICS trajectory_setpoint offboard_trajectory_setpoint diff --git a/msg/VehicleAttitudeSetpoint.msg b/msg/VehicleAttitudeSetpoint.msg index e4f66c446a..323c00dc5a 100644 --- a/msg/VehicleAttitudeSetpoint.msg +++ b/msg/VehicleAttitudeSetpoint.msg @@ -17,4 +17,4 @@ bool reset_integral # Reset roll/pitch/yaw integrals (navigation logic change) bool fw_control_yaw_wheel # control heading with steering wheel (used for auto takeoff on runway) -# TOPICS vehicle_attitude_setpoint mc_virtual_attitude_setpoint fw_virtual_attitude_setpoint +# TOPICS vehicle_attitude_setpoint mc_virtual_attitude_setpoint fw_virtual_attitude_setpoint offboard_attitude_setpoint diff --git a/msg/VehicleRatesSetpoint.msg b/msg/VehicleRatesSetpoint.msg index 88adcf3bea..2da4049b62 100644 --- a/msg/VehicleRatesSetpoint.msg +++ b/msg/VehicleRatesSetpoint.msg @@ -10,3 +10,5 @@ float32 yaw # [rad/s] yaw rate setpoint float32[3] thrust_body # Normalized thrust command in body NED frame [-1,1] bool reset_integral # Reset roll/pitch/yaw integrals (navigation logic change) + +# TOPICS vehicle_rates_setpoint offboard_rates_setpoint diff --git a/src/modules/offboard_switch/CMakeLists.txt b/src/modules/offboard_switch/CMakeLists.txt new file mode 100644 index 0000000000..9b05a9db41 --- /dev/null +++ b/src/modules/offboard_switch/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_module( + MODULE modules__offboard_switch + MAIN offboard_switch + SRCS + offboard_switch.cpp + offboard_switch.hpp + DEPENDS + px4_work_queue + ) diff --git a/src/modules/offboard_switch/Kconfig b/src/modules/offboard_switch/Kconfig new file mode 100644 index 0000000000..8e657958c7 --- /dev/null +++ b/src/modules/offboard_switch/Kconfig @@ -0,0 +1,12 @@ +menuconfig MODULES_OFFBOARD_SWITCH + bool "offboard_switch" + default n + ---help--- + Enable support for fw_att_control + +menuconfig USER_MODULES_OFFBOARD_SWITCH + bool "fw_rate_control running as userspace module" + default n + depends on BOARD_PROTECTED + ---help--- + Put offboard_switch in userspace memory diff --git a/src/modules/offboard_switch/offboard_switch.cpp b/src/modules/offboard_switch/offboard_switch.cpp new file mode 100644 index 0000000000..bcb6cdf58b --- /dev/null +++ b/src/modules/offboard_switch/offboard_switch.cpp @@ -0,0 +1,228 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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 "offboard_switch.hpp" + +using namespace time_literals; +using namespace matrix; + +OffboardSwitch::OffboardSwitch() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers), + _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) +{ + /* fetch initial parameter values */ + parameters_update(); +} + +OffboardSwitch::~OffboardSwitch() +{ + perf_free(_loop_perf); +} + +bool +OffboardSwitch::init() +{ + if (!_vehicle_angular_velocity_sub.registerCallback()) { + PX4_ERR("callback registration failed"); + return false; + } + + return true; +} + +int +OffboardSwitch::parameters_update() +{ + + return PX4_OK; +} + +// void OffboardSwitch::trajectorySetpointCheck() { +// if () +// } + +void OffboardSwitch::Run() +{ + if (should_exit()) { + _vehicle_angular_velocity_sub.unregisterCallback(); + exit_and_cleanup(); + return; + } + + perf_begin(_loop_perf); + + // only run controller if angular velocity changed + + ///TODO: Timeout if setpoints fall out of sync + ///TODO: Package offboard control mode messages to the correct level, depending on the setpoint + offboard_control_mode_s ocm{}; + trajectory_setpoint_s trajectory_setpoint; + + vehicle_status_s vehicle_status{}; + _vehicle_status_sub.copy(&vehicle_status); + + bool data_is_recent{false}; + + if (_offboard_trajectory_setpoint_sub.update(&trajectory_setpoint)) { + data_is_recent = data_is_recent || (hrt_absolute_time() < trajectory_setpoint.timestamp + + static_cast(_param_com_of_loss_t.get() * 1_s)); + + if (data_is_recent) { + if (PX4_ISFINITE(trajectory_setpoint.position[0]) && PX4_ISFINITE(trajectory_setpoint.position[1]) + && PX4_ISFINITE(trajectory_setpoint.position[2])) { + ocm.position = true; + } + + if (PX4_ISFINITE(trajectory_setpoint.velocity[0]) && PX4_ISFINITE(trajectory_setpoint.velocity[1]) + && PX4_ISFINITE(trajectory_setpoint.velocity[2])) { + ocm.velocity = true; + } + + if (PX4_ISFINITE(trajectory_setpoint.acceleration[0]) && PX4_ISFINITE(trajectory_setpoint.acceleration[1]) + && PX4_ISFINITE(trajectory_setpoint.acceleration[2])) { + ocm.acceleration = true; + } + + if ((ocm.position || ocm.velocity || ocm.acceleration) + && (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD)) { + _trajectory_setpoint_pub.publish(trajectory_setpoint); + } + } + } + + vehicle_attitude_setpoint_s attitude_setpoint; + + if (_offboard_vehicle_attitude_setpoint_sub.update(&attitude_setpoint)) { + data_is_recent = data_is_recent || (hrt_absolute_time() < attitude_setpoint.timestamp + + static_cast(_param_com_of_loss_t.get() * 1_s)); + PX4_INFO("Data is recent: %f", double(data_is_recent)); + + if (data_is_recent) { + if (PX4_ISFINITE(attitude_setpoint.q_d[0]) && PX4_ISFINITE(attitude_setpoint.q_d[1]) + && PX4_ISFINITE(attitude_setpoint.q_d[2]) && PX4_ISFINITE(attitude_setpoint.q_d[3])) { + ocm.attitude = true; + } + + if (ocm.attitude + && (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD)) { + _vehicle_attitude_setpoint_pub.publish(attitude_setpoint); + } + } + } + + vehicle_rates_setpoint_s rates_setpoint; + + if (_offboard_vehicle_rates_setpoint_sub.update(&rates_setpoint)) { + data_is_recent = data_is_recent || (hrt_absolute_time() < rates_setpoint.timestamp + + static_cast(_param_com_of_loss_t.get() * 1_s)); + + if (data_is_recent) { + if (PX4_ISFINITE(rates_setpoint.roll) && PX4_ISFINITE(rates_setpoint.pitch) + && PX4_ISFINITE(rates_setpoint.yaw)) { + ocm.body_rate = true; + } + + if (ocm.body_rate + && (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD)) { + _vehicle_rates_setpoint_pub.publish(rates_setpoint); + } + } + } + + ///TODO: Handle corner cases where ill-posed setpoints are mixed + bool valid_position_setpoint = ocm.position || ocm.velocity || ocm.acceleration; + bool valid_attitude_setpoint = ocm.attitude || ocm.body_rate; + bool valid_actuator_setpooint = ocm.thrust_and_torque || ocm.direct_actuator; + + if (data_is_recent && (valid_position_setpoint || valid_attitude_setpoint || valid_actuator_setpooint)) { + // publish offboard_control_mode + ocm.timestamp = hrt_absolute_time(); + _offboard_control_mode_pub.publish(ocm); + } + + perf_end(_loop_perf); +} + +int OffboardSwitch::task_spawn(int argc, char *argv[]) +{ + OffboardSwitch *instance = new OffboardSwitch(); + + 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 OffboardSwitch::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int OffboardSwitch::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +offboard_switch filters and regulated offboard setpoints. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("offboard_switch", "controller"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int offboard_switch_main(int argc, char *argv[]) +{ + return OffboardSwitch::main(argc, argv); +} diff --git a/src/modules/offboard_switch/offboard_switch.hpp b/src/modules/offboard_switch/offboard_switch.hpp new file mode 100644 index 0000000000..84a9bc8de7 --- /dev/null +++ b/src/modules/offboard_switch/offboard_switch.hpp @@ -0,0 +1,114 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using matrix::Eulerf; +using matrix::Quatf; + +using uORB::SubscriptionData; + +using namespace time_literals; + +class OffboardSwitch final : public ModuleBase, public ModuleParams, + public px4::ScheduledWorkItem +{ +public: + OffboardSwitch(); + ~OffboardSwitch() override; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + bool init(); + +private: + void Run() override; + + uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)}; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::Publication _offboard_control_mode_pub{ORB_ID(offboard_control_mode)}; + uORB::Publication _trajectory_setpoint_pub{ORB_ID(trajectory_setpoint)}; + uORB::Publication _vehicle_attitude_setpoint_pub{ORB_ID(vehicle_attitude_setpoint)}; + uORB::Publication _vehicle_rates_setpoint_pub{ORB_ID(vehicle_rates_setpoint)}; + + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _offboard_trajectory_setpoint_sub{ORB_ID(offboard_trajectory_setpoint)}; + uORB::Subscription _offboard_vehicle_attitude_setpoint_sub{ORB_ID(offboard_attitude_setpoint)}; + uORB::Subscription _offboard_vehicle_rates_setpoint_sub{ORB_ID(offboard_rates_setpoint)}; + + perf_counter_t _loop_perf; + + DEFINE_PARAMETERS( + (ParamFloat) _param_com_of_loss_t + ) + + /** + * Update our local parameter cache. + */ + int parameters_update(); + + +}; diff --git a/src/modules/offboard_switch/offboard_switch_params.c b/src/modules/offboard_switch/offboard_switch_params.c new file mode 100644 index 0000000000..03621b3523 --- /dev/null +++ b/src/modules/offboard_switch/offboard_switch_params.c @@ -0,0 +1,40 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2023 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 offboard_switch_params.c + * + * Parameters defined by the offboard_switch + * + * @author Jaeyoung Lim + */ diff --git a/src/modules/uxrce_dds_client/dds_topics.yaml b/src/modules/uxrce_dds_client/dds_topics.yaml index e467c61929..4001ab442b 100644 --- a/src/modules/uxrce_dds_client/dds_topics.yaml +++ b/src/modules/uxrce_dds_client/dds_topics.yaml @@ -115,15 +115,24 @@ subscriptions: - topic: /fmu/in/trajectory_setpoint type: px4_msgs::msg::TrajectorySetpoint + - topic: /fmu/in/offboard_trajectory_setpoint + type: px4_msgs::msg::TrajectorySetpoint + - topic: /fmu/in/vehicle_attitude_setpoint type: px4_msgs::msg::VehicleAttitudeSetpoint + - topic: /fmu/in/offboard_attitude_setpoint + type: px4_msgs::msg::VehicleAttitudeSetpoint + - topic: /fmu/in/vehicle_mocap_odometry type: px4_msgs::msg::VehicleOdometry - topic: /fmu/in/vehicle_rates_setpoint type: px4_msgs::msg::VehicleRatesSetpoint + - topic: /fmu/in/offboard_rates_setpoint + type: px4_msgs::msg::VehicleRatesSetpoint + - topic: /fmu/in/vehicle_visual_odometry type: px4_msgs::msg::VehicleOdometry