Difference between revisions of "Booz"

From PaparazziUAV
Jump to navigation Jump to search
Line 238: Line 238:


== Sensors Calibration ==
== Sensors Calibration ==
All our sensors needs to be calibrated in order to provide useful informations.
Accelerometers and Magnetometers calibration is critical for AHRS performances and can be performed using no special hardware. For the magnetometer, it is even very important that the calibration be performed in the fully assembled vehicle, with all systems powered. This is the so-called hard-iron calibration and will allow us to compensate for any constant parasitic magnetic field generated by the vehicle.
The calibration process consist in finding a set of neutrals and scale factors for each sensor, such as
<math>
\begin{pmatrix}physical\_value_x\\physical\_value_y\\physical\_value_z\end{pmatrix} = \begin{pmatrix}sf_x&0&0\\0&sf_y&0\\0&0&sf_z\end{pmatrix} * (\begin{pmatrix}sensor_x\\sensor_y\\sensor_z\end{pmatrix}-\begin{pmatrix}n_x\\n_y\\n_z\end{pmatrix})
</math>
The principle of the calibration is the following : An accelerometer, on a vehicle at rest measures a constant vector ( the opposite of gravity ) in the earth frame, expressed in the vehicle frame. 
<math>
DCM * \begin{pmatrix}0\\0\\-9.81\end{pmatrix} =
\begin{pmatrix}sf_x&0&0\\0&sf_y&0\\0&0&sf_z\end{pmatrix} * (\begin{pmatrix}sensor_x\\sensor_y\\sensor_z\end{pmatrix}-\begin{pmatrix}n_x\\n_y\\n_z\end{pmatrix})
</math>
DCM is a rotation matrix that converts between earth frame and body frame. It will change when we change the orientation of the vehicle. Nevertheless, a rotation conserves the norm of a vector. We can thus obtain the following scalar equation that doesn't depend on the vehicle orientation :
<math>
9.81^2 = ( sf_x(sensor_x-n_x) )^2 +  (sf_y(sensor_y-n_y) )^2 +  (sf_z(sensor_z-n_z) )^2
</math>
We can then record an important number of measurements in different orientations and find the set of scale factor and neutral giving the norm closest to 9.81
Booz comes with a ( very unfriendly ) scilab script to perform this operation ( sw/tools/calibration/calib_accel_mag.sce ). Here is the way to use it
Switch to the "raw sensors" telemetry mode and launch "server" to record a log.
Move the quad in different orientations ( upright, inverted, on nose, on tail, on right side, on left side ) . You can also take some measurememts banking 45 degres.
Try to get an homogeneous distribution of your measurements. I find it better to let the quad rest while measuring. You can then run the scilab script to get your calibration coefficients.  It first makes an initial guess using min and max, ie for each axis
neutral = 0.5 * (max + min)
sensitivity = 0.5*(max-min)
It then uses scilab's "datafit" algorithm to optimise the initial guess
[[Image:calib8accel.png]]
Note for magnetometer:

Revision as of 11:03, 6 January 2010

Overview

Booz is an extension of Paparazzi to VTOLs. At the current stage of the project, the system provides attitude stabilization, vertical guidance and automatic navigation. It is able to use simple Paparazzi flight plans ( only go instructions ) and uses Paparazzi telemetry and datalink, which means all the Paparazzi ground segment applications are available( plotter, settings, gcs, etc...)

The current autopilot consist in 3 boards

Booz2 main.jpg Booz2 imu.jpg Booz2 gps.jpg

  • The main board, comprising power supply, a LPC2148 and a barometer.
  • The IMU board, comprising gyroscopes, accelerometers, magnetometers and a 16 bits ADC.
  • The GPS board, using a LEA-5H by ublox.

It flies on a variety of Quad-rotor platforms

Small quad.jpg Booz2 a2.jpg Booz2 a1.jpg

and less common vehicles

