Skip to content

Commit 66a83a7

Browse files
xieyixieyi
xieyi
authored and
xieyi
committed
modified: inc/GlobalXYZ.h
 modified: inc/Vector.h modified: src/GlobalXYZ.cpp modified: src/Motor.cpp modified: src/main.cpp
1 parent 889806a commit 66a83a7

File tree

5 files changed

+122
-2
lines changed

5 files changed

+122
-2
lines changed

‎inc/GlobalXYZ.h

+7
Original file line numberDiff line numberDiff line change
@@ -55,6 +55,13 @@ class GlobalXYZ {
5555
*/
5656
//获得飞机的滚动角，俯仰角，偏航角
5757
static Matrix<double> getRPY(double & roll,double & pitch,double & yaw);
58+
/**
59+
* Get the quaternion representation of the rotation
60+
*
61+
* @return the quaterion vector.
62+
*/
63+
//获得四元数表示的旋转向量
64+
static Vector<double> getQuaternion();
5865
};
5966

6067
#endif

‎inc/Vector.h

+34
Original file line numberDiff line numberDiff line change
@@ -52,6 +52,10 @@ class Vector {
5252
* @param b the other vector whose value will be copied.
5353
*/
5454
Vector<T> & operator=(const Vector<T> & b);
55+
/**
56+
* The negative of the vector.
57+
*/
58+
Vector<T> operator-();
5559

5660
/**
5761
* The inner product of two vectors.
@@ -73,6 +77,10 @@ class Vector {
7377
* Scale a vector.
7478
*/
7579
template<typename T1> friend Vector<T1> operator*(const Vector<T1> & a,double s);
80+
/**
81+
* Scale a vector.
82+
*/
83+
template<typename T1> friend Vector<T1> & operator*=(Vector<T1> & a,double s);
7684
/**
7785
* Scale a vector.
7886
*/
@@ -81,6 +89,10 @@ class Vector {
8189
* Scale a vector.
8290
*/
8391
template<typename T1> friend Vector<T1> operator/(const Vector<T1> & a,double s);
92+
/**
93+
* Scale a vector.
94+
*/
95+
template<typename T1> friend Vector<T1> & operator/=(Vector<T1> & a,double s);
8496
};
8597

8698
template<typename T>
@@ -140,6 +152,14 @@ Vector<T> & Vector<T>::operator=(const Vector<T> & b)
140152
return *this;
141153
}
142154

155+
template<typename T>
156+
Vector<T> Vector<T>::operator-()
157+
{
158+
Vector<T> retVal(sz);
159+
for(int i = 0 ; i < sz ; i++) retVal(i) = -data[i];
160+
return retVal;
161+
}
162+
143163
template<typename T1,typename T2>
144164
T1 inner_prod(const Vector<T1> & a,const Vector<T2> & b)
145165
{
@@ -203,6 +223,13 @@ Vector<T1> operator*(const Vector<T1> & a,double s)
203223
return retVal;
204224
}
205225

226+
template<typename T1>
227+
Vector<T1> & operator*=(Vector<T1> & a,double s)
228+
{
229+
for(int i = 0 ; i < a.sz ; i++) a(i) *= s;
230+
return a;
231+
}
232+
206233
template<typename T1>
207234
Vector<T1> operator*(double s,const Vector<T1> & a)
208235
{
@@ -217,4 +244,11 @@ Vector<T1> operator/(const Vector<T1> & a,double s)
217244
return retVal;
218245
}
219246

247+
template<typename T1>
248+
Vector<T1> & operator/=(Vector<T1> & a,double s)
249+
{
250+
for(int i = 0 ; i < a.sz ; i++) a(i) /= s;
251+
return a;
252+
}
253+
220254
#endif

‎src/GlobalXYZ.cpp

+42
Original file line numberDiff line numberDiff line change
@@ -127,3 +127,45 @@ Matrix<double> GlobalXYZ::getRPY(double & roll,double & pitch,double & yaw)
127127
yaw = atan2(DCM(1,0),DCM(0,0));
128128
return DCM;
129129
}
130+
131+
Vector<double> GlobalXYZ::getQuaternion()
132+
{
133+
Vector<double> quaternion(4);
134+
Vector<double> newX,newY,newZ;
135+
getXYZ(newX,newY,newZ);
136+
Matrix<double> DCM(3,3);
137+
//计算体坐标系->全局坐标系的转换矩阵
138+
DCM(0,0) = newX(0); DCM(0,1) = newX(1); DCM(0,2) = newX(2);
139+
DCM(1,0) = newY(0); DCM(1,1) = newY(1); DCM(1,2) = newY(2);
140+
DCM(2,0) = newZ(0); DCM(2,1) = newZ(1); DCM(2,2) = newZ(2);
141+
double trace = DCM(0,0) + DCM(1,1) + DCM(2,2);
142+
if(trace > 0) {
143+
double s = 0.5 / sqrt(trace + 1.0);
144+
quaternion(0) = 0.25 / s;
145+
quaternion(1) = (DCM(2,1) - DCM(1,2)) * s;
146+
quaternion(2) = (DCM(0,2) - DCM(2,0)) * s;
147+
quaternion(3) = (DCM(1,0) - DCM(0,1)) * s;
148+
} else {
149+
if(DCM(0,0) > DCM(1,1) && DCM(0,0) > DCM(2,2)) {
150+
double s = 2.0 * sqrt(1.0 + DCM(0,0) - DCM(1,1) - DCM(2,2));
151+
quaternion(0) = (DCM(2,1) - DCM(1,2)) / s;
152+
quaternion(1) = 0.25 * s;
153+
quaternion(2) = (DCM(0,1) + DCM(1,0)) / s;
154+
quaternion(3) = (DCM(0,2) + DCM(2,0)) / s;
155+
} else if(DCM(1,1) > DCM(2,2)) {
156+
double s = 2.0 * sqrt(1.0 + DCM(1,1) - DCM(0,0) - DCM(2,2));
157+
quaternion(0) = (DCM(0,2) - DCM(2,0)) / s;
158+
quaternion(1) = (DCM(0,1) - DCM(1,0)) / s;
159+
quaternion(2) = 0.25 * s;
160+
quaternion(3) = (DCM(1,2) + DCM(2,1)) / s;
161+
} else {
162+
double s = 2.0 * sqrt(1.0 + DCM(2,2) - DCM(0,0) - DCM(1,1));
163+
quaternion(0) = (DCM(1,0) - DCM(0,1)) / s;
164+
quaternion(1) = (DCM(0,2) + DCM(2,0)) / s;
165+
quaternion(2) = (DCM(1,2) + DCM(2,1)) / s;
166+
quaternion(3) = 0.25 * s;
167+
}
168+
}
169+
170+
return quaternion;
171+
}

