/* This function updates continuous states using the ODE4 fixed-step
* solver algorithm
*/
static void rt_ertODEUpdateContinuousStates(RTWSolverInfo *si , int_T tid)
{
time_T t = rtsiGetT(si);
time_T tnew = rtsiGetSolverStopTime(si);
time_T h = rtsiGetStepSize(si);
real_T *x = rtsiGetContStates(si);
ODE4_IntgData *id = rtsiGetSolverData(si);
real_T *y = id->y;
real_T *f0 = id->f[0];
real_T *f1 = id->f[1];
real_T *f2 = id->f[2];
real_T *f3 = id->f[3];
real_T temp;
int_T i;
int_T nXc = 1;
rtsiSetSimTimeStep(si,MINOR_TIME_STEP);
/* Save the state values at time t in y, we'll use x as ynew. */
(void)memcpy(y, x, nXc*sizeof(real_T));
/* Assumes that rtsiSetT and ModelOutputs are up-to-date */
/* f0 = f(t,y) */
rtsiSetdX(si, f0);
m1006_derivatives();
/* f1 = f(t + (h/2), y + (h/2)*f0) */
temp = 0.5 * h;
for (i = 0; i < nXc; i++) x[i] = y[i] + (temp*f0[i]);
rtsiSetT(si, t + temp);
rtsiSetdX(si, f1);
m1006_output(0);
m1006_derivatives();
/* f2 = f(t + (h/2), y + (h/2)*f1) */
for (i = 0; i < nXc; i++) x[i] = y[i] + (temp*f1[i]);
rtsiSetdX(si, f2);
m1006_output(0);
m1006_derivatives();
/* f3 = f(t + h, y + h*f2) */
for (i = 0; i < nXc; i++) x[i] = y[i] + (h*f2[i]);
rtsiSetT(si, tnew);
rtsiSetdX(si, f3);
m1006_output(0);
m1006_derivatives();
/* tnew = t + h
ynew = y + (h/6)*(f0 + 2*f1 + 2*f2 + 2*f3) */
temp = h / 6.0;
for (i = 0; i < nXc; i++) {
x[i] = y[i] + temp*(f0[i] + 2.0*f1[i] + 2.0*f2[i] + f3[i]);
}
rtsiSetSimTimeStep(si,MAJOR_TIME_STEP);
}
/*
* This function updates continuous states using the ODE2 fixed-step
* solver algorithm
*/
void Position_TiltModelClass::rt_ertODEUpdateContinuousStates(RTWSolverInfo *si )
{
time_T tnew = rtsiGetSolverStopTime(si);
time_T h = rtsiGetStepSize(si);
real_T *x = rtsiGetContStates(si);
ODE2_IntgData *id = (ODE2_IntgData *)rtsiGetSolverData(si);
real_T *y = id->y;
real_T *f0 = id->f[0];
real_T *f1 = id->f[1];
real_T temp;
int_T i;
int_T nXc = 2;
rtsiSetSimTimeStep(si,MINOR_TIME_STEP);
/* Save the state values at time t in y, we'll use x as ynew. */
(void) memcpy(y, x,
(uint_T)nXc*sizeof(real_T));
/* Assumes that rtsiSetT and ModelOutputs are up-to-date */
/* f0 = f(t,y) */
rtsiSetdX(si, f0);
Position_Tilt_derivatives();
/* f1 = f(t + h, y + h*f0) */
for (i = 0; i < nXc; i++) {
x[i] = y[i] + (h*f0[i]);
}
rtsiSetT(si, tnew);
rtsiSetdX(si, f1);
this->step();
Position_Tilt_derivatives();
/* tnew = t + h
ynew = y + (h/2)*(f0 + f1) */
temp = 0.5*h;
for (i = 0; i < nXc; i++) {
x[i] = y[i] + temp*(f0[i] + f1[i]);
}
rtsiSetSimTimeStep(si,MAJOR_TIME_STEP);
}
/*
* This function updates continuous states using the ODE1 fixed-step
* solver algorithm
*/
static void rt_ertODEUpdateContinuousStates(RTWSolverInfo *si )
{
time_T tnew = rtsiGetSolverStopTime(si);
time_T h = rtsiGetStepSize(si);
real_T *x = rtsiGetContStates(si);
ODE1_IntgData *id = (ODE1_IntgData *)rtsiGetSolverData(si);
real_T *f0 = id->f[0];
int_T i;
int_T nXc = 2;
rtsiSetSimTimeStep(si,MINOR_TIME_STEP);
rtsiSetdX(si, f0);
motor_io_position_derivatives();
rtsiSetT(si, tnew);
for (i = 0; i < nXc; i++) {
*x += h * f0[i];
x++;
}
rtsiSetSimTimeStep(si,MAJOR_TIME_STEP);
}
/*
* This function updates continuous states using the ODE3 fixed-step
* solver algorithm
*/
static void rt_ertODEUpdateContinuousStates(RTWSolverInfo *si )
{
/* Solver Matrices */
static const real_T rt_ODE3_A[3] = {
1.0/2.0, 3.0/4.0, 1.0
};
static const real_T rt_ODE3_B[3][3] = {
{ 1.0/2.0, 0.0, 0.0 },
{ 0.0, 3.0/4.0, 0.0 },
{ 2.0/9.0, 1.0/3.0, 4.0/9.0 }
};
time_T t = rtsiGetT(si);
time_T tnew = rtsiGetSolverStopTime(si);
time_T h = rtsiGetStepSize(si);
real_T *x = rtsiGetContStates(si);
ODE3_IntgData *id = (ODE3_IntgData *)rtsiGetSolverData(si);
real_T *y = id->y;
real_T *f0 = id->f[0];
real_T *f1 = id->f[1];
real_T *f2 = id->f[2];
real_T hB[3];
int_T i;
int_T nXc = 4;
rtsiSetSimTimeStep(si,MINOR_TIME_STEP);
/* Save the state values at time t in y, we'll use x as ynew. */
(void) memcpy(y, x,
(uint_T)nXc*sizeof(real_T));
/* Assumes that rtsiSetT and ModelOutputs are up-to-date */
/* f0 = f(t,y) */
rtsiSetdX(si, f0);
trajectoryModel_derivatives();
/* f(:,2) = feval(odefile, t + hA(1), y + f*hB(:,1), args(:)(*)); */
hB[0] = h * rt_ODE3_B[0][0];
for (i = 0; i < nXc; i++) {
x[i] = y[i] + (f0[i]*hB[0]);
}
rtsiSetT(si, t + h*rt_ODE3_A[0]);
rtsiSetdX(si, f1);
trajectoryModel_step();
trajectoryModel_derivatives();
/* f(:,3) = feval(odefile, t + hA(2), y + f*hB(:,2), args(:)(*)); */
for (i = 0; i <= 1; i++) {
hB[i] = h * rt_ODE3_B[1][i];
}
for (i = 0; i < nXc; i++) {
x[i] = y[i] + (f0[i]*hB[0] + f1[i]*hB[1]);
}
rtsiSetT(si, t + h*rt_ODE3_A[1]);
rtsiSetdX(si, f2);
trajectoryModel_step();
trajectoryModel_derivatives();
/* tnew = t + hA(3);
ynew = y + f*hB(:,3); */
for (i = 0; i <= 2; i++) {
hB[i] = h * rt_ODE3_B[2][i];
}
for (i = 0; i < nXc; i++) {
x[i] = y[i] + (f0[i]*hB[0] + f1[i]*hB[1] + f2[i]*hB[2]);
}
rtsiSetT(si, tnew);
rtsiSetSimTimeStep(si,MAJOR_TIME_STEP);
}
/* This function updates continuous states using the ODE3 fixed-step
* solver algorithm
*/
static void rt_ertODEUpdateContinuousStates(RTWSolverInfo *si )
{
time_T t = rtsiGetT(si);
time_T tnew = rtsiGetSolverStopTime(si);
time_T h = rtsiGetStepSize(si);
real_T *x = rtsiGetContStates(si);
ODE3_IntgData *id = (ODE3_IntgData *)rtsiGetSolverData(si);
real_T *y = id->y;
real_T *f0 = id->f[0];
real_T *f1 = id->f[1];
real_T *f2 = id->f[2];
real_T hB[3];
int_T i;
int_T nXc = 10;
rtsiSetSimTimeStep(si,MINOR_TIME_STEP);
/* Save the state values at time t in y, we'll use x as ynew. */
(void) memcpy(y,x,
nXc*sizeof(real_T));
/* Assumes that rtsiSetT and ModelOutputs are up-to-date */
/* f0 = f(t,y) */
rtsiSetdX(si, f0);
Mechanics_derivatives();
/* f(:,2) = feval(odefile, t + hA(1), y + f*hB(:,1), args(:)(*)); */
hB[0] = h * rt_ODE3_B[0][0];
for (i = 0; i < nXc; i++) {
x[i] = y[i] + (f0[i]*hB[0]);
}
rtsiSetT(si, t + h*rt_ODE3_A[0]);
rtsiSetdX(si, f1);
Mechanics_output(0);
Mechanics_derivatives();
/* f(:,3) = feval(odefile, t + hA(2), y + f*hB(:,2), args(:)(*)); */
for (i = 0; i <= 1; i++)
hB[i] = h * rt_ODE3_B[1][i];
for (i = 0; i < nXc; i++) {
x[i] = y[i] + (f0[i]*hB[0] + f1[i]*hB[1]);
}
rtsiSetT(si, t + h*rt_ODE3_A[1]);
rtsiSetdX(si, f2);
Mechanics_output(0);
Mechanics_derivatives();
/* tnew = t + hA(3);
ynew = y + f*hB(:,3); */
for (i = 0; i <= 2; i++)
hB[i] = h * rt_ODE3_B[2][i];
for (i = 0; i < nXc; i++) {
x[i] = y[i] + (f0[i]*hB[0] + f1[i]*hB[1] + f2[i]*hB[2]);
}
rtsiSetT(si, tnew);
Mechanics_output(0);
Mechanics_projection();
rtsiSetSimTimeStep(si,MAJOR_TIME_STEP);
}
void rt_ODEUpdateContinuousStates(RTWSolverInfo *si)
{
time_T t = rtsiGetT(si);
time_T tnew = rtsiGetSolverStopTime(si);
time_T h = rtsiGetStepSize(si);
real_T *x = rtsiGetContStates(si);
IntgData *id = rtsiGetSolverData(si);
real_T *y = id->y;
real_T *f0 = id->f[0];
real_T *f1 = id->f[1];
real_T *f2 = id->f[2];
real_T hB[3];
int_T i;
#ifdef NCSTATES
int_T nXc = NCSTATES;
#else
int_T nXc = rtsiGetNumContStates(si);
#endif
rtsiSetSimTimeStep(si,MINOR_TIME_STEP);
/* Save the state values at time t in y, we'll use x as ynew. */
(void)memcpy(y, x, nXc*sizeof(real_T));
/* Assumes that rtsiSetT and ModelOutputs are up-to-date */
/* f0 = f(t,y) */
rtsiSetdX(si, f0);
DERIVATIVES(si);
/* f(:,2) = feval(odefile, t + hA(1), y + f*hB(:,1), args(:)(*)); */
hB[0] = h * rt_ODE3_B[0][0];
for (i = 0; i < nXc; i++) {
x[i] = y[i] + (f0[i]*hB[0]);
}
rtsiSetT(si, t + h*rt_ODE3_A[0]);
rtsiSetdX(si, f1);
OUTPUTS(si,0);
DERIVATIVES(si);
/* f(:,3) = feval(odefile, t + hA(2), y + f*hB(:,2), args(:)(*)); */
for (i = 0; i <= 1; i++) hB[i] = h * rt_ODE3_B[1][i];
for (i = 0; i < nXc; i++) {
x[i] = y[i] + (f0[i]*hB[0] + f1[i]*hB[1]);
}
rtsiSetT(si, t + h*rt_ODE3_A[1]);
rtsiSetdX(si, f2);
OUTPUTS(si,0);
DERIVATIVES(si);
/* tnew = t + hA(3);
ynew = y + f*hB(:,3); */
for (i = 0; i <= 2; i++) hB[i] = h * rt_ODE3_B[2][i];
for (i = 0; i < nXc; i++) {
x[i] = y[i] + (f0[i]*hB[0] + f1[i]*hB[1] + f2[i]*hB[2]);
}
rtsiSetT(si, tnew);
PROJECTION(si);
rtsiSetSimTimeStep(si,MAJOR_TIME_STEP);
}