Beispiel #1
0
////////////////////////////////////////////////////////////////////////////////////
// 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;
  }
}
Beispiel #2
0
////////////////////////////////////////////////////////////////////////////////////
// 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;
  }
}
////////////////////////////////////////////////////////////////////////////////////
// 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);
    }
}
Beispiel #4
0
////////////////////////////////////////////////////////////////////////////////////
// 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;
    }
}
Beispiel #5
0
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);
    }
}
Beispiel #6
0
int main (int argc, char** argv) {
	Graph *G;
	CayleyGraph *C;
	
	G = get_D(6);
	C = G -> make_cayley_graph();
	std::cout << C -> identify_group() << std::endl;
	delete C,G;
	
	G = get_Q8();
	C = G -> make_cayley_graph();
	std::cout << C -> identify_group() << std::endl;
	delete C,G;
	
	Graph *G1, *G2, *G3, *G4;
	CayleyGraph *C1,*C2,*C3;
	
	G1 = get_Z(4);
	G2 = get_Z(1);
	G3 = get_Q8();
	G4 = get_Z(1);
	
	C1 = direct_product(*(G1->make_cayley_graph()), *(G2->make_cayley_graph()));
	C2 = direct_product(*(G3->make_cayley_graph()), *(G4->make_cayley_graph()));
	C3 = direct_product(*C1,*C2);
	
	std::cout << "Direct product 1: " << C1 -> identify_group() << std::endl;
	std::cout << "Direct product 2: " << C2 -> identify_group() << std::endl;
	std::cout << "Direct product 3: " << C3 -> identify_group() << std::endl;
	delete G1,G2,G3,G4,C1,C2,C3;
	
	return 0;
}
Beispiel #7
0
////////////////////////////////////////////////////////////////////////////////////
// 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;
    }
}