diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/71002_gz_spacecraft_2d b/ROMFS/px4fmu_common/init.d-posix/airframes/71002_gz_spacecraft_2d index 4cc0a02e413f..4396d9add04c 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/71002_gz_spacecraft_2d +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/71002_gz_spacecraft_2d @@ -20,7 +20,7 @@ param set-default COM_ARM_CHK_ESCS 0 # We don't have ESCs param set-default FD_ESCS_EN 0 # We don't have ESCs - but maybe we need this later? param set-default CA_AIRFRAME 14 -param set-default MAV_TYPE 99 +param set-default MAV_TYPE 7 # Using Airship param set-default CA_THRUSTER_CNT 8 param set-default CA_R_REV 0 diff --git a/ROMFS/px4fmu_common/init.d/airframes/70000_spacecraft_2d b/ROMFS/px4fmu_common/init.d/airframes/70000_spacecraft_2d index c0f55737f5ab..4bf69a47f63f 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/70000_spacecraft_2d +++ b/ROMFS/px4fmu_common/init.d/airframes/70000_spacecraft_2d @@ -11,7 +11,7 @@ . ${R}etc/init.d/rc.sc_defaults param set-default CA_AIRFRAME 14 -param set-default MAV_TYPE 99 +param set-default MAV_TYPE 7 param set-default CA_THRUSTER_CNT 8 param set-default CA_R_REV 0 diff --git a/ROMFS/px4fmu_common/init.d/rc.sc_defaults b/ROMFS/px4fmu_common/init.d/rc.sc_defaults index 8dc8867b222b..5892ea62e5f8 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sc_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.sc_defaults @@ -5,8 +5,8 @@ set VEHICLE_TYPE sc -# MAV_TYPE_QUADROTOR 2 -#param set-default MAV_TYPE 12 +# MAV_TYPE_SPACECRAFT +param set-default MAV_TYPE 7 # Set micro-dds-client to use ethernet and IP-address 192.168.0.1 param set-default UXRCE_DDS_AG_IP -1062731775 diff --git a/ROMFS/px4fmu_common/init.d/rc.vehicle_setup b/ROMFS/px4fmu_common/init.d/rc.vehicle_setup index ae34645ad890..b0557a31e582 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vehicle_setup +++ b/ROMFS/px4fmu_common/init.d/rc.vehicle_setup @@ -32,6 +32,15 @@ then . ${R}etc/init.d/rc.rover_apps fi +# +# Spapcecraft setup. +# +if [ $VEHICLE_TYPE = sc ] +then + # Start standard multicopter apps. + . ${R}etc/init.d/rc.sc_apps +fi + # # Differential Rover setup. # diff --git a/msg/versioned/VehicleStatus.msg b/msg/versioned/VehicleStatus.msg index 81bb3ba1c027..d98cf55b7d68 100644 --- a/msg/versioned/VehicleStatus.msg +++ b/msg/versioned/VehicleStatus.msg @@ -92,6 +92,7 @@ uint8 vehicle_type uint8 VEHICLE_TYPE_ROTARY_WING = 0 uint8 VEHICLE_TYPE_FIXED_WING = 1 uint8 VEHICLE_TYPE_ROVER = 2 +uint8 VEHICLE_TYPE_SPACECRAFT = 7 uint8 FAILSAFE_DEFER_STATE_DISABLED = 0 uint8 FAILSAFE_DEFER_STATE_ENABLED = 1 diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index cebc08ae9416..cce5e9b1cef7 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1734,6 +1734,9 @@ void Commander::updateParameters() } else if (is_ground) { _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROVER; + + } else if (is_spacecraft(_vehicle_status)) { + _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_SPACECRAFT; } _vehicle_status.is_vtol = is_vtol(_vehicle_status); diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 48c93982495a..aae8c7caac25 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -78,6 +78,7 @@ #define VEHICLE_TYPE_VTOL_TILTROTOR 21 #define VEHICLE_TYPE_VTOL_FIXEDROTOR 22 // VTOL standard #define VEHICLE_TYPE_VTOL_TAILSITTER 23 +#define VEHICLE_TYPE_SPACECRAFT 7 #define BLINK_MSG_TIME 700000 // 3 fast blinks (in us) @@ -122,6 +123,11 @@ bool is_ground_vehicle(const vehicle_status_s ¤t_status) return (current_status.system_type == VEHICLE_TYPE_BOAT || current_status.system_type == VEHICLE_TYPE_GROUND_ROVER); } +bool is_spacecraft(const vehicle_status_s ¤t_status) +{ + return (current_status.system_type == VEHICLE_TYPE_SPACECRAFT); +} + // End time for currently blinking LED message, 0 if no blink message static hrt_abstime blink_msg_end = 0; static int fd_leds{-1}; diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index 3e96a636e575..631025817f88 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -56,6 +56,7 @@ bool is_vtol(const vehicle_status_s ¤t_status); bool is_vtol_tailsitter(const vehicle_status_s ¤t_status); bool is_fixed_wing(const vehicle_status_s ¤t_status); bool is_ground_vehicle(const vehicle_status_s ¤t_status); +bool is_spacecraft(const vehicle_status_s ¤t_status); int buzzer_init(void); void buzzer_deinit(void); diff --git a/src/modules/spacecraft/CMakeLists.txt b/src/modules/spacecraft/CMakeLists.txt index 88a18e9c01ec..330385c9f0af 100644 --- a/src/modules/spacecraft/CMakeLists.txt +++ b/src/modules/spacecraft/CMakeLists.txt @@ -32,6 +32,7 @@ ############################################################################ include_directories(${CMAKE_CURRENT_SOURCE_DIR}) +add_subdirectory(SpacecraftRateControl) px4_add_module( MODULE modules__spacecraft @@ -48,4 +49,5 @@ px4_add_module( DEPENDS mathlib px4_work_queue + SpacecraftRateControl ) diff --git a/src/modules/spacecraft/SpacecraftHandler.cpp b/src/modules/spacecraft/SpacecraftHandler.cpp index 92cdf866557b..935380ba6818 100644 --- a/src/modules/spacecraft/SpacecraftHandler.cpp +++ b/src/modules/spacecraft/SpacecraftHandler.cpp @@ -34,16 +34,75 @@ /** * @file SpacecraftHandler.cpp * - * Control allocator. + * Spacecraft control handler. * - * @author Julien Lecoeur + * @author Pedro Roque */ #include "SpacecraftHandler.hpp" + +using namespace time_literals; + +SpacecraftHandler::SpacecraftHandler() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl) +{ + updateParams(); +} + +bool SpacecraftHandler::init() +{ + ScheduleOnInterval(4_ms); // 250 Hz + return true; +} + +void SpacecraftHandler::updateParams() +{ + ModuleParams::updateParams(); +} + +void SpacecraftHandler::Run() +{ + if (_parameter_update_sub.updated()) { + updateParams(); + } + + const hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + _dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; + + _spacecraft_rate_control.updateRateControl(); + + // TODO: Prepare allocation + // if (_vehicle_control_mode.flag_armed) { + // generateActuatorSetpoint(); + + // } + +} + int SpacecraftHandler::task_spawn(int argc, char *argv[]) { - return 0; + 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() @@ -75,6 +134,7 @@ int SpacecraftHandler::print_usage(const char *reason) PRINT_MODULE_USAGE_NAME("spacecraft", "controller"); PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_COMMAND("status"); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0; diff --git a/src/modules/spacecraft/SpacecraftHandler.hpp b/src/modules/spacecraft/SpacecraftHandler.hpp index 8fc954dc71c5..7d445a193126 100644 --- a/src/modules/spacecraft/SpacecraftHandler.hpp +++ b/src/modules/spacecraft/SpacecraftHandler.hpp @@ -62,13 +62,16 @@ #include #include +// Local includes +#include "SpacecraftRateControl/SpacecraftRateControl.hpp" + class SpacecraftHandler : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { public: SpacecraftHandler(); - virtual ~SpacecraftHandler(); + ~SpacecraftHandler() override = default; /** @see ModuleBase */ static int task_spawn(int argc, char *argv[]); @@ -82,6 +85,31 @@ class SpacecraftHandler : public ModuleBase, public ModulePar /** @see ModuleBase::print_status() */ int print_status() override; -private: /**< loop duration performance counter */ + bool init(); + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + void Run() override; + + // uORB subscriptions + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)}; + vehicle_control_mode_s _vehicle_control_mode{}; + + // uORB publications + uORB::PublicationMulti _actuator_motors_pub{ORB_ID(actuator_motors)}; + + // Class instances + SpacecraftRateControl _spacecraft_rate_control{this}; + + // Variables + hrt_abstime _timestamp{0}; + float _dt{0.f}; }; diff --git a/src/modules/spacecraft/SpacecraftRateControl/CMakeLists.txt b/src/modules/spacecraft/SpacecraftRateControl/CMakeLists.txt new file mode 100644 index 000000000000..b4f33ce25a46 --- /dev/null +++ b/src/modules/spacecraft/SpacecraftRateControl/CMakeLists.txt @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2025 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_library(SpacecraftRateControl + SpacecraftRateControl.cpp +) + +target_link_libraries(SpacecraftRateControl PUBLIC RateControl) +target_link_libraries(SpacecraftRateControl PUBLIC mathlib) +target_link_libraries(SpacecraftRateControl PUBLIC circuit_breaker) +target_include_directories(SpacecraftRateControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/spacecraft/SpacecraftRateControl/SpacecraftRateControl.cpp b/src/modules/spacecraft/SpacecraftRateControl/SpacecraftRateControl.cpp new file mode 100644 index 000000000000..a9f07e76d7dd --- /dev/null +++ b/src/modules/spacecraft/SpacecraftRateControl/SpacecraftRateControl.cpp @@ -0,0 +1,261 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#include "SpacecraftRateControl.hpp" + +#include +#include +#include +#include +#include + +using namespace matrix; +using namespace time_literals; +using math::radians; + +SpacecraftRateControl::SpacecraftRateControl(ModuleParams *parent) : ModuleParams(parent) +{ + _controller_status_pub.advertise(); + updateParams(); +} + +void SpacecraftRateControl::updateParams() +{ + ModuleParams::updateParams(); + // rate control parameters + // The controller gain K is used to convert the parallel (P + I/s + sD) form + // to the ideal (K * [1 + 1/sTi + sTd]) form + const Vector3f rate_k = Vector3f(_param_sc_rollrate_k.get(), _param_sc_pitchrate_k.get(), _param_sc_yawrate_k.get()); + + _rate_control.setPidGains( + rate_k.emult(Vector3f(_param_sc_rollrate_p.get(), _param_sc_pitchrate_p.get(), _param_sc_yawrate_p.get())), + rate_k.emult(Vector3f(_param_sc_rollrate_i.get(), _param_sc_pitchrate_i.get(), _param_sc_yawrate_i.get())), + rate_k.emult(Vector3f(_param_sc_rollrate_d.get(), _param_sc_pitchrate_d.get(), _param_sc_yawrate_d.get()))); + + _rate_control.setIntegratorLimit( + Vector3f(_param_sc_rr_int_lim.get(), _param_sc_pr_int_lim.get(), _param_sc_yr_int_lim.get())); + + _rate_control.setFeedForwardGain( + Vector3f(_param_sc_rollrate_ff.get(), _param_sc_pitchrate_ff.get(), _param_sc_yawrate_ff.get())); + + // manual rate control acro mode rate limits + _acro_rate_max = Vector3f(radians(_param_sc_acro_r_max.get()), radians(_param_sc_acro_p_max.get()), + radians(_param_sc_acro_y_max.get())); + _manual_force_max = _param_sc_manual_f_max.get(); + _manual_torque_max = _param_sc_manual_t_max.get(); +} + +void SpacecraftRateControl::updateRateControl() +{ + if (_vehicle_angular_velocity_sub.update(&angular_velocity)) { + const hrt_abstime now = angular_velocity.timestamp_sample; + + // Guard against too small (< 0.125ms) and too large (> 20ms) dt's. + const float dt = math::constrain(((now - _last_run) * 1e-6f), 0.000125f, 0.02f); + _last_run = now; + + const Vector3f rates{angular_velocity.xyz}; + const Vector3f angular_accel{angular_velocity.xyz_derivative}; + + /* check for updates in other topics */ + _vehicle_control_mode_sub.update(&_vehicle_control_mode); + _vehicle_status_sub.update(&_vehicle_status); + + // use rates setpoint topic + vehicle_rates_setpoint_s vehicle_rates_setpoint{}; + + if (_vehicle_control_mode.flag_control_manual_enabled && + !_vehicle_control_mode.flag_control_attitude_enabled) { + // Here we can be in: Manual Mode or Acro Mode + // generate the rate setpoint from sticks + manual_control_setpoint_s manual_control_setpoint; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + if (_vehicle_control_mode.flag_control_rates_enabled) { + // manual rates control - ACRO mode + const Vector3f man_rate_sp{manual_control_setpoint.roll, + -manual_control_setpoint.pitch, + manual_control_setpoint.yaw}; + + _rates_setpoint = man_rate_sp * 5; + _thrust_setpoint(2) = -manual_control_setpoint.throttle; + _thrust_setpoint(0) = _thrust_setpoint(1) = 0.f; + + // publish rate setpoint + vehicle_rates_setpoint.roll = _rates_setpoint(0); + vehicle_rates_setpoint.pitch = _rates_setpoint(1); + vehicle_rates_setpoint.yaw = _rates_setpoint(2); + _thrust_setpoint.copyTo(vehicle_rates_setpoint.thrust_body); + vehicle_rates_setpoint.timestamp = hrt_absolute_time(); + + _vehicle_rates_setpoint_pub.publish(vehicle_rates_setpoint); + + } else if (!_vehicle_control_mode.flag_control_rates_enabled) { + // Manual/direct control + // Yaw stick commands rotational moment, Roll/Pitch stick commands translational forces + // All other axis are set as zero (We only have four channels on the manual control inputs) + _thrust_setpoint(0) = math::constrain((manual_control_setpoint.pitch * _manual_force_max), -1.f, 1.f); + _thrust_setpoint(1) = math::constrain((manual_control_setpoint.roll * _manual_force_max), -1.f, 1.f); + _thrust_setpoint(2) = 0.0; + + _torque_setpoint(0) = _torque_setpoint(1) = 0.0; + _torque_setpoint(2) = math::constrain((manual_control_setpoint.yaw * _manual_torque_max), -1.f, 1.f); + + // publish thrust and torque setpoints + vehicle_thrust_setpoint_s vehicle_thrust_setpoint{}; + vehicle_torque_setpoint_s vehicle_torque_setpoint{}; + + _thrust_setpoint.copyTo(vehicle_thrust_setpoint.xyz); + _torque_setpoint.copyTo(vehicle_torque_setpoint.xyz); + + vehicle_thrust_setpoint.timestamp_sample = angular_velocity.timestamp_sample; + vehicle_thrust_setpoint.timestamp = hrt_absolute_time(); + + vehicle_torque_setpoint.timestamp = hrt_absolute_time(); + vehicle_torque_setpoint.timestamp_sample = angular_velocity.timestamp_sample; + + _vehicle_thrust_setpoint_pub.publish(vehicle_thrust_setpoint); + _vehicle_torque_setpoint_pub.publish(vehicle_torque_setpoint); + + updateActuatorControlsStatus(vehicle_torque_setpoint, dt); + } + } + + } else if (_vehicle_rates_setpoint_sub.update(&vehicle_rates_setpoint)) { + // Get rates from other controllers (e.g. position or attitude controller) + if (_vehicle_rates_setpoint_sub.copy(&vehicle_rates_setpoint)) { + _rates_setpoint(0) = PX4_ISFINITE(vehicle_rates_setpoint.roll) ? vehicle_rates_setpoint.roll : rates(0); + _rates_setpoint(1) = PX4_ISFINITE(vehicle_rates_setpoint.pitch) ? vehicle_rates_setpoint.pitch : rates(1); + _rates_setpoint(2) = PX4_ISFINITE(vehicle_rates_setpoint.yaw) ? vehicle_rates_setpoint.yaw : rates(2); + _thrust_setpoint = Vector3f(vehicle_rates_setpoint.thrust_body); + } + } + + // run the rate controller + if (_vehicle_control_mode.flag_control_rates_enabled) { + // reset integral if disarmed + if (!_vehicle_control_mode.flag_armed) { + _rate_control.resetIntegral(); + } + + // update saturation status from control allocation feedback + control_allocator_status_s control_allocator_status; + + if (_control_allocator_status_sub.update(&control_allocator_status)) { + Vector saturation_positive; + Vector saturation_negative; + + if (!control_allocator_status.torque_setpoint_achieved) { + for (size_t i = 0; i < 3; i++) { + if (control_allocator_status.unallocated_torque[i] > FLT_EPSILON) { + saturation_positive(i) = true; + + } else if (control_allocator_status.unallocated_torque[i] < -FLT_EPSILON) { + saturation_negative(i) = true; + } + } + } + } + + const Vector3f torque_sp = _rate_control.update(rates, _rates_setpoint, angular_accel, dt, false); + + // publish rate controller status + rate_ctrl_status_s rate_ctrl_status{}; + _rate_control.getRateControlStatus(rate_ctrl_status); + rate_ctrl_status.timestamp = hrt_absolute_time(); + _controller_status_pub.publish(rate_ctrl_status); + + // publish thrust and torque setpoints + vehicle_thrust_setpoint_s vehicle_thrust_setpoint{}; + vehicle_torque_setpoint_s vehicle_torque_setpoint{}; + + _thrust_setpoint.copyTo(vehicle_thrust_setpoint.xyz); + vehicle_torque_setpoint.xyz[0] = PX4_ISFINITE(torque_sp(0)) ? torque_sp(0) : 0.f; + vehicle_torque_setpoint.xyz[1] = PX4_ISFINITE(torque_sp(1)) ? torque_sp(1) : 0.f; + vehicle_torque_setpoint.xyz[2] = PX4_ISFINITE(torque_sp(2)) ? torque_sp(2) : 0.f; + + // scale setpoints by battery status if enabled + if (_param_sc_bat_scale_en.get()) { + if (_battery_status_sub.updated()) { + battery_status_s battery_status; + + if (_battery_status_sub.copy(&battery_status) && battery_status.connected && battery_status.scale > 0.f) { + _battery_status_scale = battery_status.scale; + } + } + + if (_battery_status_scale > 0.f) { + for (int i = 0; i < 3; i++) { + vehicle_thrust_setpoint.xyz[i] = + math::constrain(vehicle_thrust_setpoint.xyz[i] * _battery_status_scale, -1.f, 1.f); + vehicle_torque_setpoint.xyz[i] = + math::constrain(vehicle_torque_setpoint.xyz[i] * _battery_status_scale, -1.f, 1.f); + } + } + } + + vehicle_thrust_setpoint.timestamp_sample = angular_velocity.timestamp_sample; + vehicle_thrust_setpoint.timestamp = hrt_absolute_time(); + _vehicle_thrust_setpoint_pub.publish(vehicle_thrust_setpoint); + + vehicle_torque_setpoint.timestamp_sample = angular_velocity.timestamp_sample; + vehicle_torque_setpoint.timestamp = hrt_absolute_time(); + _vehicle_torque_setpoint_pub.publish(vehicle_torque_setpoint); + + updateActuatorControlsStatus(vehicle_torque_setpoint, dt); + } + } +} + +void SpacecraftRateControl::updateActuatorControlsStatus(const vehicle_torque_setpoint_s &vehicle_torque_setpoint, + float dt) +{ + for (int i = 0; i < 3; i++) { + _control_energy[i] += vehicle_torque_setpoint.xyz[i] * vehicle_torque_setpoint.xyz[i] * dt; + } + + _energy_integration_time += dt; + + if (_energy_integration_time > 500e-3f) { + actuator_controls_status_s status; + status.timestamp = vehicle_torque_setpoint.timestamp; + + for (int i = 0; i < 3; i++) { + status.control_power[i] = _control_energy[i] / _energy_integration_time; + _control_energy[i] = 0.f; + } + + _actuator_controls_status_pub.publish(status); + _energy_integration_time = 0.f; + } +} diff --git a/src/modules/spacecraft/SpacecraftRateControl/SpacecraftRateControl.hpp b/src/modules/spacecraft/SpacecraftRateControl/SpacecraftRateControl.hpp new file mode 100644 index 000000000000..cfb7a620cda8 --- /dev/null +++ b/src/modules/spacecraft/SpacecraftRateControl/SpacecraftRateControl.hpp @@ -0,0 +1,160 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#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 +#include +#include + +using namespace time_literals; + +class SpacecraftRateControl : public ModuleParams +{ +public: + SpacecraftRateControl(ModuleParams *parent); + ~SpacecraftRateControl() = default; + + /** + * @brief Update rate controller. + */ + void updateRateControl(); + +protected: + void updateParams() override; + +private: + void updateActuatorControlsStatus(const vehicle_torque_setpoint_s &vehicle_torque_setpoint, float dt); + + RateControl _rate_control; ///< class for rate control calculations + + uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; + uORB::Subscription _control_allocator_status_sub{ORB_ID(control_allocator_status)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; + uORB::Subscription _vehicle_rates_setpoint_sub{ORB_ID(vehicle_rates_setpoint)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + uORB::Publication _actuator_controls_status_pub{ORB_ID(actuator_controls_status_0)}; + uORB::PublicationMulti _controller_status_pub{ORB_ID(rate_ctrl_status)}; + uORB::Publication _vehicle_rates_setpoint_pub{ORB_ID(vehicle_rates_setpoint)}; + uORB::Publication _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)}; + uORB::Publication _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)}; + + vehicle_control_mode_s _vehicle_control_mode{}; + vehicle_status_s _vehicle_status{}; + vehicle_angular_velocity_s angular_velocity{}; + + bool _landed{true}; + bool _maybe_landed{true}; + + hrt_abstime _last_run{0}; + + perf_counter_t _loop_perf; /**< loop duration performance counter */ + + // keep setpoint values between updates + matrix::Vector3f _acro_rate_max; /**< max attitude rates in acro mode */ + matrix::Vector3f _rates_setpoint{}; + float _manual_torque_max{1.0}; + float _manual_force_max{1.0}; + + float _battery_status_scale{0.0f}; + matrix::Vector3f _thrust_setpoint{}; + matrix::Vector3f _torque_setpoint{}; + + float _energy_integration_time{0.0f}; + float _control_energy[4] {}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_sc_rollrate_p, + (ParamFloat) _param_sc_rollrate_i, + (ParamFloat) _param_sc_rr_int_lim, + (ParamFloat) _param_sc_rollrate_d, + (ParamFloat) _param_sc_rollrate_ff, + (ParamFloat) _param_sc_rollrate_k, + + (ParamFloat) _param_sc_pitchrate_p, + (ParamFloat) _param_sc_pitchrate_i, + (ParamFloat) _param_sc_pr_int_lim, + (ParamFloat) _param_sc_pitchrate_d, + (ParamFloat) _param_sc_pitchrate_ff, + (ParamFloat) _param_sc_pitchrate_k, + + (ParamFloat) _param_sc_yawrate_p, + (ParamFloat) _param_sc_yawrate_i, + (ParamFloat) _param_sc_yr_int_lim, + (ParamFloat) _param_sc_yawrate_d, + (ParamFloat) _param_sc_yawrate_ff, + (ParamFloat) _param_sc_yawrate_k, + + (ParamFloat) _param_sc_acro_r_max, + (ParamFloat) _param_sc_acro_p_max, + (ParamFloat) _param_sc_acro_y_max, + (ParamFloat) _param_sc_acro_expo, /**< expo stick curve shape (roll & pitch) */ + (ParamFloat) _param_sc_acro_expo_y, /**< expo stick curve shape (yaw) */ + (ParamFloat) _param_sc_acro_supexpo, /**< superexpo stick curve shape (roll & pitch) */ + (ParamFloat) _param_sc_acro_supexpoy, /**< superexpo stick curve shape (yaw) */ + + (ParamFloat) _param_sc_manual_f_max, + (ParamFloat) _param_sc_manual_t_max, + + (ParamBool) _param_sc_bat_scale_en + ) +}; diff --git a/src/modules/spacecraft/module.yaml b/src/modules/spacecraft/module.yaml index 5b2b5a264da7..1d6c2b576b04 100644 --- a/src/modules/spacecraft/module.yaml +++ b/src/modules/spacecraft/module.yaml @@ -1,9 +1,8 @@ __max_num_mc_motors: &max_num_mc_motors 12 __max_num_thrusters: &max_num_thrusters 12 __max_num_servos: &max_num_servos 8 -__max_num_tilts: &max_num_tilts 4 -module_name: Control Allocation +module_name: Spacecraft parameters: - group: Geometry @@ -172,6 +171,8 @@ parameters: max: 100 default: 6.5 + + # Mixer mixer: actuator_types: diff --git a/src/modules/spacecraft/spacecraft_params.c b/src/modules/spacecraft/spacecraft_params.c new file mode 100644 index 000000000000..4afb818a424d --- /dev/null +++ b/src/modules/spacecraft/spacecraft_params.c @@ -0,0 +1,421 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2025 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 spacecraft_params.c + * Parameters for spacecraft vehicle type. + * + * @author Pedro Roque + */ + +/** + * Roll rate P gain + * + * Roll rate proportional gain, i.e. control output for angular speed error 1 rad/s. + * + * @min 0.01 + * @max 0.5 + * @decimal 3 + * @increment 0.01 + * @group Spacecraft Rate Control + */ +PARAM_DEFINE_FLOAT(SC_ROLLRATE_P, 0.15f); + +/** + * Roll rate I gain + * + * Roll rate integral gain. Can be set to compensate static thrust difference or gravity center offset. + * + * @min 0.0 + * @decimal 3 + * @increment 0.01 + * @group Spacecraft Rate Control + */ +PARAM_DEFINE_FLOAT(SC_ROLLRATE_I, 0.2f); + +/** + * Roll rate integrator limit + * + * Roll rate integrator limit. Can be set to increase the amount of integrator available to counteract disturbances or reduced to improve settling time after large roll moment trim changes. + * + * @min 0.0 + * @decimal 2 + * @increment 0.01 + * @group Spacecraft Rate Control + */ +PARAM_DEFINE_FLOAT(SC_RR_INT_LIM, 0.30f); + +/** + * Roll rate D gain + * + * Roll rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + * + * @min 0.0 + * @max 0.01 + * @decimal 4 + * @increment 0.0005 + * @group Spacecraft Rate Control + */ +PARAM_DEFINE_FLOAT(SC_ROLLRATE_D, 0.003f); + +/** + * Roll rate feedforward + * + * Improves tracking performance. + * + * @min 0.0 + * @decimal 4 + * @group Spacecraft Rate Control + */ +PARAM_DEFINE_FLOAT(SC_ROLLRATE_FF, 0.0f); + +/** + * Roll rate controller gain + * + * Global gain of the controller. + * + * This gain scales the P, I and D terms of the controller: + * output = SC_ROLLRATE_K * (SC_ROLLRATE_P * error + * + SC_ROLLRATE_I * error_integral + * + SC_ROLLRATE_D * error_derivative) + * Set SC_ROLLRATE_P=1 to implement a PID in the ideal form. + * Set SC_ROLLRATE_K=1 to implement a PID in the parallel form. + * + * @min 0.01 + * @max 5.0 + * @decimal 4 + * @increment 0.0005 + * @group Spacecraft Rate Control + */ +PARAM_DEFINE_FLOAT(SC_ROLLRATE_K, 1.0f); + +/** + * Pitch rate P gain + * + * Pitch rate proportional gain, i.e. control output for angular speed error 1 rad/s. + * + * @min 0.01 + * @max 0.6 + * @decimal 3 + * @increment 0.01 + * @group Spacecraft Rate Control + */ +PARAM_DEFINE_FLOAT(SC_PITCHRATE_P, 0.15f); + +/** + * Pitch rate I gain + * + * Pitch rate integral gain. Can be set to compensate static thrust difference or gravity center offset. + * + * @min 0.0 + * @decimal 3 + * @increment 0.01 + * @group Spacecraft Rate Control + */ +PARAM_DEFINE_FLOAT(SC_PITCHRATE_I, 0.2f); + +/** + * Pitch rate integrator limit + * + * Pitch rate integrator limit. Can be set to increase the amount of integrator available to counteract disturbances or reduced to improve settling time after large pitch moment trim changes. + * + * @min 0.0 + * @decimal 2 + * @increment 0.01 + * @group Spacecraft Rate Control + */ +PARAM_DEFINE_FLOAT(SC_PR_INT_LIM, 0.30f); + +/** + * Pitch rate D gain + * + * Pitch rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + * + * @min 0.0 + * @decimal 4 + * @increment 0.0005 + * @group Spacecraft Rate Control + */ +PARAM_DEFINE_FLOAT(SC_PITCHRATE_D, 0.003f); + +/** + * Pitch rate feedforward + * + * Improves tracking performance. + * + * @min 0.0 + * @decimal 4 + * @group Spacecraft Rate Control + */ +PARAM_DEFINE_FLOAT(SC_PITCHRATE_FF, 0.0f); + +/** + * Pitch rate controller gain + * + * Global gain of the controller. + * + * This gain scales the P, I and D terms of the controller: + * output = SC_PITCHRATE_K * (SC_PITCHRATE_P * error + * + SC_PITCHRATE_I * error_integral + * + SC_PITCHRATE_D * error_derivative) + * Set SC_PITCHRATE_P=1 to implement a PID in the ideal form. + * Set SC_PITCHRATE_K=1 to implement a PID in the parallel form. + * + * @min 0.01 + * @max 5.0 + * @decimal 4 + * @increment 0.0005 + * @group Spacecraft Rate Control + */ +PARAM_DEFINE_FLOAT(SC_PITCHRATE_K, 1.0f); + +/** + * Yaw rate P gain + * + * Yaw rate proportional gain, i.e. control output for angular speed error 1 rad/s. + * + * @min 0.0 + * @max 10.0 + * @decimal 2 + * @increment 0.01 + * @group Spacecraft Rate Control + */ +PARAM_DEFINE_FLOAT(SC_YAWRATE_P, 10.0f); + +/** + * Yaw rate I gain + * + * Yaw rate integral gain. Can be set to compensate static thrust difference or gravity center offset. + * + * @min 0.0 + * @decimal 2 + * @increment 0.01 + * @group Spacecraft Rate Control + */ +PARAM_DEFINE_FLOAT(SC_YAWRATE_I, 0.865f); + +/** + * Yaw rate integrator limit + * + * Yaw rate integrator limit. Can be set to increase the amount of integrator available to counteract disturbances or reduced to improve settling time after large yaw moment trim changes. + * + * @min 0.0 + * @decimal 2 + * @increment 0.01 + * @group Spacecraft Rate Control + */ +PARAM_DEFINE_FLOAT(SC_YR_INT_LIM, 0.2f); + +/** + * Yaw rate D gain + * + * Yaw rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + * + * @min 0.0 + * @decimal 2 + * @increment 0.01 + * @group Spacecraft Rate Control + */ +PARAM_DEFINE_FLOAT(SC_YAWRATE_D, 0.0f); + +/** + * Yaw rate feedforward + * + * Improves tracking performance. + * + * @min 0.0 + * @decimal 4 + * @increment 0.01 + * @group Spacecraft Rate Control + */ +PARAM_DEFINE_FLOAT(SC_YAWRATE_FF, 0.0f); + +/** + * Yaw rate controller gain + * + * Global gain of the controller. + * + * This gain scales the P, I and D terms of the controller: + * output = SC_YAWRATE_K * (SC_YAWRATE_P * error + * + SC_YAWRATE_I * error_integral + * + SC_YAWRATE_D * error_derivative) + * Set SC_YAWRATE_P=1 to implement a PID in the ideal form. + * Set SC_YAWRATE_K=1 to implement a PID in the parallel form. + * + * @min 0.0 + * @max 5.0 + * @decimal 4 + * @increment 0.0005 + * @group Spacecraft Rate Control + */ +PARAM_DEFINE_FLOAT(SC_YAWRATE_K, 1.0f); + +/** + * Max acro roll rate + * + * default: 2 turns per second + * + * @unit deg/s + * @min 0.0 + * @max 1800.0 + * @decimal 1 + * @increment 5 + * @group Spacecraft Rate Control + */ +PARAM_DEFINE_FLOAT(SC_ACRO_R_MAX, 720.0f); + +/** + * Max acro pitch rate + * + * default: 2 turns per second + * + * @unit deg/s + * @min 0.0 + * @max 1800.0 + * @decimal 1 + * @increment 5 + * @group Spacecraft Rate Control + */ +PARAM_DEFINE_FLOAT(SC_ACRO_P_MAX, 720.0f); + +/** + * Max acro yaw rate + * + * default 1.5 turns per second + * + * @unit deg/s + * @min 0.0 + * @max 1800.0 + * @decimal 1 + * @increment 5 + * @group Spacecraft Rate Control + */ +PARAM_DEFINE_FLOAT(SC_ACRO_Y_MAX, 540.0f); + +/** + * Acro mode Expo factor for Roll and Pitch. + * + * Exponential factor for tuning the input curve shape. + * + * 0 Purely linear input curve + * 1 Purely cubic input curve + * + * @min 0 + * @max 1 + * @decimal 2 + * @group Spacecraft Rate Control + */ +PARAM_DEFINE_FLOAT(SC_ACRO_EXPO, 0.69f); + +/** + * Acro mode Expo factor for Yaw. + * + * Exponential factor for tuning the input curve shape. + * + * 0 Purely linear input curve + * 1 Purely cubic input curve + * + * @min 0 + * @max 1 + * @decimal 2 + * @group Spacecraft Rate Control + */ +PARAM_DEFINE_FLOAT(SC_ACRO_EXPO_Y, 0.69f); + +/** + * Acro mode SuperExpo factor for Roll and Pitch. + * + * SuperExpo factor for refining the input curve shape tuned using SC_ACRO_EXPO. + * + * 0 Pure Expo function + * 0.7 reasonable shape enhancement for intuitive stick feel + * 0.95 very strong bent input curve only near maxima have effect + * + * @min 0 + * @max 0.95 + * @decimal 2 + * @group Spacecraft Rate Control + */ +PARAM_DEFINE_FLOAT(SC_ACRO_SUPEXPO, 0.7f); + +/** + * Acro mode SuperExpo factor for Yaw. + * + * SuperExpo factor for refining the input curve shape tuned using SC_ACRO_EXPO_Y. + * + * 0 Pure Expo function + * 0.7 reasonable shape enhancement for intuitive stick feel + * 0.95 very strong bent input curve only near maxima have effect + * + * @min 0 + * @max 0.95 + * @decimal 2 + * @group Spacecraft Rate Control + */ +PARAM_DEFINE_FLOAT(SC_ACRO_SUPEXPOY, 0.7f); + +/** + * Battery power level scaler + * + * This compensates for voltage drop of the battery over time by attempting to + * normalize performance across the operating range of the battery. The copter + * should constantly behave as if it was fully charged with reduced max acceleration + * at lower battery percentages. i.e. if hover is at 0.5 throttle at 100% battery, + * it will still be 0.5 at 60% battery. + * + * @boolean + * @group Spacecraft Rate Control + */ +PARAM_DEFINE_INT32(SC_BAT_SCALE_EN, 0); + +/** + * Manual mode maximum force. + * + * * + * @min 0 + * @max 1.0 + * @decimal 2 + * @group Spacecraft Rate Control + */ +PARAM_DEFINE_FLOAT(SC_MAN_F_MAX, 1.0f); + +/** + * Manual mode maximum torque. + * + * * + * @min 0 + * @max 1.0 + * @decimal 2 + * @group Spacecraft Rate Control + */ +PARAM_DEFINE_FLOAT(SC_MAN_T_MAX, 1.0f);