// Reset velocity states to last GPS measurement if available or to zero if in constant position mode or if PV aiding is not absolute
// Do not reset vertical velocity using GPS as there is baro alt available to constrain drift
void NavEKF2_core::ResetVelocity(void)
{
    // Store the position before the reset so that we can record the reset delta
    velResetNE.x = stateStruct.velocity.x;
    velResetNE.y = stateStruct.velocity.y;

    if (PV_AidingMode != AID_ABSOLUTE) {
        stateStruct.velocity.zero();
    } else {
        // reset horizontal velocity states to the GPS velocity
        stateStruct.velocity.x  = gpsDataNew.vel.x; // north velocity from blended accel data
        stateStruct.velocity.y  = gpsDataNew.vel.y; // east velocity from blended accel data
    }
    for (uint8_t i=0; i<imu_buffer_length; i++) {
        storedOutput[i].velocity.x = stateStruct.velocity.x;
        storedOutput[i].velocity.y = stateStruct.velocity.y;
    }
    outputDataNew.velocity.x = stateStruct.velocity.x;
    outputDataNew.velocity.y = stateStruct.velocity.y;
    outputDataDelayed.velocity.x = stateStruct.velocity.x;
    outputDataDelayed.velocity.y = stateStruct.velocity.y;

    // Calculate the position jump due to the reset
    velResetNE.x = stateStruct.velocity.x - velResetNE.x;
    velResetNE.y = stateStruct.velocity.y - velResetNE.y;

    // store the time of the reset
    lastVelReset_ms = imuSampleTime_ms;

    // reset the corresponding covariances
    zeroRows(P,3,4);
    zeroCols(P,3,4);

    // set the variances to the measurement variance
    P[4][4] = P[3][3] = sq(frontend->_gpsHorizVelNoise);

}
Example #2
0
/*
  calculate a new aerodynamic_load_factor and limit nav_roll_cd to
  ensure that the load factor does not take us below the sustainable
  airspeed
 */
void Plane::update_load_factor(void)
{
    float demanded_roll = fabsf(nav_roll_cd*0.01f);
    if (demanded_roll > 85) {
        // limit to 85 degrees to prevent numerical errors
        demanded_roll = 85;
    }
    aerodynamic_load_factor = 1.0f / safe_sqrt(cosf(radians(demanded_roll)));

    if (!aparm.stall_prevention) {
        // stall prevention is disabled
        return;
    }
    if (fly_inverted()) {
        // no roll limits when inverted
        return;
    }

    float max_load_factor = smoothed_airspeed / aparm.airspeed_min;
    if (max_load_factor <= 1) {
        // our airspeed is below the minimum airspeed. Limit roll to
        // 25 degrees
        nav_roll_cd = constrain_int32(nav_roll_cd, -2500, 2500);
    } else if (max_load_factor < aerodynamic_load_factor) {
        // the demanded nav_roll would take us past the aerodymamic
        // load limit. Limit our roll to a bank angle that will keep
        // the load within what the airframe can handle. We always
        // allow at least 25 degrees of roll however, to ensure the
        // aircraft can be maneuvered with a bad airspeed estimate. At
        // 25 degrees the load factor is 1.1 (10%)
        int32_t roll_limit = degrees(acosf(sq(1.0f / max_load_factor)))*100;
        if (roll_limit < 2500) {
            roll_limit = 2500;
        }
        nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit, roll_limit);
    }
}
Example #3
0
Csf SpinAdapted::CSFUTIL::applyTensorOp(const TensorOp& newop, int spinL)
{
  //for (int k=0; k<newop.Szops[newop.Szops.size()-1-newop.Spin].size(); k++) {
  map<Slater, double> m;
  SpinQuantum sq(newop.optypes.size(), SpinSpace(newop.Spin), IrrepSpace(newop.irrep)); 
  for (int k=0; k<newop.Szops[spinL].size(); k++) {
    if (fabs(newop.Szops[spinL][k]) > 1e-10) {
      std::vector<bool> occ_rep(Slater().size(), 0);
      Slater s(occ_rep,1);
      for (int k2=newop.opindices[k].size()-1; k2>=0; k2--) {
	s.c(newop.opindices[k][k2]);
      }
      if (s.isempty()) {
	continue;
      }
      map<Slater, double>::iterator itout = m.find(s);

      if (itout == m.end()) {
	int sign = s.alpha.getSign();
	s.setSign(1);
	m[s] = sign*newop.Szops[spinL][k];
      }	
      else
	m[s] += s.alpha.getSign()*newop.Szops[spinL][k];
    }
  }
  int irreprow = spinL/(newop.Spin+1); 
  int sz = -2*(spinL%(newop.Spin+1)) + newop.Spin;

  

  Csf csf = Csf(m,sq.particleNumber, sq.totalSpin, sz, IrrepVector(sq.get_symm().getirrep(),irreprow)) ; 
  if (!csf.isempty() && fabs(csf.norm()) > 1e-14)
    csf.normalize();
  
  return csf;
}
Example #4
0
double
eval_nb_dfda
(
   double a,
   const tab_t *tab
)
{

   double retval;
   double prev;
   // Convenience variables.
   const unsigned int *val = tab->val;
   const unsigned int *num = tab->num;

   size_t nobs = num[0];
   double mean = num[0] * val[0];

   prev = trigamma(a + val[0]);
   retval = num[0] * prev;
   // Iterate over the occurrences and compute the new value
   // of trigamma either by the recurrence relation, or by
   // a new call to 'trigamma()', whichever is faster.
   for (size_t i = 1 ; i < tab->size ; i++) {
      nobs += num[i];
      mean += num[i] * val[i];
      prev = (val[i] - val[i-1] == 1) ?
         prev - 1.0 / sq(a-1 +val[i]) :
         trigamma(a + val[i]);
      retval += num[i] * prev;
   }

   mean /= nobs;
   retval += nobs*(mean/(a*(a+mean)) - trigamma(a));

   return retval;

}
Example #5
0
/**
 * Morgan SCARA Inverse Kinematics. Results in delta[].
 *
 * See http://forums.reprap.org/read.php?185,283327
 *
 * Maths and first version by QHARLEY.
 * Integrated into Marlin and slightly restructured by Joachim Cerny.
 */
