remi
05-08-09, 05:01 PM
Hi,
just a little words to present you my project using a ServoPod-USB. Its objective is to build an UAV (Unmanned Aerial Vehicule), more precisely a small quadrocopter (http://en.wikipedia.org/wiki/Quadrocopter). As this kind of vehicule is pretty unstable, it needs very fast responding and very accurate sensors to assist either the pilot or the auto-pilot.
One of the critical and difficult parts of this is the inertial measurement unit (IMU) whose role is to tell the position of the vehicule in space in terms of pitch, roll and yaw, and I decided to start with this part. I'm quite at an early stage of the project: I have an IMU that begins to work getting an attitude by merging information from two kind of sensors and reporting it.
Here (http://www.dailymotion.com/crashtest44/video/15425887) is a sample of the results I got. The top line represents the yaw, the middle 3 lines represent the pitch and the bottom 3 lines represent the roll.
For this IMU, I use a 3 axis ADXL 330 accelerometer (http://www.sparkfun.com/commerce/product_info.php?products_id=692) and two dual axis IDG-500 gyros (http://www.sparkfun.com/commerce/product_info.php?products_id=9094). The accelerometer allows to measure pitch and roll by comparing measures on 3 orthogonal axis to the gravity (see this explanation (http://tom.pycke.be/mav/69/accelerometer-to-attitude)) The gyros allow to measure angular rates for 3 axis (I don't use the 4th available). It's then possible to integrate these rates to determine the pitch, roll and yaw (see this explanation (http://tom.pycke.be/mav/70/gyroscope-to-roll-pitch-and-yaw)). These sensors are however not perfect: the accelerometer is influenced by the vehicule movements and the gyros are subject to errors introducing drift in the calculated angles (for the IDG-500 I use, this drift can be as large as 5 degrees/sec.). So the values measured from the accelerometer and the gyros have to be combined to determine the correct attitude of the vehicule.
There are several ways to merge these two kind of data. One popular way is Kalman filtering, but it relies on very hard mathematics with plenty of matrices and is known to be computer intensive. I've chosen another simpler approache: the complement filter. It is based on the remark that the accelerometers can give a reliable result at a low frequency (e.g. once per second) but not at an higher frequency and the gyros can give a reliable result at higher frequency, but, not at low frequency because of the drift phenomenom. Thus, a correct measure can be obtained by applying a low pass filter to the values read from the accelerometer (in red on my video sample), an high pass filter to the values read from the gyros (in light blue on my video sample) and summing them (in dark blue on my video sample).
Everything including filters is implemented in soft on the ServoPod, the sensors are directly connected to the analog inputs with no electronics. The IsoMax code is included below. It's my first "big" (other than the led switching examples) Isomax code, so feel free to tell me if there are mistakes or if I can improve it some ways. The display is done using a perl script generating postscript code (ok, I know that programming in postscript is a bit outdated, but it's quite close to Forth and Isomax so it makes me a good training :) ) and displayed using ghostscript.
Next, I wish to start making my proto more autonomous, by powering it with a battery instead of the USB port and, communicating to the ServoPod using an Xbee dongle (that's the issue of my question in another thread (http://www.newmicros.com/discussion/showthread.php?t=7805) ;) ). I will start to control the quadro four motors too, by using RC model brushless motors controllers that are controled using the same PWM signals as standard RC servos. I hope that these ESC will be 67 Hz tolerant as I read that the ServoPod cannot output 50 Hz PWM without setting down its clock halfspeed. If you have information on 67 Hz tolerant ESC, I'm interrested since I don't have ordered the ESCs yet.
Sincerely,
Remi
\ Complement filter ratio (gyro/accelero)
0.8e FCONSTANT COMPRATIO
1.0e COMPRATIO F- FCONSTANT 1-COMPRATIO
\ Low pass filter cutoff frequency for the ADXL330
0.05e FCONSTANT ACC:A \ approximatively 0.8 Hz for a 5000 PERIOD
1.0e ACC:A F- FCONSTANT ACC:1-A
\ Voltage reference for the ADXL-330 (VREF / 2)
1.65e FCONSTANT ACC:VREF
\ High pass filter cutoff frequency for the two IDG500
0.9999e FCONSTANT GY:HP:1-A \ approximatively 0.016 Hz for a 5000 PERIOD
\ IDG500 sensitivity
\ 1.5e FCONSTANT GY:SENS
0.00000025e FCONSTANT GY:SENS
\ Low pass filtered values computed from ADXL330 inputs
FVARIABLE ACC:XOUT 0.0e ACC:XOUT F!
FVARIABLE ACC:YOUT 0.0e ACC:YOUT F!
FVARIABLE ACC:ZOUT 0.0e ACC:ZOUT F!
\ Computed pitch and roll (from ADXL330 measures)
FVARIABLE ACC:PITCH
FVARIABLE ACC:ROLL
\ High pass filtered values of the angular rates
FVARIABLE GY1:XOUT 0.0e GY1:XOUT F!
FVARIABLE GY1:YOUT 0.0e GY1:YOUT F!
VARIABLE GY1:XMEM 0 GY1:XMEM !
VARIABLE GY1:YMEM 0 GY1:YMEM !
\ Integration (sum) of angular rates to provide angles
FVARIABLE GY1:XINT 0.0e GY1:XINT F!
FVARIABLE GY1:YINT 0.0e GY1:YINT F!
\ Complement filter :
\ GY1:XINTCOMP averages GY1:XOUT and ACC:PITCH
\ GY1:YINTCOMP averages GY1:YOUT and ACC:ROLL
FVARIABLE GY1:XINTCOMP 0.0e GY1:XINTCOMP F!
FVARIABLE GY1:YINTCOMP 0.0e GY1:YINTCOMP F!
\ 2nd (vertical) IDG500 : these values are the same as for GY1
FVARIABLE GY2:YOUT 0.0e GY2:YOUT F!
VARIABLE GY2:YMEM 0 GY2:YMEM !
FVARIABLE GY2:YINT 0.0e GY2:YINT F!
\ ADXL-330 sensitivity (divisor)
9930.0e FCONSTANT ACC:SENS
\ Low pass filter :
\
\ x = x0*a + x*(1-a)
\
\ High pass filter :
\
\ x = (x0 - x_mem) + x*(1-a)
\ x_mem = x0
\
\ Note: a = h * 2*pi * fc
\
\ fc: cut frequency
\ h: sample period (sec.) = IsoMax period / 5000000
\
\ sample + low pass filter for accelerometer x, y and z axis:
: ACC-SAMPLE
ADC0 ANALOGIN S>F ACC:SENS F/ ACC:A F* ACC:XOUT F@ ACC:1-A F* F+ ACC:XOUT F!
ADC2 ANALOGIN S>F ACC:SENS F/ ACC:A F* ACC:YOUT F@ ACC:1-A F* F+ ACC:YOUT F!
ADC4 ANALOGIN S>F ACC:SENS F/ ACC:A F* ACC:ZOUT F@ ACC:1-A F* F+ ACC:ZOUT F!
;
\ Accelerometer: pitch = atan2(x,z)
\ roll = atan2(y,z)
\
\ Complement filter:
\
\ intcomp[n+1] = intcomp[n] * compratio + angle * (1-compratio)
\ angle = angle computed from accelerometer
\
: COMPUTE-PITCH-ROLL
ACC:ZOUT F@ ACC:VREF F- FDUP
ACC:XOUT F@ ACC:VREF F- FSWAP FATAN2 FDUP ACC:PITCH F!
GY:SENS F/ 1-COMPRATIO F* GY1:XINTCOMP F@ COMPRATIO F* F+ GY1:XINTCOMP F!
ACC:YOUT F@ ACC:VREF F- FSWAP FATAN2 FDUP ACC:ROLL F!
GY:SENS F/ 1-COMPRATIO F* GY1:YINTCOMP F@ COMPRATIO F* F+ GY1:YINTCOMP F!
;
\ gyros sample:
: GY-SAMPLE
ADC7 ANALOGIN DUP \ GY1:VREF
ADC8 ANALOGIN - \ GY1:XIN
\ high pass filter :
DUP GY1:XMEM @ - SWAP GY1:XMEM ! GY1:XOUT F@ GY:HP:1-A F* S>F F+
\ store the value in GY1:XOUT and integrate it in GY1:XINT and GY1:XINTCOMP
FDUP FDUP GY1:XOUT F!
GY1:XINT F@ F+ GY1:XINT F! GY1:XINTCOMP F@ F+ GY1:XINTCOMP F!
ADC12 ANALOGIN - \ GY1:YIN
\ the same as for GY1:XIN
DUP GY1:YMEM @ - SWAP GY1:YMEM ! GY1:YOUT F@ GY:HP:1-A F* S>F F+
FDUP FDUP GY1:YOUT F!
GY1:YINT F@ F+ GY1:YINT F! GY1:YINTCOMP F@ F+ GY1:YINTCOMP F!
ADC9 ANALOGIN \ GY2:YIN
ADC3 ANALOGIN - \ GY2:VREF : reversed values, the sensor is mounted reversed
DUP GY2:YMEM @ - SWAP GY2:YMEM ! GY2:YOUT F@ GY:HP:1-A F* S>F F+
FDUP GY2:YOUT F!
GY2:YINT F@ F+ GY2:YINT F!
;
: PRINT-SAMPLE
ACC:PITCH F@ F.
ACC:ROLL F@ F.
GY1:XINT F@ GY:SENS F* F.
GY1:YINT F@ GY:SENS F* F.
GY1:XINTCOMP F@ GY:SENS F* F.
GY1:YINTCOMP F@ GY:SENS F* F.
GY2:YINT F@ GY:SENS F* F.
GY2:YOUT F@ F.
CR
;
\ 4 asynchronous machines :
\
\ ACC: accelerometer sample (100 Hz rate)
\ PITCH-ROLL: compute pitch and roll + complement filter (25 Hz rate)
\ GY: gyros sample (1 KHz rate)
\ PRINT-AD: output results (10 Hz rate)
\
LOOPINDEX ACC-COUNTER
10 ACC-COUNTER END
1 ACC-COUNTER START
MACHINE ACC ON-MACHINE ACC APPEND-STATE S1
IN-STATE S1 CONDITION ACC-COUNTER COUNT CAUSES ACC-SAMPLE
THEN-STATE S1 TO-HAPPEN
S1 SET-STATE
LOOPINDEX PITCH-ROLL-COUNTER
40 PITCH-ROLL-COUNTER END
1 PITCH-ROLL-COUNTER START
MACHINE PITCH-ROLL ON-MACHINE PITCH-ROLL APPEND-STATE S2
IN-STATE S2 CONDITION PITCH-ROLL-COUNTER COUNT CAUSES COMPUTE-PITCH-ROLL
THEN-STATE S2 TO-HAPPEN
S2 SET-STATE
MACHINE GY ON-MACHINE GY APPEND-STATE S3
IN-STATE S3 CONDITION TRUE CAUSES GY-SAMPLE
THEN-STATE S3 TO-HAPPEN
S3 SET-STATE
LOOPINDEX PRINT-COUNTER
100 PRINT-COUNTER END
1 PRINT-COUNTER START
MACHINE PRINT-AD ON-MACHINE PRINT-AD APPEND-STATE S4
IN-STATE S4 CONDITION PRINT-COUNTER COUNT CAUSES PRINT-SAMPLE
THEN-STATE S4 TO-HAPPEN
S4 SET-STATE
5000 PERIOD
INSTALL ACC INSTALL PITCH-ROLL INSTALL GY INSTALL PRINT-AD
just a little words to present you my project using a ServoPod-USB. Its objective is to build an UAV (Unmanned Aerial Vehicule), more precisely a small quadrocopter (http://en.wikipedia.org/wiki/Quadrocopter). As this kind of vehicule is pretty unstable, it needs very fast responding and very accurate sensors to assist either the pilot or the auto-pilot.
One of the critical and difficult parts of this is the inertial measurement unit (IMU) whose role is to tell the position of the vehicule in space in terms of pitch, roll and yaw, and I decided to start with this part. I'm quite at an early stage of the project: I have an IMU that begins to work getting an attitude by merging information from two kind of sensors and reporting it.
Here (http://www.dailymotion.com/crashtest44/video/15425887) is a sample of the results I got. The top line represents the yaw, the middle 3 lines represent the pitch and the bottom 3 lines represent the roll.
For this IMU, I use a 3 axis ADXL 330 accelerometer (http://www.sparkfun.com/commerce/product_info.php?products_id=692) and two dual axis IDG-500 gyros (http://www.sparkfun.com/commerce/product_info.php?products_id=9094). The accelerometer allows to measure pitch and roll by comparing measures on 3 orthogonal axis to the gravity (see this explanation (http://tom.pycke.be/mav/69/accelerometer-to-attitude)) The gyros allow to measure angular rates for 3 axis (I don't use the 4th available). It's then possible to integrate these rates to determine the pitch, roll and yaw (see this explanation (http://tom.pycke.be/mav/70/gyroscope-to-roll-pitch-and-yaw)). These sensors are however not perfect: the accelerometer is influenced by the vehicule movements and the gyros are subject to errors introducing drift in the calculated angles (for the IDG-500 I use, this drift can be as large as 5 degrees/sec.). So the values measured from the accelerometer and the gyros have to be combined to determine the correct attitude of the vehicule.
There are several ways to merge these two kind of data. One popular way is Kalman filtering, but it relies on very hard mathematics with plenty of matrices and is known to be computer intensive. I've chosen another simpler approache: the complement filter. It is based on the remark that the accelerometers can give a reliable result at a low frequency (e.g. once per second) but not at an higher frequency and the gyros can give a reliable result at higher frequency, but, not at low frequency because of the drift phenomenom. Thus, a correct measure can be obtained by applying a low pass filter to the values read from the accelerometer (in red on my video sample), an high pass filter to the values read from the gyros (in light blue on my video sample) and summing them (in dark blue on my video sample).
Everything including filters is implemented in soft on the ServoPod, the sensors are directly connected to the analog inputs with no electronics. The IsoMax code is included below. It's my first "big" (other than the led switching examples) Isomax code, so feel free to tell me if there are mistakes or if I can improve it some ways. The display is done using a perl script generating postscript code (ok, I know that programming in postscript is a bit outdated, but it's quite close to Forth and Isomax so it makes me a good training :) ) and displayed using ghostscript.
Next, I wish to start making my proto more autonomous, by powering it with a battery instead of the USB port and, communicating to the ServoPod using an Xbee dongle (that's the issue of my question in another thread (http://www.newmicros.com/discussion/showthread.php?t=7805) ;) ). I will start to control the quadro four motors too, by using RC model brushless motors controllers that are controled using the same PWM signals as standard RC servos. I hope that these ESC will be 67 Hz tolerant as I read that the ServoPod cannot output 50 Hz PWM without setting down its clock halfspeed. If you have information on 67 Hz tolerant ESC, I'm interrested since I don't have ordered the ESCs yet.
Sincerely,
Remi
\ Complement filter ratio (gyro/accelero)
0.8e FCONSTANT COMPRATIO
1.0e COMPRATIO F- FCONSTANT 1-COMPRATIO
\ Low pass filter cutoff frequency for the ADXL330
0.05e FCONSTANT ACC:A \ approximatively 0.8 Hz for a 5000 PERIOD
1.0e ACC:A F- FCONSTANT ACC:1-A
\ Voltage reference for the ADXL-330 (VREF / 2)
1.65e FCONSTANT ACC:VREF
\ High pass filter cutoff frequency for the two IDG500
0.9999e FCONSTANT GY:HP:1-A \ approximatively 0.016 Hz for a 5000 PERIOD
\ IDG500 sensitivity
\ 1.5e FCONSTANT GY:SENS
0.00000025e FCONSTANT GY:SENS
\ Low pass filtered values computed from ADXL330 inputs
FVARIABLE ACC:XOUT 0.0e ACC:XOUT F!
FVARIABLE ACC:YOUT 0.0e ACC:YOUT F!
FVARIABLE ACC:ZOUT 0.0e ACC:ZOUT F!
\ Computed pitch and roll (from ADXL330 measures)
FVARIABLE ACC:PITCH
FVARIABLE ACC:ROLL
\ High pass filtered values of the angular rates
FVARIABLE GY1:XOUT 0.0e GY1:XOUT F!
FVARIABLE GY1:YOUT 0.0e GY1:YOUT F!
VARIABLE GY1:XMEM 0 GY1:XMEM !
VARIABLE GY1:YMEM 0 GY1:YMEM !
\ Integration (sum) of angular rates to provide angles
FVARIABLE GY1:XINT 0.0e GY1:XINT F!
FVARIABLE GY1:YINT 0.0e GY1:YINT F!
\ Complement filter :
\ GY1:XINTCOMP averages GY1:XOUT and ACC:PITCH
\ GY1:YINTCOMP averages GY1:YOUT and ACC:ROLL
FVARIABLE GY1:XINTCOMP 0.0e GY1:XINTCOMP F!
FVARIABLE GY1:YINTCOMP 0.0e GY1:YINTCOMP F!
\ 2nd (vertical) IDG500 : these values are the same as for GY1
FVARIABLE GY2:YOUT 0.0e GY2:YOUT F!
VARIABLE GY2:YMEM 0 GY2:YMEM !
FVARIABLE GY2:YINT 0.0e GY2:YINT F!
\ ADXL-330 sensitivity (divisor)
9930.0e FCONSTANT ACC:SENS
\ Low pass filter :
\
\ x = x0*a + x*(1-a)
\
\ High pass filter :
\
\ x = (x0 - x_mem) + x*(1-a)
\ x_mem = x0
\
\ Note: a = h * 2*pi * fc
\
\ fc: cut frequency
\ h: sample period (sec.) = IsoMax period / 5000000
\
\ sample + low pass filter for accelerometer x, y and z axis:
: ACC-SAMPLE
ADC0 ANALOGIN S>F ACC:SENS F/ ACC:A F* ACC:XOUT F@ ACC:1-A F* F+ ACC:XOUT F!
ADC2 ANALOGIN S>F ACC:SENS F/ ACC:A F* ACC:YOUT F@ ACC:1-A F* F+ ACC:YOUT F!
ADC4 ANALOGIN S>F ACC:SENS F/ ACC:A F* ACC:ZOUT F@ ACC:1-A F* F+ ACC:ZOUT F!
;
\ Accelerometer: pitch = atan2(x,z)
\ roll = atan2(y,z)
\
\ Complement filter:
\
\ intcomp[n+1] = intcomp[n] * compratio + angle * (1-compratio)
\ angle = angle computed from accelerometer
\
: COMPUTE-PITCH-ROLL
ACC:ZOUT F@ ACC:VREF F- FDUP
ACC:XOUT F@ ACC:VREF F- FSWAP FATAN2 FDUP ACC:PITCH F!
GY:SENS F/ 1-COMPRATIO F* GY1:XINTCOMP F@ COMPRATIO F* F+ GY1:XINTCOMP F!
ACC:YOUT F@ ACC:VREF F- FSWAP FATAN2 FDUP ACC:ROLL F!
GY:SENS F/ 1-COMPRATIO F* GY1:YINTCOMP F@ COMPRATIO F* F+ GY1:YINTCOMP F!
;
\ gyros sample:
: GY-SAMPLE
ADC7 ANALOGIN DUP \ GY1:VREF
ADC8 ANALOGIN - \ GY1:XIN
\ high pass filter :
DUP GY1:XMEM @ - SWAP GY1:XMEM ! GY1:XOUT F@ GY:HP:1-A F* S>F F+
\ store the value in GY1:XOUT and integrate it in GY1:XINT and GY1:XINTCOMP
FDUP FDUP GY1:XOUT F!
GY1:XINT F@ F+ GY1:XINT F! GY1:XINTCOMP F@ F+ GY1:XINTCOMP F!
ADC12 ANALOGIN - \ GY1:YIN
\ the same as for GY1:XIN
DUP GY1:YMEM @ - SWAP GY1:YMEM ! GY1:YOUT F@ GY:HP:1-A F* S>F F+
FDUP FDUP GY1:YOUT F!
GY1:YINT F@ F+ GY1:YINT F! GY1:YINTCOMP F@ F+ GY1:YINTCOMP F!
ADC9 ANALOGIN \ GY2:YIN
ADC3 ANALOGIN - \ GY2:VREF : reversed values, the sensor is mounted reversed
DUP GY2:YMEM @ - SWAP GY2:YMEM ! GY2:YOUT F@ GY:HP:1-A F* S>F F+
FDUP GY2:YOUT F!
GY2:YINT F@ F+ GY2:YINT F!
;
: PRINT-SAMPLE
ACC:PITCH F@ F.
ACC:ROLL F@ F.
GY1:XINT F@ GY:SENS F* F.
GY1:YINT F@ GY:SENS F* F.
GY1:XINTCOMP F@ GY:SENS F* F.
GY1:YINTCOMP F@ GY:SENS F* F.
GY2:YINT F@ GY:SENS F* F.
GY2:YOUT F@ F.
CR
;
\ 4 asynchronous machines :
\
\ ACC: accelerometer sample (100 Hz rate)
\ PITCH-ROLL: compute pitch and roll + complement filter (25 Hz rate)
\ GY: gyros sample (1 KHz rate)
\ PRINT-AD: output results (10 Hz rate)
\
LOOPINDEX ACC-COUNTER
10 ACC-COUNTER END
1 ACC-COUNTER START
MACHINE ACC ON-MACHINE ACC APPEND-STATE S1
IN-STATE S1 CONDITION ACC-COUNTER COUNT CAUSES ACC-SAMPLE
THEN-STATE S1 TO-HAPPEN
S1 SET-STATE
LOOPINDEX PITCH-ROLL-COUNTER
40 PITCH-ROLL-COUNTER END
1 PITCH-ROLL-COUNTER START
MACHINE PITCH-ROLL ON-MACHINE PITCH-ROLL APPEND-STATE S2
IN-STATE S2 CONDITION PITCH-ROLL-COUNTER COUNT CAUSES COMPUTE-PITCH-ROLL
THEN-STATE S2 TO-HAPPEN
S2 SET-STATE
MACHINE GY ON-MACHINE GY APPEND-STATE S3
IN-STATE S3 CONDITION TRUE CAUSES GY-SAMPLE
THEN-STATE S3 TO-HAPPEN
S3 SET-STATE
LOOPINDEX PRINT-COUNTER
100 PRINT-COUNTER END
1 PRINT-COUNTER START
MACHINE PRINT-AD ON-MACHINE PRINT-AD APPEND-STATE S4
IN-STATE S4 CONDITION PRINT-COUNTER COUNT CAUSES PRINT-SAMPLE
THEN-STATE S4 TO-HAPPEN
S4 SET-STATE
5000 PERIOD
INSTALL ACC INSTALL PITCH-ROLL INSTALL GY INSTALL PRINT-AD