From fa4189f66941591a9402ca8d99500a6cee45a2e3 Mon Sep 17 00:00:00 2001 From: Pedro-Roque Date: Wed, 5 Feb 2025 17:18:45 +0100 Subject: [PATCH] rft: spacecraft into a single module with multiple controllers --- ROMFS/px4fmu_common/init.d/CMakeLists.txt | 2 +- boards/px4/sitl/spacecraft.px4board | 2 +- .../sc_control_allocator/ControlAllocator.cpp | 91 ----- src/modules/sc_control_allocator/Kconfig | 12 - .../CMakeLists.txt | 8 +- src/modules/spacecraft/Kconfig | 12 + src/modules/spacecraft/SpacecraftHandler.cpp | 340 ++++++++++++++++++ .../SpacecraftHandler.hpp} | 64 +++- .../module.yaml | 0 9 files changed, 413 insertions(+), 118 deletions(-) delete mode 100644 src/modules/sc_control_allocator/ControlAllocator.cpp delete mode 100644 src/modules/sc_control_allocator/Kconfig rename src/modules/{sc_control_allocator => spacecraft}/CMakeLists.txt (94%) create mode 100644 src/modules/spacecraft/Kconfig create mode 100644 src/modules/spacecraft/SpacecraftHandler.cpp rename src/modules/{sc_control_allocator/ControlAllocator.hpp => spacecraft/SpacecraftHandler.hpp} (56%) rename src/modules/{sc_control_allocator => spacecraft}/module.yaml (100%) diff --git a/ROMFS/px4fmu_common/init.d/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/CMakeLists.txt index e9b777fcdd..7c6f8f4369 100644 --- a/ROMFS/px4fmu_common/init.d/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/CMakeLists.txt @@ -92,7 +92,7 @@ if(CONFIG_MODULES_ROVER_ACKERMANN) ) endif() -if(CONFIG_MODULES_SC_CONTROL_ALLOCATOR) +if(CONFIG_MODULES_SPACECRAFT) px4_add_romfs_files( rc.sc_apps rc.sc_defaults diff --git a/boards/px4/sitl/spacecraft.px4board b/boards/px4/sitl/spacecraft.px4board index 99f75f302e..a5c735f328 100644 --- a/boards/px4/sitl/spacecraft.px4board +++ b/boards/px4/sitl/spacecraft.px4board @@ -14,4 +14,4 @@ CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_MODULES_DIFFERENTIAL_DRIVE=n CONFIG_EKF2_AUX_GLOBAL_POSITION=y CONFIG_MODULES_CONTROL_ALLOCATOR=n -CONFIG_MODULES_SC_CONTROL_ALLOCATOR=y +CONFIG_MODULES_SPACECRAFT=y diff --git a/src/modules/sc_control_allocator/ControlAllocator.cpp b/src/modules/sc_control_allocator/ControlAllocator.cpp deleted file mode 100644 index 01f7afbcca..0000000000 --- a/src/modules/sc_control_allocator/ControlAllocator.cpp +++ /dev/null @@ -1,91 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013-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. - * - ****************************************************************************/ - -/** - * @file ControlAllocator.cpp - * - * Control allocator. - * - * @author Julien Lecoeur - */ - -#include "ControlAllocator.hpp" - -int ControlAllocator::task_spawn(int argc, char *argv[]) -{ - return 0; -} - -int ControlAllocator::print_status() -{ - PX4_INFO("Running"); - - return 0; -} - -int ControlAllocator::custom_command(int argc, char *argv[]) -{ - return print_usage("unknown command"); -} - -int ControlAllocator::print_usage(const char *reason) -{ - if (reason) { - PX4_WARN("%s\n", reason); - } - - PRINT_MODULE_DESCRIPTION( - R"DESCR_STR( - ### Description - This implements control allocation for spacecraft vehicles. - It takes torque and thrust setpoints as inputs and outputs - actuator setpoint messages. - )DESCR_STR" - ); - - PRINT_MODULE_USAGE_NAME("sc_control_allocator", "controller"); - PRINT_MODULE_USAGE_COMMAND("start"); - PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); - - return 0; -} - -/** - * Control Allocator app start / stop handling function - */ -extern "C" __EXPORT int sc_control_allocator_main(int argc, char *argv[]); - -int sc_control_allocator_main(int argc, char *argv[]) -{ - return ControlAllocator::main(argc, argv); -} diff --git a/src/modules/sc_control_allocator/Kconfig b/src/modules/sc_control_allocator/Kconfig deleted file mode 100644 index 867b560463..0000000000 --- a/src/modules/sc_control_allocator/Kconfig +++ /dev/null @@ -1,12 +0,0 @@ -menuconfig MODULES_SC_CONTROL_ALLOCATOR - bool "sc_control_allocator" - default n - ---help--- - Enable support for control_allocator - -menuconfig USER_SC_CONTROL_ALLOCATOR - bool "control_allocator running as userspace module" - default y - depends on BOARD_PROTECTED && MODULES_SC_CONTROL_ALLOCATOR - ---help--- - Put sc_control_allocator in userspace memory diff --git a/src/modules/sc_control_allocator/CMakeLists.txt b/src/modules/spacecraft/CMakeLists.txt similarity index 94% rename from src/modules/sc_control_allocator/CMakeLists.txt rename to src/modules/spacecraft/CMakeLists.txt index eae5d85aaa..88a18e9c01 100644 --- a/src/modules/sc_control_allocator/CMakeLists.txt +++ b/src/modules/spacecraft/CMakeLists.txt @@ -34,15 +34,15 @@ include_directories(${CMAKE_CURRENT_SOURCE_DIR}) px4_add_module( - MODULE modules__sc_control_allocator - MAIN sc_control_allocator + MODULE modules__spacecraft + MAIN spacecraft COMPILE_FLAGS ${MAX_CUSTOM_OPT_LEVEL} STACK_MAIN 3000 SRCS - ControlAllocator.cpp - ControlAllocator.hpp + SpacecraftHandler.cpp + SpacecraftHandler.hpp MODULE_CONFIG module.yaml DEPENDS diff --git a/src/modules/spacecraft/Kconfig b/src/modules/spacecraft/Kconfig new file mode 100644 index 0000000000..1424606226 --- /dev/null +++ b/src/modules/spacecraft/Kconfig @@ -0,0 +1,12 @@ +menuconfig MODULES_SPACECRAFT + bool "spacecraft" + default n + ---help--- + Enable support for spacecraft + +menuconfig USER_SPACECRAFT + bool "spacecraft running as userspace module" + default y + depends on BOARD_PROTECTED && MODULES_SPACECRAFT + ---help--- + Put spacecraft in userspace memory diff --git a/src/modules/spacecraft/SpacecraftHandler.cpp b/src/modules/spacecraft/SpacecraftHandler.cpp new file mode 100644 index 0000000000..c7cccd1e6d --- /dev/null +++ b/src/modules/spacecraft/SpacecraftHandler.cpp @@ -0,0 +1,340 @@ +/**************************************************************************** + * + * Copyright (c) 2013-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. + * + ****************************************************************************/ + +/** + * @file SpacecraftHandler.cpp + * + * Control allocator. + * + * @author Julien Lecoeur + */ + +#include "SpacecraftHandler.hpp" + + +SpacecraftHandler::SpacecraftHandler() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl) +{ + updateParams(); + _rover_differential_setpoint_pub.advertise(); +} + +bool SpacecraftHandler::init() +{ + ScheduleOnInterval(10_ms); // 100 Hz + return true; +} + +void SpacecraftHandler::updateParams() +{ + ModuleParams::updateParams(); + _max_yaw_rate = _param_rd_max_yaw_rate.get() * M_DEG_TO_RAD_F; +} + +void SpacecraftHandler::Run() +{ + if (should_exit()) { + ScheduleClear(); + exit_and_cleanup(); + return; + } + + updateSubscriptions(); + + // Generate and publish attitude, rate and speed setpoints + hrt_abstime timestamp = hrt_absolute_time(); + + switch (_nav_state) { + case vehicle_status_s::NAVIGATION_STATE_MANUAL: { + manual_control_setpoint_s manual_control_setpoint{}; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + rover_differential_setpoint_s rover_differential_setpoint{}; + rover_differential_setpoint.timestamp = timestamp; + rover_differential_setpoint.forward_speed_setpoint = NAN; + rover_differential_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle; + rover_differential_setpoint.yaw_setpoint = NAN; + + if (_max_yaw_rate > FLT_EPSILON && _param_rd_max_thr_yaw_r.get() > FLT_EPSILON) { + const float scaled_yaw_rate_input = math::interpolate(manual_control_setpoint.roll, + -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); + const float speed_diff = scaled_yaw_rate_input * _param_rd_wheel_track.get() / 2.f; + rover_differential_setpoint.speed_diff_setpoint_normalized = math::interpolate(speed_diff, + -_param_rd_max_thr_yaw_r.get(), _param_rd_max_thr_yaw_r.get(), -1.f, 1.f); + + } else { + rover_differential_setpoint.speed_diff_setpoint_normalized = manual_control_setpoint.roll; + + } + + rover_differential_setpoint.yaw_rate_setpoint = NAN; + _rover_differential_setpoint_pub.publish(rover_differential_setpoint); + } + } break; + + case vehicle_status_s::NAVIGATION_STATE_ACRO: { + manual_control_setpoint_s manual_control_setpoint{}; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + rover_differential_setpoint_s rover_differential_setpoint{}; + rover_differential_setpoint.timestamp = timestamp; + rover_differential_setpoint.forward_speed_setpoint = NAN; + rover_differential_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle; + rover_differential_setpoint.yaw_rate_setpoint = math::interpolate(manual_control_setpoint.roll, + -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); + rover_differential_setpoint.speed_diff_setpoint_normalized = NAN; + rover_differential_setpoint.yaw_setpoint = NAN; + _rover_differential_setpoint_pub.publish(rover_differential_setpoint); + } + + } break; + + case vehicle_status_s::NAVIGATION_STATE_STAB: { + manual_control_setpoint_s manual_control_setpoint{}; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + rover_differential_setpoint_s rover_differential_setpoint{}; + rover_differential_setpoint.timestamp = timestamp; + rover_differential_setpoint.forward_speed_setpoint = NAN; + rover_differential_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle; + rover_differential_setpoint.yaw_rate_setpoint = math::interpolate(math::deadzone(manual_control_setpoint.roll, + STICK_DEADZONE), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); + rover_differential_setpoint.speed_diff_setpoint_normalized = NAN; + rover_differential_setpoint.yaw_setpoint = NAN; + + if (fabsf(rover_differential_setpoint.yaw_rate_setpoint) > FLT_EPSILON + || fabsf(rover_differential_setpoint.forward_speed_setpoint_normalized) < FLT_EPSILON) { // Closed loop yaw rate control + _yaw_ctl = false; + + + } else { // Closed loop yaw control if the yaw rate input is zero (keep current yaw) + if (!_yaw_ctl) { + _stab_desired_yaw = _vehicle_yaw; + _yaw_ctl = true; + } + + rover_differential_setpoint.yaw_setpoint = _stab_desired_yaw; + rover_differential_setpoint.yaw_rate_setpoint = NAN; + + } + + _rover_differential_setpoint_pub.publish(rover_differential_setpoint); + } + + } break; + + case vehicle_status_s::NAVIGATION_STATE_POSCTL: { + manual_control_setpoint_s manual_control_setpoint{}; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + rover_differential_setpoint_s rover_differential_setpoint{}; + rover_differential_setpoint.timestamp = timestamp; + rover_differential_setpoint.forward_speed_setpoint = math::interpolate(manual_control_setpoint.throttle, + -1.f, 1.f, -_param_rd_max_speed.get(), _param_rd_max_speed.get()); + rover_differential_setpoint.forward_speed_setpoint_normalized = NAN; + rover_differential_setpoint.yaw_rate_setpoint = math::interpolate(math::deadzone(manual_control_setpoint.roll, + STICK_DEADZONE), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); + rover_differential_setpoint.speed_diff_setpoint_normalized = NAN; + rover_differential_setpoint.yaw_setpoint = NAN; + + if (fabsf(rover_differential_setpoint.yaw_rate_setpoint) > FLT_EPSILON + || fabsf(rover_differential_setpoint.forward_speed_setpoint) < FLT_EPSILON) { // Closed loop yaw rate control + _yaw_ctl = false; + + + } else { // Course control if the yaw rate input is zero (keep driving on a straight line) + if (!_yaw_ctl) { + _pos_ctl_course_direction = Vector2f(cos(_vehicle_yaw), sin(_vehicle_yaw)); + _pos_ctl_start_position_ned = _curr_pos_ned; + _yaw_ctl = true; + } + + // Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover + const float vector_scaling = sqrtf(powf(_param_pp_lookahd_max.get(), + 2) + powf(_posctl_pure_pursuit.getCrosstrackError(), 2)) + _posctl_pure_pursuit.getDistanceOnLineSegment(); + const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + sign( + rover_differential_setpoint.forward_speed_setpoint) * + vector_scaling * _pos_ctl_course_direction; + // Calculate yaw setpoint + const float yaw_setpoint = _posctl_pure_pursuit.calcDesiredHeading(target_waypoint_ned, + _pos_ctl_start_position_ned, _curr_pos_ned, fabsf(_vehicle_forward_speed)); + rover_differential_setpoint.yaw_setpoint = sign(rover_differential_setpoint.forward_speed_setpoint) >= 0 ? + yaw_setpoint : matrix::wrap_pi(M_PI_F + yaw_setpoint); // Flip yaw setpoint when driving backwards + rover_differential_setpoint.yaw_rate_setpoint = NAN; + + } + + _rover_differential_setpoint_pub.publish(rover_differential_setpoint); + } + + } break; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: + case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: + _rover_differential_guidance.computeGuidance(_vehicle_yaw, _vehicle_forward_speed, _nav_state); + break; + + default: // Unimplemented nav states will stop the rover + _rover_differential_control.resetControllers(); + _yaw_ctl = false; + rover_differential_setpoint_s rover_differential_setpoint{}; + rover_differential_setpoint.forward_speed_setpoint = NAN; + rover_differential_setpoint.forward_speed_setpoint_normalized = 0.f; + rover_differential_setpoint.yaw_rate_setpoint = NAN; + rover_differential_setpoint.speed_diff_setpoint_normalized = 0.f; + rover_differential_setpoint.yaw_setpoint = NAN; + _rover_differential_setpoint_pub.publish(rover_differential_setpoint); + break; + } + + if (!_armed) { // Reset when disarmed + _rover_differential_control.resetControllers(); + _yaw_ctl = false; + } + + _rover_differential_control.computeMotorCommands(_vehicle_yaw, _vehicle_yaw_rate, _vehicle_forward_speed); + +} + +void SpacecraftHandler::updateSubscriptions() +{ + + if (_parameter_update_sub.updated()) { + parameter_update_s parameter_update; + _parameter_update_sub.copy(¶meter_update); + updateParams(); + } + + if (_vehicle_status_sub.updated()) { + vehicle_status_s vehicle_status{}; + _vehicle_status_sub.copy(&vehicle_status); + + if (vehicle_status.nav_state != _nav_state) { // Reset on mode change + _rover_differential_control.resetControllers(); + _yaw_ctl = false; + } + + _nav_state = vehicle_status.nav_state; + _armed = vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED; + } + + if (_vehicle_angular_velocity_sub.updated()) { + vehicle_angular_velocity_s vehicle_angular_velocity{}; + _vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity); + _vehicle_yaw_rate = fabsf(vehicle_angular_velocity.xyz[2]) > YAW_RATE_THRESHOLD ? vehicle_angular_velocity.xyz[2] : 0.f; + } + + if (_vehicle_attitude_sub.updated()) { + vehicle_attitude_s vehicle_attitude{}; + _vehicle_attitude_sub.copy(&vehicle_attitude); + _vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q); + _vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi(); + } + + if (_vehicle_local_position_sub.updated()) { + vehicle_local_position_s vehicle_local_position{}; + _vehicle_local_position_sub.copy(&vehicle_local_position); + _curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y); + Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz); + Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame); + _vehicle_forward_speed = fabsf(velocity_in_body_frame(0)) > SPEED_THRESHOLD ? velocity_in_body_frame(0) : 0.f; + } +} + +int SpacecraftHandler::task_spawn(int argc, char *argv[]) +{ + SpacecraftHandler *instance = new SpacecraftHandler(); + + 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 SpacecraftHandler::print_status() +{ + PX4_INFO("Running"); + + return 0; +} + +int SpacecraftHandler::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int SpacecraftHandler::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( + ### Description + This implements control allocation for spacecraft vehicles. + It takes torque and thrust setpoints as inputs and outputs + actuator setpoint messages. + )DESCR_STR" + ); + + PRINT_MODULE_USAGE_NAME("sc_control_allocator", "controller"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +/** + * Control Allocator app start / stop handling function + */ +extern "C" __EXPORT int spacecraft_main(int argc, char *argv[]); + +int spacecraft_main(int argc, char *argv[]) +{ + return SpacecraftHandler::main(argc, argv); +} diff --git a/src/modules/sc_control_allocator/ControlAllocator.hpp b/src/modules/spacecraft/SpacecraftHandler.hpp similarity index 56% rename from src/modules/sc_control_allocator/ControlAllocator.hpp rename to src/modules/spacecraft/SpacecraftHandler.hpp index 210b39d979..a0308e8fe3 100644 --- a/src/modules/sc_control_allocator/ControlAllocator.hpp +++ b/src/modules/spacecraft/SpacecraftHandler.hpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file ControlAllocator.hpp + * @file SpacecraftHandler.hpp * * Control allocator. * @@ -62,13 +62,20 @@ #include #include -class ControlAllocator : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem +// Constants +static constexpr float STICK_DEADZONE = + 0.1f; // [0, 1] Percentage of stick input range that will be interpreted as zero around the stick centered value +static constexpr float YAW_RATE_THRESHOLD = + 0.02f; // [rad/s] The minimum threshold for the yaw rate measurement not to be interpreted as zero +static constexpr float SPEED_THRESHOLD = + 0.1f; // [m/s] The minimum threshold for the speed measurement not to be interpreted as zero + +class SpacecraftHandler : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { + public: - - ControlAllocator(); - - virtual ~ControlAllocator(); + SpacecraftHandler(); + ~SpacecraftHandler() override = default; /** @see ModuleBase */ static int task_spawn(int argc, char *argv[]); @@ -79,9 +86,48 @@ public: /** @see ModuleBase */ static int print_usage(const char *reason = nullptr); - /** @see ModuleBase::print_status() */ - int print_status() override; + bool init(); + +protected: + void updateParams() override; + +private: + void Run() override; + + /** + * @brief Update uORB subscriptions. + */ + void updateSubscriptions(); + + // uORB Subscriptions + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; + uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + + // uORB Publications + uORB::Publication _rover_differential_setpoint_pub{ORB_ID(rover_differential_setpoint)}; + + // Instances + SpacecraftHandlerGuidance _rover_differential_guidance{this}; + SpacecraftHandlerControl _rover_differential_control{this}; + PurePursuit _posctl_pure_pursuit{this}; // Pure pursuit library + + // Variables + Vector2f _curr_pos_ned{}; + matrix::Quatf _vehicle_attitude_quaternion{}; + float _vehicle_yaw_rate{0.f}; + float _vehicle_forward_speed{0.f}; + float _vehicle_yaw{0.f}; + float _max_yaw_rate{0.f}; + int _nav_state{0}; + bool _armed{false}; + bool _yaw_ctl{false}; // Indicates if the rover is doing yaw or yaw rate control in Stabilized and Position mode + float _stab_desired_yaw{0.f}; // Yaw setpoint for Stabilized mode + Vector2f _pos_ctl_course_direction{}; // Course direction for Position mode + Vector2f _pos_ctl_start_position_ned{}; // Initial rover position for course control in Position mode -private: /**< loop duration performance counter */ }; diff --git a/src/modules/sc_control_allocator/module.yaml b/src/modules/spacecraft/module.yaml similarity index 100% rename from src/modules/sc_control_allocator/module.yaml rename to src/modules/spacecraft/module.yaml