void inverse_kinematics(const float (&raw)[XYZ]) {

  static float C2, S2, SK1, SK2, THETA, PSI;

  float sx = raw[X_AXIS] - SCARA_OFFSET_X,  // Translate SCARA to standard X Y
        sy = raw[Y_AXIS] - SCARA_OFFSET_Y;  // With scaling factor.

  if (L1 == L2)
    C2 = HYPOT2(sx, sy) / L1_2_2 - 1;
  else
    C2 = (HYPOT2(sx, sy) - (L1_2 + L2_2)) / (2.0 * L1 * L2);

  S2 = SQRT(1 - sq(C2));

  // Unrotated Arm1 plus rotated Arm2 gives the distance from Center to End
  SK1 = L1 + L2 * C2;

  // Rotated Arm2 gives the distance from Arm1 to Arm2
  SK2 = L2 * S2;

  // Angle of Arm1 is the difference between Center-to-End angle and the Center-to-Elbow
  THETA = ATAN2(SK1, SK2) - ATAN2(sx, sy);

  // Angle of Arm2
  PSI = ATAN2(S2, C2);

  delta[A_AXIS] = DEGREES(THETA);        // theta is support arm angle
  delta[B_AXIS] = DEGREES(THETA + PSI);  // equal to sub arm angle (inverted motor)
  delta[C_AXIS] = raw[Z_AXIS];

  /*
    DEBUG_POS("SCARA IK", raw);
    DEBUG_POS("SCARA IK", delta);
    SERIAL_ECHOLNPAIR("  SCARA (x,y) ", sx, ",", sy, " C2=", C2, " S2=", S2, " Theta=", THETA, " Phi=", PHI);
  //*/
}
void main()
{
	int i,n,m;
	float a,x0,x1,epsilon,delta,relativeerror;
	clrscr();
	cout<<"Enter any number: ";
	cin>>a;
	cout<<"Enter the value of n: ";
	cin>>m;
	cout<<"Enter the initial approximation: ";
	cin>>x0;
	cout<<"Enter the number of iterations: ";
	cin>>epsilon;
	cout<<"Enter the lower boundary: ";
	cin>>delta;

	for(i=1;i<n;i++)
	{
		if(fabs(dfsq(x0,m))<=delta)
		{
			cout<<"Slope too small!!\n";
			exit(0);
		}
	}
	x1=(float)(x0-(sq(x0,a,m)/dfsq(x0,m)));
	relativeerror=fabs((x1-x0)/x1);
	x0=x1;

	if(relativeerror>=epsilon)
	{
		cout<<"Root of the given number is: "<<setpricision(6)<<x1;
		exit(0);
	}
	cout<<"Solution does not converge in "<<n<<" iterations"<<endl;
	getch();
}
Example #7
0
File: rm501.c Project: koppi/rm501
void rotate_m_axyz(double *mat, double angle, double x, double y, double z ) {
    int i;

    double m[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 };
    double s = sin( deg2rad(angle) );
    double c = cos( deg2rad(angle) );

    double mag = sqrt(sq(x) + sq(y) + sq(z));
    if (mag <= 1.0e-4) { return; } // no rotation, leave mat as-is
    x /= mag; y /= mag; z /= mag;

    double one_c = 1.0 - c;
#define M(row,col)  m[col*4+row]
    M(0,0) = (one_c * sq(x)) + c;
    M(0,1) = (one_c * x*y) - z*s;
    M(0,2) = (one_c * z*x) + y*s;

    M(1,0) = (one_c * x*y) + z*s;
    M(1,1) = (one_c * sq(y)) + c;
    M(1,2) = (one_c * y*z) - x*s;

    M(2,0) = (one_c * z*x) - y*s;
    M(2,1) = (one_c * y*z) + x*s;
    M(2,2) = (one_c * sq(z)) + c;
#undef M

    for (i = 0; i < 4; i++) {
#define A(row,col)  mat[(col<<2)+row]
#define B(row,col)  m[(col<<2)+row]
        double ai0=A(i,0), ai1=A(i,1), ai2=A(i,2), ai3=A(i,3);
        A(i,0) = ai0 * B(0,0) + ai1 * B(1,0) + ai2 * B(2,0) + ai3 * B(3,0);
        A(i,1) = ai0 * B(0,1) + ai1 * B(1,1) + ai2 * B(2,1) + ai3 * B(3,1);
        A(i,2) = ai0 * B(0,2) + ai1 * B(1,2) + ai2 * B(2,2) + ai3 * B(3,2);
        A(i,3) = ai0 * B(0,3) + ai1 * B(1,3) + ai2 * B(2,3) + ai3 * B(3,3);
#undef A
#undef B
    }

}
/*
*Contains algorithm to pixelate and image with given params
*Parameters:
*pixAmount: the amount to pixelate our image by
*img: a pointer to our image in memory.
*
*return 0 if no errors were encountered, 1 otherwise.
*/
int pixelate(Image* img, int pixAmount) {
	//if we don't have a file in memory, complain.
	if(img->data == NULL) {
	fprintf(stderr, "Error, no file currently in memory\n");
		return 1;
	}
	//ints that we can change without messing with our image
	int numCols = img->colNum;
	int numRows = img->rowNum;
	//this sets the variables above to ones evenly divisible by pixAmount
	while( ((numCols % pixAmount) != 0) || ((numRows % pixAmount) != 0)) {
		if((numCols % pixAmount) != 0) {
			numCols--;
		}
		if((numRows % pixAmount) != 0) {
			numRows--;
		}
	}
	//we crop our image, and lose the part that we can't easily pixelate. 
	cropImage(0,  numCols, 0, numRows, img);
	//set our number of pixels in our pixelated image
	numCols = numCols/pixAmount;
	numRows = numRows/pixAmount;
	//variables to compute what each pixel should be
	int pixSumR = 0;
	int pixSumG = 0;
	int pixSumB = 0;
	// the number of pixels in one "square" of our image
	int numPix = (int)sq(pixAmount);
	//for loop that loops through all the rows & cols of our pix'd image
	for(int j=0; j<numRows; j++) {
		for(int i=0; i<numCols; i++) {
			//these loops loop through each pixel of our image in memory, and change the pixSums as needed.
			for(int y=0; y<pixAmount; y++) {
				for(int x=0; x<pixAmount; x++) {
					pixSumR+=img->data[(j*pixAmount+y)*(img->colNum)+(i*pixAmount+x)].r;
					pixSumG+=img->data[(j*pixAmount+y)*(img->colNum)+(i*pixAmount+x)].g;
					pixSumB+=img->data[(j*pixAmount+y)*(img->colNum)+(i*pixAmount+x)].b;
				}
			}
		//averages it for each color
		pixSumR = (pixSumR/numPix);
		pixSumG = (pixSumG/numPix);
		pixSumB = (pixSumB/numPix);	
		//turns our image into a pixelated one with the (almost) same resolution as the original.
		for(int y=0; y<pixAmount; y++) {
				for(int x=0; x<pixAmount; x++) {
					img->data[(j*pixAmount+y)*(img->colNum)+(i*pixAmount+x)].r =pixSumR;
					img->data[(j*pixAmount+y)*(img->colNum)+(i*pixAmount+x)].g =pixSumG;
					img->data[(j*pixAmount+y)*(img->colNum)+(i*pixAmount+x)].b =pixSumB;
				}
			}
		//reset the sums
		pixSumR = 0;
		pixSumG = 0;
		pixSumB = 0;
		}
	}
	
	return 0; 
}
/*
*computes the gaussian value for a matrix given params
*Parameters:
*sigma: the sigma to use in the equation.
*dx: x distance from the center of the matrix
*dy: y distance from the center of the matrix
*
*/
double gaussian(double sigma, int dx, int dy) {
	//equation to comput the gaussian value
	double g = (1.0 / (2.0 * PI * sq(sigma))) * exp( -(sq(dx) + sq(dy)) / (2 * sq(sigma)));
	return g;		
}
Example #10
0
/* calculate best fit from i+.5 to j+.5.  Assume i<j (cyclically).
   Return 0 and set badness and parameters (alpha, beta), if
   possible. Return 1 if impossible. */
