Błąd kompilacji - ISO C++ forbids declaration of 'GetYawPitchRoll' with no type [-fpermissive]

0

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
3

Funkcja GetYawPitchRoll(float *data, Quaternion &q) musi mieć określony zwracany typ.
U Ciebie zwraca zero, więc deklaracja powinna wyglądać tak:
int GetYawPitchRoll(float *data, Quaternion &q)

0

Tam raczej powinien być void i bez return'a i już działa. Dzięki za pomoc.

0

@smarq: Pytanie, dlaczego Ci działało do tej pory... :)

1 użytkowników online, w tym zalogowanych: 0, gości: 1