////////////////////////////////////////////////////////////////////////////////////
// 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();
}