‎src/Motor.cpp

+5
Original file line numberDiff line numberDiff line change
@@ -10,6 +10,11 @@ Motor Motor::motor4(D12);
1010
Motor::Motor(unsigned char p)
1111
:pin(p),data(0)
1212
{
13+
//原先写在这里的pinMode被移动到setup()函数中
14+
control1(0);
15+
control2(0);
16+
control3(0);
17+
control4(0);
1318
}
1419

1520
Motor::Motor(const Motor & motor)

‎src/main.cpp

+34-2
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,7 @@
1616
#include "wirish.h"
1717
#include "GlobalXYZ.h"
1818
#include "Pressure.h"
19+
#include "Motor.h"
1920

2021
#define PWM_PIN 2
2122

@@ -42,14 +43,30 @@ void setup()
4243
void loop()
4344
{
4445
while(1) {
46+
#if 1
47+
Vector<double> quaternion = GlobalXYZ::getQuaternion();
48+
for(int i = 0 ; i < quaternion.size() ; i++) {
49+
float n = quaternion(i);
50+
unsigned char * ptr = reinterpret_cast<unsigned char*>(&n);
51+
for(int i = 0 ; i < sizeof(float) ; i++) {
52+
unsigned char b1 = (ptr[i] >> 4) & 0x0f;
53+
unsigned char b2 = (ptr[i] & 0x0f);
54+
char c1 = (b1 < 10) ? ('0' + b1) : 'A' + b1 - 10;
55+
char c2 = (b2 < 10) ? ('0' + b2) : 'A' + b2 - 10;
56+
SerialUSB.print(c1);
57+
SerialUSB.print(c2);
58+
}
59+
SerialUSB.print(",");
60+
}
61+
#endif
4562
#if 0
4663
Vector<double> x,y,z;
4764
GlobalXYZ::getXYZ(x,y,z);
4865
SerialUSB.print("X = ("); SerialUSB.print(x(0)); SerialUSB.print(","); SerialUSB.print(x(1)); SerialUSB.print(","); SerialUSB.print(x(2)); SerialUSB.print(") ");
4966
SerialUSB.print("Y = ("); SerialUSB.print(y(0)); SerialUSB.print(","); SerialUSB.print(y(1)); SerialUSB.print(","); SerialUSB.print(y(2)); SerialUSB.print(") ");
5067
SerialUSB.print("Z = ("); SerialUSB.print(z(0)); SerialUSB.print(","); SerialUSB.print(z(1)); SerialUSB.print(","); SerialUSB.print(z(2)); SerialUSB.print(")");
5168
#endif
52-
#if 1
69+
#if 0
5370
double roll = 0,pitch = 0,yaw = 0;
5471
GlobalXYZ::getRPY(roll,pitch,yaw);
5572
roll *= 180 / 3.1415926; pitch *= 180 / 3.1415926; yaw *= 180 / 3.1415926;
@@ -60,6 +77,12 @@ void loop()
6077
SerialUSB.print("pressure = "); SerialUSB.print(pressure(0)); SerialUSB.print("pa\t");
6178
SerialUSB.print("temperature = "); SerialUSB.print(pressure(1) * 0.1); SerialUSB.print("C\t");
6279
SerialUSB.print("altitude = "); SerialUSB.print(pressure(2)); SerialUSB.print("m\t");
80+
toggleLED();
81+
static int level = 0;
82+
Motor::control1(500 + (level++) % 500);
83+
Motor::control2(500 + (100 + level++) % 500);
84+
Motor::control3(500 + (200 + level++) % 500);
85+
Motor::control4(500 + (300 + level++) % 500);
6386
#endif
6487
#if 0
6588
Vector<double> retVal = Accelerometer::getReading();
@@ -82,9 +105,18 @@ void loop()
82105
#if 0
83106
Compass::calibrate();
84107
#endif
108+
#if 0
109+
static int k = 0;
110+
k = (k >= 1000)?1000:k+1;
111+
Motor::control1(k);
112+
Motor::control2(k);
113+
Motor::control3(k);
114+
Motor::control4(k);
115+
delay(3000);
116+
#endif
85117

86118
SerialUSB.println();
87-
delay(100);
119+
delay(200);
88120
}
89121

90122
SerialUSB.println();

0 commit comments

Comments
 (0)