Booz2 a4.jpg Booz2 a4 2.jpg Booz2 a5.jpg

Program Sources

Everything is at savannah.

-hardware in paparazzi4/trunk/hw/booz
-code in paparazzi3/trunk

Hardware

Purchasing Hardware

There are now vendors offering Booz hardware, Please see the Get Hardware page for details.

Building Tips

Since the Booz uses the same LPC2148 as the Tiny and TWOG autopilots and runs the same core code the documented steps are nearly the same for booz. Below more Booz specific tests are given. However after assembly you load the bootloader in the same way to enable programming via the USB interface.

Assembly Tips

  • BOM
  • Assembly Notes
  • Expected voltages for newly created boards

Loading the USB Boot Loader

This is virtually the same steps as for all other LPC2148 based Paparazzi autopilots. Just the cable is different. Using UART0/serial to load the image.

Create an adapter cable that has Tx/Rx and Ground along with the ability to hold P0.14 low (i.e. grounded). My cable has P0.14 pin connected to ground as I have found P0.14 should be held low during the entire upload process. See the Eagle Schematic for the pinouts on the GPS connector you will use to upload (same connector as the Tiny/TWOG). NOTE: You can not use the same cable as the Tiny/TWOG.

Steps:

  1. Connect the "USB to FTDI cable" (#1 above) TTL ends to 8-pin Picoblade to FTDI board cable
  2. Connect the USB end to your Linux laptop (/var/log/messages should show a new device on /dev/ttyUSB0).
  3. Connect 8-pin adapter cable to "GPS" 8-pin Pico-Female on Booz Main "GPS" labeled connector
  4. Using common ground" set BOOT (P0.14) "low" and keep it held low (on GPS connector)
  5. Power on (~5v to 12v current limited power supply suggested)
  6. Enter this command in a terminal:
    make upload_bl PROC=GENERIC

NOTE: Success will mean you see this at the end in the terminal:

Synchronizing. OK
Read bootcode version: 2.12.0
Read part ID: LPC2148, 512 kiB ROM / 40 kiB SRAM (67305253)
Sector 0: .................................................. .............................................
Sector 1: .................................................. .........................
Download Finished... taking 6 seconds
Now launching the brand new code
ioctl get failed
ioctl set ok, status = 0
ioctl get ok, status = 2
ioctl get ok, status = 2
ioctl set ok, status = 2
ioctl set ok, status = 0
ioctl set ok, status = 0

What you need to see is the "Download Finished" message. Now you can remove Jumper and FTDI cable. From now on all programming is loaded using USB Adapter cable

USB Programming

Once the USB boot code is loaded (see above) you now program the autopilot via USB. To enable the LPC2148 to wait for programming on the USB interface you must power the board while the USB is connected to the ground station computer. When properly wired P0.21 will be fed 5v from the USB interface and at boot time that tells the LPC2148 to wait for programming over USB. Once programmed the board will rest itself and run the programming (even if you leave the cable attached).

Steps

  1. Connect the USB cable from the computer to the Booz Main USB connector
  2. Power on the Booz Main board
  3. On the Linux (computer) side dmesg should show you a new detected USB device
  4. In Paparazzi Center click "Upload" and wait for the ####### message to show the code is being uploaded.

That's it.

Running a simulator

Quick start guide

Booz contains a sensors model and can use Jsbsim to allow arbitrarily complex flight dynamic models.


  • Extract a copy of paparazzi3 svn
 svn co svn://svn.savannah.nongnu.org/paparazzi/paparazzi3/trunk paparazzi3
  • Install all required dependancies
apt-get install paparazzi-dev libtool


  • Compile JSBSIM
 cvs -z3 -d:pserver:anonymous@jsbsim.cvs.sourceforge.net:/cvsroot/jsbsim co -P JSBSim 
 cd JSBSim
 ./autogen.sh
 ./configure --enable-maintainer-mode --enable-compile-warnings --enable-libraries --enable-shared --prefix=/opt/jsbsim
 make
 sudo make install

Here is a patch to silent warnings. Also be sure automake and autoconf are installed. If not sudo apt-get install automake autoconf

  • Compile paparazzi
 cd paparazzi3/trunk
 make
  • Compile the vehicle
 make AIRCRAFT=BOOZ2_A1 clean_ac sim


  • Start paparazzi_center if you want click to start programs
./paparazzi
  • Start messages to monitor the middleware activity ( from the tool menu of paparazzi center) or with
./sw/ground_segment/tmtc/messages 
  • Start the sim
LD_LIBRARY_PATH=/opt/jsbsim/lib ./var/BOOZ2_A1/sim/simsitl

You should now see activity in the "messages" window

  • Plot the value of a message field.

start 'plotter' ( from the tool menu of paparazzi center) or with

 ./sw/logalizer/plotter

for example drag the label 'int32 phi' from the BOOZ2_FP message to the drawing area of the plotter


  • Use the datalink to change the temetry mode

start 'settings' ( from the tool menu of paparazzi center) or with

 ./sw/ground_segment/tmtc/settings -ac BOOZ2_A1

start 'server' to dispatch datalink messages ( from the tool menu of paparazzi center) or with

./sw/ground_segment/tmtc/server 

change the field "telemetry" on the first page to "Att loop" and send by pressing the green check button. THe label on the left or the drop box should change to "Att loop" confirming your essage has been received. "message" should now show that the message "BOOZ2_STAB_ATTITUDE" is received

  • Use flightgear to visualize your vehicle

If you want a view of a quadrotor in flightgear, make a link from

/usr/share/games/FlightGear/Models/Aircraft/paparazzi to PAPARAZZI_SRC/conf/simulator/flightgear/

start flighgear with

fgfs --fdm=null --native-gui=socket,in,30,,5501,udp --prop:/sim/model/path=Models/Aircraft/paparazzi/mikrokopter.xml

restart your simulator with

 LD_LIBRARY_PATH=/opt/jsbsim/lib ./var/BOOZ2_A1/sim/simsitl --fg_host 127.0.0.1
  • Save you session

Tunning the attitude control loop

Here we are going to use the simulator to demonstrate a way of tunning the attitude control loop.

  • Restart your previous session
  • Set telemetry mode to "Att loop"
  • Display two real time plotter windows

In the first one, plot the field "m_phi" from the "BOOZ2_STAB_ATTITUDE" message. This is our estimation of roll angle. On top of that, plot the field "phi" from the "BOOZ2_STAB_ATTITUDE_REFERENCE" message. This is our reference roll angle, that is, the roll value we are trying to achieve.

In the second plotter, plot the fields "delta_a_fb" and "delta_a_ff". Those are respectively the feddback and feedforward part of our roll command. The sum of those two terms is what is used as roll command.The feedforward part is the part used to follow our trajectory and the feedback part is the part used to reject perturbations.


  • In "Settings", go to the "Att Loop" tab

We notice that the vehicle doesn't follow accurately the step trajectory we are trying to make him do.

Start by setting the value of the proportional gain ('pgain_phi') to 1000 instead of 400. The vehicle now follows the trajectory faster but overshoots. To prevent that, increase the value of the derivative gain ('dgain p') from -300 to -700.

If you look at the plotter where you're ploting the commands, you'll notice that during steps, the feedback command has to work hard. This means that our feedforward command is badly tunned, and namely not working hard enough.Increase the value of the feedforward gain ('ddgain p') from 300 to 540. You'll notice that now the feedback command has becomed marginal during the steps. This is the right value for the gain. Anything bigger will make the feedback command fight against the feedforward command during steps, anything smaller will make the feedback command have to complement the feedforward command.

Something else

  • try starting flightgear with
fgfs --fdm=null --native-gui=socket,in,30,,5501,udp --prop:/sim/model/path=Models/Aircraft/paparazzi/simple_bipe.xml

an the sim with

LD_LIBRARY_PATH=/opt/jsbsim/lib ./var/BOOZ2_A1/sim/simsitl --fg_host 127.0.0.1 --rc_script 1


Hardware Test

Booz comes with a number of simple test programs that you can use to validate a newly assembled board or learn how booz code works in case you want to extend it. The Makefile for those is in conf/autopilot/booz2_test_progs.makefile


test_downlink

Paparazzi's Makefile allow you to build different TARGETS (aka programs) using a doted notation. The begining of the Makefile reads

 #
 # test downlink
 #
 test_downlink.ARCHDIR = $(ARCHI)
 test_downlink.ARCH = arm7tdmi
 test_downlink.TARGET = test_downlink
 test_downlink.TARGETDIR = test_downlink
 #
 test_downlink.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) $(BOOZ_CFLAGS)
 test_downlink.CFLAGS += -DPERIPHERALS_AUTO_INIT
 test_downlink.srcs   += $(SRC_BOOZ_TEST)/booz2_test_downlink.c
 test_downlink.CFLAGS += -DUSE_LED
 test_downlink.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./10.))' -DTIME_LED=1
 test_downlink.srcs   += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c
 #
 test_downlink.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600
 test_downlink.srcs   += $(SRC_ARCH)/uart_hw.c
 #
 test_downlink.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart1 
 test_downlink.srcs   += downlink.c pprz_transport.c

