From 60a9336bfca27556c06e9a3f426dedf1995ed863 Mon Sep 17 00:00:00 2001 From: "Penn, John M 047828115" <john.m.penn@nasa.gov> Date: Tue, 16 Jun 2020 16:16:16 -0500 Subject: [PATCH] Fix RCS pitch control. Tweat gains. #1006 --- .../models/lander/include/Lander.hh | 4 ++ .../SIM_lander/models/lander/src/Lander.cpp | 67 ++++++++++--------- 2 files changed, 40 insertions(+), 31 deletions(-) diff --git a/trick_sims/SIM_lander/models/lander/include/Lander.hh b/trick_sims/SIM_lander/models/lander/include/Lander.hh index 39e586fe..7f4be2f5 100755 --- a/trick_sims/SIM_lander/models/lander/include/Lander.hh +++ b/trick_sims/SIM_lander/models/lander/include/Lander.hh @@ -45,6 +45,10 @@ class Lander { PIDController* nozzlePitchController; PIDController* nozzlePitchRateController; PIDController* RCSpitchController; + PIDController* RCSpitchRateController; + + double RCS_pitch_dot_limit; + double RCS_pitch_ddot_limit; double x_cmd; double y_cmd; diff --git a/trick_sims/SIM_lander/models/lander/src/Lander.cpp b/trick_sims/SIM_lander/models/lander/src/Lander.cpp index d600fc78..78393ac1 100755 --- a/trick_sims/SIM_lander/models/lander/src/Lander.cpp +++ b/trick_sims/SIM_lander/models/lander/src/Lander.cpp @@ -10,9 +10,7 @@ LIBRARY DEPENDENCY: #include <iostream> #define RAD_PER_DEG (M_PI / 180.0) -#define DEG_PER_RAD (180.0/ M_PI ) - -int pid_debug = 0; +#define DEG_PER_RAD (180.0/ M_PI) int Lander::default_data() { @@ -32,6 +30,8 @@ int Lander::default_data() { main_engine_offset = 1.0; g = 1.62; // Gravitional acceleration on the moon. + RCS_pitch_dot_limit = (RAD_PER_DEG * 5.0); + nozzle_angle = 0.0; throttle = 0; rcs_command = 0; @@ -51,64 +51,70 @@ int Lander::default_data() { int Lander::state_init() { + RCS_pitch_ddot_limit = (rcs_thrust * rcs_offset) / moment_of_inertia; + // ========================================================================= // Horizontal Position Controllers - // ------------------------------------------------------------------------- + // Horizontal Position Controller // Generates x-axis (horizontal) velocity commands. - posxCntrl = new PIDController( 0.20, 0.0, 0.5 , 3.0, -3.0, 0.10, 0.10 ); - // ------------------------------------------------------------------------- + posxCntrl = new PIDController( 0.08, 0.002, 0.20, 10.0, -10.0, 0.10, 0.10 ); + // Horizontal Velocity Controller // Generates x-axis (horizontal) acceleration commands. - // We chose to limit it to %5 of maximum possible acceleration. - double ax_limit = 0.05 * (thrust_max / mass); - velxCntrl = new PIDController( 0.20, 0.0, 0.5, ax_limit, -ax_limit, 0.10, 0.10 ); + double ax_limit = 0.30 * (thrust_max / mass); + velxCntrl = new PIDController( 0.18, 0.0, 0.0, ax_limit, -ax_limit, 0.10, 0.10 ); // ========================================================================= // Vertical Position Controllers (Below 100 meters) - // ------------------------------------------------------------------------- + // Vertical Position Controller // Generates y-axis (vertical) velocity commands. - posyCntrl = new PIDController( 0.50, 0.1, 0.5, 5.0, -2.0, 0.10, 0.10 ); - // ------------------------------------------------------------------------- + posyCntrl = new PIDController( 0.4, 0.0001, 0.2, 5.0, -2.0, 0.10, 0.10 ); + // Vertical Velocity Controller // Generates y-axis (vertical) acceleration commands. - // We chose to limit it to %95 of maximum possible acceleration. double ay_limit = 0.95 * (thrust_max / mass); - velyCntrl = new PIDController( 0.50, 0.1, 0.5, ay_limit, -ay_limit, 0.10, 0.10 ); + velyCntrl = new PIDController( 0.5, 0.1, 0.0, ay_limit, -ay_limit, 0.10, 0.10 ); // ========================================================================= // Vertical Position Controller (Above 100 meters) // Generates y-axis (vertical) acceleration commands. - // Setpoint: Potential Energy per Kg due to gravity between the commanded altitude and the surface. - // Measured value: Total Energy per Kg (Potential + Kinetic) due to the current altitude and altitude rate. - // Output: Commanded vertical acceleration. - energy_controller = new PIDController( 0.1, 0.01, 0.1, 30.0, 0.00, 0.10, 0.10 ); + // Setpoint: Potential Energy per Kg due to gravity between the commanded + // altitude and the surface. + // Measured value: Total Energy per Kg (Potential + Kinetic) due to the + // current altitude and altitude rate. + energy_controller = new PIDController( 0.1, 0.0001, 0.2, 30.0, 0.00, 0.10, 0.10 ); // ========================================================================= // Pitch Controllers (when throttle > 10) - // ------------------------------------------------------------------------- + // Pitch Controller // Generates pitch rate commands (radians / sec) - nozzlePitchController = new PIDController( 0.50, 0.0, 0.5, 0.2, -0.2, 0.10 , 0.10); + nozzlePitchController = new PIDController( 0.50, 0.0, 0.0, RAD_PER_DEG * 20.0, -RAD_PER_DEG * 20.0, 0.10 , 0.10); - // ------------------------------------------------------------------------- // Pitch Rate Controller // Generates pitch angular acceleration commands (radians /sec^2) - nozzlePitchRateController = new PIDController( 0.50, 0.1, 0.5, 0.2, -0.2, 0.10 , 0.10); + nozzlePitchRateController = new PIDController( 0.50, 0.0, 0.0, RAD_PER_DEG * 10.0, -RAD_PER_DEG * 10.0, 0.10 , 0.10); // ========================================================================= // Pitch Controller (when throttle < 10) - // ------------------------------------------------------------------------- + // RCS Pitch Controller // Generates pitch rate commands (radians / sec) for the RCS. - RCSpitchController = new PIDController( 0.10, 0.5, 0.0, (RAD_PER_DEG * 2.0), -(RAD_PER_DEG * 2.0), 0.10 , 0.10); + RCSpitchController = new PIDController( 0.30, 0.0, 0.0, RCS_pitch_dot_limit, -RCS_pitch_dot_limit, 0.10 , 0.10); + + // RCS Pitch Rate Controller + // Generates pitch rate commands (radians / sec) for the RCS. + RCSpitchRateController = new PIDController( 0.60, 0.0, 0.0, RCS_pitch_ddot_limit, -RCS_pitch_ddot_limit, 0.10 , 0.10); return (0); } int Lander::control() { + rcs_command = 0; + if (altitudeCtrlEnabled) { double Eppkg = g * y_cmd; // Potential Energy per kg. @@ -137,8 +143,6 @@ int Lander::control() { pitch_cmd = 0.0; } - rcs_command = 0; - pitch_dot_cmd = nozzlePitchController->getOutput(pitch_cmd, pitch); pitch_ddot_cmd = nozzlePitchRateController->getOutput(pitch_dot_cmd, pitchDot); double thrust = (throttle / 100.0) * thrust_max; @@ -149,16 +153,17 @@ int Lander::control() { } else { vx_cmd = 0.0; ax_cmd = 0.0; - pitch_cmd = 0.0; - nozzle_angle = 0.0; + pitch_cmd = 0.0; pitch_dot_cmd = RCSpitchController->getOutput(pitch_cmd, pitch); - - if (( pitch_dot_cmd - pitchDot ) > (RAD_PER_DEG * 1.0) ) { + pitch_ddot_cmd = RCSpitchRateController->getOutput(pitch_dot_cmd, pitchDot); + if ( pitch_ddot_cmd > 0.1 * RCS_pitch_ddot_limit) { rcs_command = 1; - } else if (( pitch_dot_cmd - pitchDot ) < (-RAD_PER_DEG * 1.0) ) { + } else if (pitch_ddot_cmd < -0.1 * RCS_pitch_ddot_limit) { rcs_command = -1; + } else { + rcs_command = 0; } }