|
MAV'RIC
|
|
MAV'RIC
|
00001 /******************************************************************************* 00002 * Copyright (c) 2009-2014, MAV'RIC Development Team 00003 * All rights reserved. 00004 * 00005 * Redistribution and use in source and binary forms, with or without 00006 * modification, are permitted provided that the following conditions are met: 00007 * 00008 * 1. Redistributions of source code must retain the above copyright notice, 00009 * this list of conditions and the following disclaimer. 00010 * 00011 * 2. Redistributions in binary form must reproduce the above copyright notice, 00012 * this list of conditions and the following disclaimer in the documentation 00013 * and/or other materials provided with the distribution. 00014 * 00015 * 3. Neither the name of the copyright holder nor the names of its contributors 00016 * may be used to endorse or promote products derived from this software without 00017 * specific prior written permission. 00018 * 00019 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00020 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00021 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00022 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 00023 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00024 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00025 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00026 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00027 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00028 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00029 * POSSIBILITY OF SUCH DAMAGE. 00030 ******************************************************************************/ 00031 00032 /******************************************************************************* 00033 * \file stabilisation_wing_default_config.hpp 00034 * 00035 * \author MAV'RIC Team 00036 * \author Simon Pyroth 00037 * 00038 * \brief Default values for cascade PID controller 00039 * 00040 ******************************************************************************/ 00041 00042 00043 #ifndef STABILISATION_WING_DEFAULT_CONFIG_HPP_ 00044 #define STABILISATION_WING_DEFAULT_CONFIG_HPP_ 00045 00046 #include "stabilisation_wing.hpp" 00047 00048 00049 static inline stabilisation_wing_conf_t stabilisation_wing_default_config() 00050 { 00051 stabilisation_wing_conf_t conf = {}; 00052 00053 conf.thrust_apriori = 0.25f; 00054 conf.pitch_angle_apriori = 0.0f; 00055 conf.pitch_angle_apriori_gain = -0.64f; 00056 conf.max_roll_angle = PI/3.0f; 00057 conf.take_off_thrust = 0.5f; 00058 conf.take_off_pitch = PI/6.0f; 00059 conf.landing_pitch = 0.0f; 00060 conf.landing_max_roll = PI/12.0f; 00061 00062 conf.stabiliser_stack = {}; 00063 00064 // ############################################################################# 00065 // ###### RATE CONTROL ####################################################### 00066 // ############################################################################# 00067 // ----------------------------------------------------------------- 00068 // ------ ROLL PID ------------------------------------------------- 00069 // ----------------------------------------------------------------- 00070 conf.stabiliser_stack.rate_stabiliser.rpy_controller[ROLL].p_gain = 0.065f; 00071 conf.stabiliser_stack.rate_stabiliser.rpy_controller[ROLL].clip_min = -1.0f; 00072 conf.stabiliser_stack.rate_stabiliser.rpy_controller[ROLL].clip_max = 1.0f; 00073 conf.stabiliser_stack.rate_stabiliser.rpy_controller[ROLL].integrator.gain = 0.0f; 00074 conf.stabiliser_stack.rate_stabiliser.rpy_controller[ROLL].integrator.clip_pre = 6.0f; 00075 conf.stabiliser_stack.rate_stabiliser.rpy_controller[ROLL].integrator.accumulator = 0.0f; 00076 conf.stabiliser_stack.rate_stabiliser.rpy_controller[ROLL].integrator.clip = 0.7f; 00077 conf.stabiliser_stack.rate_stabiliser.rpy_controller[ROLL].differentiator.gain = 0.0f; 00078 conf.stabiliser_stack.rate_stabiliser.rpy_controller[ROLL].differentiator.previous = 0.0f; 00079 conf.stabiliser_stack.rate_stabiliser.rpy_controller[ROLL].differentiator.clip = 0.7f; 00080 conf.stabiliser_stack.rate_stabiliser.rpy_controller[ROLL].output = 0.0f; 00081 conf.stabiliser_stack.rate_stabiliser.rpy_controller[ROLL].error = 0.0f; 00082 conf.stabiliser_stack.rate_stabiliser.rpy_controller[ROLL].last_update_s = 0.0f; 00083 conf.stabiliser_stack.rate_stabiliser.rpy_controller[ROLL].dt_s = 1; 00084 conf.stabiliser_stack.rate_stabiliser.rpy_controller[ROLL].soft_zone_width = 0.0f; 00085 // ----------------------------------------------------------------- 00086 // ------ PITCH PID ------------------------------------------------ 00087 // ----------------------------------------------------------------- 00088 conf.stabiliser_stack.rate_stabiliser.rpy_controller[PITCH].p_gain = 0.055f; 00089 conf.stabiliser_stack.rate_stabiliser.rpy_controller[PITCH].clip_min = -1.0f; 00090 conf.stabiliser_stack.rate_stabiliser.rpy_controller[PITCH].clip_max = 1.0f; 00091 conf.stabiliser_stack.rate_stabiliser.rpy_controller[PITCH].integrator.gain = 0.0f; 00092 conf.stabiliser_stack.rate_stabiliser.rpy_controller[PITCH].integrator.clip_pre = 6.0f; 00093 conf.stabiliser_stack.rate_stabiliser.rpy_controller[PITCH].integrator.accumulator = 0.0f; 00094 conf.stabiliser_stack.rate_stabiliser.rpy_controller[PITCH].integrator.clip = 0.7f; 00095 conf.stabiliser_stack.rate_stabiliser.rpy_controller[PITCH].differentiator.gain = 0.000f; 00096 conf.stabiliser_stack.rate_stabiliser.rpy_controller[PITCH].differentiator.previous = 0.0f; 00097 conf.stabiliser_stack.rate_stabiliser.rpy_controller[PITCH].differentiator.clip = 0.7f; 00098 conf.stabiliser_stack.rate_stabiliser.rpy_controller[PITCH].output = 0.0f; 00099 conf.stabiliser_stack.rate_stabiliser.rpy_controller[PITCH].error = 0.0f; 00100 conf.stabiliser_stack.rate_stabiliser.rpy_controller[PITCH].last_update_s = 0.0f; 00101 conf.stabiliser_stack.rate_stabiliser.rpy_controller[PITCH].dt_s = 1; 00102 conf.stabiliser_stack.rate_stabiliser.rpy_controller[PITCH].soft_zone_width = 0.0f; 00103 // ----------------------------------------------------------------- 00104 // ------ YAW PID -------------------------------------------------- 00105 // ----------------------------------------------------------------- 00106 conf.stabiliser_stack.rate_stabiliser.rpy_controller[YAW].p_gain = 0.0f; 00107 conf.stabiliser_stack.rate_stabiliser.rpy_controller[YAW].clip_min = -1000, 00108 conf.stabiliser_stack.rate_stabiliser.rpy_controller[YAW].clip_max = 1000, 00109 conf.stabiliser_stack.rate_stabiliser.rpy_controller[YAW].integrator.gain = 0.0f; 00110 conf.stabiliser_stack.rate_stabiliser.rpy_controller[YAW].integrator.clip_pre = 0.0f; 00111 conf.stabiliser_stack.rate_stabiliser.rpy_controller[YAW].integrator.accumulator = 0.0f; 00112 conf.stabiliser_stack.rate_stabiliser.rpy_controller[YAW].integrator.clip = 0.0f; 00113 conf.stabiliser_stack.rate_stabiliser.rpy_controller[YAW].differentiator.gain = 0.0f; 00114 conf.stabiliser_stack.rate_stabiliser.rpy_controller[YAW].differentiator.previous = 0.0f; 00115 conf.stabiliser_stack.rate_stabiliser.rpy_controller[YAW].differentiator.clip = 0.0f; 00116 conf.stabiliser_stack.rate_stabiliser.rpy_controller[YAW].output = 0.0f; 00117 conf.stabiliser_stack.rate_stabiliser.rpy_controller[YAW].error = 0.0f; 00118 conf.stabiliser_stack.rate_stabiliser.rpy_controller[YAW].last_update_s = 0.0f; 00119 conf.stabiliser_stack.rate_stabiliser.rpy_controller[YAW].dt_s = 1; 00120 conf.stabiliser_stack.rate_stabiliser.rpy_controller[YAW].soft_zone_width = 0.0f; 00121 // --------------------------------------------------------------------- 00122 // ------ THRUST PID --------------------------------------------------- 00123 // --------------------------------------------------------------------- 00124 conf.stabiliser_stack.rate_stabiliser.thrust_controller.p_gain = 1.0f; 00125 conf.stabiliser_stack.rate_stabiliser.thrust_controller.clip_min = -1000, 00126 conf.stabiliser_stack.rate_stabiliser.thrust_controller.clip_max = 1000, 00127 conf.stabiliser_stack.rate_stabiliser.thrust_controller.integrator.gain = 0.0f; 00128 conf.stabiliser_stack.rate_stabiliser.thrust_controller.integrator.clip_pre = 0.0f; 00129 conf.stabiliser_stack.rate_stabiliser.thrust_controller.integrator.accumulator = 0.0f; 00130 conf.stabiliser_stack.rate_stabiliser.thrust_controller.integrator.clip = 0.0f; 00131 conf.stabiliser_stack.rate_stabiliser.thrust_controller.differentiator.gain = 0.0f; 00132 conf.stabiliser_stack.rate_stabiliser.thrust_controller.differentiator.previous = 0.0f; 00133 conf.stabiliser_stack.rate_stabiliser.thrust_controller.differentiator.clip = 0.0f; 00134 conf.stabiliser_stack.rate_stabiliser.thrust_controller.output = 0.0f; 00135 conf.stabiliser_stack.rate_stabiliser.thrust_controller.error = 0.0f; 00136 conf.stabiliser_stack.rate_stabiliser.thrust_controller.last_update_s = 0.0f; 00137 conf.stabiliser_stack.rate_stabiliser.thrust_controller.dt_s = 1; 00138 conf.stabiliser_stack.rate_stabiliser.thrust_controller.soft_zone_width = 0.0f; 00139 // --------------------------------------------------------------------- 00140 // ------ OUTPUT ------------------------------------------------------- 00141 // --------------------------------------------------------------------- 00142 conf.stabiliser_stack.rate_stabiliser.output = {}; 00143 conf.stabiliser_stack.rate_stabiliser.output.rpy[0] = 0.0f; 00144 conf.stabiliser_stack.rate_stabiliser.output.rpy[1] = 0.0f; 00145 conf.stabiliser_stack.rate_stabiliser.output.rpy[2] = 0.0f; 00146 conf.stabiliser_stack.rate_stabiliser.output.thrust = 0.0f; 00147 conf.stabiliser_stack.rate_stabiliser.output.tvel[0] = 0.0f; 00148 conf.stabiliser_stack.rate_stabiliser.output.tvel[1] = 0.0f; 00149 conf.stabiliser_stack.rate_stabiliser.output.tvel[2] = 0.0f; 00150 conf.stabiliser_stack.rate_stabiliser.output.theading = 0.0f; 00151 conf.stabiliser_stack.rate_stabiliser.output.control_mode = RATE_COMMAND_MODE; 00152 conf.stabiliser_stack.rate_stabiliser.output.yaw_mode = YAW_RELATIVE; 00153 // ############################################################################# 00154 // ###### ATTITUDE CONTROL ################################################### 00155 // ############################################################################# 00156 // ----------------------------------------------------------------- 00157 // ------ ROLL PID ------------------------------------------------- 00158 // ----------------------------------------------------------------- 00159 conf.stabiliser_stack.attitude_stabiliser.rpy_controller[ROLL].p_gain = 15.0f; 00160 conf.stabiliser_stack.attitude_stabiliser.rpy_controller[ROLL].clip_min = -100.0f; 00161 conf.stabiliser_stack.attitude_stabiliser.rpy_controller[ROLL].clip_max = 100.0f; 00162 conf.stabiliser_stack.attitude_stabiliser.rpy_controller[ROLL].integrator.gain = 0.0f; 00163 conf.stabiliser_stack.attitude_stabiliser.rpy_controller[ROLL].integrator.clip_pre = 0.0f; 00164 conf.stabiliser_stack.attitude_stabiliser.rpy_controller[ROLL].integrator.accumulator = 0.0f; 00165 conf.stabiliser_stack.attitude_stabiliser.rpy_controller[ROLL].integrator.clip = 4.0f; 00166 conf.stabiliser_stack.attitude_stabiliser.rpy_controller[ROLL].differentiator.gain = 0.15f; 00167 conf.stabiliser_stack.attitude_stabiliser.rpy_controller[ROLL].differentiator.previous = 0.0f; 00168 conf.stabiliser_stack.attitude_stabiliser.rpy_controller[ROLL].differentiator.clip = 4.0f; 00169 conf.stabiliser_stack.attitude_stabiliser.rpy_controller[ROLL].output = 0.0f; 00170 conf.stabiliser_stack.attitude_stabiliser.rpy_controller[ROLL].error = 0.0f; 00171 conf.stabiliser_stack.attitude_stabiliser.rpy_controller[ROLL].last_update_s = 0.0f; 00172 conf.stabiliser_stack.attitude_stabiliser.rpy_controller[ROLL].dt_s = 1; 00173 conf.stabiliser_stack.attitude_stabiliser.rpy_controller[ROLL].soft_zone_width = 0.0f; 00174 // ----------------------------------------------------------------- 00175 // ------ PITCH PID ------------------------------------------------ 00176 // ----------------------------------------------------------------- 00177 conf.stabiliser_stack.attitude_stabiliser.rpy_controller[PITCH].p_gain = 15.0f; 00178 conf.stabiliser_stack.attitude_stabiliser.rpy_controller[PITCH].clip_min = -100.0f; 00179 conf.stabiliser_stack.attitude_stabiliser.rpy_controller[PITCH].clip_max = 100.0f; 00180 conf.stabiliser_stack.attitude_stabiliser.rpy_controller[PITCH].integrator.gain = 0.0f; 00181 conf.stabiliser_stack.attitude_stabiliser.rpy_controller[PITCH].integrator.clip_pre = 0.0f; 00182 conf.stabiliser_stack.attitude_stabiliser.rpy_controller[PITCH].integrator.accumulator = 0.0f; 00183 conf.stabiliser_stack.attitude_stabiliser.rpy_controller[PITCH].integrator.clip = 4.0f; 00184 conf.stabiliser_stack.attitude_stabiliser.rpy_controller[PITCH].differentiator.gain = 0.0f; 00185 conf.stabiliser_stack.attitude_stabiliser.rpy_controller[PITCH].differentiator.previous = 0.0f; 00186 conf.stabiliser_stack.attitude_stabiliser.rpy_controller[PITCH].differentiator.clip = 4.0f; 00187 conf.stabiliser_stack.attitude_stabiliser.rpy_controller[PITCH].output = 0.0f; 00188 conf.stabiliser_stack.attitude_stabiliser.rpy_controller[PITCH].error = 0.0f; 00189 conf.stabiliser_stack.attitude_stabiliser.rpy_controller[PITCH].last_update_s = 0.0f; 00190 conf.stabiliser_stack.attitude_stabiliser.rpy_controller[PITCH].dt_s = 1; 00191 conf.stabiliser_stack.attitude_stabiliser.rpy_controller[PITCH].soft_zone_width = 0.0f; 00192 // ----------------------------------------------------------------- 00193 // ------ YAW PID -------------------------------------------------- 00194 // ----------------------------------------------------------------- 00195 conf.stabiliser_stack.attitude_stabiliser.rpy_controller[YAW].p_gain = 3.0f; 00196 conf.stabiliser_stack.attitude_stabiliser.rpy_controller[YAW].clip_min = -1.5f; 00197 conf.stabiliser_stack.attitude_stabiliser.rpy_controller[YAW].clip_max = 1.5f; 00198 conf.stabiliser_stack.attitude_stabiliser.rpy_controller[YAW].integrator.gain = 0.0f; 00199 conf.stabiliser_stack.attitude_stabiliser.rpy_controller[YAW].integrator.clip_pre = 0.0f; 00200 conf.stabiliser_stack.attitude_stabiliser.rpy_controller[YAW].integrator.accumulator = 0.0f; 00201 conf.stabiliser_stack.attitude_stabiliser.rpy_controller[YAW].integrator.clip = 0.0f; 00202 conf.stabiliser_stack.attitude_stabiliser.rpy_controller[YAW].differentiator.gain = 0.0f; 00203 conf.stabiliser_stack.attitude_stabiliser.rpy_controller[YAW].differentiator.previous = 0.0f; 00204 conf.stabiliser_stack.attitude_stabiliser.rpy_controller[YAW].differentiator.clip = 0.0f; 00205 conf.stabiliser_stack.attitude_stabiliser.rpy_controller[YAW].output = 0.0f; 00206 conf.stabiliser_stack.attitude_stabiliser.rpy_controller[YAW].error = 0.0f; 00207 conf.stabiliser_stack.attitude_stabiliser.rpy_controller[YAW].last_update_s = 0.0f; 00208 conf.stabiliser_stack.attitude_stabiliser.rpy_controller[YAW].dt_s = 1; 00209 conf.stabiliser_stack.attitude_stabiliser.rpy_controller[YAW].soft_zone_width = 0.0f; 00210 // --------------------------------------------------------------------- 00211 // ------ THRUST PID --------------------------------------------------- 00212 // --------------------------------------------------------------------- 00213 conf.stabiliser_stack.attitude_stabiliser.thrust_controller.p_gain = 1.0f; 00214 conf.stabiliser_stack.attitude_stabiliser.thrust_controller.clip_min = -1, 00215 conf.stabiliser_stack.attitude_stabiliser.thrust_controller.clip_max = 1; 00216 conf.stabiliser_stack.attitude_stabiliser.thrust_controller.integrator.gain = 0.0f; 00217 conf.stabiliser_stack.attitude_stabiliser.thrust_controller.integrator.clip_pre = 0.0f; 00218 conf.stabiliser_stack.attitude_stabiliser.thrust_controller.integrator.accumulator = 0.0f; 00219 conf.stabiliser_stack.attitude_stabiliser.thrust_controller.integrator.clip = 0.0f; 00220 conf.stabiliser_stack.attitude_stabiliser.thrust_controller.differentiator.gain = 0.0f; 00221 conf.stabiliser_stack.attitude_stabiliser.thrust_controller.differentiator.previous = 0.0f; 00222 conf.stabiliser_stack.attitude_stabiliser.thrust_controller.differentiator.clip = 0.0f; 00223 conf.stabiliser_stack.attitude_stabiliser.thrust_controller.output = 0.0f; 00224 conf.stabiliser_stack.attitude_stabiliser.thrust_controller.error = 0.0f; 00225 conf.stabiliser_stack.attitude_stabiliser.thrust_controller.last_update_s = 0.0f; 00226 conf.stabiliser_stack.attitude_stabiliser.thrust_controller.dt_s = 1; 00227 conf.stabiliser_stack.attitude_stabiliser.thrust_controller.soft_zone_width = 0.0f; 00228 // --------------------------------------------------------------------- 00229 // ------ OUTPUT ------------------------------------------------------- 00230 // --------------------------------------------------------------------- 00231 conf.stabiliser_stack.attitude_stabiliser.output = {}; 00232 conf.stabiliser_stack.attitude_stabiliser.output.rpy[0] = 0.0f; 00233 conf.stabiliser_stack.attitude_stabiliser.output.rpy[1] = 0.0f; 00234 conf.stabiliser_stack.attitude_stabiliser.output.rpy[2] = 0.0f; 00235 conf.stabiliser_stack.attitude_stabiliser.output.thrust = 0.0f; 00236 conf.stabiliser_stack.attitude_stabiliser.output.tvel[0] = 0.0f; 00237 conf.stabiliser_stack.attitude_stabiliser.output.tvel[1] = 0.0f; 00238 conf.stabiliser_stack.attitude_stabiliser.output.tvel[2] = 0.0f; 00239 conf.stabiliser_stack.attitude_stabiliser.output.theading = 0.0f; 00240 conf.stabiliser_stack.attitude_stabiliser.output.control_mode = RATE_COMMAND_MODE; 00241 conf.stabiliser_stack.attitude_stabiliser.output.yaw_mode = YAW_RELATIVE; 00242 // ############################################################################# 00243 // ###### VELOCITY CONTROL ################################################### 00244 // ############################################################################# 00245 // ----------------------------------------------------------------- 00246 // ------ ROLL PID ------------------------------------------------- 00247 // ----------------------------------------------------------------- 00248 conf.stabiliser_stack.velocity_stabiliser.rpy_controller[ROLL].p_gain = 1.2f; 00249 conf.stabiliser_stack.velocity_stabiliser.rpy_controller[ROLL].clip_min = -100.0f; 00250 conf.stabiliser_stack.velocity_stabiliser.rpy_controller[ROLL].clip_max = 100.0f; 00251 conf.stabiliser_stack.velocity_stabiliser.rpy_controller[ROLL].integrator.gain = 0.0f; 00252 conf.stabiliser_stack.velocity_stabiliser.rpy_controller[ROLL].integrator.clip_pre = 1.0f; 00253 conf.stabiliser_stack.velocity_stabiliser.rpy_controller[ROLL].integrator.accumulator = 0.0f; 00254 conf.stabiliser_stack.velocity_stabiliser.rpy_controller[ROLL].integrator.clip = 0.5f; 00255 conf.stabiliser_stack.velocity_stabiliser.rpy_controller[ROLL].differentiator.gain = 0.0f; 00256 conf.stabiliser_stack.velocity_stabiliser.rpy_controller[ROLL].differentiator.previous = 0.0f; 00257 conf.stabiliser_stack.velocity_stabiliser.rpy_controller[ROLL].differentiator.clip = 1.0f; 00258 conf.stabiliser_stack.velocity_stabiliser.rpy_controller[ROLL].output = 0.0f; 00259 conf.stabiliser_stack.velocity_stabiliser.rpy_controller[ROLL].error = 0.0f; 00260 conf.stabiliser_stack.velocity_stabiliser.rpy_controller[ROLL].last_update_s = 0.0f; 00261 conf.stabiliser_stack.velocity_stabiliser.rpy_controller[ROLL].dt_s = 1; 00262 conf.stabiliser_stack.velocity_stabiliser.rpy_controller[ROLL].soft_zone_width = 0.0f; 00263 // ----------------------------------------------------------------- 00264 // ------ PITCH PID ------------------------------------------------ 00265 // ----------------------------------------------------------------- 00266 conf.stabiliser_stack.velocity_stabiliser.rpy_controller[PITCH].p_gain = 0.2f; 00267 conf.stabiliser_stack.velocity_stabiliser.rpy_controller[PITCH].clip_min = -PI/4.0f; // To avoid stall 00268 conf.stabiliser_stack.velocity_stabiliser.rpy_controller[PITCH].clip_max = PI/3.0f; 00269 conf.stabiliser_stack.velocity_stabiliser.rpy_controller[PITCH].integrator.gain = 0.08f; 00270 conf.stabiliser_stack.velocity_stabiliser.rpy_controller[PITCH].integrator.clip_pre = 100.0f; 00271 conf.stabiliser_stack.velocity_stabiliser.rpy_controller[PITCH].integrator.accumulator = 0.0f; 00272 conf.stabiliser_stack.velocity_stabiliser.rpy_controller[PITCH].integrator.clip = PI/6.0f; 00273 conf.stabiliser_stack.velocity_stabiliser.rpy_controller[PITCH].differentiator.gain = 0.0f; 00274 conf.stabiliser_stack.velocity_stabiliser.rpy_controller[PITCH].differentiator.previous = 0.0f; 00275 conf.stabiliser_stack.velocity_stabiliser.rpy_controller[PITCH].differentiator.clip = 1.0f; 00276 conf.stabiliser_stack.velocity_stabiliser.rpy_controller[PITCH].output = 0.0f; 00277 conf.stabiliser_stack.velocity_stabiliser.rpy_controller[PITCH].error = 0.0f; 00278 conf.stabiliser_stack.velocity_stabiliser.rpy_controller[PITCH].last_update_s = 0.0f; 00279 conf.stabiliser_stack.velocity_stabiliser.rpy_controller[PITCH].dt_s = 1; 00280 conf.stabiliser_stack.velocity_stabiliser.rpy_controller[PITCH].soft_zone_width = 0.0f; 00281 // ----------------------------------------------------------------- 00282 // ------ YAW PID -------------------------------------------------- 00283 // ----------------------------------------------------------------- 00284 conf.stabiliser_stack.velocity_stabiliser.rpy_controller[YAW].p_gain = 0.0f; 00285 conf.stabiliser_stack.velocity_stabiliser.rpy_controller[YAW].clip_min = -1, 00286 conf.stabiliser_stack.velocity_stabiliser.rpy_controller[YAW].clip_max = 1; 00287 conf.stabiliser_stack.velocity_stabiliser.rpy_controller[YAW].integrator.gain = 0.0f; 00288 conf.stabiliser_stack.velocity_stabiliser.rpy_controller[YAW].integrator.clip_pre = 0.0f; 00289 conf.stabiliser_stack.velocity_stabiliser.rpy_controller[YAW].integrator.accumulator = 0.0f; 00290 conf.stabiliser_stack.velocity_stabiliser.rpy_controller[YAW].integrator.clip = 0.0f; 00291 conf.stabiliser_stack.velocity_stabiliser.rpy_controller[YAW].differentiator.gain = 0.0f; 00292 conf.stabiliser_stack.velocity_stabiliser.rpy_controller[YAW].differentiator.previous = 0.0f; 00293 conf.stabiliser_stack.velocity_stabiliser.rpy_controller[YAW].differentiator.clip = 0.0f; 00294 conf.stabiliser_stack.velocity_stabiliser.rpy_controller[YAW].output = 0.0f; 00295 conf.stabiliser_stack.velocity_stabiliser.rpy_controller[YAW].error = 0.0f; 00296 conf.stabiliser_stack.velocity_stabiliser.rpy_controller[YAW].last_update_s = 0.0f; 00297 conf.stabiliser_stack.velocity_stabiliser.rpy_controller[YAW].dt_s = 1; 00298 conf.stabiliser_stack.velocity_stabiliser.rpy_controller[YAW].soft_zone_width = 0.0f; 00299 // --------------------------------------------------------------------- 00300 // ------ THRUST PID --------------------------------------------------- 00301 // --------------------------------------------------------------------- 00302 conf.stabiliser_stack.velocity_stabiliser.thrust_controller.p_gain = 0.20f; 00303 conf.stabiliser_stack.velocity_stabiliser.thrust_controller.clip_min = -1.0f; 00304 conf.stabiliser_stack.velocity_stabiliser.thrust_controller.clip_max = 1.0f; 00305 conf.stabiliser_stack.velocity_stabiliser.thrust_controller.integrator.gain = 0.025f; 00306 conf.stabiliser_stack.velocity_stabiliser.thrust_controller.integrator.clip_pre = 2.0f; 00307 conf.stabiliser_stack.velocity_stabiliser.thrust_controller.integrator.accumulator = 0.0f; 00308 conf.stabiliser_stack.velocity_stabiliser.thrust_controller.integrator.clip = 0.3f; 00309 conf.stabiliser_stack.velocity_stabiliser.thrust_controller.differentiator.gain = 0.0f; 00310 conf.stabiliser_stack.velocity_stabiliser.thrust_controller.differentiator.previous = 0.0f; 00311 conf.stabiliser_stack.velocity_stabiliser.thrust_controller.differentiator.clip = 0.04f; 00312 conf.stabiliser_stack.velocity_stabiliser.thrust_controller.output = 0.0f; 00313 conf.stabiliser_stack.velocity_stabiliser.thrust_controller.error = 0.0f; 00314 conf.stabiliser_stack.velocity_stabiliser.thrust_controller.last_update_s = 0.0f; 00315 conf.stabiliser_stack.velocity_stabiliser.thrust_controller.dt_s = 1; 00316 conf.stabiliser_stack.velocity_stabiliser.thrust_controller.soft_zone_width = 0.0f; 00317 // --------------------------------------------------------------------- 00318 // ------ OUTPUT ------------------------------------------------------- 00319 // --------------------------------------------------------------------- 00320 conf.stabiliser_stack.velocity_stabiliser.output = {}; 00321 conf.stabiliser_stack.velocity_stabiliser.output.rpy[0] = 0.0f; 00322 conf.stabiliser_stack.velocity_stabiliser.output.rpy[1] = 0.0f; 00323 conf.stabiliser_stack.velocity_stabiliser.output.rpy[2] = 0.0f; 00324 conf.stabiliser_stack.velocity_stabiliser.output.thrust = 0.0f; 00325 conf.stabiliser_stack.velocity_stabiliser.output.tvel[0] = 0.0f; 00326 conf.stabiliser_stack.velocity_stabiliser.output.tvel[1] = 0.0f; 00327 conf.stabiliser_stack.velocity_stabiliser.output.tvel[2] = 0.0f; 00328 conf.stabiliser_stack.velocity_stabiliser.output.theading = 0.0f; 00329 conf.stabiliser_stack.velocity_stabiliser.output.control_mode = RATE_COMMAND_MODE; 00330 conf.stabiliser_stack.velocity_stabiliser.output.yaw_mode = YAW_RELATIVE; 00331 // ############################################################################# 00332 // ###### POSITION CONTROL ################################################### 00333 // ############################################################################# 00334 //TODO check gains before using 00335 // ----------------------------------------------------------------- 00336 // ------ ROLL PID ------------------------------------------------- 00337 // ----------------------------------------------------------------- 00338 conf.stabiliser_stack.position_stabiliser.rpy_controller[ROLL].p_gain = 0.01f; 00339 conf.stabiliser_stack.position_stabiliser.rpy_controller[ROLL].clip_min = -0.5f; 00340 conf.stabiliser_stack.position_stabiliser.rpy_controller[ROLL].clip_max = 0.5f; 00341 conf.stabiliser_stack.position_stabiliser.rpy_controller[ROLL].integrator.gain = 0.0f; 00342 conf.stabiliser_stack.position_stabiliser.rpy_controller[ROLL].integrator.clip_pre = 0.0f; 00343 conf.stabiliser_stack.position_stabiliser.rpy_controller[ROLL].integrator.accumulator = 0.0f; 00344 conf.stabiliser_stack.position_stabiliser.rpy_controller[ROLL].integrator.clip = 0.0f; 00345 conf.stabiliser_stack.position_stabiliser.rpy_controller[ROLL].differentiator.gain = 0.00005f; 00346 conf.stabiliser_stack.position_stabiliser.rpy_controller[ROLL].differentiator.previous = 0.0f; 00347 conf.stabiliser_stack.position_stabiliser.rpy_controller[ROLL].differentiator.clip = 0.005f; 00348 conf.stabiliser_stack.position_stabiliser.rpy_controller[ROLL].output = 0.0f; 00349 conf.stabiliser_stack.position_stabiliser.rpy_controller[ROLL].error = 0.0f; 00350 conf.stabiliser_stack.position_stabiliser.rpy_controller[ROLL].last_update_s = 0.0f; 00351 conf.stabiliser_stack.position_stabiliser.rpy_controller[ROLL].dt_s = 1; 00352 conf.stabiliser_stack.position_stabiliser.rpy_controller[ROLL].soft_zone_width = 0.2f; 00353 // ----------------------------------------------------------------- 00354 // ------ PITCH PID ------------------------------------------------ 00355 // ----------------------------------------------------------------- 00356 conf.stabiliser_stack.position_stabiliser.rpy_controller[PITCH].p_gain = 0.01f; 00357 conf.stabiliser_stack.position_stabiliser.rpy_controller[PITCH].clip_min = -0.5f; 00358 conf.stabiliser_stack.position_stabiliser.rpy_controller[PITCH].clip_max = 0.5f; 00359 conf.stabiliser_stack.position_stabiliser.rpy_controller[PITCH].integrator.gain = 0.0f; 00360 conf.stabiliser_stack.position_stabiliser.rpy_controller[PITCH].integrator.clip_pre = 0.0f; 00361 conf.stabiliser_stack.position_stabiliser.rpy_controller[PITCH].integrator.accumulator = 0.0f; 00362 conf.stabiliser_stack.position_stabiliser.rpy_controller[PITCH].integrator.clip = 0.0f; 00363 conf.stabiliser_stack.position_stabiliser.rpy_controller[PITCH].differentiator.gain = 0.00005f; 00364 conf.stabiliser_stack.position_stabiliser.rpy_controller[PITCH].differentiator.previous = 0.0f; 00365 conf.stabiliser_stack.position_stabiliser.rpy_controller[PITCH].differentiator.clip = 0.005f; 00366 conf.stabiliser_stack.position_stabiliser.rpy_controller[PITCH].output = 0.0f; 00367 conf.stabiliser_stack.position_stabiliser.rpy_controller[PITCH].error = 0.0f; 00368 conf.stabiliser_stack.position_stabiliser.rpy_controller[PITCH].last_update_s = 0.0f; 00369 conf.stabiliser_stack.position_stabiliser.rpy_controller[PITCH].dt_s = 1; 00370 conf.stabiliser_stack.position_stabiliser.rpy_controller[PITCH].soft_zone_width = 0.2f; 00371 // ----------------------------------------------------------------- 00372 // ------ YAW PID -------------------------------------------------- 00373 // ----------------------------------------------------------------- 00374 conf.stabiliser_stack.position_stabiliser.rpy_controller[YAW].p_gain = 1.0f; 00375 conf.stabiliser_stack.position_stabiliser.rpy_controller[YAW].clip_min = -1, 00376 conf.stabiliser_stack.position_stabiliser.rpy_controller[YAW].clip_max = 1; 00377 conf.stabiliser_stack.position_stabiliser.rpy_controller[YAW].integrator.gain = 0.0f; 00378 conf.stabiliser_stack.position_stabiliser.rpy_controller[YAW].integrator.clip_pre = 0.0f; 00379 conf.stabiliser_stack.position_stabiliser.rpy_controller[YAW].integrator.accumulator = 0.0f; 00380 conf.stabiliser_stack.position_stabiliser.rpy_controller[YAW].integrator.clip = 0.0f; 00381 conf.stabiliser_stack.position_stabiliser.rpy_controller[YAW].differentiator.gain = 0.0f; 00382 conf.stabiliser_stack.position_stabiliser.rpy_controller[YAW].differentiator.previous = 0.0f; 00383 conf.stabiliser_stack.position_stabiliser.rpy_controller[YAW].differentiator.clip = 0.0f; 00384 conf.stabiliser_stack.position_stabiliser.rpy_controller[YAW].output = 0.0f; 00385 conf.stabiliser_stack.position_stabiliser.rpy_controller[YAW].error = 0.0f; 00386 conf.stabiliser_stack.position_stabiliser.rpy_controller[YAW].last_update_s = 0.0f; 00387 conf.stabiliser_stack.position_stabiliser.rpy_controller[YAW].dt_s = 1; 00388 conf.stabiliser_stack.position_stabiliser.rpy_controller[YAW].soft_zone_width = 0.0f; 00389 // --------------------------------------------------------------------- 00390 // ------ THRUST PID --------------------------------------------------- 00391 // --------------------------------------------------------------------- 00392 conf.stabiliser_stack.position_stabiliser.thrust_controller = {}; 00393 conf.stabiliser_stack.position_stabiliser.thrust_controller.p_gain = 0.05f; 00394 conf.stabiliser_stack.position_stabiliser.thrust_controller.clip_min = -0.9f; 00395 conf.stabiliser_stack.position_stabiliser.thrust_controller.clip_max = 0.65f; 00396 conf.stabiliser_stack.position_stabiliser.thrust_controller.integrator.gain = 0.00005f; 00397 conf.stabiliser_stack.position_stabiliser.thrust_controller.integrator.clip_pre = 1.0f; 00398 conf.stabiliser_stack.position_stabiliser.thrust_controller.integrator.accumulator = 0.0f; 00399 conf.stabiliser_stack.position_stabiliser.thrust_controller.integrator.clip = 0.025f; 00400 conf.stabiliser_stack.position_stabiliser.thrust_controller.differentiator.gain = 0.005f; 00401 conf.stabiliser_stack.position_stabiliser.thrust_controller.differentiator.previous = 0.0f; 00402 conf.stabiliser_stack.position_stabiliser.thrust_controller.differentiator.clip = 0.01f; 00403 conf.stabiliser_stack.position_stabiliser.thrust_controller.output = 0.0f; 00404 conf.stabiliser_stack.position_stabiliser.thrust_controller.error = 0.0f; 00405 conf.stabiliser_stack.position_stabiliser.thrust_controller.last_update_s = 0.0f; 00406 conf.stabiliser_stack.position_stabiliser.thrust_controller.dt_s = 1; 00407 conf.stabiliser_stack.position_stabiliser.thrust_controller.soft_zone_width = 0.2f; 00408 // --------------------------------------------------------------------- 00409 // ------ OUTPUT ------------------------------------------------------- 00410 // --------------------------------------------------------------------- 00411 conf.stabiliser_stack.position_stabiliser.output = {}; 00412 conf.stabiliser_stack.position_stabiliser.output.rpy[0] = 0.0f; 00413 conf.stabiliser_stack.position_stabiliser.output.rpy[1] = 0.0f; 00414 conf.stabiliser_stack.position_stabiliser.output.rpy[2] = 0.0f; 00415 conf.stabiliser_stack.position_stabiliser.output.thrust = 0.0f; 00416 conf.stabiliser_stack.position_stabiliser.output.tvel[0] = 0.0f; 00417 conf.stabiliser_stack.position_stabiliser.output.tvel[1] = 0.0f; 00418 conf.stabiliser_stack.position_stabiliser.output.tvel[2] = 0.0f; 00419 conf.stabiliser_stack.position_stabiliser.output.theading = 0.0f; 00420 conf.stabiliser_stack.position_stabiliser.output.control_mode = RATE_COMMAND_MODE; 00421 conf.stabiliser_stack.position_stabiliser.output.yaw_mode = YAW_RELATIVE; 00422 00423 00424 return conf; 00425 }; 00426 00427 #endif /* STABILISATION_WING_DEFAULT_CONFIG_HPP_ */
1.7.6.1
