ExternalVendorCode/trick
1
0
Fork 0
mirror of https://github.com/nasa/trick.git synced 2024-12-19 21:27:54 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

Add ability to reset the integrator state (#952)

Browse Source 
* Add ability to reset the integrator state

* Make reset logic changes conditional on USE_ER7_UTILS_INTEGRATORS
This commit is contained in:
jmpenn 2020-02-18 15:18:57 -06:00 committed by GitHub
parent 05acf0d92f
commit 5c49bf8fef
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
15 changed files with 108 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
include/trick/Integrator.hh
View File

@ -63,6 +63,10 @@ namespace Trick {
virtual int integrate_2nd_order_ode (
double const* accel, double* velocity, double* position);
#ifndef SWIGPYTHON
void state_reset ();
#endif
#ifndef SWIGPYTHON
void state_in (double* arg1, va_list argp);
#endif
@ -123,6 +127,9 @@ namespace Trick {
bool use_deriv2; // -- set by integration technique
double dt; // -- set by IntegLoopSimObject.cpp
#ifndef USE_ER7_UTILS_INTEGRATORS
double **state_origin;
#endif
double *state;
double **deriv;
double **deriv2;
@ -145,10 +152,6 @@ namespace Trick {
Integrator* getIntegrator( Integrator_type Alg, unsigned int State_size, double Dt = 0.0 );
// Integrator* getEr7Integrator(
// er7_utils::Integration::Technique, unsigned int State_size, double Dt);
// void deleteIntegrator( Integrator*&);
}
#endif

1
include/trick/integrator_c_intf.h
View File

@ -12,6 +12,7 @@ int integrate_1st_order_ode(const double* deriv, double* state);
int integrate_2nd_order_ode(const double* acc, double* vel, double * pos);
double get_integ_time(void);
void set_integ_time(double time_value);
void reset_state();
void load_state(double* arg1, ... );
void load_deriv( double* arg1, ...);
void load_deriv2( double* arg1, ...);

1
trick_source/er7_utils/trick/integration/src/trick_integrator.cc
View File

@ -209,6 +209,7 @@ TrickIntegrator::initialize_trick_workspace (
deriv = alloc::allocate_array<double*> (buf_size);
deriv2 = alloc::allocate_array<double*> (buf_size);
state_ws = alloc::allocate_array<double*> (buf_size);
for (unsigned int ii = 0; ii < buf_size; ++ii) {
deriv[ii] = er7_deriv;
deriv2[ii] = er7_deriv2;

45
trick_source/sim_services/Integrator/src/Integrator.cpp
View File

@ -19,6 +19,9 @@ Trick::Integrator::Integrator() {
deriv = NULL;
deriv2 = NULL;
state_ws = NULL;
#ifndef USE_ER7_UTILS_INTEGRATORS
state_origin = NULL;
#endif
time = 0.0;
time_0 = 0.0;
verbosity = 0 ;
@ -85,18 +88,56 @@ int Trick::Integrator::integrate_2nd_order_ode (
return rc;
}
void Trick::Integrator::state_reset () {
#ifdef USE_ER7_UTILS_INTEGRATORS
#else
if (intermediate_step == 0) {
int i = 0;
double* next_arg = state_origin[i];
if (verbosity) std::cout << "RESET STATE: \n";
while (next_arg != (double*) NULL) {
*next_arg = state[i];
if (verbosity) std::cout << " " << *next_arg << "\n";
i++;
next_arg = state_origin[i];
}
if (verbosity) std::cout << std::endl;
}
#endif
}
#ifdef USE_ER7_UTILS_INTEGRATORS
void Trick::Integrator::state_in (double* arg1, va_list argp) {
int i = 0;
double* next_arg = arg1;
if (verbosity) message_publish(MSG_DEBUG, "LOAD STATE: ");
while (next_arg != (double*) NULL) {
state[i] = *next_arg;
if (verbosity) message_publish(MSG_DEBUG," %g", *next_arg);
if (verbosity) message_publish(MSG_DEBUG, " %g", *next_arg);
next_arg = va_arg(argp, double*);
i++;
}
if (verbosity) message_publish(MSG_DEBUG,"\n");
if (verbosity) message_publish(MSG_DEBUG, "\n");
}
#else
void Trick::Integrator::state_in (double* arg1, va_list argp) {
if (intermediate_step == 0) {
int i = 0;
double* next_arg = arg1;
state_origin[i] = next_arg;
if (verbosity) std::cout << "LOAD STATE: \n";
while (next_arg != (double*) NULL) {
state_origin[i] = next_arg;
state[i] = *next_arg;
if (verbosity) std::cout << " " << *next_arg << "\n";
next_arg = va_arg(argp, double*);
i++;
}
state_origin[i] = (double*)NULL;
if (verbosity) std::cout << std::endl;
}
}
#endif
void Trick::Integrator::state_in (double* arg1, ...) {
va_list argp;

7
trick_source/sim_services/Integrator/src/Integrator_C_Intf.cpp
View File

@ -29,6 +29,13 @@ extern "C" void set_integ_time(double time_value) {
trick_curr_integ->time = time_value;
}
extern "C" void reset_state() {
#ifdef USE_ER7_UTILS_INTEGRATORS
#else
trick_curr_integ->state_reset();
#endif
}
extern "C" void load_state(double* arg1, ... ) {
va_list argp;
if (trick_curr_integ != NULL) {

5
trick_source/sim_services/Integrator/trick_algorithms/ABM_Integrator.cpp
View File

@ -11,6 +11,9 @@ void Trick::ABM_Integrator::initialize(int State_size, double Dt) {
dt = Dt;
num_state = State_size;
state_origin = INTEG_ALLOC( double*, num_state );
state_origin[0] = (double*)NULL;
/** Allocate the state vector.*/
state = INTEG_ALLOC( double, num_state );
@ -46,6 +49,8 @@ Trick::ABM_Integrator::~ABM_Integrator() {
int i;
if (state_origin) INTEG_FREE(state_origin);
if (state) INTEG_FREE(state);
for(i=0; i<4 ; i++) {

5
trick_source/sim_services/Integrator/trick_algorithms/Euler_Cromer_Integrator.cpp
View File

@ -13,6 +13,9 @@ void Trick::Euler_Cromer_Integrator::initialize(int State_size, double Dt) {
dt = Dt;
num_state = State_size;
state_origin = INTEG_ALLOC( double*, num_state );
state_origin[0] = (double*)NULL;
/** Allocate the state vector.*/
state = INTEG_ALLOC( double, num_state );
@ -51,6 +54,8 @@ Trick::Euler_Cromer_Integrator::~Euler_Cromer_Integrator() {
const int n_steps = 1;
int i;
if (state_origin) INTEG_FREE(state_origin);
/** Free the state vector.*/
if (state) INTEG_FREE(state);

4
trick_source/sim_services/Integrator/trick_algorithms/Euler_Integrator.cpp
View File

@ -15,6 +15,9 @@ void Trick::Euler_Integrator::initialize(int State_size, double Dt) {
dt = Dt;
num_state = State_size;
state_origin = INTEG_ALLOC( double*, num_state );
state_origin[0] = (double*)NULL;
/** Allocate the state vector.*/
state = INTEG_ALLOC( double, num_state );
@ -40,6 +43,7 @@ Trick::Euler_Integrator::Euler_Integrator(int State_size, double Dt) {
*/
Trick::Euler_Integrator::~Euler_Integrator() {
if (state_origin) INTEG_FREE(state_origin);
if (state) INTEG_FREE(state);
if (deriv[0]) INTEG_FREE(deriv[0]);
if (deriv) INTEG_FREE(deriv);

5
trick_source/sim_services/Integrator/trick_algorithms/MM4_Integrator.cpp
View File

@ -12,6 +12,9 @@ void Trick::MM4_Integrator::initialize(int State_size, double Dt) {
dt = Dt;
num_state = State_size;
state_origin = INTEG_ALLOC( double*, num_state );
state_origin[0] = (double*)NULL;
/** Allocate the state vector.*/
state = INTEG_ALLOC( double, num_state );
@ -44,6 +47,8 @@ Trick::MM4_Integrator::~MM4_Integrator() {
const int n_steps = 3;
int i;
if (state_origin) INTEG_FREE(state_origin);
/** Free the state vector.*/
if (state) INTEG_FREE(state);

5
trick_source/sim_services/Integrator/trick_algorithms/NL2_Integrator.cpp
View File

@ -13,6 +13,9 @@ void Trick::NL2_Integrator::initialize(int State_size, double Dt) {
dt = Dt;
num_state = State_size;
state_origin = INTEG_ALLOC( double*, num_state );
state_origin[0] = (double*)NULL;
/** Allocate the state vector.*/
state = INTEG_ALLOC( double, num_state );
@ -45,6 +48,8 @@ Trick::NL2_Integrator::~NL2_Integrator() {
const int n_steps = 2;
int i;
if (state_origin) INTEG_FREE(state_origin);
/** Free the state vector.*/
if (state) INTEG_FREE(state);

5
trick_source/sim_services/Integrator/trick_algorithms/RK2_Integrator.cpp
View File

@ -14,6 +14,9 @@ void Trick::RK2_Integrator::initialize(int State_size, double Dt) {
num_state = State_size;
dt = Dt;
state_origin = INTEG_ALLOC( double*, num_state );
state_origin[0] = (double*)NULL;
/** Allocate the state vector.*/
state = INTEG_ALLOC( double, num_state );
@ -46,6 +49,8 @@ Trick::RK2_Integrator::~RK2_Integrator() {
const int n_steps = 2;
int i;
if (state_origin) INTEG_FREE(state_origin);
/** Free the state vector.*/
if (state) INTEG_FREE(state);

5
trick_source/sim_services/Integrator/trick_algorithms/RK4_Integrator.cpp
View File

@ -12,6 +12,9 @@ void Trick::RK4_Integrator::initialize(int State_size, double Dt) {
dt = Dt;
num_state = State_size;
state_origin = INTEG_ALLOC( double*, num_state );
state_origin[0] = (double*)NULL;
/** Allocate the state vector.*/
state = INTEG_ALLOC( double, num_state );
@ -44,6 +47,8 @@ Trick::RK4_Integrator::~RK4_Integrator() {
const int n_steps = 4;
int i;
if (state_origin) INTEG_FREE(state_origin);
/** Free the state vector.*/
if (state) INTEG_FREE(state);

5
trick_source/sim_services/Integrator/trick_algorithms/RKF45_Integrator.cpp
View File

@ -12,6 +12,9 @@ void Trick::RKF45_Integrator::initialize(int State_size, double Dt) {
dt = Dt;
num_state = State_size;
state_origin = INTEG_ALLOC( double*, num_state );
state_origin[0] = (double*)NULL;
/** Allocate the state vector.*/
state = INTEG_ALLOC( double, num_state );
@ -44,6 +47,8 @@ Trick::RKF45_Integrator::~RKF45_Integrator() {
const int n_steps = 6;
int i;
if (state_origin) INTEG_FREE(state_origin);
/** Free the state vector.*/
if (state) INTEG_FREE(state);

5
trick_source/sim_services/Integrator/trick_algorithms/RKF78_Integrator.cpp
View File

@ -12,6 +12,9 @@ void Trick::RKF78_Integrator::initialize(int State_size, double Dt) {
dt = Dt;
num_state = State_size;
state_origin = INTEG_ALLOC( double*, num_state );
state_origin[0] = (double*)NULL;
/** Allocate the state vector.*/
state = INTEG_ALLOC( double, num_state );
@ -44,6 +47,8 @@ Trick::RKF78_Integrator::~RKF78_Integrator() {
const int n_steps = 12;
int i;
if (state_origin) INTEG_FREE(state_origin);
/** Free the state vector.*/
if (state) INTEG_FREE(state);

5
trick_source/sim_services/Integrator/trick_algorithms/RKG4_Integrator.cpp
View File

@ -12,6 +12,9 @@ void Trick::RKG4_Integrator::initialize(int State_size, double Dt) {
dt = Dt;
num_state = State_size;
state_origin = INTEG_ALLOC( double*, num_state );
state_origin[0] = (double*)NULL;
/** Allocate the state vector.*/
state = INTEG_ALLOC( double, num_state );
@ -45,6 +48,8 @@ Trick::RKG4_Integrator::~RKG4_Integrator() {
const int n_steps = 4;
int i;
if (state_origin) INTEG_FREE(state_origin);
/** Free the state vector.*/
if (state) INTEG_FREE(state);