static int opti_penalty(privpath_t *pp, int i, int j, opti_t *res, double opttolerance, int *convc, double *areac) {
  int m = pp->curve.n;
  int k, k1, k2, conv, i1;
  double area, alpha, d, d1, d2;
  dpoint_t p0, p1, p2, p3, pt;
  double A, R, A1, A2, A3, A4;
  double s, t;

  /* check convexity, corner-freeness, and maximum bend < 179 degrees */

  if (i==j) {  /* sanity - a full loop can never be an opticurve */
    return 1;
  }

  k = i;
  i1 = mod(i+1, m);
  k1 = mod(k+1, m);
  conv = convc[k1];
  if (conv == 0) {
    return 1;
  }
  d = ddist(pp->curve.vertex[i], pp->curve.vertex[i1]);
  for (k=k1; k!=j; k=k1) {
    k1 = mod(k+1, m);
    k2 = mod(k+2, m);
    if (convc[k1] != conv) {
      return 1;
    }
    if (sign(cprod(pp->curve.vertex[i], pp->curve.vertex[i1], pp->curve.vertex[k1], pp->curve.vertex[k2])) != conv) {
      return 1;
    }
    if (iprod1(pp->curve.vertex[i], pp->curve.vertex[i1], pp->curve.vertex[k1], pp->curve.vertex[k2]) < d * ddist(pp->curve.vertex[k1], pp->curve.vertex[k2]) * COS179) {
      return 1;
    }
  }

  /* the curve we're working in: */
  p0 = pp->curve.c[mod(i,m)][2];
  p1 = pp->curve.vertex[mod(i+1,m)];
  p2 = pp->curve.vertex[mod(j,m)];
  p3 = pp->curve.c[mod(j,m)][2];

  /* determine its area */
  area = areac[j] - areac[i];
  area -= dpara(pp->curve.vertex[0], pp->curve.c[i][2], pp->curve.c[j][2])/2;
  if (i>=j) {
    area += areac[m];
  }

  /* find intersection o of p0p1 and p2p3. Let t,s such that o =
     interval(t,p0,p1) = interval(s,p3,p2). Let A be the area of the
     triangle (p0,o,p3). */

  A1 = dpara(p0, p1, p2);
  A2 = dpara(p0, p1, p3);
  A3 = dpara(p0, p2, p3);
  /* A4 = dpara(p1, p2, p3); */
  A4 = A1+A3-A2;    
  
  if (A2 == A1) {  /* this should never happen */
    return 1;
  }

  t = A3/(A3-A4);
  s = A2/(A2-A1);
  A = A2 * t / 2.0;
  
  if (A == 0.0) {  /* this should never happen */
    return 1;
  }

  R = area / A;	 /* relative area */
  alpha = 2 - sqrt(4 - R / 0.3);  /* overall alpha for p0-o-p3 curve */

  res->c[0] = interval(t * alpha, p0, p1);
  res->c[1] = interval(s * alpha, p3, p2);
  res->alpha = alpha;
  res->t = t;
  res->s = s;

  p1 = res->c[0];
  p2 = res->c[1];  /* the proposed curve is now (p0,p1,p2,p3) */

  res->pen = 0;

  /* calculate penalty */
  /* check tangency with edges */
  for (k=mod(i+1,m); k!=j; k=k1) {
    k1 = mod(k+1,m);
    t = tangent(p0, p1, p2, p3, pp->curve.vertex[k], pp->curve.vertex[k1]);
    if (t<-.5) {
      return 1;
    }
    pt = bezier(t, p0, p1, p2, p3);
    d = ddist(pp->curve.vertex[k], pp->curve.vertex[k1]);
    if (d == 0.0) {  /* this should never happen */
      return 1;
    }
    d1 = dpara(pp->curve.vertex[k], pp->curve.vertex[k1], pt) / d;
    if (fabs(d1) > opttolerance) {
      return 1;
    }
    if (iprod(pp->curve.vertex[k], pp->curve.vertex[k1], pt) < 0 || iprod(pp->curve.vertex[k1], pp->curve.vertex[k], pt) < 0) {
      return 1;
    }
    res->pen += sq(d1);
  }

  /* check corners */
  for (k=i; k!=j; k=k1) {
    k1 = mod(k+1,m);
    t = tangent(p0, p1, p2, p3, pp->curve.c[k][2], pp->curve.c[k1][2]);
    if (t<-.5) {
      return 1;
    }
    pt = bezier(t, p0, p1, p2, p3);
    d = ddist(pp->curve.c[k][2], pp->curve.c[k1][2]);
    if (d == 0.0) {  /* this should never happen */
      return 1;
    }
    d1 = dpara(pp->curve.c[k][2], pp->curve.c[k1][2], pt) / d;
    d2 = dpara(pp->curve.c[k][2], pp->curve.c[k1][2], pp->curve.vertex[k1]) / d;
    d2 *= 0.75 * pp->curve.alpha[k1];
    if (d2 < 0) {
      d1 = -d1;
      d2 = -d2;
    }
    if (d1 < d2 - opttolerance) {
      return 1;
    }
    if (d1 < d2) {
      res->pen += sq(d1 - d2);
    }
  }

  return 0;
}
Example #11
0
real SimpleStats::var() const {
  real v = (_mean2 - sq(_mean));
  return max(v, (real)0); // make sure the result is >= 0
}
// Detect if we are in flight or on ground
void NavEKF2_core::detectFlight()
{
    /*
        If we are a fly forward type vehicle (eg plane), then in-air status can be determined through a combination of speed and height criteria.
        Because of the differing certainty requirements of algorithms that need the in-flight / on-ground status we use two booleans where
        onGround indicates a high certainty we are not flying and inFlight indicates a high certainty we are flying. It is possible for
        both onGround and inFlight to be false if the status is uncertain, but they cannot both be true.

        If we are a plane as indicated by the assume_zero_sideslip() status, then different logic is used

        TODO - this logic should be moved out of the EKF and into the flight vehicle code.
    */

    if (assume_zero_sideslip()) {
        // To be confident we are in the air we use a criteria which combines arm status, ground speed, airspeed and height change
        float gndSpdSq = sq(gpsDataDelayed.vel.x) + sq(gpsDataDelayed.vel.y);
        bool highGndSpd = false;
        bool highAirSpd = false;
        bool largeHgtChange = false;

        // trigger at 8 m/s airspeed
        if (_ahrs->airspeed_sensor_enabled()) {
            const AP_Airspeed *airspeed = _ahrs->get_airspeed();
            if (airspeed->get_airspeed() * airspeed->get_EAS2TAS() > 10.0f) {
                highAirSpd = true;
            }
        }

        // trigger at 10 m/s GPS velocity, but not if GPS is reporting bad velocity errors
        if (gndSpdSq > 100.0f && gpsSpdAccuracy < 1.0f) {
            highGndSpd = true;
        }

        // trigger if more than 10m away from initial height
        if (fabsf(baroDataDelayed.hgt) > 10.0f) {
            largeHgtChange = true;
        }

        // Determine to a high certainty we are flying
        if (motorsArmed && highGndSpd && (highAirSpd || largeHgtChange)) {
            onGround = false;
            inFlight = true;
        }

        // if is possible we are in flight, set the time this condition was last detected
        if (motorsArmed && (highGndSpd || highAirSpd || largeHgtChange)) {
            airborneDetectTime_ms = imuSampleTime_ms;
            onGround = false;
        }

        // Determine if is is possible we are on the ground
        if (highGndSpd || highAirSpd || largeHgtChange) {
            inFlight = false;
        }

        // Determine to a high certainty we are not flying
        // after 5 seconds of not detecting a possible flight condition or we are disarmed, we transition to on-ground mode
        if(!motorsArmed || ((imuSampleTime_ms - airborneDetectTime_ms) > 5000)) {
            onGround = true;
            inFlight = false;
        }
    } else {
        // Non fly forward vehicle, so can only use height and motor arm status

        // If the motors are armed then we could be flying and if they are not armed then we are definitely not flying
        if (motorsArmed) {
            onGround = false;
        } else {
            inFlight = false;
            onGround = true;
        }

        // If height has increased since exiting on-ground, then we definitely are flying
        if (!onGround && ((stateStruct.position.z - posDownAtTakeoff) < -1.5f)) {
            inFlight = true;
        }

        // If rangefinder has increased since exiting on-ground, then we definitely are flying
        if (!onGround && ((rngMea - rngAtStartOfFlight) > 0.5f)) {
            inFlight = true;
        }

    }

    // store current on-ground  and in-air status for next time
    prevOnGround = onGround;
    prevInFlight = inFlight;

    // Store vehicle height and range prior to takeoff for use in post takeoff checks
    if (!onGround && !prevOnGround) {
        // store vertical position at start of flight to use as a reference for ground relative checks
        posDownAtTakeoff = stateStruct.position.z;
        // store the range finder measurement which will be used as a reference to detect when we have taken off
        rngAtStartOfFlight = rngMea;
    }

}
Example #13
0
void calcTargetDistanceAndHeading(geoCoordinate_t *tracker, geoCoordinate_t *target) {
  int16_t dLat = tracker->lat - target->lat;
  int16_t dLon = (tracker->lon - target->lon);// * lonScale;
  target->distance = uint16_t(sqrt(sq(dLat) + sq(dLon)) * 1.113195f);
  target->heading = uint16_t(atan2(dLon, dLat) * 572.90f);
}
Example #14
0
double LogisticNormalLogPdf::f(double mu, double sigma, double x)
{
    return -fastlog(sigma) - fastlog(sqrt(2*M_PI)) -
           sq(fastlog(x / (1 - x)) - mu) / (2 * sq(sigma)) -
           fastlog(x) - fastlog(1-x);
}
Example #15
0
float cov(const vector<float>& x) {
    float ssd = sum(sq(add(-mean(x), x)));
    return ssd / x.size();
}
Example #16
0
double LogNormalLogPdf::df_dx(double mu, double sigma, double x)
{
    return (mu - fastlog(x)) / (x * sq(sigma)) - 1.0 / x;
}
Example #17
0
double NormalLogPdf::f(double mu, double sigma, const double x)
{
    double part1 = NEG_LOG_2_PI_DIV_2 - fastlog(sigma);
    double part2 = sq(x - mu) / (2 * sq(sigma));
    return part1 - part2;
}
Example #18
0
// Determine if learning of wind and magnetic field will be enabled and set corresponding indexing limits to
// avoid unnecessary operations
void NavEKF2_core::setWindMagStateLearningMode()
{
    // If we are on ground, or in constant position mode, or don't have the right vehicle and sensing to estimate wind, inhibit wind states
    bool setWindInhibit = (!useAirspeed() && !assume_zero_sideslip()) || onGround || (PV_AidingMode == AID_NONE);
    if (!inhibitWindStates && setWindInhibit) {
        inhibitWindStates = true;
    } else if (inhibitWindStates && !setWindInhibit) {
        inhibitWindStates = false;
        // set states and variances
        if (yawAlignComplete && useAirspeed()) {
            // if we have airspeed and a valid heading, set the wind states to the reciprocal of the vehicle heading
            // which assumes the vehicle has launched into the wind
             Vector3f tempEuler;
            stateStruct.quat.to_euler(tempEuler.x, tempEuler.y, tempEuler.z);
            float windSpeed =  sqrtf(sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y)) - tasDataDelayed.tas;
            stateStruct.wind_vel.x = windSpeed * cosf(tempEuler.z);
            stateStruct.wind_vel.y = windSpeed * sinf(tempEuler.z);

            // set the wind sate variances to the measurement uncertainty
            for (uint8_t index=22; index<=23; index++) {
                P[index][index] = sq(constrain_float(frontend->_easNoise, 0.5f, 5.0f) * constrain_float(_ahrs->get_EAS2TAS(), 0.9f, 10.0f));
            }
        } else {
            // set the variances using a typical wind speed
            for (uint8_t index=22; index<=23; index++) {
                P[index][index] = sq(5.0f);
            }
        }
    }

    // determine if the vehicle is manoevring
    if (accNavMagHoriz > 0.5f) {
        manoeuvring = true;
    } else {
        manoeuvring = false;
    }

    // Determine if learning of magnetic field states has been requested by the user
    uint8_t magCal = effective_magCal();
    bool magCalRequested =
            ((magCal == 0) && inFlight) || // when flying
            ((magCal == 1) && manoeuvring)  || // when manoeuvring
            ((magCal == 3) && finalInflightYawInit && finalInflightMagInit) || // when initial in-air yaw and mag field reset is complete
            (magCal == 4); // all the time

    // Deny mag calibration request if we aren't using the compass, it has been inhibited by the user,
    // we do not have an absolute position reference or are on the ground (unless explicitly requested by the user)
    bool magCalDenied = !use_compass() || (magCal == 2) || (onGround && magCal != 4);

    // Inhibit the magnetic field calibration if not requested or denied
    bool setMagInhibit = !magCalRequested || magCalDenied;
    if (!inhibitMagStates && setMagInhibit) {
        inhibitMagStates = true;
    } else if (inhibitMagStates && !setMagInhibit) {
        inhibitMagStates = false;
        if (magFieldLearned) {
            // if we have already learned the field states, then retain the learned variances
            P[16][16] = earthMagFieldVar.x;
            P[17][17] = earthMagFieldVar.y;
            P[18][18] = earthMagFieldVar.z;
            P[19][19] = bodyMagFieldVar.x;
            P[20][20] = bodyMagFieldVar.y;
            P[21][21] = bodyMagFieldVar.z;
        } else {
            // set the variances equal to the observation variances
            for (uint8_t index=18; index<=21; index++) {
                P[index][index] = sq(frontend->_magNoise);
            }

            // set the NE earth magnetic field states using the published declination
            // and set the corresponding variances and covariances
            alignMagStateDeclination();

        }
        // request a reset of the yaw and magnetic field states if not done before
        if (!magStateInitComplete || (!finalInflightMagInit && inFlight)) {
            magYawResetRequest = true;
        }
    }

    // If on ground we clear the flag indicating that the magnetic field in-flight initialisation has been completed
    // because we want it re-done for each takeoff
    if (onGround) {
        finalInflightYawInit = false;
        finalInflightMagInit = false;
    }

    // Adjust the indexing limits used to address the covariance, states and other EKF arrays to avoid unnecessary operations
    // if we are not using those states
    if (inhibitMagStates && inhibitWindStates) {
        stateIndexLim = 15;
    } else if (inhibitWindStates) {
        stateIndexLim = 21;
    } else {
        stateIndexLim = 23;
    }
}
Example #19
0
void PitchTracker::run() {
    for (;;) {
        busy = false;
        sem_wait(&m_trig);
        if (error) {
            continue;
        }
        float sum = 0.0;
        for (int k = 0; k < m_buffersize; ++k) {
            sum += fabs(m_input[k]);
        }
        float threshold = (m_audioLevel ? signal_threshold_off : signal_threshold_on);
        m_audioLevel = (sum / m_buffersize >= threshold);
        if ( m_audioLevel == false ) {
	    if (m_freq != 0) {
		m_freq = 0;
		new_freq();
	    }
            continue;
        }

        memcpy(m_fftwBufferTime, m_input, m_buffersize * sizeof(*m_fftwBufferTime));
        memset(m_fftwBufferTime+m_buffersize, 0, (m_fftSize - m_buffersize) * sizeof(*m_fftwBufferTime));
        fftwf_execute(m_fftwPlanFFT);
        for (int k = 1; k < m_fftSize/2; k++) {
            m_fftwBufferFreq[k] = sq(m_fftwBufferFreq[k]) + sq(m_fftwBufferFreq[m_fftSize-k]);
            m_fftwBufferFreq[m_fftSize-k] = 0.0;
        }
        m_fftwBufferFreq[0] = sq(m_fftwBufferFreq[0]);
        m_fftwBufferFreq[m_fftSize/2] = sq(m_fftwBufferFreq[m_fftSize/2]);

        fftwf_execute(m_fftwPlanIFFT);

        double sumSq = 2.0 * static_cast<double>(m_fftwBufferTime[0]) / static_cast<double>(m_fftSize);
        for (int k = 0; k < m_fftSize - m_buffersize; k++) {
            m_fftwBufferTime[k] = m_fftwBufferTime[k+1] / static_cast<float>(m_fftSize);
        }

        int count = (m_buffersize + 1) / 2;
        for (int k = 0; k < count; k++) {
            sumSq  -= sq(m_input[m_buffersize-1-k]) + sq(m_input[k]);
            // dividing by zero is very slow, so deal with it seperately
            if (sumSq > 0.0) {
                m_fftwBufferTime[k] *= 2.0 / sumSq;
            } else {
                m_fftwBufferTime[k] = 0.0;
            }
        }
	const float thres = 0.99; // was 0.6
        int maxAutocorrIndex = findsubMaximum(m_fftwBufferTime, count, thres);

        float x = 0.0;
        if (maxAutocorrIndex >= 0) {
            parabolaTurningPoint(m_fftwBufferTime[maxAutocorrIndex-1],
                                 m_fftwBufferTime[maxAutocorrIndex],
                                 m_fftwBufferTime[maxAutocorrIndex+1],
                                 maxAutocorrIndex+1, &x);
            x = m_sampleRate / x;
            if (x > 999.0) {  // precision drops above 1000 Hz
                x = 0.0;
            }
        }
	if (m_freq != x) {
	    m_freq = x;
	    new_freq();
	}
    }
}
/*
  update the state of the airspeed calibration - needs to be called
  once a second

  On an AVR2560 this costs 1.9 milliseconds per call
 */
