// 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);
}
/*
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);
}
}
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;
}
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;
}
/**
* 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();
}
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;
}
/* 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;
}
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;
}
}
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);
}
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);
}
float cov(const vector<float>& x) {
float ssd = sum(sq(add(-mean(x), x)));
return ssd / x.size();
}
double LogNormalLogPdf::df_dx(double mu, double sigma, double x)
{
return (mu - fastlog(x)) / (x * sq(sigma)) - 1.0 / x;
}
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;
}
// 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;
}
}
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;
}
double NormalLogPdf::df_dx(double mu, double sigma, const double x)
{
return (mu - x) / sq(sigma);
}
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);
}
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;
}
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();
}
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);
}
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;
}
}
// Evaluation
double eval(const X::Vector& x) const {
return sq(x[0]-Real(3.))+sq(x[1]-Real(2.));
}
/* 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));
}
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
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;
}