//////////////////////////////////////////////////////////////////////////////////// // Crashpilot NEW PH Logic // #define GPS_X 1 // #define GPS_Y 0 // #define LON 1 // #define LAT 0 // VelEast; // 1 // VelNorth; // 0 // 0 is NICK part // 1 is ROLL part // actual_speed[GPS_Y] = VelNorth; void GPS_calc_posholdCrashpilot(bool overspeed) { uint8_t axis; float p, i, d, rate_error, AbsPosErrorToVel, tmp0, tmp1; float maxbank100new; for (axis = 0; axis < 2; axis++) { maxbank100new = maxbank100; if (overspeed) { tmp1 = abs(ACC_speed[axis]); if (tmp1 == 0) tmp1 = 1.0f; tmp0 = (float)cfg.gps_ph_settlespeed / tmp1; tmp1 = (float)cfg.gps_ph_minbrakepercent * 0.01f; // this is the minimal break percentage tmp0 = constrain(sqrt(tmp0), tmp1, 1.0f); // 1 - 100% maxbank100new = (float)maxbankbrake100 * tmp0; } tmp1 = LocError[axis]; if (abs(tmp1) < cfg.gps_ph_abstub && cfg.gps_ph_abstub != 0) // Keep linear Stuff beyond x cm { if (tmp1 < 0) tmp0 = -GpsPhAbsTub; // Do flatter response below x cm else tmp0 = GpsPhAbsTub; tmp1 = tmp1 * tmp1 * tmp0; // Get a peace around current position } AbsPosErrorToVel = get_P(tmp1, &posholdPID_PARAM); // Do absolute position error here, that means transform positionerror to a velocityerror rate_error = AbsPosErrorToVel - ACC_speed[axis]; // Calculate Rate Error for actual axis rate_error = constrain(rate_error, -1000, 1000); // +- 10m/s p = get_P(rate_error, &poshold_ratePID_PARAM); i = get_I(AbsPosErrorToVel, &dTnav, &poshold_ratePID[axis], &poshold_ratePID_PARAM); // Constrained to half max P d = get_D(rate_error , &dTnav, &poshold_ratePID[axis], &poshold_ratePID_PARAM); // Constrained to half max P nav[axis] = constrain(p + i + d, -maxbank100new, maxbank100new); } }
//////////////////////////////////////////////////////////////////////////////////// // Calculate nav_lat and nav_lon from the x and y error and the speed // static void GPS_calc_poshold(void) { int32_t d; int32_t target_speed; int axis; for (axis = 0; axis < 2; axis++) { target_speed = get_P(error[axis], &posholdPID_PARAM); // calculate desired speed from lon error rate_error[axis] = target_speed - actual_speed[axis]; // calc the speed error nav[axis] = get_P(rate_error[axis], &poshold_ratePID_PARAM) + get_I(rate_error[axis] + error[axis], &dTnav, &poshold_ratePID[axis], &poshold_ratePID_PARAM); d = get_D(error[axis], &dTnav, &poshold_ratePID[axis], &poshold_ratePID_PARAM); d = constrain(d, -2000, 2000); // get rid of noise #if defined(GPS_LOW_SPEED_D_FILTER) if (abs(actual_speed[axis]) < 50) d = 0; #endif nav[axis] += d; nav[axis] = constrain(nav[axis], -NAV_BANK_MAX, NAV_BANK_MAX); navPID[axis].integrator = poshold_ratePID[axis].integrator; } }
//////////////////////////////////////////////////////////////////////////////////// // Calculate nav_lat and nav_lon from the x and y error and the speed // static void GPS_calc_poshold(void) { int32_t d; int32_t target_speed; uint8_t axis; for (axis=0;axis<2;axis++) { target_speed = get_P(error[axis], &posholdPID_PARAM); // calculate desired speed from lat/lon error target_speed = constrain(target_speed,-100,100); // Constrain the target speed in poshold mode to 1m/s it helps avoid runaways.. rate_error[axis] = target_speed - actual_speed[axis]; // calc the speed error nav[axis] = get_P(rate_error[axis], &poshold_ratePID_PARAM) +get_I(rate_error[axis] + error[axis], &dTnav, &poshold_ratePID[axis], &poshold_ratePID_PARAM); d = get_D(error[axis], &dTnav, &poshold_ratePID[axis], &poshold_ratePID_PARAM); d = constrain(d, -2000, 2000); // get rid of noise if(fabs(actual_speed[axis]) < 50) d = 0; nav[axis] +=d; nav[axis] = constrain(nav[axis], -NAV_BANK_MAX, NAV_BANK_MAX); navPID[axis].integrator = poshold_ratePID[axis].integrator; } }
void GPS_calc_posholdCrashpilot(bool overspeed) { uint8_t axis; float rate_error, tilt, tmp0, tmp1; float maxbank100new; for (axis = 0; axis < 2; axis++) { maxbank100new = (float)maxbank100; if (overspeed) { tmp1 = fabs(ACC_speed[axis]); if (!tmp1) tmp1 = 1.0f; tmp0 = (float)cfg.gps_ph_settlespeed / tmp1; tmp1 = (float)cfg.gps_ph_minbrakepercent * 0.01f; // this is the minimal break percentage tmp0 = constrain(sqrtf(tmp0), tmp1, 1.0f); // 1 - 100% maxbank100new = (float)maxbankbrake100 * tmp0; } rate_error = get_P(LocError[axis], &posholdPID_PARAM); // Do absolute position error here, that means transform positionerror to a velocityerror rate_error += get_I(LocError[axis], &ACCDeltaTimeINS, &posholdPID[axis], &posholdPID_PARAM); rate_error = constrain(rate_error - ACC_speed[axis], -1000, 1000); // +- 10m/s; // Calculate velocity Error for actual axis tilt = get_P(rate_error, &poshold_ratePID_PARAM); tilt += get_D(rate_error, &ACCDeltaTimeINS, &poshold_ratePID[axis], &poshold_ratePID_PARAM); // Constrained to half max P nav[axis] = constrain(tilt, -maxbank100new, maxbank100new); } }
MatrixXd MultivariateFNormalSufficient::evaluate_derivative_Sigma() const { //d(-log(p))/dSigma = 1/2 (N P - N P epsilon transpose(epsilon) P - PWP) LOG( "MVN: evaluate_derivative_Sigma() = " << std::endl); MatrixXd R; if (N_==1) // O(M) if up to date, O(M^3) if Sigma new { R = 0.5*(get_P()-compute_PTP()/SQUARE(factor_)); } else { // O(M^2) if up to date, O(M^3) if Sigma new double f2=SQUARE(factor_); R = 0.5*(N_*(get_P()-compute_PTP()/f2)-compute_PWP()/f2); } return R; }
MatrixXd MultivariateFNormalSufficient::evaluate_second_derivative_FM_FM() const { if (N_!=1) IMP_THROW("not implemented when N>1", ModelException); MatrixXd ret(get_P()/IMP::square(factor_)); return ret; }
MatrixXd MultivariateFNormalSufficient::evaluate_derivative_Sigma() const { timer_.start(DSIGMA); //d(-log(p))/dSigma = 1/2 (N P - N P epsilon transpose(epsilon) P - PWP) IMP_LOG(TERSE, "MVN: evaluate_derivative_Sigma() = " << std::endl); MatrixXd R; if (N_==1) // O(M) if up to date, O(M^3) if Sigma new { R = 0.5*(get_P()-compute_PTP()/IMP::square(factor_)); } else { // O(M^2) if up to date, O(M^3) if Sigma new double f2=IMP::square(factor_); R = 0.5*(N_*(get_P()-compute_PTP()/f2)-compute_PWP()/f2); } timer_.stop(DSIGMA); return R; }
//////////////////////////////////////////////////////////////////////////////////// // Calculate the desired nav_lat and nav_lon for distance flying such as RTH // static void GPS_calc_nav_rate(uint16_t max_speed) { float trig[2]; uint8_t axis; float temp; // push us towards the original track GPS_update_crosstrack(); // nav_bearing includes crosstrack temp = (9000l - nav_bearing) * RADX100; trig[_X] = cos(temp); trig[_Y] = sin(temp); for (axis=0;axis<2;axis++) { rate_error[axis] = (trig[axis] * max_speed) - actual_speed[axis]; rate_error[axis] = constrain(rate_error[axis], -1000, 1000); // P + I + D nav[axis] = get_P(rate_error[axis], &navPID_PARAM) +get_I(rate_error[axis], &dTnav, &navPID[axis], &navPID_PARAM) +get_D(rate_error[axis], &dTnav, &navPID[axis], &navPID_PARAM); nav[axis] = constrain(nav[axis], -NAV_BANK_MAX, NAV_BANK_MAX); poshold_ratePID[axis].integrator = navPID[axis].integrator; } }
double calc_corr_at_R(double R,double*k,double*P,int Nk,int N,double h){ double zero,psi,x,t,dpsi,f,PIsinht; double PI_h = PI/h; double PI_2 = PI*0.5; gsl_spline*Pspl = gsl_spline_alloc(gsl_interp_cspline,Nk); gsl_spline_init(Pspl,k,P,Nk); gsl_interp_accel*acc= gsl_interp_accel_alloc(); double sum = 0; int i; for(i=0;i<N;i++){ zero = i+1; psi = h*zero*tanh(sinh(h*zero)*PI_2); x = psi*PI_h; t = h*zero; PIsinht = PI*sinh(t); dpsi = (PI*t*cosh(t)+sinh(PIsinht))/(1+cosh(PIsinht)); if (dpsi!=dpsi) dpsi=1.0; f = x*get_P(x,R,k,P,Nk,Pspl,acc); sum += f*sin(x)*dpsi; } gsl_spline_free(Pspl),gsl_interp_accel_free(acc); return sum/(R*R*R*PI*2); }
MatrixXd MultivariateFNormalSufficient::evaluate_second_derivative_FM_FM() const { CHECK(N_==1, "not implemented when N>1"); MatrixXd ret(get_P()/SQUARE(factor_)); return ret; }
MatrixXd MultivariateFNormalSufficient::evaluate_second_derivative_FM_Sigma( unsigned k) const { CHECK(N_==1, "not implemented when N>1"); MatrixXd P(get_P()); VectorXd Peps(get_Peps()); MatrixXd ret(P.transpose().col(k)*Peps.transpose()); return ret/SQUARE(factor_); }
MatrixXd MultivariateFNormalSufficient::evaluate_second_derivative_FM_Sigma( unsigned k) const { if (N_!=1) IMP_THROW("not implemented when N>1", ModelException); MatrixXd P(get_P()); VectorXd Peps(get_Peps()); MatrixXd ret(P.transpose().col(k)*Peps.transpose()); return ret/IMP::square(factor_); }
MatrixXd MultivariateFNormalSufficient::evaluate_second_derivative_Sigma_Sigma( unsigned m, unsigned n) const { if (N_!=1) IMP_THROW("not implemented when N>1", ModelException); MatrixXd P(get_P()); VectorXd Peps(get_Peps()); MatrixXd tmp(P.col(m)*Peps.transpose()); MatrixXd ret(0.5*(-P.col(n)*P.row(m) +Peps(n)*(tmp+tmp.transpose()))); return ret/IMP::square(factor_); }
MatrixXd MultivariateFNormalSufficient::evaluate_second_derivative_Sigma_Sigma( unsigned m, unsigned n) const { CHECK(N_==1, "not implemented when N>1"); MatrixXd P(get_P()); VectorXd Peps(get_Peps()); MatrixXd tmp(P.col(m)*Peps.transpose()); MatrixXd ret(0.5*(-P.col(n)*P.row(m) +Peps(n)*(tmp+tmp.transpose()))); return ret/SQUARE(factor_); }
double MultivariateFNormalSufficient::trace_WP() const { double trace; if (N_==1) { trace=0; } else { trace = (get_P()*get_W()).trace(); LOG( "MVN: trace(WP) = " << trace << std::endl); } return trace; }
MatrixXd MultivariateFNormalSufficient::compute_PWP() const { //compute PWP MatrixXd tmp(M_,M_); if (N_==1) { LOG( "MVN: W = 0 => PWP = 0" << std::endl); tmp.setZero(); } else { LOG( "MVN: computing PWP" << std::endl); MatrixXd P(get_P()); MatrixXd W(get_W()); tmp=P*W*P; } return tmp; }
double MultivariateFNormalSufficient::trace_WP() const { timer_.start(TRWP); double trace; if (N_==1) { trace=0; } else { if (use_cg_) { trace = get_PW().trace(); } else { trace = (get_P()*get_W()).trace(); } IMP_LOG(TERSE, "MVN: trace(WP) = " << trace << std::endl); } timer_.stop(TRWP); return trace; }
node* get_T(){ node* val = node_new(); val = get_P(); if (syntax_errno) return 0; if (*cur_c == '*' || *cur_c == '/'){ node* operation = node_new(); operation -> data = *cur_c; ++cur_c; operation -> left = val; operation -> right = get_T(); return operation; } return val; }
//////////////////////////////////////////////////////////////////////////////////// // Calculate the desired nav_lat and nav_lon for distance flying such as RTH void GPS_calc_nav_rate(uint16_t max_speed) { float trig[2], rate_error; float temp, tiltcomp, trgtspeed; uint8_t axis; int32_t crosstrack_error; if ((abs(wrap_18000(target_bearing - original_target_bearing)) < 4500) && cfg.nav_ctrkgain != 0)// If we are too far off or too close we don't do track following { temp = (float)(target_bearing - original_target_bearing) * RADX100; crosstrack_error = sinf(temp) * (float)wp_distance * cfg.nav_ctrkgain; // Meters we are off track line nav_bearing = target_bearing + constrain(crosstrack_error, -3000, 3000); while (nav_bearing < 0) nav_bearing += 36000; // nav_bearing = wrap_36000(nav_bearing); while (nav_bearing >= 36000) nav_bearing -= 36000; } else nav_bearing = target_bearing; temp = (float)(9000l - nav_bearing) * RADX100; // nav_bearing and maybe crosstrack trig[GPS_X] = cosf(temp); trig[GPS_Y] = sinf(temp); for (axis = 0; axis < 2; axis++) { trgtspeed = (trig[axis] * (float)max_speed); // Target speed rate_error = trgtspeed - MIX_speed[axis]; // Since my INS Stuff is shit, reduce ACC influence to 50% anyway better than leadfilter rate_error = constrain(rate_error, -1000, 1000); nav[axis] = get_P(rate_error, &navPID_PARAM) + // P + I + D get_I(rate_error, &ACCDeltaTimeINS, &navPID[axis], &navPID_PARAM) + get_D(rate_error, &ACCDeltaTimeINS, &navPID[axis], &navPID_PARAM); if (cfg.nav_tiltcomp) // Do the apm 2.9.1 magic tiltcompensation { tiltcomp = trgtspeed * trgtspeed * ((float)cfg.nav_tiltcomp * 0.0001f); if(trgtspeed < 0) tiltcomp = -tiltcomp; } else tiltcomp = 0; nav[axis] = nav[axis] + tiltcomp; // Constrain is done at the end of all GPS stuff poshold_ratePID[axis].integrator = navPID[axis].integrator; } }
MatrixXd MultivariateFNormalSufficient::compute_PWP() const { timer_.start(PWP); //compute PWP MatrixXd tmp(M_,M_); if (N_==1) { IMP_LOG(TERSE, "MVN: W = 0 => PWP = 0" << std::endl); tmp.setZero(); } else { IMP_LOG(TERSE, "MVN: computing PWP" << std::endl); /*if (use_cg_) { if (first_PW_) { PW = compute_PW_direct(); (*const_cast<bool *>(&first_PW_))=false; } else { PW = compute_PW_cg(); } } else { PW = compute_PW_direct(); }*/ //if (use_cg_) //{ //MatrixXd WP(get_PW().transpose()); //return get_ldlt().solve(WP); //return get_P()*WP; //} else { MatrixXd P(get_P()); MatrixXd W(get_W()); tmp=P*W*P; //} } timer_.stop(PWP); return tmp; }
double MultivariateFNormalSufficient::get_Sigma_condition_number() const { return get_Sigma().norm()*get_P().norm(); }