float Airspeed_Calibration::update(float airspeed, const Vector3f &vg)
{
    // Perform the covariance prediction
    // Q is a diagonal matrix so only need to add three terms in
    // C code implementation
    // P = P + Q;
    P.a.x += Q0;
    P.b.y += Q0;
    P.c.z += Q1;
    
    // Perform the predicted measurement using the current state estimates
    // No state prediction required because states are assumed to be time
    // invariant plus process noise
    // Ignore vertical wind component
    float TAS_pred = state.z * pythagorous3(vg.x - state.x, vg.y - state.y, vg.z);
    float TAS_mea  = airspeed;
    
    // Calculate the observation Jacobian H_TAS
    float SH1 = sq(vg.y - state.y) + sq(vg.x - state.x);
    if (SH1 < 0.000001f) {
        // avoid division by a small number
        return state.z;
    }
    float SH2 = 1/sqrtf(SH1);

    // observation Jacobian
    Vector3f H_TAS(
        -(state.z*SH2*(2*vg.x - 2*state.x))/2,
        -(state.z*SH2*(2*vg.y - 2*state.y))/2,
        1/SH2);
    
    // Calculate the fusion innovaton covariance assuming a TAS measurement
    // noise of 1.0 m/s
    // S = H_TAS*P*H_TAS' + 1.0; % [1 x 3] * [3 x 3] * [3 x 1] + [1 x 1]
    Vector3f PH = P * H_TAS;
    float S = H_TAS * PH + 1.0f;
    
    // Calculate the Kalman gain
    // [3 x 3] * [3 x 1] / [1 x 1]
    Vector3f KG = PH / S; 
    
    // Update the states
    state += KG*(TAS_mea - TAS_pred); // [3 x 1] + [3 x 1] * [1 x 1]
    
    // Update the covariance matrix
    Vector3f HP2 = H_TAS * P;
    P -= KG.mul_rowcol(HP2);
    
    // force symmetry on the covariance matrix - necessary due to rounding
    // errors
	float P12 = 0.5f * (P.a.y + P.b.x);
	float P13 = 0.5f * (P.a.z + P.c.x);
	float P23 = 0.5f * (P.b.z + P.c.y);
	P.a.y = P.b.x = P12;
	P.a.z = P.c.x = P13;
	P.b.z = P.c.y = P23;

    // Constrain diagonals to be non-negative - protects against rounding errors
    P.a.x = max(P.a.x, 0.0f);
    P.b.y = max(P.b.y, 0.0f);
    P.c.z = max(P.c.z, 0.0f);

    state.x = constrain_float(state.x, -aparm.airspeed_max, aparm.airspeed_max);
    state.y = constrain_float(state.y, -aparm.airspeed_max, aparm.airspeed_max);
    state.z = constrain_float(state.z, 0.5f, 1.0f);

    return state.z;
}
Example #21
0
double NormalLogPdf::df_dx(double mu, double sigma, const double x)
{
    return (mu - x) / sq(sigma);
}
Example #22
0
File: rm501.c Project: koppi/rm501
void kins_inv(bot_t* bot) {

    double nx = -bot->t[0], ny =  bot->t[2];
    double ox =  bot->t[8], oy = -bot->t[10];
    double ax = -bot->t[4], ay =  bot->t[6],  az = -bot->t[5];

    double px = bot->t[12];
    double pz = bot->t[13];
    double py = bot->t[14];

    double th1;

    if (py == 0 && px == 0) {     // point on the Z0 axis
        if (ay == 0 && ax == 0) { // wrist pointing straight up/down
            th1 = 0;
        } else {
            th1 = atan2(ay, ax);
        }
    } else {
        th1 = atan2(py, px);
    }

    double c1 = cos(th1);
    double s1 = sin(th1);

    double t234 = atan2(c1*ax + s1*ay, -az);
    double c234 = cos(t234);
    double s234 = sin(t234);

    // joint 3 - elbow
    double tp1 = c1*px + s1*py - bot->d5*s234;
    double tp2 = pz - bot->d1 + bot->d5*c234;
    double c3  = (sq(tp1) + sq(tp2) - sq(bot->a3) - sq(bot->a2)) / (2*bot->a2*bot->a3);
    double s3  = -sqrt(1 - sq(c3));
    double th3 = atan2(s3, c3);

    // joint 2 - shoulder
    double num = tp2*(bot->a3*c3 + bot->a2) - bot->a3*s3*tp1;
    double den = tp1*(bot->a3*c3 + bot->a2) + bot->a3*s3*tp2;
    double th2 = atan2(num, den);

    // joint 4 - pitch
    double th4 = (t234 - th3 - th2);

    // joint 5 - roll
    double th5 = atan2(s1*nx - c1*ny, s1*ox - c1*oy);

    char msg[5][256];
    double th[] = {th1, th2, th3, th4, th5};
    int i;

    bot->err = 0;

    for (i = 0; i < 5; i++) {
#ifdef KINS_INV_IGNORE_LIMITS
        if (!isnan(th[i])) {
#else
            if (!isnan(th[i]) && th[i] >= deg2rad(bot->j[i].min) && th[i] <= deg2rad(bot->j[i].max)) {
#endif
                bot->j[i].pos = rad2deg(th[i]);
            } else {
                bot->err |= (1 << i);
            }

            // pretty print results

            if (isnan(th[i])) {
                snprintf(msg[i], sizeof(msg[i]), "%7s ", "nan");
            } else if (th[i] < deg2rad(bot->j[i].min)) {
                snprintf(msg[i], sizeof(msg[i]), "%7.2fv", rad2deg(th[i]));
            } else if (th[i] > deg2rad(bot->j[i].max)) {
                snprintf(msg[i], sizeof(msg[i]), "%7.2f^", rad2deg(th[i]));
            } else {
                snprintf(msg[i], sizeof(msg[i]), "%7.2f ", rad2deg(th[i]));
            }
        }

        snprintf(bot->msg, sizeof(bot->msg), "kin_inv(%d): %s %s %s %s %s", bot->err, msg[0], msg[1], msg[2], msg[3], msg[4]);
    }

#ifdef HAVE_SDL
void cross(float th, float l) {
    glLineWidth(th);
    glBegin(GL_LINES);
    glColor3f(1, 0, 0); glVertex3f(0, 0, 0); glVertex3f(l, 0, 0);
    glColor3f(0, 1, 0); glVertex3f(0, 0, 0); glVertex3f(0, l, 0);
    glColor3f(0, 0, 1); glVertex3f(0, 0, 0); glVertex3f(0, 0, l);
    glEnd();
    glLineWidth(1);
}
Example #23
0
float AltGammaLogPdf::df_dshape(float mean, float shape, float x)
{
    float scale = mean / shape;
    float part = fastlog(x) - x / mean;
    return (-boost::math::digamma(shape) + fastlog(scale) * (mean/sq(shape))) + part;
}
Example #24
0
static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
                                bool useAcc, float ax, float ay, float az,
                                bool useMag, float mx, float my, float mz,
                                bool useYaw, float yawError)
{
    static float integralFBx = 0.0f,  integralFBy = 0.0f, integralFBz = 0.0f;    // integral error terms scaled by Ki
    float recipNorm;
    float hx, hy, bx;
    float ex = 0, ey = 0, ez = 0;
    float qa, qb, qc;

    // Calculate general spin rate (rad/s)
    float spin_rate = sqrtf(sq(gx) + sq(gy) + sq(gz));

    // Use raw heading error (from GPS or whatever else)
    if (useYaw) {
        while (yawError >  M_PIf) yawError -= (2.0f * M_PIf);
        while (yawError < -M_PIf) yawError += (2.0f * M_PIf);

        ez += sin_approx(yawError / 2.0f);
    }

    // Use measured magnetic field vector
    recipNorm = sq(mx) + sq(my) + sq(mz);
    if (useMag && recipNorm > 0.01f) {
        // Normalise magnetometer measurement
        recipNorm = invSqrt(recipNorm);
        mx *= recipNorm;
        my *= recipNorm;
        mz *= recipNorm;

        // For magnetometer correction we make an assumption that magnetic field is perpendicular to gravity (ignore Z-component in EF).
        // This way magnetic field will only affect heading and wont mess roll/pitch angles

        // (hx; hy; 0) - measured mag field vector in EF (assuming Z-component is zero)
        // (bx; 0; 0) - reference mag field vector heading due North in EF (assuming Z-component is zero)
        hx = rMat[0][0] * mx + rMat[0][1] * my + rMat[0][2] * mz;
        hy = rMat[1][0] * mx + rMat[1][1] * my + rMat[1][2] * mz;
        bx = sqrtf(hx * hx + hy * hy);

        // magnetometer error is cross product between estimated magnetic north and measured magnetic north (calculated in EF)
        float ez_ef = -(hy * bx);

        // Rotate mag error vector back to BF and accumulate
        ex += rMat[2][0] * ez_ef;
        ey += rMat[2][1] * ez_ef;
        ez += rMat[2][2] * ez_ef;
    }

    // Use measured acceleration vector
    recipNorm = sq(ax) + sq(ay) + sq(az);
    if (useAcc && recipNorm > 0.01f) {
        // Normalise accelerometer measurement
        recipNorm = invSqrt(recipNorm);
        ax *= recipNorm;
        ay *= recipNorm;
        az *= recipNorm;

        // Error is sum of cross product between estimated direction and measured direction of gravity
        ex += (ay * rMat[2][2] - az * rMat[2][1]);
        ey += (az * rMat[2][0] - ax * rMat[2][2]);
        ez += (ax * rMat[2][1] - ay * rMat[2][0]);
    }

    // Compute and apply integral feedback if enabled
    if(imuRuntimeConfig->dcm_ki > 0.0f) {
        // Stop integrating if spinning beyond the certain limit
        if (spin_rate < DEGREES_TO_RADIANS(SPIN_RATE_LIMIT)) {
            float dcmKiGain = imuRuntimeConfig->dcm_ki;
            integralFBx += dcmKiGain * ex * dt;    // integral error scaled by Ki
            integralFBy += dcmKiGain * ey * dt;
            integralFBz += dcmKiGain * ez * dt;
        }
    }
    else {
        integralFBx = 0.0f;    // prevent integral windup
        integralFBy = 0.0f;
        integralFBz = 0.0f;
    }

    // Calculate kP gain. If we are acquiring initial attitude (not armed and within 20 sec from powerup) scale the kP to converge faster
    float dcmKpGain = imuRuntimeConfig->dcm_kp * imuGetPGainScaleFactor();

    // Apply proportional and integral feedback
    gx += dcmKpGain * ex + integralFBx;
    gy += dcmKpGain * ey + integralFBy;
    gz += dcmKpGain * ez + integralFBz;

    // Integrate rate of change of quaternion
    gx *= (0.5f * dt);
    gy *= (0.5f * dt);
    gz *= (0.5f * dt);

    qa = q0;
    qb = q1;
    qc = q2;
    q0 += (-qb * gx - qc * gy - q3 * gz);
    q1 += (qa * gx + qc * gz - q3 * gy);
    q2 += (qa * gy - qb * gz + q3 * gx);
    q3 += (qa * gz + qb * gy - qc * gx);

    // Normalise quaternion
    recipNorm = invSqrt(sq(q0) + sq(q1) + sq(q2) + sq(q3));
    q0 *= recipNorm;
    q1 *= recipNorm;
    q2 *= recipNorm;
    q3 *= recipNorm;

    // Pre-compute rotation matrix from quaternion
    imuComputeRotationMatrix();
}
Example #25
0
double LogisticNormalLogPdf::df_dx(double mu, double sigma, double x)
{
    double y = fastlog(x / (1 - x));
    return (1/(1-x)) - (1/x) - (mu - y) / (sq(sigma) * (x - 1) * x);
}
Example #26
0
void do_mysql_creds(std::fstream &pf, std::unordered_map<std::string, std::string> &dbinfo)
{
	dbinfo["type"] = "mysql";
	bool prompt = true, serve = false, ssl = false, sock = false;
	std::string dbname, user, pass, host, server, port, sslca, sslcert, servestr, sslstr, socktest, sockstr;
	while(prompt){
		std::cout << "Enter the name of the database: ";
		std::getline(std::cin, dbname);
		dbinfo["mysql_db_name"] = dbname; 
		std::cout << "Enter the username: "******"mysql_user"] = user;
		std::cout << "Enter the password: "******"mysql_pass"] = pass;
		std::cout << "Do you use a custom socket?(y/n)";
		std::getline(std::cin, socktest);
		if(socktest == "y" || socktest == "yes" || socktest == "true"){
			std::cout << "Enter socket: ";
			std::getline(std::cin, sockstr);
			dbinfo["mysql_sock"] = sockstr;
			sock = true;
		}
		std::cout << "Is this hosted on a separate server?(y/n) ";
		std::getline(std::cin, servestr);
		if(servestr == "y" || servestr == "yes" || servestr == "true"){
			serve = true;
			dbinfo["mysql_external_server"] = "true";
		}
		if(serve){
			std::cout << "Enter server address: ";
			std::getline(std::cin, server);
			dbinfo["mysql_host"] = server;
			std::cout << "Enter port: ";
			std::getline(std::cin, port);
			dbinfo["mysql_port"] = port;
			std::cout << "Do you use ssl? (y/n) ";
			std::getline(std::cin, sslstr);
			if(sslstr == "y" || sslstr == "yes" || sslstr == "true"){
				ssl = true;
			}
			if(ssl){
				std::cout << "CA: ";
				std::getline(std::cin, sslca);
				dbinfo["mysql_sslca"] = sslca;
				std::cout << "Cert path: ";
				std::getline(std::cin, sslcert);
				dbinfo["mysql_cert_path"] = sslcert;
			}

		}
		std::cout << "Attempting login..." << std::endl;
		std::string connstr = utils::db::build_db_connstr(dbinfo, false);
		try{
			soci::session sq(connstr);
			prompt = false;
		} catch(soci::mysql_soci_error const &e){
			std::cout << e.what() << std::endl;
			prompt = true;
		} catch(soci::soci_error const &e){
			std::cout << e.what() << std::endl;
			prompt = true;
		}

	}
	std::cout << "Login OK!" << std::endl << "Storing Credentials";
	pf << "type:mysql" << std::endl << "mysql_db_name:" << dbname << std::endl << "mysql_user:"******"mysql_pass:"******"mysql_server:" << server << std::endl << "mysql_port:" << port;
		if(ssl){
			pf << "mysql_sslca:" << sslca << std::endl << "mysql_cert_path:" << sslcert;
		}
	}
	if(sock){
		pf << "mysql_sock:" << sockstr << std::endl;
	}
}
Example #27
0
 // Evaluation 
 double eval(const X::Vector& x) const {
     return sq(x[0]-Real(3.))+sq(x[1]-Real(2.));
 }
