void BlockLocalPositionEstimator::publishLocalPos()
{
const Vector<float, n_x> &xLP = _xLowPass.getState();
// publish local position
if (PX4_ISFINITE(_x(X_x)) && PX4_ISFINITE(_x(X_y)) && PX4_ISFINITE(_x(X_z)) &&
PX4_ISFINITE(_x(X_vx)) && PX4_ISFINITE(_x(X_vy))
&& PX4_ISFINITE(_x(X_vz))) {
_pub_lpos.get().timestamp = _timeStamp;
_pub_lpos.get().xy_valid = _validXY;
_pub_lpos.get().z_valid = _validZ;
_pub_lpos.get().v_xy_valid = _validXY;
_pub_lpos.get().v_z_valid = _validZ;
_pub_lpos.get().x = xLP(X_x); // north
_pub_lpos.get().y = xLP(X_y); // east
_pub_lpos.get().z = xLP(X_z); // down
_pub_lpos.get().vx = xLP(X_vx); // north
_pub_lpos.get().vy = xLP(X_vy); // east
_pub_lpos.get().vz = xLP(X_vz); // down
_pub_lpos.get().yaw = _sub_att.get().yaw;
_pub_lpos.get().xy_global = _sub_home.get().timestamp != 0; // need home for reference
_pub_lpos.get().z_global = _baroInitialized;
_pub_lpos.get().ref_timestamp = _sub_home.get().timestamp;
_pub_lpos.get().ref_lat = _map_ref.lat_rad * 180 / M_PI;
_pub_lpos.get().ref_lon = _map_ref.lon_rad * 180 / M_PI;
_pub_lpos.get().ref_alt = _sub_home.get().alt;
_pub_lpos.get().dist_bottom = agl();
_pub_lpos.get().dist_bottom_rate = - xLP(X_vz);
_pub_lpos.get().surface_bottom_timestamp = _timeStamp;
_pub_lpos.get().dist_bottom_valid = _validTZ && _validZ;
_pub_lpos.get().eph = sqrtf(_P(X_x, X_x) + _P(X_y, X_y));
_pub_lpos.get().epv = sqrtf(_P(X_z, X_z));
_pub_lpos.update();
}
}
void BlockLocalPositionEstimator::predict()
{
// if can't update anything, don't propagate
// state or covariance
if (!_validXY && !_validZ) { return; }
if (integrate) {
matrix::Quaternion<float> q(&_sub_att.get().q[0]);
_eul = matrix::Euler<float>(q);
_R_att = matrix::Dcm<float>(q);
Vector3f a(_sub_sensor.get().accelerometer_m_s2);
// note, bias is removed in dynamics function
_u = _R_att * a;
_u(U_az) += 9.81f; // add g
} else {
_u = Vector3f(0, 0, 0);
}
// update state space based on new states
updateSSStates();
// continuous time kalman filter prediction
// integrate runge kutta 4th order
// TODO move rk4 algorithm to matrixlib
// https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta_methods
float h = getDt();
Vector<float, n_x> k1, k2, k3, k4;
k1 = dynamics(0, _x, _u);
k2 = dynamics(h / 2, _x + k1 * h / 2, _u);
k3 = dynamics(h / 2, _x + k2 * h / 2, _u);
k4 = dynamics(h, _x + k3 * h, _u);
Vector<float, n_x> dx = (k1 + k2 * 2 + k3 * 2 + k4) * (h / 6);
// propagate
correctionLogic(dx);
_x += dx;
Matrix<float, n_x, n_x> dP = (_A * _P + _P * _A.transpose() +
_B * _R * _B.transpose() + _Q) * getDt();
covPropagationLogic(dP);
_P += dP;
_xLowPass.update(_x);
_aglLowPass.update(agl());
}
int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
{
// check for agl
if (agl() < flow_min_agl) {
return -1;
}
// check quality
float qual = _sub_flow.get().quality;
if (qual < _flow_min_q.get()) {
return -1;
}
// calculate range to center of image for flow
if (!_validTZ) {
return -1;
}
float d = agl() * cosf(_sub_att.get().roll) * cosf(_sub_att.get().pitch);
// check for global accuracy
if (_gpsInitialized) {
double lat = _sub_gps.get().lat * 1.0e-7;
double lon = _sub_gps.get().lon * 1.0e-7;
float px = 0;
float py = 0;
map_projection_project(&_map_ref, lat, lon, &px, &py);
Vector2f delta(px - _flowX, py - _flowY);
if (delta.norm() > 3) {
mavlink_and_console_log_info(&mavlink_log_pub,
"[lpe] flow too far from GPS, resetting position");
_flowX = px;
_flowY = py;
return -1;
}
}
// optical flow in x, y axis
float flow_x_rad = _sub_flow.get().pixel_flow_x_integral;
float flow_y_rad = _sub_flow.get().pixel_flow_y_integral;
// angular rotation in x, y axis
float gyro_x_rad = _flow_gyro_x_high_pass.update(
_sub_flow.get().gyro_x_rate_integral);
float gyro_y_rad = _flow_gyro_y_high_pass.update(
_sub_flow.get().gyro_y_rate_integral);
//warnx("flow x: %10.4f y: %10.4f gyro_x: %10.4f gyro_y: %10.4f d: %10.4f",
//double(flow_x_rad), double(flow_y_rad), double(gyro_x_rad), double(gyro_y_rad), double(d));
// compute velocities in camera frame using ground distance
// assume camera frame is body frame
Vector3f delta_b(
-(flow_x_rad - gyro_x_rad)*d,
-(flow_y_rad - gyro_y_rad)*d,
0);
// rotation of flow from body to nav frame
Matrix3f R_nb(_sub_att.get().R);
Vector3f delta_n = R_nb * delta_b;
// flow integration
_flowX += delta_n(0);
_flowY += delta_n(1);
// measurement
y(Y_flow_x) = _flowX;
y(Y_flow_y) = _flowY;
_flowQStats.update(Scalarf(_sub_flow.get().quality));
// imporant to timestamp flow even if distance is bad
_time_last_flow = _timeStamp;
return OK;
}
void BlockLocalPositionEstimator::flowCorrect()
{
// measure flow
Vector<float, n_y_flow> y;
if (flowMeasure(y) != OK) { return; }
// flow measurement matrix and noise matrix
Matrix<float, n_y_flow, n_x> C;
C.setZero();
C(Y_flow_x, X_x) = 1;
C(Y_flow_y, X_y) = 1;
SquareMatrix<float, n_y_flow> R;
R.setZero();
float d = agl() * cosf(_sub_att.get().roll) * cosf(_sub_att.get().pitch);
float flow_xy_stddev = _flow_xy_stddev.get() + _flow_xy_d_stddev.get() * d ;
R(Y_flow_x, Y_flow_x) = flow_xy_stddev * flow_xy_stddev;
R(Y_flow_y, Y_flow_y) = R(Y_flow_x, Y_flow_x);
// residual
Vector<float, 2> r = y - C * _x;
_pub_innov.get().flow_innov[0] = r(0);
_pub_innov.get().flow_innov[1] = r(1);
_pub_innov.get().flow_innov_var[0] = R(0, 0);
_pub_innov.get().flow_innov_var[1] = R(1, 1);
// residual covariance, (inverse)
Matrix<float, n_y_flow, n_y_flow> S_I =
inv<float, n_y_flow>(C * _P * C.transpose() + R);
// fault detection
float beta = (r.transpose() * (S_I * r))(0, 0);
if (beta > BETA_TABLE[n_y_flow]) {
if (_flowFault < FAULT_MINOR) {
//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] flow fault, beta %5.2f", double(beta));
_flowFault = FAULT_MINOR;
}
} else if (_flowFault) {
_flowFault = FAULT_NONE;
//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] flow OK");
}
if (_flowFault < fault_lvl_disable) {
Matrix<float, n_x, n_y_flow> K =
_P * C.transpose() * S_I;
Vector<float, n_x> dx = K * r;
correctionLogic(dx);
_x += dx;
_P -= K * C * _P;
} else {
// reset flow integral to current estimate of position
// if a fault occurred
_flowX = _x(X_x);
_flowY = _x(X_y);
}
}
/*This function computes the agl in num_wave directions*/
void mexFunction(int nlhs, mxArray *plhs[], /* Output variables */
int nrhs, const mxArray *prhs[]) /* Input variables */
{
#define agl(ai,bi,j) agl[ai+dims[0]*(bi+dims[1]*j)]
#define R(ai,bi,j) R[ai+dims[0]*(bi+dims[1]*j)]
#define TTEng_1st(ai,bi,j) TTEng_1st[ai+dims[0]*(bi+dims[1]*j)]
#define TTEng_2nd(ai,bi,j) TTEng_2nd[ai+dims[0]*(bi+dims[1]*j)]
#define W_sec(ai,bi,j) W_sec[ai+dims[0]*(bi+dims[1]*j)]
#define ss_energy(ci,di,ai,bi) ss_energy[ci+Nss[0]*(di+Nss[1]*(ai+Nss[2]*bi))]
#define ss_avgdx(ci,di,ai,bi) ss_avgdx[ci+Nss[0]*(di+Nss[1]*(ai+Nss[2]*bi))]
#define ss_avgdy(ci,di,ai,bi) ss_avgdy[ci+Nss[0]*(di+Nss[1]*(ai+Nss[2]*bi))]
#define aglPos(ai,bi) aglPos[ai+bi*Nss[2]]
#define checktemp2(di,ai,bi) checktemp2[di+dims2[0]*(ai+dims2[1]*bi)]
/* #define agl2(ai,bi,j) agl2[ai+dims[0]*(bi+dims[1]*j)]
#define R2(ai,bi,j) R2[ai+dims[0]*(bi+dims[1]*j)] */
#define pi 3.141592653589793
size_t ai, bi, ci, k, j;
int di,cnt2,cnt;
int num_wave, pos_temp;
double *ss_energy, *ss_avgdx, *ss_avgdy, *agl, *R, *TTEng_1st, *TTEng_2nd, *W_sec, *aglPos, *checktemp2; /* R2, agl2 */
double temp_avgdx, temp_avgdy, sumEng;
ss_energy = mxGetPr(prhs[0]);
const mwSize *Nss = mxGetDimensions(prhs[0]);
ss_avgdx = mxGetPr(prhs[1]);
ss_avgdy = mxGetPr(prhs[2]);
num_wave = mxGetScalar(prhs[3]);
aglPos = mxGetPr(prhs[4]);
nrhs = 5;
nlhs = 6;
int ndim = 3, dims[3] = {Nss[2],Nss[3],num_wave}, numm = (int)Nss[1]/num_wave/8, numm_R = (int)floor(Nss[0]/4.0);
int LL = (int)Nss[1]/num_wave/4;
plhs[0] = mxCreateNumericArray(ndim,dims,mxDOUBLE_CLASS,mxREAL);
plhs[1] = mxCreateNumericArray(ndim,dims,mxDOUBLE_CLASS,mxREAL);
plhs[2] = mxCreateNumericArray(ndim,dims,mxDOUBLE_CLASS,mxREAL);
plhs[3] = mxCreateNumericArray(ndim,dims,mxDOUBLE_CLASS,mxREAL);
plhs[4] = mxCreateNumericArray(ndim,dims,mxDOUBLE_CLASS,mxREAL);
/* plhs[5] = mxCreateNumericArray(ndim,dims,mxDOUBLE_CLASS,mxREAL); */
/* plhs[6] = mxCreateNumericArray(ndim,dims,mxDOUBLE_CLASS,mxREAL); */
agl = mxGetPr(plhs[0]);
R = mxGetPr(plhs[1]);
TTEng_1st = mxGetPr(plhs[2]);
TTEng_2nd = mxGetPr(plhs[3]);
W_sec = mxGetPr(plhs[4]);
/* agl2 = mxGetPr(plhs[5]); */
/* R2 = mxGetPr(plhs[6]); */
int dims2[3] = {Nss[1],Nss[2],Nss[3]};
plhs[5] = mxCreateNumericArray(ndim,dims2,mxDOUBLE_CLASS,mxREAL);
checktemp2 = mxGetPr(plhs[5]);
int L = ceil(1.0*Nss[1]/num_wave), maxpos, st, ed, stR, edR, moveStep, pos, sgn, temp_di, markPos;
double *temp, energy_sum, *temp_R, *temp2, *tempss, tpR;
temp = (double *)mxMalloc(sizeof(double)*L);
temp2 = (double *)mxMalloc(sizeof(double)*Nss[1]);
tempss = (double *)mxMalloc(sizeof(double)*Nss[1]);
temp_R = (double *)mxMalloc(sizeof(double)*Nss[0]);
for (ai=0;ai<Nss[2];ai++) {
for (bi=0;bi<Nss[3];bi++) {
/*compute tempss*/
for (di=0;di<Nss[1];di++){
temp2[di] = 0;
}
for (cnt=0;cnt<num_wave;cnt++) {
for (cnt2=-LL+1;cnt2<LL;cnt2++){
di = cnt2 + aglPos(ai,bi) + cnt*L;
if (di<0) {
di = di + Nss[1];
}else if (di>=Nss[1]) {
di = di - Nss[1];
}
/*mexPrintf("%d %f %d %d %d %d\n",di,aglPos(ai,bi),LL,L,cnt,cnt2);*/
/*for (di=0;di<Nss[1];di++) {*/
for (ci=0;ci<Nss[0];ci++) {
if (ci==0)
temp2[di] = ss_energy(ci,di,ai,bi);
else
temp2[di] = temp2[di] + ss_energy(ci,di,ai,bi);
}
}
}
for (di=0;di<Nss[1];di++)
checktemp2(di,ai,bi) = temp2[di];
/*mexPrintf("(%d,%f) %f\n",LL,aglPos(ai,bi),aglPos[0]);*/
moveStep = floor(L/4.0);
/*maxpos = fmod(findmax(temp2,Nss[1]),L);*/
maxpos = findmax(temp2,Nss[1]);
markPos = floor(1.0*maxpos/L);
maxpos = maxpos - markPos*L;
moveStep = floor(maxpos-L/2.0);
if (moveStep>=0) {
sgn = 1;
for (cnt=0;cnt<Nss[1]-moveStep;cnt++) {
tempss[cnt] = temp2[cnt+moveStep];
}
for (cnt=Nss[1]-moveStep;cnt<Nss[1];cnt++) {
tempss[cnt] = temp2[cnt -(Nss[1]-moveStep)];
} }
if (moveStep<0) {
moveStep = -moveStep;
sgn = -1;
for (cnt = 0; cnt < Nss[1]-moveStep; cnt++) {
tempss[cnt+moveStep] = temp2[cnt];
}
for (cnt = Nss[1]-moveStep; cnt < Nss[1]; cnt++) {
tempss[cnt-(Nss[1]-moveStep)] = temp2[cnt];
} }
for (j=0;j<num_wave;j++) {
for (cnt=0;cnt<L;cnt++) {
W_sec(ai,bi,j) = W_sec(ai,bi,j) + tempss[cnt+j*L];
}
}
/*tempss[:] computed*/
for (j=0;j<num_wave;j++) {
maxpos = findmax2(tempss,j*L,L);
ed = maxpos;
cnt = 1;
while (ed<Nss[1]-1 & cnt==1 & tempss[ed+1]>0 & ed-maxpos < numm) {
if (tempss[ed]>=tempss[ed+1])
ed++;
else
cnt = 0;
}
st = maxpos;
cnt = 1;
while (st>0 & cnt == 1 & tempss[st-1]>0 & maxpos-st < numm) {
if (tempss[st] >= tempss[st-1])
st--;
else
cnt = 0;
}
energy_sum = 0;
for (cnt = st; cnt <= ed; cnt++)
energy_sum = energy_sum + tempss[cnt];
/*compute TTEng_1st(ai,bi,j)*/
TTEng_1st(ai,bi,j) = energy_sum;
/* /\*compute agl(ai,bi,j)*\/ */
for (cnt = st; cnt <= ed; cnt++) {
/* agl(ai,bi,j) = agl(ai,bi,j) + tempss[cnt]*cnt/energy_sum; */
tempss[cnt] = 0;
}
/* agl(ai,bi,j) = agl(ai,bi,j) + sgn*moveStep; */
/*compute R(ai,bi,j)*/
/* change (st,ed) back to original coordinate */
st = st + sgn*moveStep;
if (st>=Nss[1]) {
st = st - Nss[1];
}
else {
if (st<0){
st = st + Nss[1];
}
}
ed = ed + sgn*moveStep;
if (ed>=Nss[1]) {
ed = ed - Nss[1];
}
else {
if (ed<0){
ed = ed + Nss[1];
}
}
int st_agl = st;
int ed_agl = ed;
/*compute R*/
/*compute temp_R*/
if (1) {
if (st<=ed) {
for (di=st;di<=ed;di++) {
for (ci = 0;ci<Nss[0];ci++) {
if (di==st) {
temp_R[ci] = ss_energy(ci,di,ai,bi);
}
else
temp_R[ci] = temp_R[ci] + ss_energy(ci,di,ai,bi);
}
}
maxpos = findmax(temp_R,Nss[0]);
ed = maxpos;
cnt = 1;
while (ed<Nss[0]-1 & cnt==1 & temp_R[ed+1]>0 & ed-maxpos < numm_R) {
if (temp_R[ed]>=temp_R[ed+1]) {
ed++;
}
else
cnt = 0;
}
st = maxpos;
cnt = 1;
while (st>0 & cnt == 1 & temp_R[st-1]>0 & maxpos-st < numm_R) {
if (temp_R[st] >= temp_R[st-1]) {
st--;
}
else
cnt = 0;
}
/*
energy_sum = 0;
for (cnt = st; cnt <= ed; cnt++) {
energy_sum = energy_sum + temp_R[cnt];
}
if (energy_sum>0) {
for (cnt = st; cnt <= ed; cnt++) {
R(ai,bi,j) = R(ai,bi,j) + temp_R[cnt]*(cnt)/energy_sum;
}
} */
sumEng = 0;
temp_avgdx = 0; temp_avgdy = 0;
for (di=st_agl;di<=ed_agl;di++) {
for (ci=st;ci<=ed;ci++) {
temp_avgdx += ss_avgdx(ci,di,ai,bi);
temp_avgdy += ss_avgdy(ci,di,ai,bi);
sumEng += ss_energy(ci,di,ai,bi);
}
}
if (sumEng==0) {
agl(ai,bi,j) = 0;
R(ai,bi,j) = 0;
/*mexPrintf("%f %f, ",sumEng,TTEng_1st(ai,bi,j));*/
}else {
temp_avgdx /= sumEng;
temp_avgdy /= sumEng;
R(ai,bi,j) = sqrt(temp_avgdx*temp_avgdx + temp_avgdy*temp_avgdy);
if (temp_avgdx>=0) {
agl(ai,bi,j) = acos(temp_avgdy/R(ai,bi,j));
}
else
agl(ai,bi,j) = pi - acos(temp_avgdy/R(ai,bi,j));
}
}
else {
ed = ed + Nss[1];
for (di=st;di<=ed;di++) {
for (ci = 0;ci<Nss[0];ci++) {
if (di==st) {
temp_R[ci] = ss_energy(ci,di,ai,bi);
}
else {
if (di>=Nss[1]) {
temp_R[ci] = temp_R[ci] + ss_energy(ci,di-Nss[1],ai,bi);
}
else
temp_R[ci] = temp_R[ci] + ss_energy(ci,di,ai,bi);
}
}
}
maxpos = findmax(temp_R,Nss[0]);
ed = maxpos;
cnt = 1;
while (ed<Nss[0]-1 & cnt==1 & temp_R[ed+1]>0 & ed-maxpos < numm_R) {
if (temp_R[ed]>=temp_R[ed+1]) {
ed++;
}
else
cnt = 0;
}
st = maxpos;
cnt = 1;
while (st>0 & cnt == 1 & temp_R[st-1]>0 & maxpos-st < numm_R) {
if (temp_R[st] >= temp_R[st-1]) {
st--;
}
else
cnt = 0;
}
/*
energy_sum = 0;
for (cnt = st; cnt <= ed; cnt++) {
energy_sum = energy_sum + temp_R[cnt];
}
if (energy_sum>0) {
for (cnt = st; cnt <= ed; cnt++) {
R(ai,bi,j) = R(ai,bi,j) + temp_R[cnt]*(cnt)/energy_sum;
}
} */
ed_agl = ed_agl + Nss[1];
sumEng = 0;
temp_avgdx = 0; temp_avgdy = 0;
for (di=st_agl;di<=ed_agl;di++) {
if (di>=Nss[1]) {
for (ci=st;ci<=ed;ci++) {
temp_avgdx -= ss_avgdx(ci,di-Nss[1],ai,bi); /* "-" due to shifting to negative half plane */
temp_avgdy -= ss_avgdy(ci,di-Nss[1],ai,bi);
sumEng += ss_energy(ci,di-Nss[1],ai,bi);
}
}
else {
for (ci=st;ci<=ed;ci++) {
temp_avgdx += ss_avgdx(ci,di,ai,bi);
temp_avgdy += ss_avgdy(ci,di,ai,bi);
sumEng += ss_energy(ci,di,ai,bi);
}
}
}
if (sumEng==0) {
agl(ai,bi,j) = 0;
R(ai,bi,j) = 0;
/*mexPrintf("%f %f, ",sumEng,TTEng_1st(ai,bi,j));*/
}else {
temp_avgdx /= sumEng;
temp_avgdy /= sumEng;
R(ai,bi,j) = sqrt(temp_avgdx*temp_avgdx + temp_avgdy*temp_avgdy);
if (temp_avgdx>=0) {
agl(ai,bi,j) = acos(temp_avgdy/R(ai,bi,j));
}
else
agl(ai,bi,j) = pi - acos(temp_avgdy/R(ai,bi,j));
}
}
}
/*compute TTEng_2nd(ai,bi,j)*/
maxpos = findmax2(tempss,j*L,L);
if (tempss[maxpos]>0) {
ed = maxpos;
cnt = 1;
while (ed<Nss[1]-1 & cnt==1 & tempss[ed+1]>0 & ed-maxpos < numm) {
if (tempss[ed]>=tempss[ed+1])
ed++;
else
cnt = 0;
}
st = maxpos;
cnt = 1;
while (st>0 & cnt == 1 & tempss[st-1]>0 & maxpos-st < numm) {
if (tempss[st] >= tempss[st-1])
st--;
else
cnt = 0;
}
energy_sum = 0;
for (cnt = st; cnt <= ed; cnt++)
energy_sum = energy_sum + tempss[cnt];
if (energy_sum<=TTEng_1st(ai,bi,j))
TTEng_2nd(ai,bi,j) = energy_sum;
else {
agl(ai,bi,j) = 0;
/* agl2(ai,bi,j) = 0;
for (cnt = st; cnt <= ed; cnt++)
agl(ai,bi,j) = agl(ai,bi,j) + tempss[cnt]*cnt/energy_sum;
agl(ai,bi,j) = agl(ai,bi,j) + sgn*moveStep; */
/*compute total energy*/
TTEng_2nd(ai,bi,j) = TTEng_1st(ai,bi,j);
TTEng_1st(ai,bi,j) = energy_sum;
R(ai,bi,j) = 0;
/* R2(ai,bi,j) = 0; */
/*compute R(ai,bi,j)*/
st = st + sgn*moveStep;
if (st>=Nss[1]) {
st = st - Nss[1];
}
else {
if (st<0){
st = st + Nss[1];
}
}
ed = ed + sgn*moveStep;
if (ed>=Nss[1]) {
ed = ed - Nss[1];
}
else {
if (ed<0){
ed = ed + Nss[1];
}
}
int st_agl = st;
int ed_agl = ed;
/*compute R*/
/*compute temp_R*/
if (1) {
if (st<=ed) {
for (di=st;di<=ed;di++) {
for (ci = 0;ci<Nss[0];ci++) {
if (di==st) {
temp_R[ci] = ss_energy(ci,di,ai,bi);
}
else
temp_R[ci] = temp_R[ci] + ss_energy(ci,di,ai,bi);
}
}
maxpos = findmax(temp_R,Nss[0]);
ed = maxpos;
cnt = 1;
while (ed<Nss[0]-1 & cnt==1 & temp_R[ed+1]>0 & ed-maxpos < numm_R) {
if (temp_R[ed]>=temp_R[ed+1]) {
ed++;
}
else
cnt = 0;
}
st = maxpos;
cnt = 1;
while (st>0 & cnt == 1 & temp_R[st-1]>0 & maxpos-st < numm_R) {
if (temp_R[st] >= temp_R[st-1]) {
st--;
}
else
cnt = 0;
}
/*
energy_sum = 0;
for (cnt = st; cnt <= ed; cnt++) {
energy_sum = energy_sum + temp_R[cnt];
}
if (energy_sum>0) {
for (cnt = st; cnt <= ed; cnt++) {
R(ai,bi,j) = R(ai,bi,j) + temp_R[cnt]*(cnt)/energy_sum;
}
} */
sumEng = 0;
temp_avgdx = 0; temp_avgdy = 0;
for (di=st_agl;di<=ed_agl;di++) {
for (ci=st;ci<=ed;ci++) {
temp_avgdx += ss_avgdx(ci,di,ai,bi);
temp_avgdy += ss_avgdy(ci,di,ai,bi);
sumEng += ss_energy(ci,di,ai,bi);
}
}
temp_avgdx /= sumEng;
temp_avgdy /= sumEng;
R(ai,bi,j) = sqrt(temp_avgdx*temp_avgdx + temp_avgdy*temp_avgdy);
if (temp_avgdx>=0) {
agl(ai,bi,j) = acos(temp_avgdy/R(ai,bi,j));
}
else
agl(ai,bi,j) = pi - acos(temp_avgdy/R(ai,bi,j));
}
else {
ed = ed + Nss[1];
for (di=st;di<=ed;di++) {
for (ci = 0;ci<Nss[0];ci++) {
if (di==st) {
temp_R[ci] = ss_energy(ci,di,ai,bi);
}
else {
if (di>=Nss[1]) {
temp_R[ci] = temp_R[ci] + ss_energy(ci,di-Nss[1],ai,bi);
}
else
temp_R[ci] = temp_R[ci] + ss_energy(ci,di,ai,bi);
}
}
}
maxpos = findmax(temp_R,Nss[0]);
ed = maxpos;
cnt = 1;
while (ed<Nss[0]-1 & cnt==1 & temp_R[ed+1]>0 & ed-maxpos < numm_R) {
if (temp_R[ed]>=temp_R[ed+1]) {
ed++;
}
else
cnt = 0;
}
st = maxpos;
cnt = 1;
while (st>0 & cnt == 1 & temp_R[st-1]>0 & maxpos-st < numm_R) {
if (temp_R[st] >= temp_R[st-1]) {
st--;
}
else
cnt = 0;
}
/*
energy_sum = 0;
for (cnt = st; cnt <= ed; cnt++) {
energy_sum = energy_sum + temp_R[cnt];
}
if (energy_sum>0) {
for (cnt = st; cnt <= ed; cnt++) {
R(ai,bi,j) = R(ai,bi,j) + temp_R[cnt]*(cnt)/energy_sum;
}
} */
ed_agl = ed_agl + Nss[1];
sumEng = 0;
temp_avgdx = 0; temp_avgdy = 0;
for (di=st_agl;di<=ed_agl;di++) {
if (di>=Nss[1]) {
for (ci=st;ci<=ed;ci++) {
temp_avgdx -= ss_avgdx(ci,di-Nss[1],ai,bi);
temp_avgdy -= ss_avgdy(ci,di-Nss[1],ai,bi);
sumEng += ss_energy(ci,di-Nss[1],ai,bi);
}
}
else {
for (ci=st;ci<=ed;ci++) {
temp_avgdx += ss_avgdx(ci,di,ai,bi);
temp_avgdy += ss_avgdy(ci,di,ai,bi);
sumEng += ss_energy(ci,di,ai,bi);
}
}
}
temp_avgdx /= sumEng;
temp_avgdy /= sumEng;
R(ai,bi,j) = sqrt(temp_avgdx*temp_avgdx + temp_avgdy*temp_avgdy);
if (temp_avgdx>=0) {
agl(ai,bi,j) = acos(temp_avgdy/R(ai,bi,j));
}
else
agl(ai,bi,j) = pi - acos(temp_avgdy/R(ai,bi,j));
}
}
}
}
else
TTEng_2nd(ai,bi,j) = 0;
}
for (j=0;j<num_wave;j++) {
W_sec(ai,bi,j) = TTEng_1st(ai,bi,j)/W_sec(ai,bi,j);
if (R(ai,bi,j)==0) {
R(ai,bi,j) = R(ai,bi,markPos);
agl(ai,bi,j) = agl(ai,bi,markPos) + (j-markPos)*pi/num_wave;
}
}
}
}
mxFree(temp);
mxFree(temp2);
mxFree(tempss);
mxFree(temp_R);
return;
}
/*NP*/
char *init_phase(float *plevel, short *phase)
/*************************************************************/
/* INIT_PHASE */
/*************************************************************/
{
short i, ok, avail;
short pIndex, zIndex, tIndex, tdIndex, oIndex;
float rh, p1, pbegin, pos, neg, p, w1;
float p_pres, p_phase, ptop, pbot;
static char st[80];
char pt[80];
*plevel = 0;
*phase = -1;
pIndex = getParmIndex("PRES");
zIndex = getParmIndex("HGHT");
tIndex = getParmIndex("TEMP");
tdIndex = getParmIndex("DWPT");
oIndex = getParmIndex("OMEG");
if (!sndg || pIndex == -1 || zIndex == -1 || tIndex == -1 ||
tdIndex == -1) {
strcpy(st, "N/A");
return st;
}
/* First, determine whether VVELS are available. If they are, */
/* use them to determine level where precipitation will develop. */
avail=0;
if (oIndex != -1) {
for( i = 0; i < numlvl; i++) {
if (qc(sndg[i][oIndex]) && (sndg[i][oIndex] < 1)) avail++;
}
}
if (avail< 5) {
/* No VVELS...must look for saturated level */
/* ----- Find the highest near-saturated 50mb
layer blo 5km agl ---- */
for(i=numlvl-1;i>0;i--) {
ok = 0;
pbegin = -999;
if (agl(sndg[i][zIndex]) < 5000.0) {
rh = mixratio(sndg[i][pIndex], sndg[i][tdIndex]) /
mixratio(sndg[i][pIndex], sndg[i][tIndex]);
if (rh > 0.8) {
p1 = sndg[i][pIndex]+50;
if ((mixratio(p1, i_dwpt(p1, I_PRES)) /
mixratio(p1, i_temp(p1, I_PRES))) > 0.8) {
ok = 1;
pbegin = p1-25.0;
break;
}
}
}
}
}
else {
/* ----- Find the highest near-saturated layer with UVV in
the lowest 5km ----- */
for(i=numlvl-1;i>0;i--) {
ok=0;
pbegin=-999;
if ((agl(sndg[i][zIndex])<5000) &&
(sndg[i][oIndex] <= 0)) {
rh = mixratio(sndg[i][pIndex],
sndg[i][tdIndex]) /
mixratio(sndg[i][pIndex], sndg[i][tIndex]);
if (rh > 0.8) {
p1 = sndg[i][pIndex]+50;
if ((mixratio(p1, i_dwpt(p1, I_PRES)) /
mixratio(p1, i_temp(p1, I_PRES))) > 0.8) {
ok = 1;
pbegin = p1-25;
break;
}
}
}
}
}
if (!ok) {
*plevel = 0;
*phase = -1;
strcpy(st, "N/A");
return st;
}
p1 = i_temp(pbegin, I_PRES);
if(p1>0) {p_phase=0; strcpy(pt, "Rain"); }
if((p1<=0) && (p1 > -5)) {p_phase=1; strcpy(pt, "Freezing Rain"); }
if((p1<=-5) && (p1 > -9)) {p_phase=1; strcpy(pt, "ZR/S Mix" ); }
if(p1 <= -9) {p_phase=3; strcpy(pt, "Snow"); }
*plevel = pbegin;
*phase = p_phase;
return pt;
}
/*NP*/
char *best_guess(struct _ptype p)
/***********************************************************************/
/* BEST_GUESS */
/***********************************************************************/
{
float x1, y1, x2, y2;
static char st[80];
/* Case: no precip */
if (p.init_phase<0) {
strcpy(st, "None.");
return st;
}
/* Case: always too warm - Rain */
if ((p.init_phase==0) && (p.tneg>=0) && (p.tsfc>0)) {
strcpy(st, "Rain.");
return st;
}
/* Case: always too cold - Snow */
if ((p.init_phase==3) && (p.tpos<=0) && (p.tsfc<=0)) {
strcpy(st, "Snow.");
return st;
}
/* Case: ZR too warm at sfc - Rain */
if ((p.init_phase==1) && (p.tpos<=0) && (p.tsfc>0)) {
strcpy(st, "Rain.");
return st;
}
/* Case: non-snow init...always too cold - Initphase&sleet */
if ((p.init_phase==1) && (p.tpos<=0) && (p.tsfc<=0)) {
if (agl(i_hght(p.init_lvl, I_PRES))>=3000) {
if (p.init_temp <= -4) {
strcpy(st, "Sleet and Snow." );
return st;
}
else {
strcpy(st, "Sleet." );
return st;
}
}
else {
strcpy(st, "Freezing Rain/Drizzle.");
return st;
}
}
/* Case: Snow...but warm at sfc */
if ((p.init_phase==3) && (p.tpos<=0) && (p.tsfc>0)) {
if (p.tsfc>4)
strcpy(st, "Rain.");
else
strcpy(st, "Snow.");
return st;
}
/* Case: Warm layer. */
if (p.tpos>0) {
x1 = p.tpos;
y1 = -p.tneg;
y2 = (.62 * x1) + 60;
if (y1 > y2)
{ strcpy(st, "Sleet." ); }
else
{
if (p.tsfc<=0)
strcpy(st, "Freezing Rain." );
else
strcpy( st, "Rain." );
}
return st;
}
strcpy(st, "Unknown.");
return st;
}
void BlockLocalPositionEstimator::flowCorrect()
{
// measure flow
Vector<float, n_y_flow> y;
if (flowMeasure(y) != OK) { return; }
// flow measurement matrix and noise matrix
Matrix<float, n_y_flow, n_x> C;
C.setZero();
C(Y_flow_vx, X_vx) = 1;
C(Y_flow_vy, X_vy) = 1;
SquareMatrix<float, n_y_flow> R;
R.setZero();
// polynomial noise model, found using least squares fit
// h, h**2, v, v*h, v*h**2
const float p[5] = {0.04005232f, -0.00656446f, -0.26265873f, 0.13686658f, -0.00397357f};
// prevent extrapolation past end of polynomial fit by bounding independent variables
float h = agl();
float v = y.norm();
const float h_min = 2.0f;
const float h_max = 8.0f;
const float v_min = 0.5f;
const float v_max = 1.0f;
if (h > h_max) {
h = h_max;
}
if (h < h_min) {
h = h_min;
}
if (v > v_max) {
v = v_max;
}
if (v < v_min) {
v = v_min;
}
// compute polynomial value
float flow_vxy_stddev = p[0] * h + p[1] * h * h + p[2] * v + p[3] * v * h + p[4] * v * h * h;
float rotrate_sq = _sub_att.get().rollspeed * _sub_att.get().rollspeed
+ _sub_att.get().pitchspeed * _sub_att.get().pitchspeed
+ _sub_att.get().yawspeed * _sub_att.get().yawspeed;
matrix::Eulerf euler(matrix::Quatf(_sub_att.get().q));
float rot_sq = euler.phi() * euler.phi() + euler.theta() * euler.theta();
R(Y_flow_vx, Y_flow_vx) = flow_vxy_stddev * flow_vxy_stddev +
_flow_r.get() * _flow_r.get() * rot_sq +
_flow_rr.get() * _flow_rr.get() * rotrate_sq;
R(Y_flow_vy, Y_flow_vy) = R(Y_flow_vx, Y_flow_vx);
// residual
Vector<float, 2> r = y - C * _x;
// residual covariance
Matrix<float, n_y_flow, n_y_flow> S = C * _P * C.transpose() + R;
// publish innovations
_pub_innov.get().flow_innov[0] = r(0);
_pub_innov.get().flow_innov[1] = r(1);
_pub_innov.get().flow_innov_var[0] = S(0, 0);
_pub_innov.get().flow_innov_var[1] = S(1, 1);
// residual covariance, (inverse)
Matrix<float, n_y_flow, n_y_flow> S_I = inv<float, n_y_flow>(S);
// fault detection
float beta = (r.transpose() * (S_I * r))(0, 0);
if (beta > BETA_TABLE[n_y_flow]) {
if (!(_sensorFault & SENSOR_FLOW)) {
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] flow fault, beta %5.2f", double(beta));
_sensorFault |= SENSOR_FLOW;
}
} else if (_sensorFault & SENSOR_FLOW) {
_sensorFault &= ~SENSOR_FLOW;
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] flow OK");
}
if (!(_sensorFault & SENSOR_FLOW)) {
Matrix<float, n_x, n_y_flow> K =
_P * C.transpose() * S_I;
Vector<float, n_x> dx = K * r;
_x += dx;
_P -= K * C * _P;
}
}
/*NP*/
void plot_windspeedprofile(void)
/*************************************************************/
/* PLOT_WINDSPEEDPROFILE */
/* John Hart SPC Norman */
/* */
/* Plots vertical profile of Wind Speeds */
/*************************************************************/
{
float lasthght;
float bothgt, tophgt, h, wsp;
short x1, y1, x2, y2, i, tlx, tly, wid, hgt;
short pIndex, zIndex, wsIndex, wdIndex;
char st[10];
pIndex = getParmIndex("PRES");
zIndex = getParmIndex("HGHT");
wsIndex = getParmIndex("SPED");
if (!sndg || pIndex == -1 || zIndex == -1 || wsIndex == -1) return;
tlx = skv.brx;
tly = skv.tly;
wid = 93;
hgt = skv.bry - skv.tly;
setcliprgn(tlx, tly, tlx+wid, tly+hgt+15);
setcolor(0);
setlinestyle( 1, 1 );
rectangle(1,tlx, tly, tlx+wid, tly+hgt);
setcolor(1);
rectangle(0, tlx, tly, tlx+wid, tly+hgt);
/* Plot scale */
setlinestyle(2, 1);
set_font(5);
for (i=20;i<=120;i+=20) {
setcolor(8);
x1 = tlx + (i/1.5);
x2 = x1;
y1 = tly + 1;
y2 = tly + hgt;
moveto(x1, y1);
lineto(x2, y2);
setcolor(1);
sprintf( st, "%d", i);
outgtext(st, x1-3, y2+2);
}
/* ----- Plot Label ----- */
setcolor(1);
set_font(5);
outgtext("Wind Speed (kt)", tlx+5, tly+3);
outgtext("vs Height", tlx+5, tly+15);
/* color code to match hodograph layers */
lasthght = 0;
setlinestyle(1, 1);
for (i=0; i<numlvl; i++)
{
if ((lasthght >= 0) && (lasthght < 3000))
{
if (qc(sndg[i][wsIndex])) {
wsp = sndg[i][wsIndex];
x1 = tlx;
x2 = tlx + (short)(wsp / 1.5);
y2 = pres_to_pix(sndg[i][pIndex]);
y1 = y2;
setcolor(2);
moveto(x1, y1);
lineto(x2, y2);
lasthght = agl(sndg[i][zIndex]);
}
}
if ((lasthght >= 3000) && (lasthght < 6000))
{
if (qc(sndg[i][wsIndex])) {
wsp = sndg[i][wsIndex];
x1 = tlx;
x2 = tlx + (short)(wsp / 1.5);
y2 = pres_to_pix(sndg[i][pIndex]);
y1 = y2;
setcolor(3);
moveto(x1, y1);
lineto(x2, y2);
lasthght = agl(sndg[i][zIndex]);
}
}
if ((lasthght >= 6000) && (lasthght < 9000))
{
if (qc(sndg[i][wsIndex])) {
wsp = sndg[i][wsIndex];
x1 = tlx;
x2 = tlx + (short)(wsp / 1.5);
y2 = pres_to_pix(sndg[i][pIndex]);
y1 = y2;
setcolor(5);
moveto(x1, y1);
lineto(x2, y2);
lasthght = agl(sndg[i][zIndex]);
}
}
if ((lasthght >= 9000) && (lasthght < 12000))
{
if (qc(sndg[i][wsIndex])) {
wsp = sndg[i][wsIndex];
x1 = tlx;
x2 = tlx + (short)(wsp / 1.5);
y2 = pres_to_pix(sndg[i][pIndex]);
y1 = y2;
setcolor(6);
moveto(x1, y1);
lineto(x2, y2);
lasthght = agl(sndg[i][zIndex]);
}
}
if (lasthght >= 12000)
{
if (qc(sndg[i][wsIndex])) {
wsp = sndg[i][wsIndex];
x1 = tlx;
x2 = tlx + (short)(wsp / 1.5);
y2 = pres_to_pix(sndg[i][pIndex]);
y1 = y2;
setcolor(29);
moveto(x1, y1);
lineto(x2, y2);
lasthght = agl(sndg[i][zIndex]);
}
}
}
setcliprgn(1, 1, xwdth, xhght);
copytodisplay();
}
/*NP*/
void plot_thetae(void)
/*************************************************************/
/* PLOT_THETAE */
/* John Hart NSSFC KCMO */
/* */
/* Plots vertical profile of Theta-E (sfc-500mb) */
/*************************************************************/
{
float bothgt, tophgt, h, cthe, ix1;
short x1, y1, x2, y2, i, tlx, tly;
short pIndex, zIndex, tIndex, tdIndex;
char st[10];
pIndex = getParmIndex("PRES");
zIndex = getParmIndex("HGHT");
tIndex = getParmIndex("TEMP");
tdIndex = getParmIndex("DWPT");
if (!sndg || pIndex == -1 || tIndex == -1 || tdIndex == -1 ||
zIndex == -1)
return;
/* tlx = hov.brx - 150;
tly = hov.tly; */
tlx = hov.tlx + 120;
tly = hov.bry;
setcliprgn(tlx, tly, tlx+134, tly+120);
setcolor(0);
setlinestyle( 1, 1 );
rectangle(1,tlx, tly, tlx+134, tly+120);
setcolor(1);
rectangle(0, tlx, tly, tlx+134, tly+120);
/* ----- Set Layer (AGL) ----- */
bothgt = 0;
tophgt = agl(i_hght(500, I_PRES));
/* ----- Plot Label ----- */
setcolor(1);
set_font(4);
outgtext("Theta-E vs", tlx+55, tly+3);
outgtext("Pressure", tlx+55, tly+15);
/* ----- Plot horizontal legend ----- */
if (800 < pIndex < 850){
cthe = (thetae(800, i_temp(800, I_PRES), i_dwpt(800, I_PRES)) +
thetae(650, i_temp(650, I_PRES), i_dwpt(650, I_PRES)) +
thetae(sndg[sfc()][pIndex], sndg[sfc()][tIndex],
sndg[sfc()][tdIndex])) / 3.0;
}
if (750 < pIndex < 800){
cthe = (thetae(750, i_temp(750, I_PRES), i_dwpt(750, I_PRES)) +
thetae(600, i_temp(600, I_PRES), i_dwpt(600, I_PRES)) +
thetae(sndg[sfc()][pIndex], sndg[sfc()][tIndex],
sndg[sfc()][tdIndex])) / 3.0;
}
if (700 < pIndex < 750){
cthe = (thetae(700, i_temp(700, I_PRES), i_dwpt(700, I_PRES)) +
thetae(500, i_temp(500, I_PRES), i_dwpt(500, I_PRES)) +
thetae(sndg[sfc()][pIndex], sndg[sfc()][tIndex],
sndg[sfc()][tdIndex])) / 3.0;
}
if (pIndex >= 850){
cthe = (thetae(850, i_temp(850, I_PRES), i_dwpt(850, I_PRES)) +
thetae(700, i_temp(700, I_PRES), i_dwpt(700, I_PRES)) +
thetae(sndg[sfc()][pIndex], sndg[sfc()][tIndex],
sndg[sfc()][tdIndex])) / 3.0;
}
setcolor(19);
set_font(5);
for(h=cthe - 30.0; h<=cthe + 30.0; h += 10) {
x1 = (short)(tlx + 60 + ((h-cthe)*2.5));
y1 = tly+120;
moveto( x1, y1);
lineto( x1, y1-5);
sprintf(st, "%.0f", h + 273.15);
outgtext(st, x1-6, y1-14);
}
/* ----- Plot vertical theta-e profile ----- */
setlinestyle(1, 2);
setcolor(2);
x2 = 999;
if (sndg[numlvl-1][pIndex] < 500) {
for (i=0; sndg[i][pIndex] >= 500; i++) {
/*printf ("i = %d, PRES = %.1f\n", i, sndg[i][pIndex]);*/
if (qc(sndg[i][tdIndex])) {
x1 = (short)(tlx + 60 + ((thetae(sndg[i][pIndex],
sndg[i][tIndex], sndg[i][tdIndex])-cthe)*2.5));
y1 = vert_coords(agl(sndg[i][zIndex]), tophgt, tly);
if(x2 == 999) { x2=x1; y2=y1; }
moveto(x1, y1);
lineto(x2, y2);
x2=x1;
y2=y1;
}
}
}
/* ----- Plot Vertical Legend ----- */
setlinestyle(1, 1);
setcolor(1);
set_font(5);
x2 = 999;
for(i=1000; i >= 600; i -= 100) {
x1 = tlx;
y1 = vert_coords(agl(i_hght(i, I_PRES)), tophgt, tly);
moveto( x1, y1);
lineto( x1+5, y1);
sprintf(st, "%d", i);
if (i<1000) outgtext(st, x1+6, y1-5);
}
setcliprgn(1, 1, xwdth, xhght);
copytodisplay();
/* plot theta-e index */
setcolor(19);
set_font(4);
sprintf( st, "TEI = %s", qc2( ThetaE_diff(&ix1), "", 0));
outgtext( st, tlx + 80, tly + 50);
}
/*NP*/
float visual1(float lower, float upper, float pres, float temp, float dwpt, int ulx, int uly, int vwid)
/*************************************************************/
/* VISUAL1 */
/* John Hart NSSFC KCMO */
/* */
/* Lifts specified parcel, given an initial 5 m/s push. */
/* parcel trajectory is then calculated, using strict */
/* parcel theory. Updraft size is assumed 1km dia. */
/* */
/* All calculations use the virtual temperature correction. */
/* */
/* lower = Bottom level of layer (mb) [ -1=SFC] */
/* upper = Top level of layer (mb) [ -1=TOP] */
/* pres = LPL pressure (mb) */
/* temp = LPL temperature (c) */
/* dwpt = LPL dew point (c) */
/*************************************************************/
{
short i, lptr, uptr, pIndex, zIndex, tIndex, tdIndex;
float te1, pe1, te2, pe2, h1, h2, lyre, tdef1, tdef2, totp, totn;
float te3, pe3, h3, tp1, tp2, tp3, tdef3, lyrf, lyrlast, pelast;
float tote, dh, restim, uvv, ix1, ix2, tottim;
float u, v, du, dv, tsu, tsv, tdist, tangle, disp, angl;
short colrx;
lyre = -1;
totp = 25;
totn = 0;
pIndex = getParmIndex("PRES");
zIndex = getParmIndex("HGHT");
tIndex = getParmIndex("TEMP");
tdIndex = getParmIndex("DWPT");
if (!sndg || pIndex == -1 || zIndex == -1 || tIndex == -1 || tdIndex == -1) return RMISSD;
/* ----- See if default layer is specified ----- */
if (lower == -1) { lower = sndg[sfc()][pIndex]; }
if (upper == -1) { upper = sndg[numlvl-1][pIndex]; }
/* ----- Make sure this is a valid layer ----- */
if( lower > pres ) { lower = pres; }
if( !qc( i_vtmp( upper , I_PRES))) { return RMISSD; }
if( !qc( i_vtmp( lower , I_PRES))) { return RMISSD; }
/* ----- Begin with Mixing Layer (LPL-LCL) ----- */
te1 = i_vtmp(pres , I_PRES);
pe1 = lower;
h1 = i_hght(pe1 , I_PRES);
tp1 = virtemp(pres, temp, dwpt);
drylift(pres, temp, dwpt, &pe2, &tp2);
h2 = i_hght(pe2 , I_PRES);
te2 = i_vtmp(pe2 , I_PRES);
if( lower > pe2 ) { lower = pe2; }
/* ----- Find lowest observation in layer ----- */
i = 0;
while( sndg[i][pIndex] > lower) { i++; }
while ( !qc(sndg[i][tdIndex]) ) { i++; }
lptr = i;
if( sndg[i][pIndex] == lower ) { lptr++; }
/* ----- Find highest observation in layer ----- */
i=numlvl-1;
while(sndg[i][pIndex] < upper) { i--; }
uptr = i;
if( sndg[i][pIndex] == upper ) { uptr--; }
/* ----- Start with interpolated bottom layer ----- */
pe1 = lower;
h1 = i_hght( pe1 , I_PRES);
te1 = i_vtmp( pe1 , I_PRES);
tp1 = wetlift(pe2, tp2, pe1);
totp = 25;
totn = 0;
tsu = 0;
tsv = 0;
restim = 0;
tottim = 0;
for (i = lptr; i < numlvl; i++) {
if (qc(sndg[i][tIndex])) {
/* ----- Calculate every level that reports a temp ----- */
pe2 = sndg[i][pIndex];
h2 = sndg[i][zIndex];
te2 = i_vtmp( pe2 , I_PRES);
tp2 = wetlift(pe1, tp1, pe2);
tdef1 = (virtemp(pe1, tp1, tp1) - te1) / (te1 + 273.15);
tdef2 = (virtemp(pe2, tp2, tp2) - te2) / (te2 + 273.15);
lyrlast = lyre;
lyre = 9.8F * (tdef1 + tdef2) / 2.0F * (h2 - h1);
if( lyre > 0 ) { totp += lyre; }
else { if(pe2 > 500) { totn += lyre; } }
tote += lyre;
uvv = (float)sqrt( totp * 2 );
dh = h2 - h1;
restim = dh / uvv;
tottim += restim;
sr_wind( pe1, pe2, st_dir, st_spd, &u, &v, &ix1, &ix2);
du = kt_to_mps(u) * restim;
dv = kt_to_mps(v) * restim;
tsu -= du;
tsv += dv;
tdist = (float)sqrt((tsu*tsu) + (tsv*tsv));
tangle = angle(tsu, tsv);
pelast = pe1;
pe1 = pe2;
h1 = h2;
te1 = te2;
tp1 = tp2;
/* ----- Is this the top of given layer ----- */
if(i >= uptr) {
pe3 = pe1;
h3 = h1;
te3 = te1;
tp3 = tp1;
lyrf = lyre;
if( lyrf > 0 )
{ totp -= lyrf; }
else
{ if(pe2 > 500) { totn -= lyrf; } }
pe2 = upper;
h2 = i_hght( pe2 , I_PRES);
te2 = i_vtmp( pe2 , I_PRES);
tp2 = wetlift(pe3, tp3, pe2);
tdef3 = (virtemp(pe3, tp3, tp3) - te3) / (te3 + 273.15);
tdef2 = (virtemp(pe2, tp2, tp2) - te2) / (te2 + 273.15);
lyrf = 9.8F * (tdef3 + tdef2) / 2.0F * (h2 - h3);
if( lyrf > 0 )
{ totp += lyrf; }
else
{ if(pe2 > 500) { totn -= lyrf; } }
if( totp == 0 ) { totn = 0; }
uvv = (float)sqrt( totp * 2 );
dh = h2 - h1;
restim = dh / uvv;
tottim += restim;
sr_wind( pe1, pe2, st_dir, st_spd, &u, &v, &ix1, &ix2);
du = kt_to_mps(u) * restim;
dv = kt_to_mps(v) * restim;
tsu -= du;
tsv += dv;
tdist = (float)sqrt((tsu*tsu) + (tsv*tsv));
tangle = angle(tsu, tsv);
colrx = 7;
vis_xy( -tsu, -tsv, colrx, ulx, uly, vwid);
angl = 90 - angle( tdist, agl(h2));
write_vis_data( tottim, angl, ulx, uly, vwid );
return 1;
}
colrx = 13;
if (h2>msl(3000)) colrx=3;
if (h2>msl(6000)) colrx=27;
if (h2>msl(9000)) colrx=20;
if (h2>msl(12000)) colrx=6;
vis_xy(-tsu, -tsv, colrx, ulx, uly, vwid);
if (sndg[i][pIndex] == 500) {
disp = tdist;
angl = 90 - angle( tdist, agl(sndg[i][zIndex]));
}
}
}
return 1.0; /* ? mkay. there was no value before. bad thing */
}
/*NP*/
float bndry_ci(float lift, float distance)
/*************************************************************/
/* BOUNDARY CONVECTIVE INITIATION */
/* Rich Thompson and John Hart SPC OUN */
/* */
/* Lifts ML parcel to find LFC, then calculates parcel */
/* trajectory relative to a boundary. If parcel stays */
/* within dist_to_bndry long enough to reach the LFC, */
/* then convective initiation is likely */
/* */
/* lift = ascent rate in m/min along boundary */
/* dist_to_bndry = width of ascent zone (m) */
/*************************************************************/
{
float ix1, ix2, u, v, pe1, pe2, lfc, height, pres;
short oldlplchoice, i, min_lift;
Parcel pcl;
oldlplchoice = lplvals.flag;
/* ----- Begin with ML parcel LFC height (m AGL) */
define_parcel(4,100);
ix1 = parcel( -1, -1, lplvals.pres, lplvals.temp, lplvals.dwpt, &pcl);
lfc = agl(i_hght(pcl.lfcpres, I_PRES));
/* ----- Calculate minutes of lift needed to reach LFC */
min_lift = (short)(lfc / lift);
printf("\nminutes of lift to LFC = %d\n", min_lift);
/* if parcel remains within distance (to bndry), continue ascent */
for (i = 0; i < min_lift; i++){
height = i * lift;
/* pe1 = i_pres(msl(height));
pe2 = i_pres(msl(height));
restim = dh / uvv;
tottim += restim;
sr_wind( pe1, pe2, st_dir, st_spd, &u, &v, &ix1, &ix2);
du = kt_to_mps(u) * restim;
dv = kt_to_mps(v) * restim;
tsu -= du;
tsv += dv;
distance = (float)sqrt((tsu*tsu) + (tsv*tsv));
tangle = angle(tsu, tsv);
*/
}
if (oldlplchoice == 3)
pres = mu_layer;
else if (oldlplchoice == 4)
pres = mml_layer;
else if (oldlplchoice == 5)
pres = user_level;
else
pres = mml_layer;
define_parcel(oldlplchoice, pres);
return min_lift;
}
/* NP */
short ww_type(short *wwtype, short *dcp)
/********************************************************************/
/* Watch type guidance */
/* A decision tree to help with ww issuance */
/* */
/* Rich Thompson SPC OUN */
/********************************************************************/
{
float ix1, ix2, ix3, ix4, lr75, shr6, t500, fzlh, mumixr, lowrh, midrh, low_mid_rh;
float mucn, mlcn, mlcp, sbcp, mucp, lr1, lr3, shr6_dir, sr46_dir, sr46_spd, shr6_sr46_diff, mmp;
float sig_tor, sig_tor_winter, sighail, wind_dmg, rm_scp, cbsig, dncp, srh1, sblcl, mllcl;
float oldlplpres, pres, pbot, ptop, shr8, bot, top, esrh, lm_scp;
short oldlplchoice, ww_choice;
short pIndex, tIndex, zIndex, tdIndex;
short x1, y1, x2, y2, tlx, tly, wid;
struct _parcel pcl;
char st[40];
tlx = hov.tlx + 409;
tly = hov.bry;
wid = 119;
setcliprgn( tlx+2, tly+2, tlx+wid+24, tly+wid+1);
setcolor(0);
setlinestyle( 1, 1 );
rectangle( 1,tlx, tly, tlx+wid+27, tly+wid+1);
setcolor(1);
rectangle( 0, tlx, tly, tlx+wid+27, tly+wid+1);
setlinestyle( 1, 1 );
moveto( tlx + 2, tly + 18);
lineto(tlx + wid + 27, tly + 18);
/* ----- Plot Label ----- */
set_font(6);
setcolor(1);
outgtext( "Psbl Watch Type", tlx + 20, tly + 3 );
*wwtype = 0;
*dcp = 0;
oldlplchoice = lplvals.flag;
tIndex = getParmIndex("TEMP");
pIndex = getParmIndex("PRES");
zIndex = getParmIndex("HGHT");
tdIndex = getParmIndex("DWPT");
/* 24 Mar 2008 */
/* effective_inflow_layer(100, -250, &pbot, &ptop);*/
/* sb parcel */
define_parcel(1,0);
ix1 = parcel(-1, -1, lplvals.pres, lplvals.temp, lplvals.dwpt, &pcl);
sbcp = pcl.bplus;
sblcl = agl(i_hght(pcl.lclpres, I_PRES));
sig_tor_winter = sigtorn_fixed(st_dir, st_spd);
/* ml parcel */
define_parcel(4,100);
ix1 = parcel(-1, -1, lplvals.pres, lplvals.temp, lplvals.dwpt, &pcl);
mlcn = pcl.bminus;
mlcp = pcl.bplus;
mllcl = agl(i_hght(pcl.lclpres, I_PRES));
sig_tor = sigtorn_cin(st_dir, st_spd);
/* mu parcel */
define_parcel(3,400);
mucp = parcel(-1, -1, lplvals.pres, lplvals.temp, lplvals.dwpt, &pcl);
mucn = pcl.bminus;
dncp = dcape(&ix1, &ix2);
/* sighail ingredients */
lr75 = lapse_rate(&ix1, 700, 500);
wind_shear(sndg[sfc()][pIndex], i_pres(msl(6000)), &ix1, &ix2, &ix3, &ix4);
shr6 = ix4;
shr6_dir = ix3;
wind_shear(sndg[sfc()][pIndex], i_pres(msl(8000)), &ix1, &ix2, &ix3, &ix4);
shr8 = ix4;
mumixr = mixratio(lplvals.pres, lplvals.dwpt);
t500 = i_temp(500, I_PRES);
fzlh = mtof(agl(i_hght(temp_lvl(0, &ix1), I_PRES)));
sighail = sig_hail(pcl.bplus, mumixr, lr75, t500, kt_to_mps(shr6), fzlh, pcl.bminus, 0, 0, 25, mlcp);
rm_scp = scp(st_dir, st_spd);
wind_dmg = damaging_wind();
sr_wind( i_pres(msl(4000)), i_pres(msl(6000)), st_dir, st_spd, &ix1, &ix2, &ix3, &ix4);
sr46_dir = ix3;
sr46_spd = ix4;
shr6_sr46_diff = (shr6_dir - sr46_dir);
srh1 = helicity(0, 1000, st_dir, st_spd, &ix1, &ix2);
bot = agl(i_hght(p_bot, I_PRES));
top = agl(i_hght(p_top, I_PRES));
esrh = helicity(bot, top, st_dir, st_spd, &ix1, &ix2);
lapse_rate(&ix2, sndg[sfc()][pIndex], i_pres(sndg[sfc()][zIndex]+1000));
lr1 = ix2;
lapse_rate(&ix2, sndg[sfc()][pIndex], i_pres(sndg[sfc()][zIndex]+3000));
lr3 = ix2;
mean_relhum(&ix1, sndg[sfc()][pIndex]-150, sndg[sfc()][pIndex]-350);
midrh = ix1;
mean_relhum(&ix1, sndg[sfc()][pIndex], sndg[sfc()][pIndex]-150);
lowrh = ix1;
low_mid_rh = ((lowrh + midrh)/2);
mmp = coniglio1();
cbsig = (mlcp * kt_to_mps(shr6));
/* 24 Mar 2008 */
/* all "p_bot" below were changed from "pbot" */
/* Decision tree below is identical to the operational "ww_type" flow chart documentation 9/23/09 RLT */
if ((sig_tor >= 3.0) && (sig_tor_winter >= 3.0) && (srh1 >= 150) && (esrh >= 150) && (sr46_spd >= 15.0) && (shr8 >= 40.0) && (sblcl < 1000) && (mllcl < 1100) && (lr1 >= 5.0) && (bot == 0.0)) {
*dcp = 1;
*wwtype = 5;
ww_choice = 5;
/*printf("\n dcp (in PDS) = %d", *dcp);*/
set_font(6);
setcolor(7);
outgtext( "PDS TOR", tlx + 45, tly + 60 );
}
/* else
if ((sig_tor_winter >= 4.0) && (sr46_spd >= 15.0) && (shr8 >= 40.0) && (sblcl < 1000) && (lr1 >= 5.0) && (bot == 0.0)) {
*dcp = 2;
*wwtype = 5;
ww_choice = 5;
set_font(6);
setcolor(7);
outgtext( "PDS TOR", tlx + 45, tly + 60 );
}
*/
else
if (((sig_tor >= 3.0) || (sig_tor_winter >= 4.0)) && (bot == 0.0)) {
*dcp = 3;
*wwtype = 4;
ww_choice = 4;
/*printf("\n dcp (in TOR) = %d", *dcp);*/
set_font(6);
setcolor(2);
outgtext( "TOR", tlx + 45, tly + 60 );
}
else
if (((sig_tor >= 1.0) || (sig_tor_winter >= 1.0)) && ((sr46_spd >= 15.0) || (shr8 >= 40.0)) && (bot == 0.0)) {
*dcp = 4;
*wwtype = 4;
ww_choice = 4;
/*printf("\n dcp (in TOR) = %d", *dcp);*/
set_font(6);
setcolor(2);
outgtext( "TOR", tlx + 45, tly + 60 );
}
else
if (((sig_tor >= 1.0) || (sig_tor_winter >= 1.0)) && (low_mid_rh >= 60) && (lr1 >= 5.0) && (bot == 0.0)) {
*dcp = 5;
*wwtype = 4;
ww_choice = 4;
/*printf("\n dcp (in TOR) = %d", *dcp);*/
set_font(6);
setcolor(2);
outgtext( "TOR", tlx + 45, tly + 60 );
}
else
if ((( sig_tor >= 1.0) || (sig_tor_winter >= 1.0)) && (bot == 0.0)) {
*dcp = 6;
*wwtype = 3;
ww_choice = 3;
/*printf("\n dcp (in mrgl TOR) = %d", *dcp);*/
set_font(6);
setcolor(2);
outgtext( "mrgl TOR", tlx + 40, tly + 60 );
}
else
if (((( sig_tor >= 0.5) && (esrh >= 150)) || ((sig_tor_winter >= 0.5) && (srh1 >= 150))) && (bot == 0.0)) {
*dcp = 7;
*wwtype = 3;
ww_choice = 3;
/*printf("\n dcp (in mrgl TOR) = %d", *dcp);*/
set_font(6);
setcolor(2);
outgtext( "mrgl TOR", tlx + 40, tly + 60 );
}
else
if ((( sig_tor_winter >= 1.0) || (rm_scp >= 4.0) || (sig_tor >= 1.0)) && (mucn >= -50.0)) {
*dcp = 8;
*wwtype = 2;
ww_choice = 2;
/*printf("\n dcp (in SVR) = %d", *dcp);*/
set_font(6);
setcolor(6);
outgtext( "SVR", tlx + 60, tly + 60 );
}
else
if ((rm_scp >= 2.0) && ((sighail >= 1.0) || (dncp >= 750)) && (mucn >= -50.0)) {
*dcp = 9;
*wwtype = 2;
ww_choice = 2;
/*printf("\n dcp (in SVR) = %d", *dcp);*/
set_font(6);
setcolor(6);
outgtext( "SVR", tlx + 60, tly + 60 );
}
else
if ((cbsig >= 30000) && (mmp >= 0.6) && (mucn >= -50.0)) {
*dcp = 10;
*wwtype = 2;
ww_choice = 2;
/*printf("\n dcp (in SVR) = %d", *dcp);*/
set_font(6);
setcolor(6);
outgtext( "SVR", tlx + 60, tly + 60 );
}
else
if ((mucn >= -75.0) && ((wind_dmg >= 0.5) || (sighail >= 0.5) || (rm_scp >= 0.5))) {
*dcp = 11;
*wwtype = 1;
ww_choice = 1;
/*printf("\n dcp (in mrgl SVR) = %d", *dcp);*/
set_font(6);
setcolor(26);
outgtext( "MRGL SVR", tlx + 40, tly + 60 );
}
else {
*dcp = 0;
/*printf("\n dcp (after logic checks) = %d", *dcp);*/
*wwtype = 0;
ww_choice = 0;
}
if (*wwtype == 0) {
set_font(6);
setcolor(19);
outgtext( "NONE", tlx + 50, tly + 60 );
}
//printf("sig_tor=%f sig_tor_winter=%f srh1=%f esrh=%f sr46_spd=%f shr8=%f sblcl=%f\n mllcl=%f lr1=%f bot=%f low_mid_rh=%f rm_scp=%f\n mucn=%f dncp=%f sighail=%f cbsig=%f wind_dmg=%f",
// sig_tor,sig_tor_winter,srh1,esrh,sr46_spd,shr8, sblcl, mllcl, lr1, bot, low_mid_rh, rm_scp,mucn, dncp, sighail, cbsig, wind_dmg);
/* define_parcel(oldlplchoice, oldlplpres); */
/* set parcel back to user selection */
if (oldlplchoice == 1)
pres = 0;
else if (oldlplchoice == 2)
pres = 0;
else if (oldlplchoice == 3)
pres = mu_layer;
else if (oldlplchoice == 4)
pres = mml_layer;
else if (oldlplchoice == 5)
pres = user_level;
else if (oldlplchoice == 6)
pres = mu_layer;
define_parcel(oldlplchoice, pres);
return ww_choice;
}
int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
{
matrix::Eulerf euler(matrix::Quatf(_sub_att.get().q));
// check for sane pitch/roll
if (euler.phi() > 0.5f || euler.theta() > 0.5f) {
return -1;
}
// check for agl
if (agl() < flow_min_agl) {
return -1;
}
// check quality
float qual = _sub_flow.get().quality;
if (qual < _flow_min_q.get()) {
return -1;
}
// calculate range to center of image for flow
if (!(_estimatorInitialized & EST_TZ)) {
return -1;
}
float d = agl() * cosf(euler.phi()) * cosf(euler.theta());
// optical flow in x, y axis
// TODO consider making flow scale a states of the kalman filter
float flow_x_rad = _sub_flow.get().pixel_flow_x_integral * _flow_scale.get();
float flow_y_rad = _sub_flow.get().pixel_flow_y_integral * _flow_scale.get();
float dt_flow = _sub_flow.get().integration_timespan / 1.0e6f;
if (dt_flow > 0.5f || dt_flow < 1.0e-6f) {
return -1;
}
// angular rotation in x, y axis
float gyro_x_rad = 0;
float gyro_y_rad = 0;
if (_fusion.get() & FUSE_FLOW_GYRO_COMP) {
gyro_x_rad = _flow_gyro_x_high_pass.update(
_sub_flow.get().gyro_x_rate_integral);
gyro_y_rad = _flow_gyro_y_high_pass.update(
_sub_flow.get().gyro_y_rate_integral);
}
//warnx("flow x: %10.4f y: %10.4f gyro_x: %10.4f gyro_y: %10.4f d: %10.4f",
//double(flow_x_rad), double(flow_y_rad), double(gyro_x_rad), double(gyro_y_rad), double(d));
// compute velocities in body frame using ground distance
// note that the integral rates in the optical_flow uORB topic are RH rotations about body axes
Vector3f delta_b(
+(flow_y_rad - gyro_y_rad) * d,
-(flow_x_rad - gyro_x_rad) * d,
0);
// rotation of flow from body to nav frame
Vector3f delta_n = _R_att * delta_b;
// imporant to timestamp flow even if distance is bad
_time_last_flow = _timeStamp;
// measurement
y(Y_flow_vx) = delta_n(0) / dt_flow;
y(Y_flow_vy) = delta_n(1) / dt_flow;
_flowQStats.update(Scalarf(_sub_flow.get().quality));
return OK;
}
void BlockLocalPositionEstimator::predict()
{
// get acceleration
matrix::Quatf q(&_sub_att.get().q[0]);
_eul = matrix::Euler<float>(q);
_R_att = matrix::Dcm<float>(q);
Vector3f a(_sub_sensor.get().accelerometer_m_s2);
// note, bias is removed in dynamics function
_u = _R_att * a;
_u(U_az) += 9.81f; // add g
// update state space based on new states
updateSSStates();
// continuous time kalman filter prediction
// integrate runge kutta 4th order
// TODO move rk4 algorithm to matrixlib
// https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta_methods
float h = getDt();
Vector<float, n_x> k1, k2, k3, k4;
k1 = dynamics(0, _x, _u);
k2 = dynamics(h / 2, _x + k1 * h / 2, _u);
k3 = dynamics(h / 2, _x + k2 * h / 2, _u);
k4 = dynamics(h, _x + k3 * h, _u);
Vector<float, n_x> dx = (k1 + k2 * 2 + k3 * 2 + k4) * (h / 6);
// don't integrate position if no valid xy data
if (!(_estimatorInitialized & EST_XY)) {
dx(X_x) = 0;
dx(X_vx) = 0;
dx(X_y) = 0;
dx(X_vy) = 0;
}
// don't integrate z if no valid z data
if (!(_estimatorInitialized & EST_Z)) {
dx(X_z) = 0;
}
// don't integrate tz if no valid tz data
if (!(_estimatorInitialized & EST_TZ)) {
dx(X_tz) = 0;
}
// saturate bias
float bx = dx(X_bx) + _x(X_bx);
float by = dx(X_by) + _x(X_by);
float bz = dx(X_bz) + _x(X_bz);
if (std::abs(bx) > BIAS_MAX) {
bx = BIAS_MAX * bx / std::abs(bx);
dx(X_bx) = bx - _x(X_bx);
}
if (std::abs(by) > BIAS_MAX) {
by = BIAS_MAX * by / std::abs(by);
dx(X_by) = by - _x(X_by);
}
if (std::abs(bz) > BIAS_MAX) {
bz = BIAS_MAX * bz / std::abs(bz);
dx(X_bz) = bz - _x(X_bz);
}
// propagate
_x += dx;
Matrix<float, n_x, n_x> dP = (_A * _P + _P * _A.transpose() +
_B * _R * _B.transpose() + _Q) * getDt();
// covariance propagation logic
for (int i = 0; i < n_x; i++) {
if (_P(i, i) > P_MAX) {
// if diagonal element greater than max, stop propagating
dP(i, i) = 0;
for (int j = 0; j < n_x; j++) {
dP(i, j) = 0;
dP(j, i) = 0;
}
}
}
_P += dP;
_xLowPass.update(_x);
_aglLowPass.update(agl());
}
/*NP*/
void write_file2( char *filename )
/*************************************************************/
/* WRITE_FILE */
/* John Hart NSSFC KCMO */
/* */
/* Writes contents of sndg array into SHARP95 file. */
/*************************************************************/
{
short i, j;
short idx[7];
float sfctemp, sfcdwpt, sfcpres, j1, j2, ix1;
struct _parcel pcl;
char st[80];
FILE *fout;
idx[1] = getParmIndex("PRES");
idx[2] = getParmIndex("HGHT");
idx[3] = getParmIndex("TEMP");
idx[4] = getParmIndex("DWPT");
idx[5] = getParmIndex("DRCT");
idx[6] = getParmIndex("SPED");
fout = fopen( filename, "wt" );
if (fout==NULL)
{
printf("Unable to write output file!\n" );
return;
}
fputs( "%TITLE%\n", fout );
fputs( raobtitle, fout );
fputs( "\n\n", fout );
fprintf( fout, " LEVEL HGHT TEMP DWPT WDIR WSPD\n");
fprintf( fout, "-------------------------------------------------------------------\n");
fputs( "%RAW%\n", fout );
for(i=0; i<numlvl; i++)
{
for(j=1; j<=5; j++) fprintf( fout, "%8.2f, ", sndg[i][idx[j]]);
fprintf( fout, "%8.2f\n", sndg[i][idx[6]]);
}
fputs( "%END%\n\n", fout );
if ((numlvl<4) || (!qc(i_dwpt(700, I_PRES)))) return;
fprintf( fout, "----- Parcel Information-----\n");
/* ----- Calculate Parcel Data ----- */
sfctemp = lplvals.temp;
sfcdwpt = lplvals.dwpt;
sfcpres = lplvals.pres;
strcpy( st, "*** " );
strcat( st, lplvals.desc );
strcat( st, " ***" );
fprintf( fout, "%s\n", st);
ix1 = parcel( -1, -1, sfcpres, sfctemp, sfcdwpt, &pcl);
fprintf( fout, "LPL: P=%.0f T=%.0fF Td=%.0fF\n", sfcpres, ctof(sfctemp), ctof(sfcdwpt));
fprintf( fout, "CAPE: %6.0f J/kg\n", pcl.bplus);
fprintf( fout, "CINH: %6.0f J/kg\n", pcl.bminus);
fprintf( fout, "LI: %6.0f C\n", pcl.li5);
fprintf( fout, "LI(300mb): %6.0f C\n", pcl.li3);
fprintf( fout, "3km Cape: %6.0f J/kg\n", pcl.cape3km);
j1 = pcl.bplus;
j2 = i_hght(pcl.elpres, I_PRES) - i_hght(pcl.lfcpres, I_PRES);
fprintf( fout, "NCAPE: %6.2f m/s2\n\n", j1/j2);
fprintf( fout, "LCL: %6.0fmb %6.0fm\n", pcl.lclpres, agl(i_hght(pcl.lclpres, I_PRES)));
fprintf( fout, "LFC: %6.0fmb %6.0fm\n", pcl.lfcpres, agl(i_hght(pcl.lfcpres, I_PRES)));
fprintf( fout, "EL: %6.0fmb %6.0fm\n", pcl.elpres, agl(i_hght(pcl.elpres, I_PRES)));
fprintf( fout, "MPL: %6.0fmb %6.0fm\n", pcl.mplpres, agl(i_hght(pcl.mplpres, I_PRES)));
fprintf( fout, "All heights AGL\n\n" );
fprintf( fout, "----- Moisture -----\n" );
strcpy( st, qc2( precip_water( &ix1, -1, -1), " in", 2 ));
fprintf( fout, "Precip Water: %s\n", st);
strcpy( st, qc2( mean_mixratio( &ix1, -1, -1 ), " g/Kg", 1 ));
fprintf( fout, "Mean W: %s\n\n", st);
fprintf( fout, "----- Lapse Rates -----\n" );
j1 = delta_t(&ix1);
j2 = lapse_rate( &ix1, 700, 500);
fprintf( fout, "700-500mb %.0f C %.1f C/km\n", j1, j2);
j1 = vert_tot(&ix1);
j2 = lapse_rate( &ix1, 850, 500);
fprintf( fout, "850-500mb %.0f C %.1f C/km\n", j1, j2);
fclose( fout );
}
/*This function uses the symmetric property and computes the agl in one direction*/
void mexFunction(int nlhs, mxArray *plhs[], /* Output variables */
int nrhs, const mxArray *prhs[]) /* Input variables */
{
#define agl(ai,bi) agl[ai+dims[0]*bi]
#define TTEng_1st(ai,bi) TTEng_1st[ai+dims[0]*bi]
#define TTEng_2nd(ai,bi) TTEng_2nd[ai+dims[0]*bi]
#define ss_energy(ci,di,ai,bi) ss_energy[ci+Nss[0]*(di+Nss[1]*(ai+Nss[2]*bi))]
size_t ai, bi, ci, di, k, j, cnt;
int num_wave;
double *ss_energy, *agl, *TTEng_1st, *TTEng_2nd;
ss_energy = mxGetPr(prhs[0]);
const mwSize *Nss = mxGetDimensions(prhs[0]);
num_wave = mxGetScalar(prhs[1]);
nrhs = 2;
nlhs = 3;
int ndim = 2, dims[2] = {Nss[2],Nss[3]}, numm = (int)(Nss[1]/num_wave/4);
plhs[0] = mxCreateNumericArray(ndim,dims,mxDOUBLE_CLASS,mxREAL);
plhs[1] = mxCreateNumericArray(ndim,dims,mxDOUBLE_CLASS,mxREAL);
plhs[2] = mxCreateNumericArray(ndim,dims,mxDOUBLE_CLASS,mxREAL);
agl = mxGetPr(plhs[0]);
TTEng_1st = mxGetPr(plhs[1]);
TTEng_2nd = mxGetPr(plhs[2]);
int L = Nss[1]/num_wave, maxpos, st, ed;
double *temp, energy_sum, *tempss;
temp = (double *)mxMalloc(sizeof(double)*L);
tempss = (double *)mxMalloc(sizeof(double)*(2*numm+1));
for (ai=0;ai<Nss[2];ai++) {
for (bi=0;bi<Nss[3];bi++) {
for (k=0;k<L;k++) {
for (j=0;j<num_wave;j++) {
di = j*L + k;
for (ci=0;ci<Nss[0];ci++) {
if (ci==0 & j==0)
temp[k] = ss_energy(ci,di,ai,bi);
else
temp[k] = temp[k] + ss_energy(ci,di,ai,bi);
}
}
}
/*temp[:] computed*/
maxpos = findmax(temp,L);
for (cnt=0;cnt<2*numm+1;cnt++) {
tempss[cnt] = temp[(int)fmod(cnt+maxpos-numm,L)];
}
ed = numm;
cnt = 1;
while (ed<2*numm & cnt==1 & tempss[ed+1]>0) {
if (tempss[ed]>=tempss[ed+1])
ed++;
else
cnt = 0;
}
st = numm;
cnt = 1;
while (st>0 & cnt == 1 & tempss[st-1]>0) {
if (tempss[st] >= tempss[st-1])
st--;
else
cnt = 0;
}
/*compute agl(ai,bi)*/
energy_sum = 0;
for (cnt = st; cnt <= ed; cnt++)
energy_sum = energy_sum + tempss[cnt];
for (cnt = st; cnt <= ed; cnt++) {
agl(ai,bi) = agl(ai,bi) + tempss[cnt]*(cnt+0.5)/energy_sum;
temp[(int)fmod(cnt+maxpos-numm,L)] = 0;
}
agl(ai,bi) = fmod(agl(ai,bi)+maxpos-numm,L);/*any problem here??????*/
/*compute TTEng_1st(ai,bi)*/
TTEng_1st(ai,bi) = energy_sum;
/*compute TTEng_2nd(ai,bi)*/
maxpos = findmax(temp,L);
if (temp[maxpos]>0) {
for (cnt=0;cnt<2*numm+1;cnt++) {
tempss[cnt] = temp[(int)fmod(cnt+maxpos-numm,L)];
}
ed = numm;
cnt = 1;
while (ed<2*numm & cnt==1 & tempss[ed+1]>0) {
if (tempss[ed]>=tempss[ed+1])
ed++;
else
cnt = 0;
}
st = numm;
cnt = 1;
while (st>0 & cnt == 1 & tempss[st-1]>0) {
if (tempss[st] >= tempss[st-1])
st--;
else
cnt = 0;
}
/*compute agl(ai,bi)*/
energy_sum = 0;
for (cnt = st; cnt <= ed; cnt++)
energy_sum = energy_sum + tempss[cnt];
if (energy_sum<=TTEng_1st(ai,bi))
TTEng_2nd(ai,bi) = energy_sum;
else {
agl(ai,bi) = 0;
for (cnt = st; cnt <= ed; cnt++) {
agl(ai,bi) = agl(ai,bi) + tempss[cnt]*(cnt+0.5)/energy_sum;
}
agl(ai,bi) = fmod(agl(ai,bi)+maxpos-numm,L);
TTEng_2nd(ai,bi) = TTEng_1st(ai,bi);
TTEng_1st(ai,bi) = energy_sum;
}
}
else
TTEng_2nd(ai,bi) = 0;
}
}
mxFree(temp);
mxFree(tempss);
return;
}
