diff --git a/src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControl.cpp b/src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControl.cpp deleted file mode 100644 index 7c3ffb2066..0000000000 --- a/src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControl.cpp +++ /dev/null @@ -1,115 +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 AngularVelocityControl.cpp - */ - -#include -#include - -using namespace matrix; - -void AngularVelocityControl::setGains(const Vector3f &P, const Vector3f &I, const Vector3f &D) -{ - _gain_p = P; - _gain_i = I; - _gain_d = D; -} - -void AngularVelocityControl::setSaturationStatus(const matrix::Vector &saturation_positive, - const matrix::Vector &saturation_negative) -{ - _saturation_positive = saturation_positive; - _saturation_negative = saturation_negative; -} - -void AngularVelocityControl::update(const Vector3f &angular_velocity, const Vector3f &angular_velocity_sp, - const Vector3f &angular_acceleration, const float dt, const bool landed) -{ - // angular rates error - Vector3f angular_velocity_error = angular_velocity_sp - angular_velocity; - - // P + D control - _angular_accel_sp = _gain_p.emult(angular_velocity_error) - _gain_d.emult(angular_acceleration); - - // I + FF control - Vector3f torque_feedforward = _angular_velocity_int + _gain_ff.emult(angular_velocity_sp); - - // compute torque setpoint - _torque_sp = _inertia * _angular_accel_sp + torque_feedforward + angular_velocity.cross(_inertia * angular_velocity); - - // update integral only if we are not landed - if (!landed) { - updateIntegral(angular_velocity_error, dt); - } -} - -void AngularVelocityControl::updateIntegral(Vector3f &angular_velocity_error, const float dt) -{ - for (int i = 0; i < 3; i++) { - // prevent further positive control saturation - if (_saturation_positive(i)) { - angular_velocity_error(i) = math::min(angular_velocity_error(i), 0.f); - } - - // prevent further negative control saturation - if (_saturation_negative(i)) { - angular_velocity_error(i) = math::max(angular_velocity_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 = angular_velocity_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 angular_velocity_i = _angular_velocity_int(i) + i_factor * _gain_i(i) * angular_velocity_error(i) * dt; - - // do not propagate the result if out of range or invalid - if (PX4_ISFINITE(angular_velocity_i)) { - _angular_velocity_int(i) = math::constrain(angular_velocity_i, -_lim_int(i), _lim_int(i)); - } - } -} - -void AngularVelocityControl::reset() -{ - _angular_velocity_int.zero(); - _torque_sp.zero(); - _angular_accel_sp.zero(); -} diff --git a/src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControl.hpp b/src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControl.hpp deleted file mode 100644 index 49de784026..0000000000 --- a/src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControl.hpp +++ /dev/null @@ -1,149 +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 AngularVelocityControl.hpp - * - * PID 3 axis angular velocity control. - */ - -#pragma once - -#include - -#include - -class AngularVelocityControl -{ -public: - AngularVelocityControl() = default; - ~AngularVelocityControl() = default; - - /** - * Set the 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 angular velocity setpoint 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; }; - - /** - * Set inertia matrix - * @see _inertia - * @param inertia inertia matrix - */ - void setInertiaMatrix(const matrix::Matrix3f &inertia) { _inertia = inertia; }; - - /** - * Set saturation status - * @see _saturation_positive - * @see _saturation_negative - * @param saturation_positive positive saturation - * @param saturation_negative negative saturation - */ - void setSaturationStatus(const matrix::Vector &saturation_positive, - const matrix::Vector &saturation_negative); - - /** - * Run one control loop cycle calculation - * @param angular_velocity estimation of the current vehicle angular velocity - * @param angular_velocity_sp desired vehicle angular velocity setpoint - * @param angular_acceleration estimation of the current vehicle angular acceleration - * @param dt elapsed time since last update - * @param landed whether the vehicle is on the ground - */ - void update(const matrix::Vector3f &angular_velocity, const matrix::Vector3f &angular_velocity_sp, - const matrix::Vector3f &angular_acceleration, const float dt, const bool landed); - - /** - * Get the desired angular acceleration - * @see _angular_accel_sp - */ - const matrix::Vector3f &getAngularAccelerationSetpoint() {return _angular_accel_sp;}; - - /** - * Get the torque vector to apply to the vehicle - * @see _torque_sp - */ - const matrix::Vector3f &getTorqueSetpoint() {return _torque_sp;}; - - /** - * Get the integral term - * @see _torque_sp - */ - const matrix::Vector3f &getIntegral() {return _angular_velocity_int;}; - - /** - * Set the integral term to 0 to prevent windup - * @see _angular_velocity_int - */ - void resetIntegral() { _angular_velocity_int.zero(); } - - /** - * ReSet the controller state - */ - void reset(); - -private: - void updateIntegral(matrix::Vector3f &angular_velocity_error, const float dt); - - // Gains - matrix::Vector3f _gain_p; ///< proportional gain for all axes x, y, z - matrix::Vector3f _gain_i; ///< integral gain - matrix::Vector3f _gain_d; ///< derivative gain - matrix::Vector3f _lim_int; ///< integrator term maximum absolute value - matrix::Vector3f _gain_ff; ///< direct angular velocity to torque feed forward gain - matrix::Matrix3f _inertia{matrix::eye()}; ///< inertia matrix - - // States - matrix::Vector3f _angular_velocity_int; - matrix::Vector _saturation_positive; - matrix::Vector _saturation_negative; - - // Output - matrix::Vector3f _angular_accel_sp; //< Angular acceleration setpoint computed using P and D gains - matrix::Vector3f _torque_sp; //< Torque setpoint to apply to the vehicle -}; diff --git a/src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControlTest.cpp b/src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControlTest.cpp deleted file mode 100644 index 9f4c85af46..0000000000 --- a/src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControlTest.cpp +++ /dev/null @@ -1,45 +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(AngularVelocityControlTest, AllZeroCase) -{ - AngularVelocityControl control; - control.update(Vector3f(), Vector3f(), Vector3f(), 0.f, false); - Vector3f torque = control.getTorqueSetpoint(); - EXPECT_EQ(torque, Vector3f()); -} diff --git a/src/modules/angular_velocity_controller/AngularVelocityControl/CMakeLists.txt b/src/modules/angular_velocity_controller/AngularVelocityControl/CMakeLists.txt deleted file mode 100644 index 05bb7dd7b2..0000000000 --- a/src/modules/angular_velocity_controller/AngularVelocityControl/CMakeLists.txt +++ /dev/null @@ -1,41 +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(AngularVelocityControl - AngularVelocityControl.cpp -) -target_compile_options(AngularVelocityControl PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) -target_include_directories(AngularVelocityControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) -target_link_libraries(AngularVelocityControl PRIVATE mathlib) - -px4_add_unit_gtest(SRC AngularVelocityControlTest.cpp LINKLIBS AngularVelocityControl) diff --git a/src/modules/angular_velocity_controller/AngularVelocityController.cpp b/src/modules/angular_velocity_controller/AngularVelocityController.cpp deleted file mode 100644 index 52f374f07b..0000000000 --- a/src/modules/angular_velocity_controller/AngularVelocityController.cpp +++ /dev/null @@ -1,338 +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. - * - ****************************************************************************/ - -#include "AngularVelocityController.hpp" - -#include -#include -#include -#include -#include - -using namespace matrix; -using namespace time_literals; - -AngularVelocityController::AngularVelocityController() : - ModuleParams(nullptr), - WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl), - _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) -{ - _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; - - parameters_updated(); - _rate_ctrl_status_pub.advertise(); -} - -AngularVelocityController::~AngularVelocityController() -{ - perf_free(_loop_perf); -} - -bool -AngularVelocityController::init() -{ - if (!_vehicle_angular_velocity_sub.registerCallback()) { - PX4_ERR("callback registration failed"); - return false; - } - - return true; -} - -void -AngularVelocityController::parameters_updated() -{ - // 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 k_gains = Vector3f(_param_avc_x_k.get(), _param_avc_y_k.get(), _param_avc_z_k.get()); - - _control.setGains( - k_gains.emult(Vector3f(_param_avc_x_p.get(), _param_avc_y_p.get(), _param_avc_z_p.get())), - k_gains.emult(Vector3f(_param_avc_x_i.get(), _param_avc_y_i.get(), _param_avc_z_i.get())), - k_gains.emult(Vector3f(_param_avc_x_d.get(), _param_avc_y_d.get(), _param_avc_z_d.get()))); - - _control.setIntegratorLimit( - Vector3f(_param_avc_x_i_lim.get(), _param_avc_y_i_lim.get(), _param_avc_z_i_lim.get())); - - _control.setFeedForwardGain( - Vector3f(_param_avc_x_ff.get(), _param_avc_y_ff.get(), _param_avc_z_ff.get())); - - // inertia matrix - const float inertia[3][3] = { - {_param_vm_inertia_xx.get(), _param_vm_inertia_xy.get(), _param_vm_inertia_xz.get()}, - {_param_vm_inertia_xy.get(), _param_vm_inertia_yy.get(), _param_vm_inertia_yz.get()}, - {_param_vm_inertia_xz.get(), _param_vm_inertia_yz.get(), _param_vm_inertia_zz.get()} - }; - _control.setInertiaMatrix(matrix::Matrix3f(inertia)); - - // Hover thrust - if (!_param_mpc_use_hte.get() - || !_vehicle_control_mode.flag_armed) { - _hover_thrust = _param_mpc_thr_hover.get(); - } -} - -void -AngularVelocityController::Run() -{ - if (should_exit()) { - _vehicle_angular_velocity_sub.unregisterCallback(); - exit_and_cleanup(); - return; - } - - perf_begin(_loop_perf); - - // Check if parameters have changed - if (_parameter_update_sub.updated()) { - // clear update - parameter_update_s param_update; - _parameter_update_sub.copy(¶m_update); - - updateParams(); - parameters_updated(); - } - - /* run controller on gyro changes */ - vehicle_angular_velocity_s vehicle_angular_velocity; - - if (_vehicle_angular_velocity_sub.update(&vehicle_angular_velocity)) { - const hrt_abstime now = hrt_absolute_time(); - _timestamp_sample = vehicle_angular_velocity.timestamp_sample; - - // Guard against too small (< 0.2ms) and too large (> 20ms) dt's. - const float dt = math::constrain(((now - _last_run) / 1e6f), 0.0002f, 0.02f); - _last_run = now; - - const Vector3f angular_velocity{vehicle_angular_velocity.xyz}; - _angular_acceleration = Vector3f(vehicle_angular_velocity.xyz_derivative); - - /* check for updates in other topics */ - _vehicle_status_sub.update(&_vehicle_status); - _vehicle_control_mode_sub.update(&_vehicle_control_mode); - - if (_vehicle_land_detected_sub.updated()) { - vehicle_land_detected_s vehicle_land_detected; - - if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) { - _landed = vehicle_land_detected.landed; - _maybe_landed = vehicle_land_detected.maybe_landed; - } - } - - // Check for updates of hover thrust - if (!_vehicle_control_mode.flag_armed) { - _hover_thrust = _param_mpc_thr_hover.get(); - - } else if (_param_mpc_use_hte.get()) { - hover_thrust_estimate_s hte; - - if (_hover_thrust_estimate_sub.update(&hte)) { - if (hte.valid) { - _hover_thrust = hte.hover_thrust; - } - } - } - - // check rates setpoint topic - vehicle_rates_setpoint_s vehicle_rates_setpoint; - - if (_vehicle_rates_setpoint_sub.update(&vehicle_rates_setpoint)) { - _angular_velocity_sp(0) = vehicle_rates_setpoint.roll; - _angular_velocity_sp(1) = vehicle_rates_setpoint.pitch; - _angular_velocity_sp(2) = vehicle_rates_setpoint.yaw; - _thrust_sp = Vector3f(vehicle_rates_setpoint.thrust_body); - - // Scale _thrust_sp in Newton, assuming _hover_thrust is equivalent to 1G - _thrust_sp *= (_param_vm_mass.get() * CONSTANTS_ONE_G / _hover_thrust); - } - - // run the controller - if (_vehicle_control_mode.flag_control_rates_enabled) { - // reset integral if disarmed - if (!_vehicle_control_mode.flag_armed || _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - _control.resetIntegral(); - } - - // update saturation status from mixer 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; - } - } - } - - _control.setSaturationStatus(saturation_positive, saturation_negative); - } - - // run rate controller - _control.update(angular_velocity, _angular_velocity_sp, _angular_acceleration, dt, _maybe_landed || _landed); - - // publish rate controller status - rate_ctrl_status_s rate_ctrl_status{}; - Vector3f integral = _control.getIntegral(); - rate_ctrl_status.rollspeed_integ = integral(0); - rate_ctrl_status.pitchspeed_integ = integral(1); - rate_ctrl_status.yawspeed_integ = integral(2); - rate_ctrl_status.timestamp = hrt_absolute_time(); - _rate_ctrl_status_pub.publish(rate_ctrl_status); - - // publish controller output - publish_angular_acceleration_setpoint(); - publish_torque_setpoint(); - publish_thrust_setpoint(); - } - } - - perf_end(_loop_perf); -} - -void -AngularVelocityController::publish_angular_acceleration_setpoint() -{ - Vector3f angular_accel_sp = _control.getAngularAccelerationSetpoint(); - vehicle_angular_acceleration_setpoint_s v_angular_accel_sp = {}; - v_angular_accel_sp.timestamp_sample = _timestamp_sample; - v_angular_accel_sp.xyz[0] = (PX4_ISFINITE(angular_accel_sp(0))) ? angular_accel_sp(0) : 0.0f; - v_angular_accel_sp.xyz[1] = (PX4_ISFINITE(angular_accel_sp(1))) ? angular_accel_sp(1) : 0.0f; - v_angular_accel_sp.xyz[2] = (PX4_ISFINITE(angular_accel_sp(2))) ? angular_accel_sp(2) : 0.0f; - v_angular_accel_sp.timestamp = hrt_absolute_time(); - _vehicle_angular_acceleration_setpoint_pub.publish(v_angular_accel_sp); -} - -void -AngularVelocityController::publish_torque_setpoint() -{ - Vector3f torque_sp = _control.getTorqueSetpoint(); - vehicle_torque_setpoint_s v_torque_sp = {}; - v_torque_sp.timestamp_sample = _timestamp_sample; - v_torque_sp.xyz[0] = (PX4_ISFINITE(torque_sp(0))) ? torque_sp(0) : 0.0f; - v_torque_sp.xyz[1] = (PX4_ISFINITE(torque_sp(1))) ? torque_sp(1) : 0.0f; - v_torque_sp.xyz[2] = (PX4_ISFINITE(torque_sp(2))) ? torque_sp(2) : 0.0f; - v_torque_sp.timestamp = hrt_absolute_time(); - _vehicle_torque_setpoint_pub.publish(v_torque_sp); -} - -void -AngularVelocityController::publish_thrust_setpoint() -{ - vehicle_thrust_setpoint_s v_thrust_sp = {}; - v_thrust_sp.timestamp_sample = _timestamp_sample; - v_thrust_sp.xyz[0] = (PX4_ISFINITE(_thrust_sp(0))) ? (_thrust_sp(0)) : 0.0f; - v_thrust_sp.xyz[1] = (PX4_ISFINITE(_thrust_sp(1))) ? (_thrust_sp(1)) : 0.0f; - v_thrust_sp.xyz[2] = (PX4_ISFINITE(_thrust_sp(2))) ? (_thrust_sp(2)) : 0.0f; - v_thrust_sp.timestamp = hrt_absolute_time(); - _vehicle_thrust_setpoint_pub.publish(v_thrust_sp); -} - -int AngularVelocityController::task_spawn(int argc, char *argv[]) -{ - AngularVelocityController *instance = new AngularVelocityController(); - - 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 AngularVelocityController::print_status() -{ - PX4_INFO("Running"); - - perf_print_counter(_loop_perf); - - return 0; -} - -int AngularVelocityController::custom_command(int argc, char *argv[]) -{ - return print_usage("unknown command"); -} - -int AngularVelocityController::print_usage(const char *reason) -{ - if (reason) { - PX4_WARN("%s\n", reason); - } - - PRINT_MODULE_DESCRIPTION( - R"DESCR_STR( -### Description -This implements the angular velocity controller. -It takes angular velocity setpoints and measured angular -velocity as inputs and outputs actuator setpoints. - -The controller has a PID loop for angular rate error. - -)DESCR_STR"); - - PRINT_MODULE_USAGE_NAME(MODULE_NAME, "controller"); - PRINT_MODULE_USAGE_COMMAND("start"); - PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); - - return 0; -} - -/** - * Angular velocity controller app start / stop handling function - */ -extern "C" __EXPORT int angular_velocity_controller_main(int argc, char *argv[]); - -int angular_velocity_controller_main(int argc, char *argv[]) -{ - return AngularVelocityController::main(argc, argv); -} diff --git a/src/modules/angular_velocity_controller/AngularVelocityController.hpp b/src/modules/angular_velocity_controller/AngularVelocityController.hpp deleted file mode 100644 index c28e36a78d..0000000000 --- a/src/modules/angular_velocity_controller/AngularVelocityController.hpp +++ /dev/null @@ -1,176 +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. - * - ****************************************************************************/ - -#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 - -using namespace time_literals; - -class AngularVelocityController : public ModuleBase, public ModuleParams, - public px4::WorkItem -{ -public: - AngularVelocityController(); - - virtual ~AngularVelocityController(); - - /** @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); - - /** @see ModuleBase::print_status() */ - int print_status() override; - - void Run() override; - - bool init(); - -private: - - /** - * initialize some vectors/matrices from parameters - */ - void parameters_updated(); - - void vehicle_status_poll(); - - void publish_angular_acceleration_setpoint(); - void publish_torque_setpoint(); - void publish_thrust_setpoint(); - void publish_actuator_controls(); - - AngularVelocityControl _control; ///< class for control calculations - - uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - - uORB::Subscription _control_allocator_status_sub{ORB_ID(control_allocator_status)}; /**< motor limits subscription */ - uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle control mode subscription */ - uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */ - uORB::Subscription _vehicle_rates_setpoint_sub{ORB_ID(vehicle_rates_setpoint)}; /**< vehicle rates setpoint subscription */ - uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */ - uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)}; - - uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)}; - - uORB::Publication _rate_ctrl_status_pub{ORB_ID(rate_ctrl_status)}; /**< controller status publication */ - uORB::Publication _vehicle_angular_acceleration_setpoint_pub{ORB_ID(vehicle_angular_acceleration_setpoint)}; /**< angular acceleration setpoint publication */ - uORB::Publication _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)}; /**< thrust setpoint publication */ - uORB::Publication _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)}; /**< torque setpoint publication */ - - vehicle_control_mode_s _vehicle_control_mode{}; - vehicle_status_s _vehicle_status{}; - - bool _landed{true}; - bool _maybe_landed{true}; - - float _battery_status_scale{0.0f}; - - perf_counter_t _loop_perf; /**< loop duration performance counter */ - - matrix::Vector3f _angular_velocity_sp; /**< angular velocity setpoint */ - matrix::Vector3f _angular_acceleration; /**< angular acceleration (estimated) */ - matrix::Vector3f _thrust_sp; /**< thrust setpoint */ - - float _hover_thrust{0.5f}; /**< Normalized hover thrust **/ - - bool _gear_state_initialized{false}; /**< true if the gear state has been initialized */ - - hrt_abstime _task_start{hrt_absolute_time()}; - hrt_abstime _last_run{0}; - hrt_abstime _timestamp_sample{0}; - - DEFINE_PARAMETERS( - (ParamFloat) _param_avc_x_p, - (ParamFloat) _param_avc_x_i, - (ParamFloat) _param_avc_x_i_lim, - (ParamFloat) _param_avc_x_d, - (ParamFloat) _param_avc_x_ff, - (ParamFloat) _param_avc_x_k, - - (ParamFloat) _param_avc_y_p, - (ParamFloat) _param_avc_y_i, - (ParamFloat) _param_avc_y_i_lim, - (ParamFloat) _param_avc_y_d, - (ParamFloat) _param_avc_y_ff, - (ParamFloat) _param_avc_y_k, - - (ParamFloat) _param_avc_z_p, - (ParamFloat) _param_avc_z_i, - (ParamFloat) _param_avc_z_i_lim, - (ParamFloat) _param_avc_z_d, - (ParamFloat) _param_avc_z_ff, - (ParamFloat) _param_avc_z_k, - - (ParamFloat) _param_vm_mass, - (ParamFloat) _param_vm_inertia_xx, - (ParamFloat) _param_vm_inertia_yy, - (ParamFloat) _param_vm_inertia_zz, - (ParamFloat) _param_vm_inertia_xy, - (ParamFloat) _param_vm_inertia_xz, - (ParamFloat) _param_vm_inertia_yz, - - (ParamFloat) _param_mpc_thr_hover, - (ParamBool) _param_mpc_use_hte - ) - -}; diff --git a/src/modules/angular_velocity_controller/CMakeLists.txt b/src/modules/angular_velocity_controller/CMakeLists.txt deleted file mode 100644 index 4cfc369db7..0000000000 --- a/src/modules/angular_velocity_controller/CMakeLists.txt +++ /dev/null @@ -1,49 +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. -# -############################################################################ - -add_subdirectory(AngularVelocityControl) - -px4_add_module( - MODULE modules__angular_velocity_control - MAIN angular_velocity_controller - COMPILE_FLAGS - ${MAX_CUSTOM_OPT_LEVEL} - SRCS - AngularVelocityController.cpp - AngularVelocityController.hpp - DEPENDS - circuit_breaker - mathlib - AngularVelocityControl - px4_work_queue - ) diff --git a/src/modules/angular_velocity_controller/Kconfig b/src/modules/angular_velocity_controller/Kconfig deleted file mode 100644 index f4698e3b1b..0000000000 --- a/src/modules/angular_velocity_controller/Kconfig +++ /dev/null @@ -1,12 +0,0 @@ -menuconfig MODULES_ANGULAR_VELOCITY_CONTROLLER - bool "angular_velocity_controller" - default n - ---help--- - Enable support for angular_velocity_controller - -menuconfig USER_ANGULAR_VELOCITY_CONTROLLER - bool "angular_velocity_controller running as userspace module" - default y - depends on BOARD_PROTECTED && MODULES_ANGULAR_VELOCITY_CONTROLLER - ---help--- - Put angular_velocity_controller in userspace memory diff --git a/src/modules/angular_velocity_controller/angular_velocity_controller_params.c b/src/modules/angular_velocity_controller/angular_velocity_controller_params.c deleted file mode 100644 index 88c61c4004..0000000000 --- a/src/modules/angular_velocity_controller/angular_velocity_controller_params.c +++ /dev/null @@ -1,297 +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 angular_velocity_controller_params.c - * Parameters for angular velocity controller. - * - * @author Lorenz Meier - * @author Anton Babushkin - * @author Julien Lecoeur - */ - -/** - * Body X axis angular velocity P gain - * - * Body X axis angular velocity proportional gain, i.e. control output for angular speed error 1 rad/s. - * - * @unit 1/s - * @min 0.0 - * @max 20.0 - * @decimal 3 - * @increment 0.01 - * @group Angular Velocity Control - */ -PARAM_DEFINE_FLOAT(AVC_X_P, 18.f); - -/** - * Body X axis angular velocity I gain - * - * Body X axis angular velocity integral gain. Can be set to compensate static thrust difference or gravity center offset. - * - * @unit Nm/rad - * @min 0.0 - * @decimal 3 - * @increment 0.01 - * @group Angular Velocity Control - */ -PARAM_DEFINE_FLOAT(AVC_X_I, 0.2f); - -/** - * Body X axis angular velocity integrator limit - * - * Body X axis angular velocity 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. - * - * @unit Nm - * @min 0.0 - * @decimal 2 - * @increment 0.01 - * @group Angular Velocity Control - */ -PARAM_DEFINE_FLOAT(AVC_X_I_LIM, 0.3f); - -/** - * Body X axis angular velocity D gain - * - * Body X axis angular velocity differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. - * - * @min 0.0 - * @max 2.0 - * @decimal 4 - * @increment 0.01 - * @group Angular Velocity Control - */ -PARAM_DEFINE_FLOAT(AVC_X_D, 0.36f); - -/** - * Body X axis angular velocity feedforward gain - * - * Improves tracking performance. - * - * @unit Nm/(rad/s) - * @min 0.0 - * @decimal 4 - * @group Angular Velocity Control - */ -PARAM_DEFINE_FLOAT(AVC_X_FF, 0.0f); - -/** - * Body X axis angular velocity controller gain - * - * Global gain of the controller. - * - * This gain scales the P, I and D terms of the controller: - * output = AVC_X_K * (AVC_X_P * error - * + AVC_X_I * error_integral - * + AVC_X_D * error_derivative) - * Set AVC_X_P=1 to implement a PID in the ideal form. - * Set AVC_X_K=1 to implement a PID in the parallel form. - * - * @min 0.0 - * @max 5.0 - * @decimal 4 - * @increment 0.0005 - * @group Angular Velocity Control - */ -PARAM_DEFINE_FLOAT(AVC_X_K, 1.0f); - -/** - * Body Y axis angular velocity P gain - * - * Body Y axis angular velocity proportional gain, i.e. control output for angular speed error 1 rad/s. - * - * @unit 1/s - * @min 0.0 - * @max 20.0 - * @decimal 3 - * @increment 0.01 - * @group Angular Velocity Control - */ -PARAM_DEFINE_FLOAT(AVC_Y_P, 18.f); - -/** - * Body Y axis angular velocity I gain - * - * Body Y axis angular velocity integral gain. Can be set to compensate static thrust difference or gravity center offset. - * - * @unit Nm/rad - * @min 0.0 - * @decimal 3 - * @increment 0.01 - * @group Angular Velocity Control - */ -PARAM_DEFINE_FLOAT(AVC_Y_I, 0.2f); - -/** - * Body Y axis angular velocity integrator limit - * - * Body Y axis angular velocity 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. - * - * @unit Nm - * @min 0.0 - * @decimal 2 - * @increment 0.01 - * @group Angular Velocity Control - */ -PARAM_DEFINE_FLOAT(AVC_Y_I_LIM, 0.3f); - -/** - * Body Y axis angular velocity D gain - * - * Body Y axis angular velocity differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. - * - * @min 0.0 - * @max 2.0 - * @decimal 4 - * @increment 0.01 - * @group Angular Velocity Control - */ -PARAM_DEFINE_FLOAT(AVC_Y_D, 0.36f); - -/** - * Body Y axis angular velocity feedforward - * - * Improves tracking performance. - * - * @unit Nm/(rad/s) - * @min 0.0 - * @decimal 4 - * @group Angular Velocity Control - */ -PARAM_DEFINE_FLOAT(AVC_Y_FF, 0.0f); - -/** - * Body Y axis angular velocity controller gain - * - * Global gain of the controller. - * - * This gain scales the P, I and D terms of the controller: - * output = AVC_Y_K * (AVC_Y_P * error - * + AVC_Y_I * error_integral - * + AVC_Y_D * error_derivative) - * Set AVC_Y_P=1 to implement a PID in the ideal form. - * Set AVC_Y_K=1 to implement a PID in the parallel form. - * - * @min 0.0 - * @max 20.0 - * @decimal 4 - * @increment 0.0005 - * @group Angular Velocity Control - */ -PARAM_DEFINE_FLOAT(AVC_Y_K, 1.0f); - -/** - * Body Z axis angular velocity P gain - * - * Body Z axis angular velocity proportional gain, i.e. control output for angular speed error 1 rad/s. - * - * @unit 1/s - * @min 0.0 - * @max 20.0 - * @decimal 2 - * @increment 0.01 - * @group Angular Velocity Control - */ -PARAM_DEFINE_FLOAT(AVC_Z_P, 7.f); - -/** - * Body Z axis angular velocity I gain - * - * Body Z axis angular velocity integral gain. Can be set to compensate static thrust difference or gravity center offset. - * - * @unit Nm/rad - * @min 0.0 - * @decimal 2 - * @increment 0.01 - * @group Angular Velocity Control - */ -PARAM_DEFINE_FLOAT(AVC_Z_I, 0.1f); - -/** - * Body Z axis angular velocity integrator limit - * - * Body Z axis angular velocity 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. - * - * @unit Nm - * @min 0.0 - * @decimal 2 - * @increment 0.01 - * @group Angular Velocity Control - */ -PARAM_DEFINE_FLOAT(AVC_Z_I_LIM, 0.30f); - -/** - * Body Z axis angular velocity D gain - * - * Body Z axis angular velocity differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. - * - * @min 0.0 - * @max 2.0 - * @decimal 2 - * @increment 0.01 - * @group Angular Velocity Control - */ -PARAM_DEFINE_FLOAT(AVC_Z_D, 0.0f); - -/** - * Body Z axis angular velocity feedforward - * - * Improves tracking performance. - * - * @unit Nm/(rad/s) - * @min 0.0 - * @decimal 4 - * @increment 0.01 - * @group Angular Velocity Control - */ -PARAM_DEFINE_FLOAT(AVC_Z_FF, 0.0f); - -/** - * Body Z axis angular velocity controller gain - * - * Global gain of the controller. - * - * This gain scales the P, I and D terms of the controller: - * output = AVC_Z_K * (AVC_Z_P * error - * + AVC_Z_I * error_integral - * + AVC_Z_D * error_derivative) - * Set AVC_Z_P=1 to implement a PID in the ideal form. - * Set AVC_Z_K=1 to implement a PID in the parallel form. - * - * @min 0.0 - * @max 5.0 - * @decimal 4 - * @increment 0.0005 - * @group Angular Velocity Control - */ -PARAM_DEFINE_FLOAT(AVC_Z_K, 1.0f); diff --git a/src/modules/angular_velocity_controller/vehicle_model_params.c b/src/modules/angular_velocity_controller/vehicle_model_params.c deleted file mode 100644 index a1417791ab..0000000000 --- a/src/modules/angular_velocity_controller/vehicle_model_params.c +++ /dev/null @@ -1,109 +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 vehicle_model_params.c - * Parameters for vehicle model. - * - * @author Julien Lecoeur - */ - -/** - * Mass - * - * @unit kg - * @decimal 5 - * @increment 0.00001 - * @group Vehicle Model - */ -PARAM_DEFINE_FLOAT(VM_MASS, 1.f); - -/** - * Inertia matrix, XX component - * - * @unit kg m^2 - * @decimal 5 - * @increment 0.00001 - * @group Vehicle Model - */ -PARAM_DEFINE_FLOAT(VM_INERTIA_XX, 0.01f); - -/** - * Inertia matrix, YY component - * - * @unit kg m^2 - * @decimal 5 - * @increment 0.00001 - * @group Vehicle Model - */ -PARAM_DEFINE_FLOAT(VM_INERTIA_YY, 0.01f); - -/** - * Inertia matrix, ZZ component - * - * @unit kg m^2 - * @decimal 5 - * @increment 0.00001 - * @group Vehicle Model - */ -PARAM_DEFINE_FLOAT(VM_INERTIA_ZZ, 0.01f); - -/** - * Inertia matrix, XY component - * - * @unit kg m^2 - * @decimal 5 - * @increment 0.00001 - * @group Vehicle Model - */ -PARAM_DEFINE_FLOAT(VM_INERTIA_XY, 0.f); - -/** - * Inertia matrix, XZ component - * - * @unit kg m^2 - * @decimal 5 - * @increment 0.00001 - * @group Vehicle Model - */ -PARAM_DEFINE_FLOAT(VM_INERTIA_XZ, 0.f); - -/** - * Inertia matrix, YZ component - * - * @unit kg m^2 - * @decimal 5 - * @increment 0.00001 - * @group Vehicle Model - */ -PARAM_DEFINE_FLOAT(VM_INERTIA_YZ, 0.f);