Example #28
0
/* calculate distance between two points */
static inline double ddist(dpoint_t p, dpoint_t q) {
  return sqrt(sq(p.x-q.x)+sq(p.y-q.y));
}
Example #29
0
void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
{
    switch (msg->msgid) {

    case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE:
    {
        // allow override of RC channel values for HIL
        // or for complete GCS control of switch position
        // and RC PWM values.
        if (msg->sysid != rover.g.sysid_my_gcs) {  // Only accept control from our gcs
            break;
        }

        uint32_t tnow = AP_HAL::millis();

        mavlink_rc_channels_override_t packet;
        mavlink_msg_rc_channels_override_decode(msg, &packet);

        RC_Channels::set_override(0, packet.chan1_raw, tnow);
        RC_Channels::set_override(1, packet.chan2_raw, tnow);
        RC_Channels::set_override(2, packet.chan3_raw, tnow);
        RC_Channels::set_override(3, packet.chan4_raw, tnow);
        RC_Channels::set_override(4, packet.chan5_raw, tnow);
        RC_Channels::set_override(5, packet.chan6_raw, tnow);
        RC_Channels::set_override(6, packet.chan7_raw, tnow);
        RC_Channels::set_override(7, packet.chan8_raw, tnow);

        break;
    }

    case MAVLINK_MSG_ID_MANUAL_CONTROL:
    {
        if (msg->sysid != rover.g.sysid_my_gcs) {  // Only accept control from our gcs
            break;
        }

        mavlink_manual_control_t packet;
        mavlink_msg_manual_control_decode(msg, &packet);

        if (packet.target != rover.g.sysid_this_mav) {
            break; // only accept control aimed at us
        }

        uint32_t tnow = AP_HAL::millis();
        
        const int16_t roll = (packet.y == INT16_MAX) ? 0 : rover.channel_steer->get_radio_min() + (rover.channel_steer->get_radio_max() - rover.channel_steer->get_radio_min()) * (packet.y + 1000) / 2000.0f;
        const int16_t throttle = (packet.z == INT16_MAX) ? 0 : rover.channel_throttle->get_radio_min() + (rover.channel_throttle->get_radio_max() - rover.channel_throttle->get_radio_min()) * (packet.z + 1000) / 2000.0f;
        RC_Channels::set_override(uint8_t(rover.rcmap.roll() - 1), roll, tnow);
        RC_Channels::set_override(uint8_t(rover.rcmap.throttle() - 1), throttle, tnow);

        break;
    }

    case MAVLINK_MSG_ID_HEARTBEAT:
        {
            // We keep track of the last time we received a heartbeat from our GCS for failsafe purposes
            if (msg->sysid != rover.g.sysid_my_gcs) {
                break;
            }

            rover.last_heartbeat_ms = AP_HAL::millis();
            rover.failsafe_trigger(FAILSAFE_EVENT_GCS, false);
            break;
        }

    case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET:   // MAV ID: 82
        {
            // decode packet
            mavlink_set_attitude_target_t packet;
            mavlink_msg_set_attitude_target_decode(msg, &packet);

            // exit if vehicle is not in Guided mode
            if (rover.control_mode != &rover.mode_guided) {
                break;
            }

            // ensure type_mask specifies to use thrust
            if ((packet.type_mask & MAVLINK_SET_ATT_TYPE_MASK_THROTTLE_IGNORE) != 0) {
                break;
            }

            // convert thrust to ground speed
            packet.thrust = constrain_float(packet.thrust, -1.0f, 1.0f);
            const float target_speed = rover.control_mode->get_speed_default() * packet.thrust;

            // if the body_yaw_rate field is ignored, convert quaternion to heading
            if ((packet.type_mask & MAVLINK_SET_ATT_TYPE_MASK_YAW_RATE_IGNORE) != 0) {
                // convert quaternion to heading
                float target_heading_cd = degrees(Quaternion(packet.q[0], packet.q[1], packet.q[2], packet.q[3]).get_euler_yaw()) * 100.0f;
                rover.mode_guided.set_desired_heading_and_speed(target_heading_cd, target_speed);
            } else {
                // use body_yaw_rate field
                rover.mode_guided.set_desired_turn_rate_and_speed((RAD_TO_DEG * packet.body_yaw_rate) * 100.0f, target_speed);
            }
            break;
        }

    case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED:     // MAV ID: 84
        {
            // decode packet
            mavlink_set_position_target_local_ned_t packet;
            mavlink_msg_set_position_target_local_ned_decode(msg, &packet);

            // exit if vehicle is not in Guided mode
            if (rover.control_mode != &rover.mode_guided) {
                break;
            }

            // check for supported coordinate frames
            if (packet.coordinate_frame != MAV_FRAME_LOCAL_NED &&
                packet.coordinate_frame != MAV_FRAME_LOCAL_OFFSET_NED &&
                packet.coordinate_frame != MAV_FRAME_BODY_NED &&
                packet.coordinate_frame != MAV_FRAME_BODY_OFFSET_NED) {
                break;
            }

            bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE;
            bool vel_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE;
            bool acc_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE;
            bool yaw_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE;
            bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE;

            // prepare target position
            Location target_loc = rover.current_loc;
            if (!pos_ignore) {
                switch (packet.coordinate_frame) {
                case MAV_FRAME_BODY_NED:
                case MAV_FRAME_BODY_OFFSET_NED: {
                    // rotate from body-frame to NE frame
                    const float ne_x = packet.x * rover.ahrs.cos_yaw() - packet.y * rover.ahrs.sin_yaw();
                    const float ne_y = packet.x * rover.ahrs.sin_yaw() + packet.y * rover.ahrs.cos_yaw();
                    // add offset to current location
                    location_offset(target_loc, ne_x, ne_y);
                    }
                    break;

                case MAV_FRAME_LOCAL_OFFSET_NED:
                    // add offset to current location
                    location_offset(target_loc, packet.x, packet.y);
                    break;

                default:
                    // MAV_FRAME_LOCAL_NED interpret as an offset from home
                    target_loc = rover.ahrs.get_home();
                    location_offset(target_loc, packet.x, packet.y);
                    break;
                }
            }

            float target_speed = 0.0f;
            float target_yaw_cd = 0.0f;

            // consume velocity and convert to target speed and heading
            if (!vel_ignore) {
                const float speed_max = rover.control_mode->get_speed_default();
                // convert vector length into a speed
                target_speed = constrain_float(safe_sqrt(sq(packet.vx) + sq(packet.vy)), -speed_max, speed_max);
                // convert vector direction to target yaw
                target_yaw_cd = degrees(atan2f(packet.vy, packet.vx)) * 100.0f;

                // rotate target yaw if provided in body-frame
                if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
                    target_yaw_cd = wrap_180_cd(target_yaw_cd + rover.ahrs.yaw_sensor);
                }
            }

            // consume yaw heading
            if (!yaw_ignore) {
                target_yaw_cd = ToDeg(packet.yaw) * 100.0f;
                // rotate target yaw if provided in body-frame
                if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
                    target_yaw_cd = wrap_180_cd(target_yaw_cd + rover.ahrs.yaw_sensor);
                }
            }
            // consume yaw rate
            float target_turn_rate_cds = 0.0f;
            if (!yaw_rate_ignore) {
                target_turn_rate_cds = ToDeg(packet.yaw_rate) * 100.0f;
            }

            // handling case when both velocity and either yaw or yaw-rate are provided
            // by default, we consider that the rover will drive forward
            float speed_dir = 1.0f;
            if (!vel_ignore && (!yaw_ignore || !yaw_rate_ignore)) {
                // Note: we are using the x-axis velocity to determine direction even though
                // the frame may have been provided in MAV_FRAME_LOCAL_OFFSET_NED or MAV_FRAME_LOCAL_NED
                if (is_negative(packet.vx)) {
                    speed_dir = -1.0f;
                }
            }

            // set guided mode targets
            if (!pos_ignore) {
                // consume position target
                rover.mode_guided.set_desired_location(target_loc);
            } else if (pos_ignore && !vel_ignore && acc_ignore && yaw_ignore && yaw_rate_ignore) {
                // consume velocity
                rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, speed_dir * target_speed);
            } else if (pos_ignore && !vel_ignore && acc_ignore && yaw_ignore && !yaw_rate_ignore) {
                // consume velocity and turn rate
                rover.mode_guided.set_desired_turn_rate_and_speed(target_turn_rate_cds, speed_dir * target_speed);
            } else if (pos_ignore && !vel_ignore && acc_ignore && !yaw_ignore && yaw_rate_ignore) {
                // consume velocity
                rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, speed_dir * target_speed);
            } else if (pos_ignore && vel_ignore && acc_ignore && !yaw_ignore && yaw_rate_ignore) {
                // consume just target heading (probably only skid steering vehicles can do this)
                rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, 0.0f);
            } else if (pos_ignore && vel_ignore && acc_ignore && yaw_ignore && !yaw_rate_ignore) {
                // consume just turn rate(probably only skid steering vehicles can do this)
                rover.mode_guided.set_desired_turn_rate_and_speed(target_turn_rate_cds, 0.0f);
            }
            break;
        }

    case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT:    // MAV ID: 86
        {
            // decode packet
            mavlink_set_position_target_global_int_t packet;
            mavlink_msg_set_position_target_global_int_decode(msg, &packet);

            // exit if vehicle is not in Guided mode
            if (rover.control_mode != &rover.mode_guided) {
                break;
            }
            // check for supported coordinate frames
            if (packet.coordinate_frame != MAV_FRAME_GLOBAL &&
                packet.coordinate_frame != MAV_FRAME_GLOBAL_INT &&
                packet.coordinate_frame != MAV_FRAME_GLOBAL_RELATIVE_ALT &&
                packet.coordinate_frame != MAV_FRAME_GLOBAL_RELATIVE_ALT_INT &&
                packet.coordinate_frame != MAV_FRAME_GLOBAL_TERRAIN_ALT &&
                packet.coordinate_frame != MAV_FRAME_GLOBAL_TERRAIN_ALT_INT) {
                break;
            }
            bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE;
            bool vel_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE;
            bool acc_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE;
            bool yaw_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE;
            bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE;

            // prepare target position
            Location target_loc = rover.current_loc;
            if (!pos_ignore) {
                // sanity check location
                if (!check_latlng(packet.lat_int, packet.lon_int)) {
                    // result = MAV_RESULT_FAILED;
                    break;
                }
                target_loc.lat = packet.lat_int;
                target_loc.lng = packet.lon_int;
            }

            float target_speed = 0.0f;
            float target_yaw_cd = 0.0f;

            // consume velocity and convert to target speed and heading
            if (!vel_ignore) {
                const float speed_max = rover.control_mode->get_speed_default();
                // convert vector length into a speed
                target_speed = constrain_float(safe_sqrt(sq(packet.vx) + sq(packet.vy)), -speed_max, speed_max);
                // convert vector direction to target yaw
                target_yaw_cd = degrees(atan2f(packet.vy, packet.vx)) * 100.0f;

                // rotate target yaw if provided in body-frame
                if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
                    target_yaw_cd = wrap_180_cd(target_yaw_cd + rover.ahrs.yaw_sensor);
                }
            }

            // consume yaw heading
            if (!yaw_ignore) {
                target_yaw_cd = ToDeg(packet.yaw) * 100.0f;
                // rotate target yaw if provided in body-frame
                if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
                    target_yaw_cd = wrap_180_cd(target_yaw_cd + rover.ahrs.yaw_sensor);
                }
            }
            // consume yaw rate
            float target_turn_rate_cds = 0.0f;
            if (!yaw_rate_ignore) {
                target_turn_rate_cds = ToDeg(packet.yaw_rate) * 100.0f;
            }

            // handling case when both velocity and either yaw or yaw-rate are provided
            // by default, we consider that the rover will drive forward
            float speed_dir = 1.0f;
            if (!vel_ignore && (!yaw_ignore || !yaw_rate_ignore)) {
                // Note: we are using the x-axis velocity to determine direction even though
                // the frame is provided in MAV_FRAME_GLOBAL_xxx
                if (is_negative(packet.vx)) {
                    speed_dir = -1.0f;
                }
            }

            // set guided mode targets
            if (!pos_ignore) {
                // consume position target
                rover.mode_guided.set_desired_location(target_loc);
            } else if (pos_ignore && !vel_ignore && acc_ignore && yaw_ignore && yaw_rate_ignore) {
                // consume velocity
                rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, speed_dir * target_speed);
            } else if (pos_ignore && !vel_ignore && acc_ignore && yaw_ignore && !yaw_rate_ignore) {
                // consume velocity and turn rate
                rover.mode_guided.set_desired_turn_rate_and_speed(target_turn_rate_cds, speed_dir * target_speed);
            } else if (pos_ignore && !vel_ignore && acc_ignore && !yaw_ignore && yaw_rate_ignore) {
                // consume velocity
                rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, speed_dir * target_speed);
            } else if (pos_ignore && vel_ignore && acc_ignore && !yaw_ignore && yaw_rate_ignore) {
                // consume just target heading (probably only skid steering vehicles can do this)
                rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, 0.0f);
            } else if (pos_ignore && vel_ignore && acc_ignore && yaw_ignore && !yaw_rate_ignore) {
                // consume just turn rate(probably only skid steering vehicles can do this)
                rover.mode_guided.set_desired_turn_rate_and_speed(target_turn_rate_cds, 0.0f);
            }
            break;
        }

