ExternalVendorCode/trick
1
0
Fork 0
mirror of https://github.com/nasa/trick.git synced 2025-02-06 10:59:27 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

Fix RCS pitch control. Tweat gains. #1006

Browse Source
This commit is contained in:
Penn, John M 047828115 2020-06-16 16:16:16 -05:00
parent c812d3c3dc
commit 60a9336bfc
2 changed files with 40 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
trick_sims/SIM_lander/models/lander/include/Lander.hh
View File

@ -45,6 +45,10 @@ class Lander {
PIDController* nozzlePitchController; PIDController* nozzlePitchController;
PIDController* nozzlePitchRateController; PIDController* nozzlePitchRateController;
PIDController* RCSpitchController; PIDController* RCSpitchController;
PIDController* RCSpitchRateController;
double RCS_pitch_dot_limit;
double RCS_pitch_ddot_limit;
double x_cmd; double x_cmd;
double y_cmd; double y_cmd;

67
trick_sims/SIM_lander/models/lander/src/Lander.cpp
View File

@ -10,9 +10,7 @@ LIBRARY DEPENDENCY:
#include <iostream> #include <iostream>
#define RAD_PER_DEG (M_PI / 180.0) #define RAD_PER_DEG (M_PI / 180.0)
#define DEG_PER_RAD (180.0/ M_PI ) #define DEG_PER_RAD (180.0/ M_PI)
int pid_debug = 0;
int Lander::default_data() { int Lander::default_data() {
@ -32,6 +30,8 @@ int Lander::default_data() {
main_engine_offset = 1.0; main_engine_offset = 1.0;
g = 1.62; // Gravitional acceleration on the moon. g = 1.62; // Gravitional acceleration on the moon.
RCS_pitch_dot_limit = (RAD_PER_DEG * 5.0);
nozzle_angle = 0.0; nozzle_angle = 0.0;
throttle = 0; throttle = 0;
rcs_command = 0; rcs_command = 0;
@ -51,64 +51,70 @@ int Lander::default_data() {
int Lander::state_init() { int Lander::state_init() {
RCS_pitch_ddot_limit = (rcs_thrust * rcs_offset) / moment_of_inertia;
// ========================================================================= // =========================================================================
// Horizontal Position Controllers // Horizontal Position Controllers
// -------------------------------------------------------------------------
// Horizontal Position Controller // Horizontal Position Controller
// Generates x-axis (horizontal) velocity commands. // 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 // Horizontal Velocity Controller
// Generates x-axis (horizontal) acceleration commands. // Generates x-axis (horizontal) acceleration commands.
// We chose to limit it to %5 of maximum possible acceleration. double ax_limit = 0.30 * (thrust_max / mass);
double ax_limit = 0.05 * (thrust_max / mass); velxCntrl = new PIDController( 0.18, 0.0, 0.0, ax_limit, -ax_limit, 0.10, 0.10 );
velxCntrl = new PIDController( 0.20, 0.0, 0.5, ax_limit, -ax_limit, 0.10, 0.10 );
// ========================================================================= // =========================================================================
// Vertical Position Controllers (Below 100 meters) // Vertical Position Controllers (Below 100 meters)
// -------------------------------------------------------------------------
// Vertical Position Controller // Vertical Position Controller
// Generates y-axis (vertical) velocity commands. // 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 // Vertical Velocity Controller
// Generates y-axis (vertical) acceleration commands. // 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); 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) // Vertical Position Controller (Above 100 meters)
// Generates y-axis (vertical) acceleration commands. // Generates y-axis (vertical) acceleration commands.
// Setpoint: Potential Energy per Kg due to gravity between the commanded altitude and the surface. // Setpoint: Potential Energy per Kg due to gravity between the commanded
// Measured value: Total Energy per Kg (Potential + Kinetic) due to the current altitude and altitude rate. // altitude and the surface.
// Output: Commanded vertical acceleration. // Measured value: Total Energy per Kg (Potential + Kinetic) due to the
energy_controller = new PIDController( 0.1, 0.01, 0.1, 30.0, 0.00, 0.10, 0.10 ); // 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 Controllers (when throttle > 10)
// -------------------------------------------------------------------------
// Pitch Controller // Pitch Controller
// Generates pitch rate commands (radians / sec) // 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 // Pitch Rate Controller
// Generates pitch angular acceleration commands (radians /sec^2) // 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) // Pitch Controller (when throttle < 10)
// -------------------------------------------------------------------------
// RCS Pitch Controller // RCS Pitch Controller
// Generates pitch rate commands (radians / sec) for the RCS. // 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); return (0);
} }
int Lander::control() { int Lander::control() {
rcs_command = 0;
if (altitudeCtrlEnabled) { if (altitudeCtrlEnabled) {
double Eppkg = g * y_cmd; // Potential Energy per kg. double Eppkg = g * y_cmd; // Potential Energy per kg.
@ -137,8 +143,6 @@ int Lander::control() {
pitch_cmd = 0.0; pitch_cmd = 0.0;
} }
rcs_command = 0;
pitch_dot_cmd = nozzlePitchController->getOutput(pitch_cmd, pitch); pitch_dot_cmd = nozzlePitchController->getOutput(pitch_cmd, pitch);
pitch_ddot_cmd = nozzlePitchRateController->getOutput(pitch_dot_cmd, pitchDot); pitch_ddot_cmd = nozzlePitchRateController->getOutput(pitch_dot_cmd, pitchDot);
double thrust = (throttle / 100.0) * thrust_max; double thrust = (throttle / 100.0) * thrust_max;
@ -149,16 +153,17 @@ int Lander::control() {
} else { } else {
vx_cmd = 0.0; vx_cmd = 0.0;
ax_cmd = 0.0; ax_cmd = 0.0;
pitch_cmd = 0.0;
nozzle_angle = 0.0; nozzle_angle = 0.0;
pitch_cmd = 0.0;
pitch_dot_cmd = RCSpitchController->getOutput(pitch_cmd, pitch); pitch_dot_cmd = RCSpitchController->getOutput(pitch_cmd, pitch);
pitch_ddot_cmd = RCSpitchRateController->getOutput(pitch_dot_cmd, pitchDot);
if (( pitch_dot_cmd - pitchDot ) > (RAD_PER_DEG * 1.0) ) { if ( pitch_ddot_cmd > 0.1 * RCS_pitch_ddot_limit) {
rcs_command = 1; 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; rcs_command = -1;
} else {
rcs_command = 0;
} }
} }