diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1070_boat b/ROMFS/px4fmu_common/init.d-posix/airframes/1070_boat index be68232e37..917a14b640 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1070_boat +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1070_boat @@ -13,7 +13,7 @@ param set-default GND_SPEED_I 8 param set-default GND_SPEED_IMAX 0.125 param set-default GND_SPEED_P 2 param set-default GND_SPEED_THR_SC 1 -param set-default GND_SPEED_TRIM 1 +param set-default GND_SPEED_TRIM 3 param set-default GND_THR_CRUISE 0.85 param set-default GND_THR_MAX 1 param set-default GND_THR_MIN 0 diff --git a/src/modules/rover_pos_control/CMakeLists.txt b/src/modules/rover_pos_control/CMakeLists.txt index 22ab31d2e4..2c42cce5fd 100644 --- a/src/modules/rover_pos_control/CMakeLists.txt +++ b/src/modules/rover_pos_control/CMakeLists.txt @@ -31,8 +31,6 @@ # ############################################################################ -add_subdirectory(RoverRateControl) - px4_add_module( MODULE modules__rover_pos_control MAIN rover_pos_control @@ -40,7 +38,7 @@ px4_add_module( RoverPositionControl.cpp RoverPositionControl.hpp DEPENDS - RoverRateControl + RateControl l1 pid ) diff --git a/src/modules/rover_pos_control/RoverPositionControl.cpp b/src/modules/rover_pos_control/RoverPositionControl.cpp index 45a78469ce..bcac375c14 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.cpp +++ b/src/modules/rover_pos_control/RoverPositionControl.cpp @@ -318,14 +318,20 @@ RoverPositionControl::control_position(const matrix::Vector2d ¤t_position, prev_wp(1)); _gnd_control.navigate_waypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed_2d); - _act_controls.control[actuator_controls_s::INDEX_THROTTLE] = mission_throttle; + ///WIP: Convert Lateral acceleration demand to rate control reference + float min_speed{1.0}; + matrix::Vector2f saturated_speed_2d = (ground_speed_2d.norm() < min_speed) ? ground_speed_2d : + ground_speed_2d.normalized() * min_speed; - float desired_r = ground_speed_2d.norm_squared() / math::abs_t(_gnd_control.nav_lateral_acceleration_demand()); - float desired_theta = (0.5f * M_PI_F) - atan2f(desired_r, _param_wheel_base.get()); - float control_effort = (desired_theta / _param_max_turn_angle.get()) * sign( - _gnd_control.nav_lateral_acceleration_demand()); - control_effort = math::constrain(control_effort, -1.0f, 1.0f); - _act_controls.control[actuator_controls_s::INDEX_YAW] = control_effort; + float desired_yaw_rate = _gnd_control.nav_lateral_acceleration_demand() / saturated_speed_2d.norm(); + + _rates_sp.roll = 0.0; + _rates_sp.pitch = 0.0; + _rates_sp.yaw = desired_yaw_rate; + _rates_sp.thrust_body[0] = math::constrain(mission_throttle, 0.0f, 1.0f); + _rates_sp.timestamp = hrt_absolute_time(); + _rates_sp_pub.publish(_rates_sp); + control_rates(_vehicle_rates, _vehicle_angular_acceleration, _local_pos, _rates_sp); } } break; @@ -433,6 +439,8 @@ RoverPositionControl::control_rates(const vehicle_angular_velocity_s &rates, con dt = hrt_elapsed_time(&_control_rates_last_called) * 1e-6f; } + _control_rates_last_called = hrt_absolute_time(); + const matrix::Vector3f current_velocity(local_pos.vx, local_pos.vy, local_pos.vz); const matrix::Vector3f vehicle_rates(rates.xyz[0], rates.xyz[1], rates.xyz[2]); const matrix::Vector3f rates_setpoint(rates_sp.roll, rates_sp.pitch, rates_sp.yaw); @@ -472,7 +480,7 @@ RoverPositionControl::Run() /* update parameters from storage */ parameters_update(); - if (!is_finite(_steering_input)) { + if (!PX4_ISFINITE(_steering_input)) { _steering_input = 0.0; } diff --git a/src/modules/rover_pos_control/RoverPositionControl.hpp b/src/modules/rover_pos_control/RoverPositionControl.hpp index 5df8338311..3ec1cb2eed 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.hpp +++ b/src/modules/rover_pos_control/RoverPositionControl.hpp @@ -41,7 +41,6 @@ * @author Marco Zorzi */ -#include #include #include @@ -50,6 +49,7 @@ #include #include #include +#include #include #include #include @@ -134,6 +134,9 @@ private: vehicle_local_position_s _local_pos{}; /**< global vehicle position */ actuator_controls_s _act_controls{}; /**< direct control of actuators */ vehicle_attitude_s _vehicle_att{}; + vehicle_angular_acceleration_s _vehicle_angular_acceleration{}; + vehicle_angular_velocity_s _vehicle_rates{}; + trajectory_setpoint_s _trajectory_setpoint{}; uORB::Publication _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)}; uORB::Publication _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)}; @@ -157,7 +160,7 @@ private: uint8_t _pos_reset_counter{0}; // captures the number of times the estimator has reset the horizontal position ECL_L1_Pos_Controller _gnd_control; - RoverRateControl _rate_control; + RateControl _rate_control; float _steering_input{0.0}; enum UGV_POSCTRL_MODE { diff --git a/src/modules/rover_pos_control/RoverRateControl/CMakeLists.txt b/src/modules/rover_pos_control/RoverRateControl/CMakeLists.txt deleted file mode 100644 index 9025c96a1c..0000000000 --- a/src/modules/rover_pos_control/RoverRateControl/CMakeLists.txt +++ /dev/null @@ -1,42 +0,0 @@ -############################################################################ -# -# Copyright (c) 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. -# -############################################################################ - -px4_add_library(RoverRateControl - RoverRateControl.cpp - RoverRateControl.hpp -) -target_compile_options(RoverRateControl PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) -target_include_directories(RoverRateControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) -target_link_libraries(RoverRateControl PRIVATE mathlib) - -px4_add_unit_gtest(SRC RoverRateControlTest.cpp LINKLIBS RoverRateControl) diff --git a/src/modules/rover_pos_control/RoverRateControl/RoverRateControl.cpp b/src/modules/rover_pos_control/RoverRateControl/RoverRateControl.cpp deleted file mode 100644 index df13da5dd6..0000000000 --- a/src/modules/rover_pos_control/RoverRateControl/RoverRateControl.cpp +++ /dev/null @@ -1,104 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 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 RoverRateControl.cpp - */ - -#include -#include - -using namespace matrix; - -void RoverRateControl::setGains(const Vector3f &P, const Vector3f &I, const Vector3f &D) -{ - _gain_p = P; - _gain_i = I; - _gain_d = D; -} - -Vector3f RoverRateControl::update(const Vector3f &rate, const Vector3f &rate_sp, const Vector3f &angular_accel, - const float dt, const bool landed) -{ - // angular rates error - Vector3f rate_error = rate_sp - rate; - - // PID control with feed forward - const Vector3f torque = _gain_p.emult(rate_error) + _rate_int - _gain_d.emult(angular_accel) + _gain_ff.emult(rate_sp); - - // update integral only if we are not landed - if (!landed) { - updateIntegral(rate_error, dt); - } - - return torque; -} - -void RoverRateControl::updateIntegral(Vector3f &rate_error, const float dt) -{ - for (int i = 0; i < 3; i++) { - // prevent further positive control saturation - if (_mixer_saturation_positive[i]) { - rate_error(i) = math::min(rate_error(i), 0.f); - } - - // prevent further negative control saturation - if (_mixer_saturation_negative[i]) { - rate_error(i) = math::max(rate_error(i), 0.f); - } - - // I term factor: reduce the I gain with increasing rate error. - // This counteracts a non-linear effect where the integral builds up quickly upon a large setpoint - // change (noticeable in a bounce-back effect after a flip). - // The formula leads to a gradual decrease w/o steps, while only affecting the cases where it should: - // with the parameter set to 400 degrees, up to 100 deg rate error, i_factor is almost 1 (having no effect), - // and up to 200 deg error leads to <25% reduction of I. - float i_factor = rate_error(i) / math::radians(400.f); - i_factor = math::max(0.0f, 1.f - i_factor * i_factor); - - // Perform the integration using a first order method - float rate_i = _rate_int(i) + i_factor * _gain_i(i) * rate_error(i) * dt; - - // do not propagate the result if out of range or invalid - if (PX4_ISFINITE(rate_i)) { - _rate_int(i) = math::constrain(rate_i, -_lim_int(i), _lim_int(i)); - } - } -} - -void RoverRateControl::getRoverRateControlStatus(rate_ctrl_status_s &rate_ctrl_status) -{ - rate_ctrl_status.rollspeed_integ = _rate_int(0); - rate_ctrl_status.pitchspeed_integ = _rate_int(1); - rate_ctrl_status.yawspeed_integ = _rate_int(2); -} diff --git a/src/modules/rover_pos_control/RoverRateControl/RoverRateControl.hpp b/src/modules/rover_pos_control/RoverRateControl/RoverRateControl.hpp deleted file mode 100644 index 83428a1d13..0000000000 --- a/src/modules/rover_pos_control/RoverRateControl/RoverRateControl.hpp +++ /dev/null @@ -1,110 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2019-2021 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 RoverRateControl.hpp - * - * PID 3 axis angular rate / angular velocity control. - */ - -#pragma once - -#include -#include -#include - -class RoverRateControl -{ -public: - RoverRateControl() = default; - ~RoverRateControl() = default; - - /** - * Set the rate control gains - * @param P 3D vector of proportional gains for body x,y,z axis - * @param I 3D vector of integral gains - * @param D 3D vector of derivative gains - */ - void setGains(const matrix::Vector3f &P, const matrix::Vector3f &I, const matrix::Vector3f &D); - - /** - * Set the mximum absolute value of the integrator for all axes - * @param integrator_limit limit value for all axes x, y, z - */ - void setIntegratorLimit(const matrix::Vector3f &integrator_limit) { _lim_int = integrator_limit; }; - - /** - * Set direct rate to torque feed forward gain - * @see _gain_ff - * @param FF 3D vector of feed forward gains for body x,y,z axis - */ - void setFeedForwardGain(const matrix::Vector3f &FF) { _gain_ff = FF; }; - - /** - * Run one control loop cycle calculation - * @param rate estimation of the current vehicle angular rate - * @param rate_sp desired vehicle angular rate setpoint - * @param dt desired vehicle angular rate setpoint - * @return [-1,1] normalized torque vector to apply to the vehicle - */ - matrix::Vector3f update(const matrix::Vector3f &rate, const matrix::Vector3f &rate_sp, - const matrix::Vector3f &angular_accel, const float dt, const bool landed); - - /** - * Set the integral term to 0 to prevent windup - * @see _rate_int - */ - void resetIntegral() { _rate_int.zero(); } - - /** - * Get status message of controller for logging/debugging - * @param rate_ctrl_status status message to fill with internal states - */ - void getRoverRateControlStatus(rate_ctrl_status_s &rate_ctrl_status); - -private: - void updateIntegral(matrix::Vector3f &rate_error, const float dt); - - // Gains - matrix::Vector3f _gain_p; ///< rate control proportional gain for all axes x, y, z - matrix::Vector3f _gain_i; ///< rate control integral gain - matrix::Vector3f _gain_d; ///< rate control derivative gain - matrix::Vector3f _lim_int; ///< integrator term maximum absolute value - matrix::Vector3f _gain_ff; ///< direct rate to torque feed forward gain only useful for helicopters - - // States - matrix::Vector3f _rate_int; ///< integral term of the rate controller - - bool _mixer_saturation_positive[3] {}; - bool _mixer_saturation_negative[3] {}; -}; diff --git a/src/modules/rover_pos_control/RoverRateControl/RoverRateControlTest.cpp b/src/modules/rover_pos_control/RoverRateControl/RoverRateControlTest.cpp deleted file mode 100644 index 9bae5a64a3..0000000000 --- a/src/modules/rover_pos_control/RoverRateControl/RoverRateControlTest.cpp +++ /dev/null @@ -1,44 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 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 -#include - -using namespace matrix; - -TEST(RoverRateControlTest, AllZeroCase) -{ - RoverRateControl rate_control; - Vector3f torque = rate_control.update(Vector3f(), Vector3f(), Vector3f(), 0.f, false); - EXPECT_EQ(torque, Vector3f()); -}