#if HIL_MODE != HIL_MODE_DISABLED
    case MAVLINK_MSG_ID_HIL_STATE:
        {
            mavlink_hil_state_t packet;
            mavlink_msg_hil_state_decode(msg, &packet);

            // sanity check location
            if (!check_latlng(packet.lat, packet.lon)) {
                break;
            }

            // set gps hil sensor
            Location loc;
            loc.lat = packet.lat;
            loc.lng = packet.lon;
            loc.alt = packet.alt/10;
            Vector3f vel(packet.vx, packet.vy, packet.vz);
            vel *= 0.01f;

            gps.setHIL(0, AP_GPS::GPS_OK_FIX_3D,
                       packet.time_usec/1000,
                       loc, vel, 10, 0);

            // rad/sec
            Vector3f gyros;
            gyros.x = packet.rollspeed;
            gyros.y = packet.pitchspeed;
            gyros.z = packet.yawspeed;

            // m/s/s
            Vector3f accels;
            accels.x = packet.xacc * (GRAVITY_MSS/1000.0f);
            accels.y = packet.yacc * (GRAVITY_MSS/1000.0f);
            accels.z = packet.zacc * (GRAVITY_MSS/1000.0f);

            ins.set_gyro(0, gyros);

            ins.set_accel(0, accels);
            compass.setHIL(0, packet.roll, packet.pitch, packet.yaw);
            compass.setHIL(1, packet.roll, packet.pitch, packet.yaw);
            break;
        }
