/****************************************************************************/ /* Copyright (c) 2000 MBARI */ /* MBARI Proprietary Information. All rights reserved. */ /****************************************************************************/ /* Summary : */ /* Filename : Simulator2.cc */ /* Author : */ /* Project : */ /* Version : 1.0 */ /* Created : 02/07/2000 */ /* Modified : */ /* Archived : */ /****************************************************************************/ /* Modification History: */ /****************************************************************************/ /* Along with Simulator.cc, this file defines the methods of the Simulator class. Had to break into 2 files, because compiler breaks on very large source files! */ #include #include #include #include #include #include "Simulator.h" #include "MathP.h" #include "VehicleConfigurationIF.h" #include "FloatAttribute.h" #include "AngleAttribute.h" #include /*----------------------------------------------------------------------* Subroutine motion() uses 4th-order Runge-Kutta method. Variables are: edot(1) edot(2) edot(3) edot(4) edot(5) edot(6) ] ] ] ] ] ] udot vdot wdot pdot qdot rdot e(1) e(2) e(3) e(4) e(5) e(6) ] ] ] ] ] ] u v w p q r p(1) p(2) p(3) p(4) p(5) p(6) ] ] ] ] ] ] x y z phi theta psi *-----------------------------------------------------------------------*/ void Simulator::motion( double e[], double p[], double omega_P, double rudder, double elevator, double dt ) { int i,j,k; double eo[7], pdot[7][5], edot[7][5]; double po[7], tp[7]; double ds4,dc4,ds5,dc5,ds6,dc6,dt5,ds5dc6,ds5ds6; double dto2 = 0.5*dt, dto6 = dt/6.0; double vel_W_N_N[3] = { northCurrent, eastCurrent, 0.}; double vel_Bo_W_B[3], vel_Bo_N_B[3], vel_W_N_B[3]; for (i=1;i<=6;i++) { eo[i]=e[i]; po[i]=p[i]; for (j=1;j<=4;j++) { edot[i][j]=0.0; pdot[i][j]=0.0; } } /* start 4th order runga kutta integration */ for (jjj=1;jjj<=4;jjj++) { /* get forces and moments: rhs[1] to rhs[6] */ hydro( e, p, omega_P, rudder, elevator, dt ); /* multiply by inverse of inertial matrix */ for (i=1;i<=6;i++) for (k=1;k<=6;k++) edot[i][jjj] = edot[i][jjj] + ainv[i][k] * rhs[k]; /* ctrn takes e to pdot */ ds4 = sin(p[4]); dc4 = cos(p[4]); ds5 = sin(p[5]); dc5 = cos(p[5]); ds6 = sin(p[6]); dc6 = cos(p[6]); dt5 = tan(p[5]); ds5dc6 = ds5 * dc6; ds5ds6 = ds5 * ds6; /* this first 3x3 block is just 3-axis rotation */ ctrn[1][1] = dc5 * dc6; ctrn[1][2] = -dc4 * ds6 + ds4 * ds5dc6; ctrn[1][3] = ds4 * ds6 + dc4 * ds5dc6; ctrn[2][1] = dc5 * ds6; ctrn[2][2] = dc4 * dc6 + ds4 * ds5ds6; ctrn[2][3] = -ds4 * dc6 + dc4 * ds5ds6; ctrn[3][1] = -ds5; ctrn[3][2] = dc5 * ds4; ctrn[3][3] = dc5 * dc4; /* this 3x3 block relates body attitude rates to Euler angle rates of change */ ctrn[4][4] = 1.0; ctrn[4][5] = ds4 * dt5; ctrn[4][6] = dc4 * dt5; ctrn[5][4] = 0.0; ctrn[5][5] = dc4; ctrn[5][6] = -ds4; ctrn[6][4] = 0.0; ctrn[6][5] = ds4/dc5; ctrn[6][6] = dc4/dc5; for (i=1;i<=6;i++) /* get pdot */ for (k=1;k<=6;k++) pdot[i][jjj] = pdot[i][jjj] + ctrn[i][k] * e[k]; /* apply a Runge-Kutta half- or whole step */ if ( jjj < 3 ) for (i=1;i<=6;i++) { e[i] = eo[i] + edot[i][jjj] * dto2; p[i] = po[i] + pdot[i][jjj] * dto2; } else for (i=1;i<=6;i++) { e[i] = eo[i] + edot[i][jjj] * dt; p[i] = po[i] + pdot[i][jjj] * dt; } } /* for (jjj=1;jjj<=4;jjj++) */ for (i=1;i<=6;i++) /* compute the weighted sum */ { e[i] = eo[i] + ( edot[i][1] + 2*(edot[i][2] + edot[i][3]) + edot[i][4] )*dto6; p[i] = po[i] + ( pdot[i][1] + 2*(pdot[i][2] + pdot[i][3]) + pdot[i][4] )*dto6; } if ( fabs(e[1]) + fabs(e[2]) + fabs(e[3]) + fabs(e[4]) + fabs(e[5]) + fabs(e[6]) > 100 ) { strcpy( _eventMsg, "UNSTABLE SIMULATION -- ABORTED\n" ); _eventLog.write(EventLogIF::Error, _eventMsg); exit( 1 ); } // // Add in steady current. It is added to pdot, but not to e[] because // the vehicle moves with the water. Adding it to e[] would give the // vehicle more speed through the water, and change the forces // computed by hydro(), which is not what is intended. // // It's not clear how to extract pdot from the pdot[][] array above. Thus I'll // recompute it here. In my notation, e(1:3) = vel_Bo_W_B, // pdot(1:3) = vel_Bo_N_N, and ctrn = T_N_B. // // BEWARE that the original arrays e[], ctrn[][], etc., start counting // with 1, and the 0th position is ignored. // for ( i=0; i<3; i++ ) { vel_W_N_B[i] = 0.; for ( k=0; k<3; k++ ) vel_W_N_B[i] = vel_W_N_B[i] + ctrn[k+1][i+1] * vel_W_N_N[k]; vel_Bo_N_B[i] = e[i+1] + vel_W_N_B[i]; p[i+1] += dt * vel_W_N_N[i]; } /* update output parameters */ _output.translationalVelocity[0] = e[1]; _output.translationalVelocity[1] = e[2]; _output.translationalVelocity[2] = e[3]; _output.rotationalVelocity[0] = e[4]; _output.rotationalVelocity[1] = e[5]; _output.rotationalVelocity[2] = e[6]; _output.vel_Bo_N_B[0] = vel_Bo_N_B[0]; _output.vel_Bo_N_B[1] = vel_Bo_N_B[1]; _output.vel_Bo_N_B[2] = vel_Bo_N_B[2]; _output.translationalBackDiff[0] = (e[1] - eo[1]) / dt; _output.translationalBackDiff[1] = (e[2] - eo[2]) / dt; _output.translationalBackDiff[2] = (e[3] - eo[3]) / dt; _output.rotationalBackDiff[0] = (e[4] - eo[4]) / dt; _output.rotationalBackDiff[1] = (e[5] - eo[5]) / dt; _output.rotationalBackDiff[2] = (e[6] - eo[6]) / dt; _output.position[0] = p[1]; _output.position[1] = p[2]; _output.position[2] = p[3]; _output.eulerAngles[0] = p[4]; _output.eulerAngles[1] = p[5]; _output.eulerAngles[2] = p[6]; //_log->write(_simTime, &_output); The old one // // Uncomment this log if you want output at every integration step. Comment // out the one just after the call to motion() in Simulator.cc. // //_log->write(); } /*------------------------------------------------------------------------- Subroutine hydro() gets the right-hand sides of the dynamic equations. ------------------------------------------------------------------------- */ void Simulator::hydro( double *e, double *p, double omega_P, double rudder, double elevator, double dt ) { Boolean debug = False; static double Xgp, Ygp, Zgp, Xbp, Ybp, Zbp, Wp, Bp; static int init=-1; double pp,qq,rr,pr,pq,qr,uv,uw,uq,ur,uu,vv,ww,ut; double vp,vr,wp,wq,etam1,cp4,cp5,sp4,sp5; double wabw,vabv,uabu,pabp,qabq,rabr,alpha,beta; double Vuw,Vuv,salpha,calpha,sbeta,cbeta; double command ; double rud_ang, elev_ang, TForce, TTorque, TVolt; double vt , wt ; static double old_rud_ang = 0 , old_elev_ang = 0. ; static double old_rud_ang_com = 0. , old_elev_ang_com = 0. ; static double old_rud_ang_shaft = 0. , old_elev_ang_shaft = 0. ; static double rud_ang_com = 0. , elev_ang_com = 0. ; double rud_ang_shaft, elev_ang_shaft ; static double rud_ang_com_delay[100], elev_ang_com_delay[100] ; int i ; static double old_TForce = 0.0, old_TTorque = 0.0, old_TVolt = 0.0; double fx1, fx2, fx3, fx4, fx5; double W,B ; /* if weight dropped, then vehicle feels 0 */ if (_dropWeight->released(DropWeightIF::Descend)) { Wdw1 = 0.0; Bdw1 = 0.0; } if (_dropWeight->released(DropWeightIF::Ascend)) { Wdw2 = 0.0; Bdw2 = 0.0; } // W = _configuration->mass(); // Should be weight??? B = _configuration->buoyancy(); W = B; /* get weight and buoyancy of vehicle */ Wp = W + Wdw1 + Wdw2; Bp = B + Bdw1 + Bdw2; Xgp = (_centerOfMass[X]*W + Xdw1*Wdw1 + Xdw2*Wdw2) / Wp; Ygp = (_centerOfMass[Y]*W + Ydw1*Wdw1 + Ydw2*Wdw2) / Wp; Zgp = (_centerOfMass[Z]*W + Zdw1*Wdw1 + Zdw2*Wdw2) / Wp; Xbp = (_centerOfBuoyancy[X]*W + Xdw1*Bdw1 + Xdw2*Bdw2) / Bp; Ybp = (_centerOfBuoyancy[Y]*W + Ydw1*Bdw1 + Ydw2*Bdw2) / Bp; Zbp = (_centerOfBuoyancy[Z]*W + Zdw1*Bdw1 + Zdw2*Bdw2) / Bp; uu = e[1] * e[1]; /* some efficient consolidation */ vv = e[2] * e[2]; ww = e[3] * e[3]; uv = e[1] * e[2]; uw = e[1] * e[3]; ur = e[1] * e[6]; uq = e[1] * e[5]; vr = e[2] * e[6]; vp = e[2] * e[4]; wp = e[3] * e[4]; wq = e[3] * e[5]; pr = e[4] * e[6]; pq = e[4] * e[5]; qr = e[5] * e[6]; pp = e[4] * e[4]; qq = e[5] * e[5]; rr = e[6] * e[6]; Vuw = sqrt(uu + ww); Vuv = sqrt(uu + vv); ut = sqrt(uu + vv + ww); uabu = e[1] * fabs(e[1]); vabv = e[2] * fabs(e[2]); wabw = e[3] * fabs(e[3]); pabp = e[4] * fabs(e[4]); qabq = e[5] * fabs(e[5]); rabr = e[6] * fabs(e[6]); cp4 = cos(p[4]); cp5 = cos(p[5]); sp4 = sin(p[4]); sp5 = sin(p[5]); vt = e[2] + r1[1]*e[6] ; wt = e[3] - r1[1]*e[5] ; if (_simTime <= 0.2) { old_rud_ang = -center_hyst_rud ; old_elev_ang = -center_hyst_elev ; } if (jjj == 1) /* if first step of the runge-kutta integration,... */ { /* two big time steps delay */ for(i=delay-1;i>=0;i--){ rud_ang_com_delay[i+1] = rud_ang_com_delay[i] ; elev_ang_com_delay[i+1] = elev_ang_com_delay[i] ; } rud_ang_com_delay[0] = rudder; elev_ang_com_delay[0] = elevator; /* enforce speed constraint on motor shaft */ rud_ang_shaft = act_model( rud_ang_com_delay[delay], old_rud_ang_shaft, dt ); /* put through a hysteresis loop to get actual rudder */ rud_ang = hysteresis( rud_ang_shaft, old_rud_ang_shaft, old_rud_ang, wide_hyst_rud, center_hyst_rud ) ; elev_ang_shaft = act_model( elev_ang_com_delay[delay], old_elev_ang_shaft, dt ); elev_ang = hysteresis( elev_ang_shaft, old_elev_ang_shaft, old_elev_ang, wide_hyst_elev, center_hyst_elev) ; TForce = thruster_force (omega_P,e[1]); /* using u should be a bit*/ TTorque= thruster_torque(omega_P, e[1]);/* more accurate than ut */ /* store the previous fin angles and thruster parameters */ old_rud_ang = rud_ang ; old_elev_ang = elev_ang ; old_rud_ang_shaft = rud_ang_shaft ; old_elev_ang_shaft = elev_ang_shaft ; old_TForce = TForce; old_TTorque = TTorque; old_TVolt = TVolt; } else { rud_ang = old_rud_ang ; elev_ang = old_elev_ang ; TForce = old_TForce; TTorque = old_TTorque; TVolt = old_TVolt; } dprintf("Simulator::hydro() - TTorque=%.8f\n", TTorque); dprintf("Simulator::hydro() - TForce =%.8f\n", TForce); _output.elevatorCmd = elevator; _output.rudderCmd = rudder ; _output.elevator = elev_ang ; _output.rudder = rud_ang ; #if 0 rud_ang = .1*sin(2.*M_PI/2.*_simTime); /* DEBUG rsm 10/20 zz */ elev_ang = .1*sin(2.*M_PI/2.*_simTime); #endif #if 0 TTorque = 1.7; TForce = 18.0; #endif fins(e,rud_ang,elev_ang); /* get fin forces and moments F1,F2,F3,F4,M1,M2,M3,M4 */ /* AXIAL FORCES */ rhs[1] = TForce - (Wp-Bp)*sp5 + _input.vehicleMass*(vr - wq + Xgp*(qq + rr) - Ygp*pq - Zgp*pr) + F1[1] + F2[1] + F3[1] + F4[1] + Xvv*vv + Xww*ww + Xvr*vr + Xwq*wq + Xrr*rr + Xqq*qq; /* apply correction in operating speed range, fsh */ if (e[1] > 0.7) rhs[1] += Xuabu*uabu*(2.455-2.175*e[1]+.75*e[1]*e[1]) ; else rhs[1] += Xuabu*uabu*(2.455-2.175*.7+.75*.7*.7) ; rhs[2] = (Wp-Bp)*cp5*sp4 + Yvabv*vabv + Yuv*uv + Yur*ur + Yrabr*rabr + Ywp*wp + _input.vehicleMass*(wp - ur + Ygp*(rr+pp) -Zgp*qr - Xgp*pq) + F1[2] + F2[2] + F3[2] + F4[2]; /* NORMAL FORCES */ rhs[3] = (Wp-Bp)*cp5*cp4 + Zwabw*wabw + Zuq*uq + Zuw*uw + Zqabq*qabq + Zvp*vp + _input.vehicleMass*(uq - vp + Zgp*(pp + qq) - Xgp*pr - Ygp*qr) + F1[3] + F2[3] + F3[3] + F4[3]; rhs[4] = -(Zgp*Wp - Zbp*Bp)*cp5*sp4 /* ROLL MOMENTS */ + (Ygp*Wp - Ybp*Bp)*cp5*cp4 + Kpabp*pabp - (Izz - Iyy)*qr + Izx*pq - Ixy*pr + Iyz*qq -Iyz*rr - _input.vehicleMass*(Ygp*(vp-uq) + Zgp*(wp-ur)) + M1[1] + M2[1] + M3[1] + M4[1] + TTorque + Kvt2*4.*vt*wt*(wt*wt-vt*vt)/(vt*vt + wt*wt + .0001) ; rhs[5] = -(Zgp*Wp - Zbp*Bp)*sp5 /* PITCH MOMENTS */ - (Xgp*Wp - Xbp*Bp)*cp5*cp4 + Mwabw*wabw + Mqabq*qabq + Muw*uw + Muq*uq + Mpr*pr + (Izz - Ixx)*pr - Izx*pp - Iyz*pq +Ixy*qr + Izx*rr - _input.vehicleMass*(Zgp*(wq - vr) + Xgp*(uq - vp)) + M1[2] + M2[2] + M3[2] + M4[2]; rhs[6] = (Ygp*Wp - Ybp*Bp)*sp5 /* YAW MOMENTS */ + (Xgp*Wp - Xbp*Bp)*cp5*sp4 + Nuv*uv + Nrabr*rabr + Nur*ur + Nvabv*vabv + Npq*pq + (Ixx - Iyy)*pq + Ixy*pp + Iyz*pr - Ixy*qq - Izx*qr - _input.vehicleMass*Xgp*(ur-wp) + _input.vehicleMass*Ygp*(wq - vr) + M1[3] + M2[3] + M3[3] + M4[3]; _output.force[0] = rhs[1]; _output.force[1] = rhs[2]; _output.force[2] = rhs[3]; _output.torque[0] = rhs[4]; _output.torque[1] = rhs[5]; _output.torque[2] = rhs[6]; _output.M1_1 = M1[1]; /*99/10/21 rsm zz */ _output.M2_1 = M2[1]; _output.M3_1 = M3[1]; _output.M4_1 = M4[1]; _output.M1_2 = M1[2]; /*99/10/21 rsm zz */ _output.M2_2 = M2[2]; _output.M3_2 = M3[2]; _output.M4_2 = M4[2]; } /* ------------------------------------------------------------------ Subroutine fins() computes forces generated by control surfaces ------------------------------------------------------------------ */ void Simulator::fins( double *e, double rud_ang, double elev_ang ) { static double scal, norm_v1, norm_v2, norm_v3, norm_v4; static double alpha1,alpha2,alpha3,alpha4; static double CL1,CL2,CL3,CL4,CD1,CD2,CD3,CD4; static double cons,CDC,aa; static double LW1,LW2,LW3,LW4,DW1,DW2,DW3,DW4; static double LF1,LF2,LF3,LF4,DF1,DF2,DF3,DF4; static double v1[4],v2[4],v3[4],v4[4]; static double s13,s24,c13,c24; static double last_rud_ang = 0.0, last_elev_ang = 0.0; static int init=-1; s13 = sin(rud_ang); s24 = sin(elev_ang); c13 = cos(rud_ang); c24 = cos(elev_ang); /* fin velocities in body coords */ v1[1] = e[1] + e[5]*r1[3] - e[6]*r1[2]; v1[2] = e[2] - e[4]*r1[3] + e[6]*r1[1]; v1[3] = e[3] + e[4]*r1[2] - e[5]*r1[1]; v2[1] = e[1] + e[5]*r2[3] - e[6]*r2[2]; v2[2] = e[2] - e[4]*r2[3] + e[6]*r2[1]; v2[3] = e[3] + e[4]*r2[2] - e[5]*r2[1]; v3[1] = e[1] + e[5]*r3[3] - e[6]*r3[2]; v3[2] = e[2] - e[4]*r3[3] + e[6]*r3[1]; v3[3] = e[3] + e[4]*r3[2] - e[5]*r3[1]; v4[1] = e[1] + e[5]*r4[3] - e[6]*r4[2]; v4[2] = e[2] - e[4]*r4[3] + e[6]*r4[1]; v4[3] = e[3] + e[4]*r4[2] - e[5]*r4[1]; /* now get angle of attack for each fin */ norm_v1 = sqrt( v1[1]*v1[1] + v1[2]*v1[2] + v1[3]*v1[3] ); if (norm_v1 < 0.001) alpha1 = 0.0; else alpha1 = asin(-v1[2]/norm_v1) + rud_ang; norm_v2 = sqrt( v2[1]*v2[1] + v2[2]*v2[2] + v2[3]*v2[3] ); if (norm_v2 < 0.001) alpha2 = 0.0; else alpha2 = atan2(v2[3],v2[1]) + elev_ang; norm_v3 = sqrt( v3[1]*v3[1] + v3[2]*v3[2] + v3[3]*v3[3] ); if (norm_v3 < 0.001) alpha3=0.0; else alpha3 = asin(-v3[2]/norm_v3) + rud_ang; norm_v4 = sqrt( v4[1]*v4[1] + v4[2]*v4[2] + v4[3]*v4[3] ); if (norm_v4 < 0.001) alpha4=0.0; else alpha4 = atan2(v4[3],v4[1]) + elev_ang; /* lift and drag coefficients */ CDC = CDc/aspect_ratio; /* note that if stall angle is exceeded: NO LIFT */ if (fabs(alpha1) < stall_angle) CL1 = dCL*alpha1 + CDC*alpha1*fabs(alpha1); else CL1 = 0. ; if (fabs(alpha2) < stall_angle) CL2 = dCL*alpha2 + CDC*alpha2*fabs(alpha2); else CL2 = 0. ; if (fabs(alpha3) < stall_angle) CL3 = dCL*alpha3 + CDC*alpha3*fabs(alpha3); else CL3 = 0. ; if (fabs(alpha4) < stall_angle) CL4 = dCL*alpha4 + CDC*alpha4*fabs(alpha4); else CL4 = 0. ; aa = 1.0/(M_PI*aspect_ratio*ec); CD1 = cdo + aa*CL1*CL1; CD2 = cdo + aa*CL2*CL2; CD3 = cdo + aa*CL3*CL3; CD4 = cdo + aa*CL4*CL4; /* lift and drag forces, in flow coords... */ cons = (rho*S)/2.0; LW1 = cons*norm_v1*norm_v1*CL1; /* positive when the lift vector is close to normal vector */ LW2 = cons*norm_v2*norm_v2*CL2; LW3 = cons*norm_v3*norm_v3*CL3; LW4 = cons*norm_v4*norm_v4*CL4; DW1 = cons*norm_v1*norm_v1*CD1; /* always positive */ DW2 = cons*norm_v2*norm_v2*CD2; DW3 = cons*norm_v3*norm_v3*CD3; DW4 = cons*norm_v4*norm_v4*CD4; LF1 = LW1*cos(alpha1) + DW1*sin(alpha1); /* force in the fin normal direction */ LF2 = LW2*cos(alpha2) + DW2*sin(alpha2); LF3 = LW3*cos(alpha3) + DW3*sin(alpha3); LF4 = LW4*cos(alpha4) + DW4*sin(alpha4); DF1 = -LW1*sin(alpha1) + DW1*cos(alpha1); /* force in the fin-aft direction */ DF2 = -LW2*sin(alpha2) + DW2*cos(alpha2); DF3 = -LW3*sin(alpha3) + DW3*cos(alpha3); DF4 = -LW4*sin(alpha4) + DW4*cos(alpha4); F1[1] = -LF1*s13 + (-DF1)*c13; /* finally, transform into the body frame */ F1[2] = LF1*c13 + (-DF1)*s13; F1[3] = 0.0; F2[1] = -LF2*s24 + (-DF2)*c24; F2[2] = 0.0; F2[3] = -LF2*c24 - (-DF2)*s24; F3[1] = -LF3*s13 + (-DF3)*c13; F3[2] = LF3*c13 + (-DF3)*s13; F3[3] = 0.0; F4[1] = -LF4*s24 + (-DF4)*c24; F4[2] = 0.0; F4[3] = -LF4*c24 - (-DF4)*s24; M1[1] = r1[2]*F1[3] - r1[3]*F1[2]; /* moments induced by these forces */ M1[2] = -r1[1]*F1[3] + r1[3]*F1[1]; M1[3] = r1[1]*F1[2] - r1[2]*F1[1]; M2[1] = r2[2]*F2[3] - r2[3]*F2[2]; M2[2] = -r2[1]*F2[3] + r2[3]*F2[1]; M2[3] = r2[1]*F2[2] - r2[2]*F2[1]; M3[1] = r3[2]*F3[3] - r3[3]*F3[2]; M3[2] = -r3[1]*F3[3] + r3[3]*F3[1]; M3[3] = r3[1]*F3[2] - r3[2]*F3[1]; M4[1] = r4[2]*F4[3] - r4[3]*F4[2]; M4[2] = -r4[1]*F4[3] + r4[3]*F4[1]; M4[3] = r4[1]*F4[2] - r4[2]*F4[1]; return; } /*--------------------- routines from actuator.c -----------------------* * act_model() : move at either zero, full forward, or full backward speed. * thruster_force() : computation of the thruster force from the Amps * thruster_torque() : computation of the thruster torque from the Amps * ----------------------------------------------------------------------*/ double Simulator::act_model(double des_angle, double angle, double dt) { double new_angle ; double can_move_in_dt ; const int debug=0; can_move_in_dt = fin_speed*dt; /* how far fin can go in dt seconds */ if (fabs(angle-des_angle) <= can_move_in_dt) { new_angle = des_angle; } else { if (angle > des_angle) new_angle = angle - can_move_in_dt; if (angle < des_angle) new_angle = angle + can_move_in_dt; } if (debug) printf("canmove in dt=%lf %lf %lf %lf\n", can_move_in_dt,des_angle,angle,new_angle); return(new_angle); } /*---------------------------------------------------------------------- Hysteresis: applied to an arbitrary input, probably elevator or rudder. Width and center of loop are defined with other physical variables. fsh 8 May 1995 ------------------------------------------------------------------------*/ double Simulator::hysteresis( double x, double x_last, double y_last, double wide, double center) { double right_intercept, left_intercept, to_right, to_left ; double dx ,y ; right_intercept = center + wide/2. ; left_intercept = center - wide/2. ; dx = x - x_last ; /* get last horizontal distance from right and left lines*/ to_right = -x_last + (y_last + right_intercept) ; to_left = -x_last + (y_last + left_intercept) ; if (dx == 0.) y = y_last ; /* CASE 1: a move inside the loop -------------------------------*/ if (dx <= to_right && dx > 0.) y = y_last ; if (dx >= to_left && dx < 0.) y = y_last ; /* CASE 2: a move that goes outside the loop ---------------------*/ if (dx > to_right && dx > 0.) y = y_last + x - (y_last + right_intercept) ; if (dx < to_left && dx < 0.) y = y_last + x - (y_last + left_intercept) ; return(y) ; } #if 0 /*===================================================================== Modified this to reflect nature of Odyssey II's current control on the thruster controller... Calculate torque (Kt*amps) then calculate propellor RPM for that torque and water speed. This in turn allows one to compute thrust.... J. G. Bellingham 1/94 fsh update 5/95 Note: Kt contains the gear ration 5.54. *====================================================================*/ double Simulator::thruster_force( double real_amps, double u ) { double omega, thrust; omega = sqrt((Kt*real_amps + .5*Area*Eta*Pit*rho*pow( u, 2. ))/ (.5*Area*pow( Eta * Pit, 3. ) * rho)); thrust = rho * Area * (pow(Eta * Pit*omega, 2.) - u * Eta * Pit*omega); // //thrust = rho * Area * Eta * Pit * ( Eta* Pit * omega_P * omega_P - // u * fabs(omega_P)); // // Note: Something seems wrong here. 2*pi*60 doesn't convert rads to rpm. // //_output.propOmega = 2 * M_PI * 60 / 5.54 * omega; _output.propOmega = omega; return thrust; } #endif ///////////////////////////////////////////////////////////////////////// // // PURPOSE: This newer version takes omega_P as the argument, and computes // thrust. The model is very simple, and is for Knut's ducted prop // design. // DATE: 00/8/8 // AUTHOR: Rob McEwen // NOTES: 00/8/8: I left the vehicle speed u as an argument in the hope // that at some future point I'll find a better model which uses it. // ///////////////////////////////////////////////////////////////////////// const double designOmega = 300.*PI/30.; //rad/sec. Also used in thr_trq() double Simulator::thruster_force( double omega_P, double u ) { double thrust; const double designThrust = 52.; // Newtons const double Kthr = designThrust/pow( designOmega, 2. ); thrust = Kthr * pow( omega_P, 2. ); _output.propOmega = omega_P; return thrust; } #if 0 /*====================================================================== Modified this to reflect nature of Odyssey II's current control on the thruster controller...torque is proportional to current... J. G. Bellingham 1/94 *======================================================================*/ double Simulator::thruster_torque( double real_amps, double u ) { double torque; torque = Kt * real_amps; return torque; } #endif ///////////////////////////////////////////////////////////////////////// // // PURPOSE: This newer version takes omega_P as the argument, and computes // the reaction torque. The model is very simple, and is for // Knut's ducted prop design. // DATE: 00/8/8 // AUTHOR: Rob McEwen // NOTES: 00/8/8: I left the vehicle speed u as an argument in the hope // that at some future point I'll find a better model which uses it. // ///////////////////////////////////////////////////////////////////////// double Simulator::thruster_torque( double omega_P, double u ) { double torque; const double designTorque = 2.78; //Nm. At 300 rpm & 52 Newtons thrust. const double Ktrq = designTorque/designOmega; torque = Ktrq * omega_P; return torque; } // // Compute relative water speed: // long Simulator::waterSpeed( double *speed ) { // // Return the simulation truth value. This should be replaced with a // sensor model, at some future point. Probably the RDI_DVL. Beware // that the actual tailcone algo computes water speed from prop omega; // it doesn't actually measure it. *speed = _output.translationalVelocity[0]; // u return 0.; } /*====================================================================*/ /*-----------------------------------------------------------------* | matrix calculations, not sure where their from, probably | numerical recipes in fortran (note array element[0] unused!) *-----------------------------------------------------------------*/ /*************************************************************** subroutine Simulator::matinv(a,ainv,n,adum) this routine computes the inverse of a matrix. matrix inversion is performed by crout elimination input matrix : a output matrix : ainv order : n error flag : ier adum : dummy matrix of order n x n . ----------------------------------------------------------------- */ int Simulator::matinv( double a[][7], double ainv[][7], int n, double adum[][7] ) { int ier,i,j,k,ic,ii,km,kp; double d,p,t,s,dp,dt; double eps = 1.0e-36; /* "ier" is a flag that is set to "1" if the matrix is singular. */ ier = 0; /* special case for n=1 */ if ( n == 1 ) { if ( fabs(a[1][1]) <= eps ) { ier=1; return (ier); } else { ainv[1][1] = 1.0/a[1][1]; return (ier); } } /* copy a into adum because a is changed during computation. Take opportunity to initialize ainv to i . */ for (i=1;i<=n;i++) for (j=1;j<=n;j++) { adum[i][j]=a[i][j]; ainv[i][j] = 0.0e0; if (i==j) ainv[i][j] = 1.0e0; } /* find the largest element in col-1. */ ic = 0; ii = 0; t = fabs(adum[1][1]); for (i=2;i<=n;i++) { if (t-fabs(adum[i][1]) < 0.0e0) { ii = i; t = fabs(adum[i][1]); } } if (ii!=0) { ic++; for (j=1;j<=n;j++) { s = ainv[1][j]; ainv[1][j] = ainv[ii][j]; ainv[ii][j] = s; } for (j=1;j<=n;j++) { s = adum[1][j]; adum[1][j] = adum[ii][j]; adum[ii][j] = s; } } p = adum[1][1]; if (fabs(p)<=eps) { /* c determinant is zero. */ d = 0.0; ier = 1; return (ier); } /* c determinant is non-zero, continue */ for (j=2;j<=n;j++) adum[1][j] = adum[1][j]/p; for (j=1;j<=n;j++) ainv[1][j] = ainv[1][j]/p; for (k=2;k<=n;k++) { /*start big loop*/ km = k - 1; t = -1.0; for (i=k;i<=n;i++) { dp = adum[i][k]; for (j=1;j<=km;j++) dp = dp - adum[i][j] * adum[j][k]; adum[i][k] = dp; if (t-fabs(adum[i][k]) < 0.0e0) { t = fabs(adum[i][k]); ii = i; } } if (ii!=k) { ic++; for (j=1;j<=n;j++) { s = ainv[k][j]; ainv[k][j] = ainv[ii][j]; ainv[ii][j] = s; } for (j=1;j<=n;j++) { s = adum[k][j]; adum[k][j] = adum[ii][j]; adum[ii][j] = s; } } dt = adum[k][k]; if (fabs(dt) <= eps) { /* c determinant is zero. */ d = 0.0; ier = 1; return (ier); } p = p * dt; if (k!=n) { kp = k + 1; for (j=kp;j<=n;j++) { dp = adum[k][j]; for (i=1;i<=km;i++) dp = dp - adum[k][i] * adum[i][j]; adum[k][j] = dp/dt; } } for (j=1;j<=n;j++) { dp = ainv[k][j]; for (i=1;i<=km;i++) dp = dp - adum[k][i] * ainv[i][j]; ainv[k][j] = dp/dt; } } /*end big loop*/ if ((ic%2)!=0) p=-p; d = p; ii = n; for (k=2;k<=n;k++) { kp = ii; ii--; for (j=1;j<=n;j++) { dp = ainv[ii][j]; for (i=kp;i<=n;i++) dp = dp - adum[ii][i] * ainv[i][j]; ainv[ii][j] = dp; } } return (ier); } #ifndef Macintosh double Simulator::gasdev( void ) { static int iset=0; static double gset; double fac,r,v1,v2; if (iset == 0) { do { v1 = 2.0*drand48()-1.0; v2 = 2.0*drand48()-1.0; r = v1*v1 + v2*v2; } while (r >= 1.0); fac = sqrt( -2.0*log(r)/r ); gset = v1*fac; iset = 1; return v2*fac; } else { iset = 0; return gset; } } #endif double Simulator::drand48( void ) { int i; double x; i = rand(); x = ((double) i)/32767; if ((x<0)||(x>1)) fprintf(stderr, "Error in drand48(), value is %lf\n", x); return(x); } Simulator::Log::Log(const char *fileName) { _outputFile = fopen(fileName, "w"); if (!_outputFile) { fprintf(stderr, "Simulator::Log::Log() - couldn't open log file %s\n", fileName); exit(1); } // Header fprintf(_outputFile, "%% %10s %7s %7s %7s %7s %7s %7s " "%7s %7s %7s %7s %7s %7s " "%7s %7s " "%7s %7s %7s %7s\n", "time", "X", "Y", "Z", "Phi", "Theta", "Psi", "u", "v", "w", "p", "q", "r", "rudder", "elev", "M1[1]", "M2[1]", "M3[1]", "M4[1]"); fprintf(_outputFile, "data1=[\n"); } Simulator::Log::~Log() { if (_outputFile) { fprintf(_outputFile, "\n];\n"); fclose(_outputFile); } } int Simulator::Log::write(double simTime, Simulator::OutputData *output) { if (!_outputFile) return -1; fprintf(_outputFile, " %10.2f " "%7.3e %7.3e %7.3e " "%7.3e %7.3e %7.3e " "%7.3e %7.3e %7.3e " "%7.3e %7.3e %7.3e " "%7.3e %7.3e " "%7.3e %7.3e " "%7.3e %7.3e %7.3e %7.3e " "%7.3e %7.3e %7.3e %7.3e\n", simTime, output->position[0], output->position[1], output->position[2], output->eulerAngles[0], output->eulerAngles[1], output->eulerAngles[2], output->translationalVelocity[0], output->translationalVelocity[1], output->translationalVelocity[2], output->rotationalVelocity[0], output->rotationalVelocity[1], output->rotationalVelocity[2], output->rudderCmd, output->elevatorCmd, output->rudder, output->elevator, output->M1_1, output->M2_1, output->M3_1, output->M4_1, output->M1_2, output->M2_2, output->M3_2, output->M4_2 ); // fflush(_outputFile); return 0; }