aboutsummaryrefslogtreecommitdiff
path: root/src/modules/fw_att_control_vector/ecl_fw_att_control_vector.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/fw_att_control_vector/ecl_fw_att_control_vector.cpp')
-rw-r--r--src/modules/fw_att_control_vector/ecl_fw_att_control_vector.cpp165
1 files changed, 0 insertions, 165 deletions
diff --git a/src/modules/fw_att_control_vector/ecl_fw_att_control_vector.cpp b/src/modules/fw_att_control_vector/ecl_fw_att_control_vector.cpp
deleted file mode 100644
index 16e60f3e0..000000000
--- a/src/modules/fw_att_control_vector/ecl_fw_att_control_vector.cpp
+++ /dev/null
@@ -1,165 +0,0 @@
-/****************************************************************************
- *
- * Copyright (c) 2013 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 ecl_fw_att_control_vector.cpp
- *
- * Fixed wing attitude controller
- *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- * @author Tobias Naegeli <naegelit@student.ethz.ch>
- *
- */
-
-#include <mathlib/mathlib.h>
-#include <systemlib/geo/geo.h>
-#include "ecl_fw_att_control_vector.h"
-
-ECL_FWAttControlVector::ECL_FWAttControlVector() :
- _integral_error(0.0f, 0.0f),
- _integral_max(1000.0f, 1000.0f),
- _rates_demanded(0.0f, 0.0f, 0.0f),
- _k_p(1.0f, 1.0f, 1.0f),
- _k_d(1.0f, 1.0f, 1.0f),
- _k_i(1.0f, 1.0f, 1.0f),
- _integral_lock(false),
- _p_airspeed_min(12.0f),
- _p_airspeed_max(24.0f),
- _p_tconst(0.1f),
- _p_roll_ffd(1.0f),
- _airspeed_enabled(false)
- {
-
- }
-
-/**
- *
- * @param F_des_in Desired force vector in body frame (NED). Straight flight is (0 0 -1)',
- * banking hard right (1 0 -1)' and pitching down (1 0 -1)'.
- */
-void ECL_FWAttControlVector::control(float dt, float airspeed, float airspeed_scaling, const math::Dcm &R_nb, float roll, float pitch, float yaw, const math::Vector &F_des_in,
- const math::Vector &angular_rates,
- math::Vector &moment_des, float &thrust)
-{
- if (!isfinite(airspeed) || !airspeed_enabled()) {
- // If airspeed is not available or Inf/NaN, use the center
- // of min / max
- airspeed = 0.5f * (_p_airspeed_min + _p_airspeed_max);
- }
-
- math::Dcm R_bn(R_nb.transpose());
- math::Matrix R_yaw_bn = math::Dcm(math::EulerAngles(0.0f, 0.0f, yaw)).transpose();
-
- // Establish actuator signs and lift compensation
- float lift_sign = (R_bn(3, 3) >= 0) ? 1.0f : -1.0f;
-
- float lift_comp = CONSTANTS_ONE_G / math::max(airspeed , float(_p_airspeed_min) * (R_nb(2,3) * R_nb(2,3)) / (R_nb(3,3) * sqrtf((R_nb(2,3) * R_nb(2,3)) + (R_nb(3,3) * R_nb(3,3)))));
- //float lift_comp = fabsf((CONSTANTS_ONE_G / math::max(airspeed , float(_p_airspeed_min)) * tanf(roll) * sinf(roll))) * _p_roll_ffd;
-
- float cy = cosf(yaw);
- float sy = sinf(yaw);
-
- //math::Matrix RYaw = math::Dcm(cy,-sy,0.0f,sy,cy,0.0f,0.0f,0.0f,1.0f);
- math::Vector z_b = math::Vector3(R_bn(0,2), R_bn(1,2), R_bn(2,2));
-
- math::Vector3 F_des = R_yaw_bn * F_des_in;
-
- // desired thrust in body frame
- // avoid division by zero
- // compensates for thrust loss due to roll/pitch
- if (F_des(2) >= 0.1f) {
- thrust = F_des(2) / R_bn(2, 2);
- } else {
- F_des(2) = 0.1f;
- thrust= F_des(2) / R_bn(2, 2);
- }
-
- math::Vector3 x_B_des;
- math::Vector3 y_B_des;
- math::Vector3 z_B_des;
-
- // desired body z axis
- z_B_des = (F_des / F_des.norm());
-
- // desired direction in world coordinates (yaw angle)
- math::Vector3 x_C(cy, sy, 0.0f);
- // desired body y axis
- y_B_des = z_B_des.cross(x_C) / (z_B_des.cross(x_C)).norm();
- // desired body x axis
- x_B_des = y_B_des.cross(z_B_des);
- // desired Rotation Matrix
- math::Dcm R_des(x_B_des(0), x_B_des(1), x_B_des(2),
- y_B_des(0), y_B_des(1), y_B_des(2),
- z_B_des(0), z_B_des(1), z_B_des(2));
-
-
- // Attitude Controller
- // P controller
-
- // error rotation matrix
- // operation executed in quaternion space to allow large differences
- // XXX switch between operations based on difference,
- // benchmark both options
- math::Quaternion e_q = math::Quaternion(R_des) - math::Quaternion(R_bn);
- // Renormalize
- e_q = e_q / e_q.norm();
- math::Matrix e_R = math::Dcm(e_q);
- //small angles: math::Matrix e_R = (R_des.transpose() * R_bn - R_bn.transpose() * R_des) * 0.5f;
-
- // error rotation vector
- math::Vector e_R_v(3);
- e_R_v(0) = e_R(1,2);
- e_R_v(1) = e_R(0,2);
- e_R_v(2) = e_R(0,1);
-
-
- // attitude integral error
- math::Vector intError = math::Vector3(0.0f, 0.0f, 0.0f);
- if (!_integral_lock) {
-
- if (fabsf(_integral_error(0)) < _integral_max(0)) {
- _integral_error(0) = _integral_error(0) + e_R_v(0) * dt;
- }
-
- if (fabsf(_integral_error(1)) < _integral_max(1)) {
- _integral_error(1) = _integral_error(1) + e_R_v(1) * dt;
- }
-
- intError(0) = _integral_error(0);
- intError(1) = _integral_error(1);
- intError(2) = 0.0f;
- }
-
- _rates_demanded = (e_R_v * _k_p(0) + angular_rates * _k_d(0) + intError * _k_i(0));
- moment_des = _rates_demanded;
-}
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-10 17:56:57 +0000