#endif  // HIL_MODE

#if MOUNT == ENABLED
    // deprecated. Use MAV_CMD_DO_MOUNT_CONFIGURE
    case MAVLINK_MSG_ID_MOUNT_CONFIGURE:
        {
            rover.camera_mount.configure_msg(msg);
            break;
        }

    // deprecated. Use MAV_CMD_DO_MOUNT_CONTROL
    case MAVLINK_MSG_ID_MOUNT_CONTROL:
        {
            rover.camera_mount.control_msg(msg);
            break;
        }
#endif  // MOUNT == ENABLED

    case MAVLINK_MSG_ID_RADIO:
    case MAVLINK_MSG_ID_RADIO_STATUS:
        {
            handle_radio_status(msg, rover.DataFlash, rover.should_log(MASK_LOG_PM));
            break;
        }

    // send or receive fence points with GCS
    case MAVLINK_MSG_ID_FENCE_POINT:  // MAV ID: 160
    case MAVLINK_MSG_ID_FENCE_FETCH_POINT:
        rover.g2.fence.handle_msg(*this, msg);
        break;

    case MAVLINK_MSG_ID_DISTANCE_SENSOR:
        rover.rangefinder.handle_msg(msg);
        rover.g2.proximity.handle_msg(msg);
        break;

    default:
        handle_common_message(msg);
        break;
    }  // end switch
}  // end handle mavlink
Example #30
0
static int adjust_vertices(privpath_t *pp) {
  int m = pp->m;
  int *po = pp->po;
  int n = pp->len;
  point_t *pt = pp->pt;
  int x0 = pp->x0;
  int y0 = pp->y0;

  dpoint_t *ctr = NULL;      /* ctr[m] */
  dpoint_t *dir = NULL;      /* dir[m] */
  quadform_t *q = NULL;      /* q[m] */
  double v[3];
  double d;
  int i, j, k, l;
  dpoint_t s;
  int r;

  SAFE_MALLOC(ctr, m, dpoint_t);
  SAFE_MALLOC(dir, m, dpoint_t);
  SAFE_MALLOC(q, m, quadform_t);

  r = privcurve_init(&pp->curve, m);
  if (r) {
    goto malloc_error;
  }
  
  /* calculate "optimal" point-slope representation for each line
     segment */
  for (i=0; i<m; i++) {
    j = po[mod(i+1,m)];
    j = mod(j-po[i],n)+po[i];
    pointslope(pp, po[i], j, &ctr[i], &dir[i]);
  }

  /* represent each line segment as a singular quadratic form; the
     distance of a point (x,y) from the line segment will be
     (x,y,1)Q(x,y,1)^t, where Q=q[i]. */
  for (i=0; i<m; i++) {
    d = sq(dir[i].x) + sq(dir[i].y);
    if (d == 0.0) {
      for (j=0; j<3; j++) {
	for (k=0; k<3; k++) {
	  q[i][j][k] = 0;
	}
      }
    } else {
      v[0] = dir[i].y;
      v[1] = -dir[i].x;
      v[2] = - v[1] * ctr[i].y - v[0] * ctr[i].x;
      for (l=0; l<3; l++) {
	for (k=0; k<3; k++) {
	  q[i][l][k] = v[l] * v[k] / d;
	}
      }
    }
  }

  /* now calculate the "intersections" of consecutive segments.
     Instead of using the actual intersection, we find the point
     within a given unit square which minimizes the square distance to
     the two lines. */
  for (i=0; i<m; i++) {
    quadform_t Q;
    dpoint_t w;
    double dx, dy;
    double det;
    double min, cand; /* minimum and candidate for minimum of quad. form */
    double xmin, ymin;	/* coordinates of minimum */
    int z;

    /* let s be the vertex, in coordinates relative to x0/y0 */
    s.x = pt[po[i]].x-x0;
    s.y = pt[po[i]].y-y0;

    /* intersect segments i-1 and i */

    j = mod(i-1,m);

    /* add quadratic forms */
    for (l=0; l<3; l++) {
      for (k=0; k<3; k++) {
	Q[l][k] = q[j][l][k] + q[i][l][k];
      }
    }
    
    while(1) {
      /* minimize the quadratic form Q on the unit square */
      /* find intersection */

#ifdef HAVE_GCC_LOOP_BUG
      /* work around gcc bug #12243 */
      free(NULL);
#endif
      
      det = Q[0][0]*Q[1][1] - Q[0][1]*Q[1][0];
      if (det != 0.0) {
	w.x = (-Q[0][2]*Q[1][1] + Q[1][2]*Q[0][1]) / det;
	w.y = ( Q[0][2]*Q[1][0] - Q[1][2]*Q[0][0]) / det;
	break;
      }

      /* matrix is singular - lines are parallel. Add another,
	 orthogonal axis, through the center of the unit square */
      if (Q[0][0]>Q[1][1]) {
	v[0] = -Q[0][1];
	v[1] = Q[0][0];
      } else if (Q[1][1]) {
	v[0] = -Q[1][1];
	v[1] = Q[1][0];
      } else {
	v[0] = 1;
	v[1] = 0;
      }
      d = sq(v[0]) + sq(v[1]);
      v[2] = - v[1] * s.y - v[0] * s.x;
      for (l=0; l<3; l++) {
	for (k=0; k<3; k++) {
	  Q[l][k] += v[l] * v[k] / d;
	}
      }
    }
    dx = fabs(w.x-s.x);
    dy = fabs(w.y-s.y);
    if (dx <= .5 && dy <= .5) {
      pp->curve.vertex[i].x = w.x+x0;
      pp->curve.vertex[i].y = w.y+y0;
      continue;
    }

    /* the minimum was not in the unit square; now minimize quadratic
       on boundary of square */
    min = quadform(Q, s);
    xmin = s.x;
    ymin = s.y;

    if (Q[0][0] == 0.0) {
      goto fixx;
    }
    for (z=0; z<2; z++) {   /* value of the y-coordinate */
      w.y = s.y-0.5+z;
      w.x = - (Q[0][1] * w.y + Q[0][2]) / Q[0][0];
      dx = fabs(w.x-s.x);
      cand = quadform(Q, w);
      if (dx <= .5 && cand < min) {
	min = cand;
	xmin = w.x;
	ymin = w.y;
      }
    }
  fixx:
    if (Q[1][1] == 0.0) {
      goto corners;
    }
    for (z=0; z<2; z++) {   /* value of the x-coordinate */
      w.x = s.x-0.5+z;
      w.y = - (Q[1][0] * w.x + Q[1][2]) / Q[1][1];
      dy = fabs(w.y-s.y);
      cand = quadform(Q, w);
      if (dy <= .5 && cand < min) {
	min = cand;
	xmin = w.x;
	ymin = w.y;
      }
    }
  corners:
    /* check four corners */
    for (l=0; l<2; l++) {
      for (k=0; k<2; k++) {
	w.x = s.x-0.5+l;
	w.y = s.y-0.5+k;
	cand = quadform(Q, w);
	if (cand < min) {
	  min = cand;
	  xmin = w.x;
	  ymin = w.y;
	}
      }
    }

    pp->curve.vertex[i].x = xmin + x0;
    pp->curve.vertex[i].y = ymin + y0;
    continue;
  }

  free(ctr);
  free(dir);
  free(q);
  return 0;

 malloc_error:
  free(ctr);
  free(dir);
  free(q);
  return 1;
}