// update L1 control for loitering void AP_L1_Control::update_loiter(const struct Location ¢er_WP, float radius, int8_t loiter_direction) { struct Location _current_loc; // Calculate guidance gains used by PD loop (used during circle tracking) float omega = (6.2832f / _L1_period); float Kx = omega * omega; float Kv = 2.0f * _L1_damping * omega; // Calculate L1 gain required for specified damping (used during waypoint capture) float K_L1 = 4.0f * _L1_damping * _L1_damping; //Get current position and velocity _ahrs->get_position(&_current_loc); // update _target_bearing_cd _target_bearing_cd = get_bearing_cd(&_current_loc, ¢er_WP); Vector2f _groundspeed_vector = _ahrs->groundspeed_vector(); //Calculate groundspeed float groundSpeed = _groundspeed_vector.length(); // Calculate time varying control parameters // Calculate the L1 length required for specified period // 0.3183099 = 1/pi _L1_dist = 0.3183099f * _L1_damping * _L1_period * groundSpeed; //Convert current location and WP positionsto 2D vectors in lat and long Vector2f A_air((_current_loc.lat*1.0e-7f), (_current_loc.lng*1.0e-7f)); Vector2f A_v((center_WP.lat*1.0e-7f), (center_WP.lng*1.0e-7f)); //Calculate the NE position of the aircraft relative to WP A A_air = _geo2planar(A_v, A_air)*RADIUS_OF_EARTH; //Calculate the unit vector from WP A to aircraft Vector2f A_air_unit = (A_air).normalized(); //Calculate Nu to capture center_WP float xtrackVelCap = A_air_unit % _groundspeed_vector; // Velocity across line - perpendicular to radial inbound to WP float ltrackVelCap = - (_groundspeed_vector * A_air_unit); // Velocity along line - radial inbound to WP float Nu = atan2f(xtrackVelCap,ltrackVelCap); Nu = constrain_float(Nu, -1.5708f, +1.5708f); //Limit Nu to +- Pi/2 //Calculate lat accln demand to capture center_WP (use L1 guidance law) float latAccDemCap = K_L1 * groundSpeed * groundSpeed / _L1_dist * sinf(Nu); //Calculate radial position and velocity errors float xtrackVelCirc = -ltrackVelCap; // Radial outbound velocity - reuse previous radial inbound velocity float xtrackErrCirc = A_air.length() - radius; // Radial distance from the loiter circle // keep crosstrack error for reporting _crosstrack_error = xtrackErrCirc; //Calculate PD control correction to circle waypoint float latAccDemCircPD = (xtrackErrCirc * Kx + xtrackVelCirc * Kv); //Calculate tangential velocity float velTangent = xtrackVelCap * float(loiter_direction); //Prevent PD demand from turning the wrong way by limiting the command when flying the wrong way if ( velTangent < 0.0f ) { latAccDemCircPD = _maxf(latAccDemCircPD , 0.0f); } // Calculate centripetal acceleration demand float latAccDemCircCtr = velTangent * velTangent / _maxf((0.5f * radius) , (radius + xtrackErrCirc)); //Sum PD control and centripetal acceleration to calculate lateral manoeuvre demand float latAccDemCirc = loiter_direction * (latAccDemCircPD + latAccDemCircCtr); // Perform switchover between 'capture' and 'circle' modes at the point where the commands cross over to achieve a seamless transfer // Only fly 'capture' mode if outside the circle if ((latAccDemCap < latAccDemCirc && loiter_direction > 0 && xtrackErrCirc > 0.0f) | (latAccDemCap > latAccDemCirc && loiter_direction < 0 && xtrackErrCirc > 0.0f)) { _latAccDem = latAccDemCap; _WPcircle = false; _bearing_error = Nu; // angle between demanded and achieved velocity vector, +ve to left of track _nav_bearing = atan2f(-A_air_unit.y , -A_air_unit.x); // bearing (radians) from AC to L1 point } else { _latAccDem = latAccDemCirc; _WPcircle = true; _bearing_error = 0.0f; // bearing error (radians), +ve to left of track _nav_bearing = atan2f(-A_air_unit.y , -A_air_unit.x); // bearing (radians)from AC to L1 point } }
// update L1 control for waypoint navigation // this function costs about 3.5 milliseconds on a AVR2560 void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct Location &next_WP) { struct Location _current_loc; float Nu; float xtrackVel; float ltrackVel; // Calculate L1 gain required for specified damping float K_L1 = 4.0f * _L1_damping * _L1_damping; // Get current position and velocity _ahrs->get_position(&_current_loc); // update _target_bearing_cd _target_bearing_cd = get_bearing_cd(&_current_loc, &next_WP); Vector2f _groundspeed_vector = _ahrs->groundspeed_vector(); //Calculate groundspeed float groundSpeed = _groundspeed_vector.length(); // Calculate time varying control parameters // Calculate the L1 length required for specified period // 0.3183099 = 1/1/pipi _L1_dist = 0.3183099f * _L1_damping * _L1_period * groundSpeed; //Convert current location and WP positions to 2D vectors in lat and long Vector2f A_air((_current_loc.lat*1.0e-7f), (_current_loc.lng*1.0e-7f)); Vector2f A_v((prev_WP.lat*1.0e-7f), (prev_WP.lng*1.0e-7f)); Vector2f B_v((next_WP.lat*1.0e-7f), (next_WP.lng*1.0e-7f)); //Calculate the NE position of the aircraft and WP B relative to WP A A_air = _geo2planar(A_v, A_air)*RADIUS_OF_EARTH; Vector2f AB = _geo2planar(A_v, B_v)*RADIUS_OF_EARTH; //Calculate the unit vector from WP A to WP B Vector2f AB_unit = (AB).normalized(); // calculate distance to target track, for reporting _crosstrack_error = AB_unit % A_air; //Determine if the aircraft is behind a +-135 degree degree arc centred on WP A //and further than L1 distance from WP A. Then use WP A as the L1 reference point //Otherwise do normal L1 guidance float WP_A_dist = A_air.length(); float alongTrackDist = A_air * AB_unit; if (WP_A_dist > _L1_dist && alongTrackDist/(WP_A_dist + 1.0f) < -0.7071f) { //Calc Nu to fly To WP A Vector2f A_air_unit = (A_air).normalized(); // Unit vector from WP A to aircraft xtrackVel = _groundspeed_vector % (-A_air_unit); // Velocity across line ltrackVel = _groundspeed_vector * (-A_air_unit); // Velocity along line Nu = atan2f(xtrackVel,ltrackVel); _nav_bearing = atan2f(-A_air_unit.y , -A_air_unit.x); // bearing (radians) from AC to L1 point } else { //Calc Nu to fly along AB line //Calculate Nu2 angle (angle of velocity vector relative to line connecting waypoints) xtrackVel = _groundspeed_vector % AB_unit; // Velocity cross track ltrackVel = _groundspeed_vector * AB_unit; // Velocity along track float Nu2 = atan2f(xtrackVel,ltrackVel); //Calculate Nu1 angle (Angle to L1 reference point) float xtrackErr = A_air % AB_unit; float sine_Nu1 = xtrackErr/_maxf(_L1_dist , 0.1f); //Limit sine of Nu1 to provide a controlled track capture angle of 45 deg sine_Nu1 = constrain_float(sine_Nu1, -0.7854f, 0.7854f); float Nu1 = asinf(sine_Nu1); Nu = Nu1 + Nu2; _nav_bearing = atan2f(AB_unit.y, AB_unit.x) + Nu1; // bearing (radians) from AC to L1 point } //Limit Nu to +-pi Nu = constrain_float(Nu, -1.5708f, +1.5708f); _latAccDem = K_L1 * groundSpeed * groundSpeed / _L1_dist * sinf(Nu); // Waypoint capture status is always false during waypoint following _WPcircle = false; _bearing_error = Nu; // bearing error angle (radians), +ve to left of track }
R744Class::R744Class() { static double alpha[40],beta[43],GAMMA[40],epsilon[40],a[43],b[43],A[43],B[43],C[43],D[43],a0[9],theta0[9]; static double n[]={0, 0.388568232032E+00, 0.293854759427E+01, -0.558671885350E+01, -0.767531995925E+00, 0.317290055804E+00, 0.548033158978E+00, 0.122794112203E+00, 0.216589615432E+01, 0.158417351097E+01, -0.231327054055E+00, 0.581169164314E-01, -0.553691372054E-00, 0.489466159094E-00, -0.242757398435E-01, 0.624947905017E-01, -0.121758602252E+00, -0.370556852701E+00, -0.167758797004E-01, -0.119607366380E+00, -0.456193625088E-01, 0.356127892703E-01, -0.744277271321E-02, -0.173957049024E-02, -0.218101212895E-01, 0.243321665592E-01, -0.374401334235E-01, 0.143387157569E-00, -0.134919690833E-00, -0.231512250535E-01, 0.123631254929E-01, 0.210583219729E-02, -0.339585190264E-03, 0.559936517716E-02, -0.303351180556E-03, -0.213654886883E+03, 0.266415691493E+05, -0.240272122046E+05, -0.283416034240E+03, 0.212472844002E+03, -0.666422765408E+00, 0.726086323499E+00, 0.550686686128E-01}; static double d[]={0, 1, 1, 1, 1, 2, 2, 3, 1, 2, 4, 5, 5, 5, 6, 6, 6, 1, 1, 4, 4, 4, 7, 8, 2, 3, 3, 5, 5, 6, 7, 8, 10, 4, 8, 2, 2, 2, 3, 3}; static double t[]={0.00, 0.00, 0.75, 1.00, 2.00, 0.75, 2.00, 0.75, 1.50, 1.50, 2.50, 0.00, 1.50, 2.00, 0.00, 1.00, 2.00, 3.00, 6.00, 3.00, 6.00, 8.00, 6.00, 0.00, 7.00, 12.00, 16.00, 22.00, 24.00, 16.00, 24.00, 8.00, 2.00, 28.00, 14.00, 1.00, 0.00, 1.00, 3.00, 3.00}; static double c[]={0,0,0,0,0,0,0,0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2, 2, 2, 2, 2, 2, 2, 3, 3, 3, 4, 4, 4, 4, 4, 4, 5, 6}; alpha[35]=25.0; alpha[36]=25.0; alpha[37]=25.0; alpha[38]=15.0; alpha[39]=20.0; beta[35]=325.0; beta[36]=300.0; beta[37]=300.0; beta[38]=275.0; beta[39]=275.0; GAMMA[35]=1.16; GAMMA[36]=1.19; GAMMA[37]=1.19; GAMMA[38]=1.25; GAMMA[39]=1.22; epsilon[35]=1.00; epsilon[36]=1.00; epsilon[37]=1.00; epsilon[38]=1.00; epsilon[39]=1.00; a[40]=3.5; a[41]=3.5; a[42]=3.0; b[40]=0.875; b[41]=0.925; b[42]=0.875; beta[40]=0.300; beta[41]=0.300; beta[42]=0.300; A[40]=0.700; A[41]=0.700; A[42]=0.700; B[40]=0.3; B[41]=0.3; B[42]=1.0; C[40]=10.0; C[41]=10.0; C[42]=12.5; D[40]=275.0; D[41]=275.0; D[42]=275.0; //Constants for ideal gas expression a0[1]=8.37304456; a0[2]=-3.70454304; a0[3]=2.500000; a0[4]=1.99427042; a0[5]=0.62105248; a0[6]=0.41195293; a0[7]=1.04028922; a0[8]=0.08327678; theta0[4]=3.15163; theta0[5]=6.11190; theta0[6]=6.77708; theta0[7]=11.32384; theta0[8]=27.08792; std::vector<double> a_v(a,a+sizeof(a)/sizeof(double)); std::vector<double> b_v(b,b+sizeof(b)/sizeof(double)); std::vector<double> A_v(A,A+sizeof(A)/sizeof(double)); std::vector<double> B_v(B,B+sizeof(B)/sizeof(double)); std::vector<double> C_v(C,C+sizeof(C)/sizeof(double)); std::vector<double> D_v(D,D+sizeof(D)/sizeof(double)); phirlist.push_back(new phir_power(n,d,t,c,1,34,35)); phirlist.push_back(new phir_gaussian(n,d,t,alpha,epsilon,beta,GAMMA,35,39,40)); phirlist.push_back(new phir_critical(n,d,t,a,b,beta,A,B,C,D,40,42,43)); std::vector<double> a0_v(a0,a0+sizeof(a0)/sizeof(double)); std::vector<double> theta0_v(theta0,theta0+sizeof(theta0)/sizeof(double)); phi0list.push_back(new phi0_lead(a0[1],a0[2])); phi0list.push_back(new phi0_logtau(a0[3])); phi0list.push_back(new phi0_Planck_Einstein(a0,theta0,4,8,9)); // Critical parameters crit.rho = 44.0098*10.6249063; crit.p = PressureUnit(7377.3, UNIT_KPA); crit.T = 304.1282; crit.v = 1.0/crit.rho; // Other fluid parameters params.molemass = 44.0098; params.Ttriple = 216.592; params.ptriple = 517.996380545; params.R_u = 8.31451; params.accentricfactor = 0.22394; // Limit parameters limits.Tmin = 216.592; limits.Tmax = 2000.0; limits.pmax = 800000.0; limits.rhomax = 37.24 * params.molemass; EOSReference.assign("\"A New Equation of State for Carbon Dioxide Covering the Fluid Region from the " "Triple Point Temperature to 1100 K at Pressures up to 800 MPa\", " "R. Span and W. Wagner, J. Phys. Chem. Ref. Data, v. 25, 1996"); TransportReference.assign("\"The Transport Properties of Carbon Dioxide\"," " V. Vesovic and W.A. Wakeham and G.A. Olchowy and " "J.V. Sengers and J.T.R. Watson and J. Millat" "J. Phys. Chem. Ref. Data, v. 19, 1990"); name.assign("CarbonDioxide"); aliases.push_back("R744"); aliases.push_back("co2"); aliases.push_back("CO2"); aliases.push_back("carbondioxide"); REFPROPname.assign("CO2"); // Adjust to the IIR reference state (h=200 kJ/kg, s = 1 kJ/kg for sat. liq at 0C) params.HSReferenceState = "IIR"; BibTeXKeys.EOS = "Span-JPCRD-1996"; BibTeXKeys.SURFACE_TENSION = "Mulero-JPCRD-2012"; BibTeXKeys.VISCOSITY = "Vesovic-JPCRD-1990"; BibTeXKeys.CONDUCTIVITY = "Vesovic-JPCRD-1990"; }