// 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";
}
