Witam.
Ostatnio zaczął mi wyskakiwać błąd taki jak w temacie. Tą bibliotekę napisałem ponad rok temu i przez ten czas wszystko działało jednak po wgraniu nowych ustawień w arduino (MiniCore) pojawił mi się ten błąd.
In file included from C:\Documents and Settings\Administrator\Pulpit\AVR_PID2\AVR_PID2.ino:1:0:
sketch\Quaternion\Quaternion.h:205:42: error: ISO C++ forbids declaration of 'GetYawPitchRoll' with no type [-fpermissive]
GetYawPitchRoll(float *data, Quaternion q) {
GetYawPitchRoll(float *data, Quaternion &q) {
float &w = q.w, &x = q.x, &y = q.y, &z = q.z;
float vx = 2 * (x * z - w * y);
float vy = 2 * (w * x + y * z);
float vz = w * w - x * x - y * y + z * z;
data[0] = atan2(2 * (x*y - w*z), 2 * (w*w + x*x) - 1); // yaw: (about Z axis)
data[1] = atan( vx / sqrt( vy * vy + vz * vz )); // pitch: (nose up/down, about Y axis)
data[2] = atan( vy / sqrt( vx * vx + vz * vz )); // roll: (tilt left/right, about X axis)
return 0;
}
plik quaternion.h:
// #ifndef _QUATERNION
// #define _QUATERNION
// #include "Euler.h"
// #include "AngularRates.h"
// https://pizer.wordpress.com/2008/10/12/fast-inverse-square-root/
float InvSqrt(float x){ // -0.001735 ... 0
float xhalf = 0.5f*x;
int i = *(int*)&x; // get bits for floating value
i = 0x5f3759df - (i>>1); // gives initial guess y0
x = *(float*)&i; // convert bits back to float
x = x*(1.5f-xhalf*x*x); // Newton step, repeating increases accuracy
return x;
}
// https://pizer.wordpress.com/2008/10/12/fast-inverse-square-root/
float InvSqrt2(float x){ // -0.00065 ... 0.00065
uint32_t i = 0x5F1F1412 - (*(uint32_t*)&x >> 1);
float tmp = *(float*)&i;
return tmp * (1.69000231f - 0.714158168f * x * tmp * tmp);
}
// http://allenchou.net/2014/02/game-math-fast-re-normalization-of-unit-vectors/
inline float FastSqrtInvAroundOne(float x)
{
const float a0 = 15.0f / 8.0f;
const float a1 = -5.0f / 4.0f;
const float a2 = 3.0f / 8.0f;
return a0 + a1 * x + a2 * x * x;
}
class Quaternion
{
public:
float w;
float x;
float y;
float z;
Quaternion() {
w = 1.0f;
x = 0.0f;
y = 0.0f;
z = 0.0f;
}
Quaternion(float nw, float nx, float ny, float nz) {
w = nw;
x = nx;
y = ny;
z = nz;
}
Quaternion getProduct(Quaternion q) {
return Quaternion(
w * q.w - x * q.x - y * q.y - z * q.z, // new w
w * q.x + x * q.w + y * q.z - z * q.y, // new x
w * q.y - x * q.z + y * q.w + z * q.x, // new y
w * q.z + x * q.y - y * q.x + z * q.w); // new z
}
Quaternion getConjugate() {
return Quaternion(w, -x, -y, -z);
}
float getMagnitude() {
return sqrt(w * w + x * x + y * y + z * z);
}
float sqSum(){
return w * w + x * x + y * y + z * z;
}
void normalize() {
// return normalize1();
return normalize2();
// return normalize3();
// return normalize4();
}
void normalize1() {
float m = getMagnitude();
w /= m;
x /= m;
y /= m;
z /= m;
}
void normalize2() {
float m = 1.0/getMagnitude();
w *= m;
x *= m;
y *= m;
z *= m;
}
void normalize3() {
float m = InvSqrt2(sqSum());
w *= m;
x *= m;
y *= m;
z *= m;
}
void normalize4() {
//https://github.com/schteppe/cannon.js/issues/22
float m = (3.0-sqSum())/2.0;
w *= m;
x *= m;
y *= m;
z *= m;
}
Quaternion getNormalized() {
Quaternion r(w, x, y, z);
r.normalize();
return r;
}
void ApplyGyroRates(int32_t gx, int32_t gy, int32_t gz, float Gyro2Rad) {
// https://www.mathworks.com/help/aeroblks/customvariablemass6dofquaternion.html
// http://www.euclideanspace.com/physics/kinematics/angularvelocity/
float dx = gx * Gyro2Rad ;
float dy = gy * Gyro2Rad ;
float dz = gz * Gyro2Rad ;
// this->normalize();
float qw = w, qx = x, qy = y, qz = z; // temp quaternion
w += qw* ( 0) + qx * (-dx) + qy * (-dy) + qz * (-dz);
x += qw* (dx) + qx * ( 0) + qy * ( dz) + qz * (-dy);
y += qw* (dy) + qx * (-dz) + qy * ( 0) + qz * ( dx);
z += qw* (dz) + qx * ( dy) + qy * (-dx) + qz * ( 0);
this->normalize();
}
// transform quaternion 'this' by a quaternion 't'
Quaternion transform(Quaternion t) { // q' = t*q
float qw=w,qx=x,qy=y,qz=z;
w = qw * t.w - qx * t.x - qy * t.y - qz * t.z;
x = qw * t.x + qx * t.w + qy * t.z - qz * t.y;
y = qw * t.y - qx * t.z + qy * t.w + qz * t.x;
z = qw * t.z + qx * t.y - qy * t.x + qz * t.w;
}
// convert gyro data (radians) into quaternion
Quaternion AngularRate2Quaternion(float *w) {
// https://github.com/jerabaul29/IntegrateGyroData/blob/master/ComputerSide/Quaternions.py
float x,y,z,l,ll,lby2,sl2l,q0,q1,q2,q3;
x = w[0];
y = w[1];
z = w[2];
float s = x*x + y*y + z*z;
l = sqrt(s);
// l = 2.0 / ( 3.0-s ); // https://github.com/schteppe/cannon.js/issues/22
if (l<=0) return Quaternion(1,0,0,0);
//===========================================
// w not normalized
//===========================================
// lby2 = l/2.0f;
// sl2l = sin(lby2)/l;
// q0 = cos(lby2);
//===========================================
// Series expansion - w not normalized // TODO - check maximum angles
//===========================================
static const float oneby48 = 1.0f/48.0f;
sl2l = oneby48 * (24.0f-l); // sin(l/2)/l
q0 = 1.0f-s/2.0f; // cos(l/2) // slower than cos(lby2)?
//===========================================
q1 = sl2l * x;
q2 = sl2l * y;
q3 = sl2l * z;
return Quaternion(q0,q1,q2,q3);
}
// update quaternion by gyro data
void ApplyGyroRates2(float gx,float gy,float gz){ // this' = (q)[gyroRad]*this
// ApplyGyroRates4(gx,gy,gz);
// return;
// quaternions-cs520.pdf - Rotation to Unit Quaternion
// this->normalize(); // TODO: czy kwaternion musi być normalizowany w tego typu obliczeniach?
float v[3]={gx,gy,gz};
Quaternion t = AngularRate2Quaternion(v); // kwaternion transformacji
transform(t); // q' = t*q
// this->normalize();
}
};
void GetGravityVector(Quaternion *q,float *data) {
float &w = q->w, &x = q->x, &y = q->y, &z = q->z;
data[0] = 2 * (x * z - w * y);
data[1] = 2 * (w * x + y * z);
data[2] = w * w - x * x - y * y + z * z;
}
//mpu.dmpGetGravity(&gravity, &q);
//mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
GetYawPitchRoll(float *data, Quaternion &q) {
float &w = q.w, &x = q.x, &y = q.y, &z = q.z;
float vx = 2 * (x * z - w * y);
float vy = 2 * (w * x + y * z);
float vz = w * w - x * x - y * y + z * z;
data[0] = atan2(2 * (x*y - w*z), 2 * (w*w + x*x) - 1); // yaw: (about Z axis)
data[1] = atan( vx / sqrt( vy * vy + vz * vz )); // pitch: (nose up/down, about Y axis)
data[2] = atan( vy / sqrt( vx * vx + vz * vz )); // roll: (tilt left/right, about X axis)
return 0;
}
// przekształca kwaternion na prędkości kątowe x,y,z
void q2w2(Quaternion q,float *data,bool deg) {
// ;http://www.euclideanspace.com/physics/kinematics/angularvelocity/index.htm
// ;W(t) = 2 * dq(t) /dt*conj(q(t))
// ;https://www.mathworks.com/help/aeroblks/rotationanglestoquaternions.html
// ;Rotation Angles to Quaternions
float w=q.w,x=q.x,y=q.y,z=q.z;
if (w<0){
w*=-1.0;
// x*=-1.0;
// y*=-1.0;
// z*=-1.0;
}
if (fabs(w)==1){ // ???
data[0] = 0;
data[1] = 0;
data[2] = 0;
return;
}
float m = ( acos(w)*2.0 )/sqrt(1-w*w);
if (deg)
m*= 180.0/M_PI;
data[0] = x*m;
data[1] = y*m;
data[2] = z*m;
}
///////////////////////////////
// Quaternion to Euler
///////////////////////////////
enum RotSeq {zyx, zyz, zxy, zxz, yxz, yxy, yzx, yzy, xyz, xyx, xzy, xzx};
void twoaxisrot(double r11, double r12, double r21, double r31, double r32, double res[]) {
res[0] = atan2( r11, r12 );
res[1] = acos ( r21 );
res[2] = atan2( r31, r32 );
}
void threeaxisrot(double r11, double r12, double r21, double r31, double r32, double res[]) {
res[0] = atan2( r31, r32 );
res[1] = asin ( r21 );
res[2] = atan2( r11, r12 );
}
void quaternion2Euler(Quaternion q, double res[], RotSeq rotSeq) {
/*
https://www.mathworks.com/matlabcentral/fileexchange/
27653-euler-angle--dcm--quaternion--and-euler-vector-conversion-teaching-gui
http://bediyap.com/programming/convert-quaternion-to-euler-rotations/
*/
float &w = q.w, &x = q.x, &y = q.y, &z = q.z;
double &phi = res[0];
double &theta = res[1];
double &psi = res[2];
switch (rotSeq) {
case zyx: //321
psi = atan2(2*(x*y+w*z),(w*w+x*x-y*y-z*z));
theta = asin (2*(w*y-x*z));
phi = atan2(2*(w*x+z*y),(w*w-x*x-y*y+z*z));
break;
case zyz: //323
psi = atan2((y*z-w*x),(x*z+w*y));
theta = acos (w*w-x*x-y*y+z*z);
phi = atan2((w*x+y*z),(w*y-x*z));
break;
case zxy: //312
psi = atan2(2*(w*z-x*y),(w*w-x*x+y*y-z*z));
theta = asin (2*(w*x+y*z));
phi = atan2(2*(w*y-x*z),(w*w-x*x-y*y+z*z));
break;
case zxz: //313
psi = atan2((x*z+w*y),(w*x-y*z));
theta = acos (w*w-x*x-y*y+z*z);
phi = atan2((x*z-w*y),(w*x+y*z));
break;
case yxz: //213
psi = atan2(2*(x*z+w*y),(w*w-x*x-y*y+z*z));
theta = asin (2*(w*x-y*z));
phi = atan2(2*(x*y+w*z),(w*w-x*x+y*y-z*z));
break;
case yxy: //212
psi = atan2((x*y-w*z),(w*x+y*z));
theta = acos (w*w-x*x+y*y-z*z);
phi = atan2((x*y+w*z),(w*x-y*z));
break;
case yzx: //231
psi = atan2(2*(w*y-x*z),(w*w+x*x-y*y-z*z));
theta = asin (2*(x*y+w*z));
phi = atan2(2*(w*x-z*y),(w*w-x*x+y*y-z*z));
break;
case yzy: //232
psi = atan2((w*x+y*z),(w*z-x*y));
theta = acos(w*w-x*x+y*y-z*z);
phi = atan2((y*z-w*x),(x*y+w*z));
break;
case xyz: //123
psi = atan2(2*(w*x-y*z),(w*w-x*x-y*y+z*z));
theta = asin (2*(x*z+w*y));
phi = atan2(2*(w*z-x*y),(w*w+x*x-y*y-z*z));
break;
case xyx: //121
psi = atan2((x*y+w*z),(w*y-x*z));
theta = acos (w*w+x*x-y*y-z*z);
phi = atan2((x*y-w*z),(x*z+w*y));
break;
case xzy: //132
psi = atan2(2*(w*x+y*z),(w*w-x*x+y*y-z*z));
theta = asin (2*(w*z-x*y));
phi = atan2(2*(x*z+w*y),(w*w+x*x-y*y-z*z));
break;
case xzx: //131
psi = atan2((x*z-w*y),(x*y+w*z));
theta = acos (w*w+x*x-y*y-z*z);
phi = atan2((x*z+w*y),(w*z-x*y));
break;
}
}
// #endif //_QUATERNION