FIXME: More on this when syntax highlighting/line numbering works

The command line to compile the "test_downlink" target for the BOOZ2_A1 aircraft would be

make AIRCRAFT=BOOZ2_A1 test_downlink.compile

and to upload this program to your board ( you don't really need to type the previous command, make is smart and will compile your program if needed when you ask him to upload it )

make AIRCRAFT=BOOZ2_A1 test_downlink.upload


test_max1168

the max1168 is the 16 bits analog to digital converter chip used on the IMU to sample gyros and accels.

make AIRCRAFT=BOOZ2_A1 test_max1168.upload


Sensors Calibration

All our sensors needs to be calibrated in order to provide useful informations.

Accelerometers and Magnetometers calibration is critical for AHRS performances and can be performed using no special hardware. For the magnetometer, it is even very important that the calibration be performed in the fully assembled vehicle, with all systems powered. This is the so-called hard-iron calibration and will allow us to compensate for any constant parasitic magnetic field generated by the vehicle. The calibration process consist in finding a set of neutrals and scale factors for each sensor, such as



The principle of the calibration is the following : An accelerometer, on a vehicle at rest measures a constant vector ( the opposite of gravity ) in the earth frame, expressed in the vehicle frame.


DCM is a rotation matrix that converts between earth frame and body frame. It will change when we change the orientation of the vehicle. Nevertheless, a rotation conserves the norm of a vector. We can thus obtain the following scalar equation that doesn't depend on the vehicle orientation :


We can then record an important number of measurements in different orientations and find the set of scale factor and neutral giving the norm closest to 9.81

Booz comes with a ( very unfriendly ) scilab script to perform this operation ( sw/tools/calibration/calib_accel_mag.sce ). Here is the way to use it

Switch to the "raw sensors" telemetry mode and launch "server" to record a log.

Move the quad in different orientations ( upright, inverted, on nose, on tail, on right side, on left side ) . You can also take some measurememts banking 45 degres.

Try to get an homogeneous distribution of your measurements. I find it better to let the quad rest while measuring. You can then run the scilab script to get your calibration coefficients. It first makes an initial guess using min and max, ie for each axis

neutral = 0.5 * (max + min)

sensitivity = 0.5*(max-min)

It then uses scilab's "datafit" algorithm to optimise the initial guess

File:Calib8accel.png


Note for magnetometer: