F Reprogrammable UAV Autopilot System (Part 2)elkaim/Documents/... · Reprogrammable UAV Autopilot...

8
36 CIRCUIT CELLAR ® www.circuitcellar.com May 2011 – Issue 250 In the first part of this series, you were introduced to the SLUGS reprogrammable UAV autopilot system. This article details the system’s software and flight test results. Reprogrammable UAV Autopilot System (Part 2) I F EATURE ARTICLE by Mariano Lizarraga, Renwick Curry, and Gabriel Elkaim (USA) n the first part of this two-part series, we introduced the hardware used in the Santa Cruz Low-cost UAV GNC System (SLUGS), an open-source unmanned aerial vehicle (UAV) autopilot primarily focused on supporting guidance, navigation, and control (GNC) research. In this article, we’ll focus on the software architecture and algo- rithms. We’ll also present results from temperature com- pensation, sensor calibration, and flight tests. SOFTWARE ARCHITECTURE The SLUGS has two Microchip Technology dsPIC33 digital signal controllers (DSCs) that serve as its main processing units, running at 40 MIPS. These are interconnected via a SPI at 10 MHz and are called the “sensor” DSC and “control” DSC, respectively. The sen- sor DSC is in charge of reading the sensor data and converting it into a high-quality estimate of the aircraft position and atti- tude (3-D orientation). The control DSC interacts with the user through the ground station, generating signals to the UAV’s control surfaces to stabilize the aircraft and fly it along the user-designated mission (at its highest level, traversing waypoints at a given altitude and airspeed). One of the key advancements in the SLUGS autopilot is that the DSCs are pro- grammed directly from MATLAB Simulink using a block and signal flow metaphor for visual programming. This offers several advantages. Most researchers in the GNC field are already familiar with MATLAB and use it to prototype and simulate new algorithms. Com- plicated algorithms and changes are easy to implement, rapidly increasing the prototype-simulate-test cycle. Lastly, the software can be easily retargeted for better processors without having to rework the low-level, back-end code. While this environment sacrifices some efficiency, any per- formance issues are made insignificant by the speed with which changes can be made and tested. The sensor DSC’s low-level software architecture (see Figure 1) is a combination of blocks provided by Lubin Kerhuel’s dsPIC Simulink Embedded Target (ET) (shown in Figure 1—Low-level software architecture for the sensor DSC Parser K C Function call Simulink blocks Modifiable by end user (Simulink blocks) Simulink ET blocks Attitude and position estimation Protocol encoder To control DSC [SPI] Calibration and temperature compensation GPS [UART] Mags [I 2 C] RCRx [IC] Analog sensors [ADC] Accels and gyros [SPI] HIL Data [UART] Testing and Results

Transcript of F Reprogrammable UAV Autopilot System (Part 2)elkaim/Documents/... · Reprogrammable UAV Autopilot...

Page 1: F Reprogrammable UAV Autopilot System (Part 2)elkaim/Documents/... · Reprogrammable UAV Autopilot System ... rate gyro, and magnetometer data are ... of temperature versus sensor

36 CIRCUIT CELLAR® • www.circuitcellar.com

May

201

1 –

Issue

250

In the first part of this series, you were introduced to theSLUGS reprogrammable UAV autopilot system. This articledetails the system’s software and flight test results.

Reprogrammable UAV AutopilotSystem (Part 2)

I

FEAT

URE

ARTICLEby Mariano Lizarraga, Renwick Curry, and Gabriel Elkaim (USA)

n the first part of this two-part series, we introducedthe hardware used in the Santa Cruz Low-cost UAV

GNC System (SLUGS), an open-source unmanned aerialvehicle (UAV) autopilot primarily focused on supportingguidance, navigation, and control (GNC) research. In thisarticle, we’ll focus on the software architecture and algo-rithms. We’ll also present results from temperature com-pensation, sensor calibration, and flight tests.

SOFTWARE ARCHITECTUREThe SLUGS has two Microchip Technology dsPIC33

digital signal controllers (DSCs) that serveas its main processing units, running at 40MIPS. These are interconnected via a SPI at10 MHz and are called the “sensor” DSCand “control” DSC, respectively. The sen-sor DSC is in charge of reading the sensordata and converting it into a high-qualityestimate of the aircraft position and atti-tude (3-D orientation). The control DSCinteracts with the user through the groundstation, generating signals to the UAV’scontrol surfaces to stabilize the aircraft andfly it along the user-designated mission (atits highest level, traversing waypoints at agiven altitude and airspeed).

One of the key advancements in theSLUGS autopilot is that the DSCs are pro-grammed directly from MATLAB Simulinkusing a block and signal flow metaphor forvisual programming. This offers severaladvantages. Most researchers in the GNCfield are already familiar with MATLAB

and use it to prototype and simulate new algorithms. Com-plicated algorithms and changes are easy to implement,rapidly increasing the prototype-simulate-test cycle. Lastly,the software can be easily retargeted for better processorswithout having to rework the low-level, back-end code.While this environment sacrifices some efficiency, any per-formance issues are made insignificant by the speed withwhich changes can be made and tested.

The sensor DSC’s low-level software architecture (seeFigure 1) is a combination of blocks provided by LubinKerhuel’s dsPIC Simulink Embedded Target (ET) (shown in

Figure 1—Low-level software architecture for the sensor DSC

Parser

K

C Function call

Simulink blocks

Modifiable by end user (Simulink blocks)

Simulink ET blocks

Attitude and position

estimation

Protocolencoder

TocontrolDSC[SPI]

Cal

ibra

tion

and

tem

pera

ture

com

pens

atio

n

GPS[UART]

Mags[I2C]

RCRx[IC]

Analogsensors[ADC]

Accels and

gyros [SPI]

HILData

[UART]

Testing and Results

2011-5-018_Lizarraga_part2_Layout 1 4/6/2011 9:48 AM Page 36

Page 2: F Reprogrammable UAV Autopilot System (Part 2)elkaim/Documents/... · Reprogrammable UAV Autopilot System ... rate gyro, and magnetometer data are ... of temperature versus sensor

ground control station (GCS) com-mands—are stored in a separate circu-lar queue. The GCS commands proto-col decoder reads the queue, interpretsthose commands, and passes them onto the navigation and control algo-rithm. The data being received by theRCRx are scaled from the PWM dutycycle to angular values and passed tothe guidance and control algorithm tobe used as initial conditions for themanual-to-autonomous transition.This ensures that no sudden transientsare experienced by the aircraft whenswitching between manual andautonomous mode. In other words, aninstant after telling the autopilot tofly the airplane, the control surfacesshould be in exactly the same placethat they were before the switch.

The guidance and control algorithmgenerates the required control surfacecommands, which are scaled fromangular values to the correct PWMduty cycle and sent to each of the con-trol surface’s servos. These commandsare also passed on (along with the datasent from the sensor DSC) to a protocolencoder, which splits them into differ-ent sentences, schedules them fortransmission, and sends them to theradio modem for ground telemetrydata. These data are used to drive theGCS display, recorded for post-flightanalysis, and used for both control looptuning and fault diagnosis in real time.

COMMUNICATIONS PROTOCOLThe complete autopilot system gen-

erates 168 different meaningful vari-ables. Attitude, position, sensor biases,temperature, absolute and differentialpressure, control surface commands,controller gains, and many more aregenerated on every cycle. These valuesare not only used by the autopilot, butalso sent to the GCS and logged tofacilitate debugging and post-flightanalysis. During our initial develop-ment phase, we produced a very sim-ple, yet effective, binary communica-tions protocol, where each messagecontained a 4-byte header and a 3-bytetrailer. Nevertheless, we soon realizedthat adding a new sentence to the pro-tocol was tedious and error-prone.After a detailed evaluation of possiblesolutions, we decided to adopt the

www.circuitcellar.com • CIRCUIT CELLAR® 37

May

201

1 –

Issue

250

blue), blocks implemented in C(shown in yellow), and basic Simulinkblocks (shown in white).

Although the ET offered ready-to-use blocks for most of the DSC’speripherals, it was decided early inthe design process that many of thesewould be rewritten in C to providegreater flexibility and full control ofthe implementation. For instance, touse less processor time while sendingdata through the serial port, it wasnecessary to use the direct memoryaccess (DMA) feature of the DSC.The ET’s UART block did not offerthis capability, and it was necessaryto rewrite it in C and provide it as aC function call block in Simulink.This is one of the key advantages ofthe ET: C functions can be mergedinto the overall generated code withgreat ease.

The GPS serial stream is read via aUART port and placed into a circularqueue data structure. An NMEA 0183protocol parser reads data from thecircular queue and decodes the proto-col sentences producing position,velocities, and course over ground(COG) information. The analog sen-sors (dynamic and static pressures,battery voltage, and temperature) areread directly from the hardware viathe DSC’s ADC and passed through adigital low-pass filter bank to attenu-ate sensor noise. The accelerometersand rate gyros are queried via a SPI,and the digital stream is decodedaccording to the device specifications.The magnetometers are configuredand queried through I2C, implemented

as an interrupt-driven state machineto decode the data.

The analog sensor, accelerometer,rate gyro, and magnetometer data arepassed through a calibration and tem-perature compensation block to pro-duce temperature-compensated datain meaningful engineering units. Thealgorithms and test procedures aredetailed later. The data being receivedby the input capture port from theradio control receiver (RCRx) arereceived and scaled from the PWMduty cycle to angular values in radianscorresponding to control surfacedeflection commands. The data areassembled into a data structure that ispassed to other parts of the code forfurther calculations.

In hardware-in-the-loop (HIL) simu-lation (see Part 1) and in real flight, theattitude and position estimation algo-rithm (shown in green) processes thedata and sends the results to the con-trol DSC via a communications proto-col encoder (which separates the datainto sentences and manages the SPIinter-processor communications bus).

The control DSC’s low-level soft-ware architecture (shown in Figure 2)is currently less resource intensivethan that of the sensor DSC. Thepackets sent from the sensor DSC arereceived and placed in a circularqueue. The inter-processor communi-cations (IPC) protocol decoder readsfrom the queue and assembles thedata for the navigation and controlalgorithm (shown in green).

In a similar manner, data receivedfrom the radio modem—containing

Figure 2—Low-level software architecture for the control DSC

C Function call

Simulink blocks

Modifiable by end user (Simulink blocks)

Simulink ET blocks

Protocolencoder

RCRx[IC]

Navigation andcontrol

IPCProtocoldecoder

GSProtocoldecoder

FromsensorDSC[SPI]

Fromradio

modem[UART]

K

K

To radiomodem[UART]

To controlsurfaceservos[PWM]

2011-5-018_Lizarraga_part2_Layout 1 4/6/2011 9:48 AM Page 37

Page 3: F Reprogrammable UAV Autopilot System (Part 2)elkaim/Documents/... · Reprogrammable UAV Autopilot System ... rate gyro, and magnetometer data are ... of temperature versus sensor

38 CIRCUIT CELLAR® • www.circuitcellar.com

May

201

1 –

Issue

250

of temperature versus sensor readings.From that plot, a simple linear coeffi-cient (delta sensor counts/delta tem-perature counts) was extracted usinga least squares fit. The sensor readingis then adjusted using this coefficient.The new sensor reading is the rawplus the coefficient multiplied by thedifference in temperature from anominal point.

The advantage is that this dramati-cally reduces the temperature drift of

the sensors; the disadvantage is that the sensor reading isnow a function of both the sensor and the temperaturesensor, possibly increasing the noise in the calibratedsensor output.

Lastly, a more careful temperature curve could be usedinstead of the simple linear correction, although this wasdeemed effective for our needs. Figure 4 shows the impor-tance of temperature compensation. It shows the tempera-ture run (top left) and compares it with the readings fromthe static pressure sensor (top center). A clear correlationexists between the two. As expected, the scatter plot (topright) confirms the dependence between the two and showsthe linear temperature coefficient. The resulting readingafter temperature compensation (bottom plot, red) shows asignificant improvement over the uncompensated reading(bottom plot, blue). Although temperature compensationdoes not completely remove the sensor’s temperaturedependence, it corrects the majority of the effect.

Bias points on the sensors are relatively easy to calibrateusing static data (and several are estimated on-the-fly bythe attitude and position estimation algorithms). In order

open-source communications protocolMAVLink developed by Lorenz Meierat ETH Zurich. This protocol is alightweight, message-marshallinglibrary for autonomous vehicles thatallows the serialization of data struc-tures (C-structs in particular) for trans-mission. But the feature that tippedthe scales toward selecting MAVLinkas the communications protocol ofchoice was the capability to automati-cally generate all of the required Ccode to serialize, pack, send, receive, and decode these datastructures. One only needs to describe the fields of thepackets at a high level using a simple XML syntax, and thetools available with the protocol will generate all of therequired C code. Furthermore, the project itself has greatdocumentation on how to integrate MAVLink into anexisting project (provided the project is coded in C).

Figure 3 shows how the protocol’s packets are structured.There is a 1-byte packet start sign (the letter U). Payloaddata specifies the size of the actual data being transmitted.A packet sequence is a counter that gets incremented aspackets are sent out. The system and component identi-fiers uniquely identify the source of the message. A mes-sage identifier indicates what type of message it is. Afterthe payload data, a 2-byte checksum is appended to themessage to verify its validity upon reception.

Due to limitations in the RF link bandwidth, some ofthese messages are sent at a high data rate (100 Hz), whileothers that do not change very often are sent at a lowerdata rate (10 Hz). The protocol implements a small set of“spare” messages for debugging that can be assigned to anyvariable or calculation accessible within the autopilot soft-ware, both for floating-point and integer values. These areautomatically logged and displayed on the GCS, and haveproven very useful when testing out new algorithms.

TEMPERATURE & SENSOR COMPENSATIONThe sensors used in the SLUGS are relatively inexpen-

sive MEMS devices. As a result, they tend to have largernoise, nonlinearities, and temperature drifts than tradi-tional (and more expensive) inertial sensors. The largesterror seen in these low-cost sensors is a null-point shift asa function of temperature. Temperature compensation isused on the SLUGS to attenuate this null shift via an on-board, low-power linear Microchip Technology MCP9701active thermistor. This is inexpensive and requires verylittle board real estate.

To determine which sensors within the suite requiredtemperature compensation, we conducted the followingexperiment. The board was sealed inside a Ziploc bag andplaced inside the lab’s freezer. After an hour, the board wastaken out, powered, started data recording, and placed ontop of an electric grill to generate a slow temperaturesweep (see Photo 1).

In order to find the temperature dependence for a givensensor, using the captured data, we performed a scatter plot

Photo 1—The SLUGS autopilot on top of an electric grill used torecord data for temperature compensation calculations

Figure 3—Communications protocol messagestructure

Component ID

Data

Checksum highChecksum low

U L PS SI CI MI CH CL

Message ID

Start of message (0x55)

Message length

Packet sequence

System ID

2011-5-018_Lizarraga_part2_Layout 1 4/6/2011 9:48 AM Page 38

Page 4: F Reprogrammable UAV Autopilot System (Part 2)elkaim/Documents/... · Reprogrammable UAV Autopilot System ... rate gyro, and magnetometer data are ... of temperature versus sensor

www.circuitcellar.com • CIRCUIT CELLAR® 39

May

201

1 –

Issue

250

to match scale factors, two different methods were useddepending on the sensors. For the gyros, they were matchedagainst a better sensor, a KVH DSP-3000 fiber optic gyro(FOG). A one-axis rate table and a least squares processwere used to find the best scale factor match between theMEMS gyros and the FOG.

For the accelerometers and magnetometers, an ellipsemethod was used to simultaneously find scale factor, nullshift, and non-orthogonality errors. The three axis sensorsmeasure the x, y, and z components of a single (nonzero)vector. If the sensors were ideal, and you rotated the bodyaround all axes and plotted the x, y, and z measurements,you would see points on the surface of a sphere. Whenplotting the real measurement instead of a sphere, you geta rotated ellipse. The center of the ellipse corresponds tothe null shift, the semi-major axes to the scale factors, andthe rotation to non-orthogonality within the axes. In thecase of the magnetometer, these are often referred to ashard- and soft-iron effects. In practice, the aircraft wastumbled out at the field while telemetry data was stored onthe ground station in order to get as close to the flying con-figuration as possible. (The specific algorithms that backout the parameters to correct the accelerometers and mag-netometers can be found in the References section of thisarticle.) Photo 2 shows the aircraft being tumbled by one ofus prior to a test flight. (This is necessary to do only once,not before every flight.)

Lastly, the UAV was carefully leveled into flying attitudein the lab using a high-accuracy spirit level. The autopilotwas turned on and allowed to converge on its attitude. Thiswas used to generate a small rotation offset matrix to correctfor the difference between the autopilot mounting and theaircraft body coordinates. This is held as a direction cosinematrix (DCM) in order to correctly account for the rotation.(You cannot simply add angles, and this does not preserve the

mathematical relationships for rotationsequences.)

POSITION & ATTITUDE ESTIMATIONThe initial design of the attitude filter used

a complementary filter with a magnetometertriad for a heading reference and estimates ofinertial velocity from the position filter. Thisdesign was placed in a Simulink simulation of6DOF UAV flying a closed course defined bywaypoints requiring changes in altitude andtrack. The original design did not include lon-gitudinal acceleration (dV/dt in body coordi-nates), but this was found to be necessary toaccount for changes in speed. Several candi-dates for computing this term were comparedon the basis of tracking errors before choosinga high-pass filter to compute the approximatenumerical derivative.

Two attitude filters were designed. Oneused magnetometers for a heading reference.The other used GPS COG. The COG designwas created because of the possibility of the

high noise magnetometer readings (which were, in fact, expe-rienced in the field) due to the UAV’s electric motor.

The initial design for the position filter was a nine-stateextended Kalman filter (EKF) with state variables consisting

Figure 4—Static pressure sensor (barometer) temperature compensation results

Photo 2—Aircraft tumbling while recording data for sensor calibration

2011-5-018_Lizarraga_part2_Layout 1 4/6/2011 9:48 AM Page 39

Page 5: F Reprogrammable UAV Autopilot System (Part 2)elkaim/Documents/... · Reprogrammable UAV Autopilot System ... rate gyro, and magnetometer data are ... of temperature versus sensor

40 CIRCUIT CELLAR® • www.circuitcellar.com

May

201

1 –

Issue

250

of position and velocity in North-East-Down (NED) coordi-nates and accelerometer biases. The input measurementswere horizontal GPS position and velocity, barometric alti-tude, accelerometer readings, and the direction cosinematrix from the attitude filter. The filter performed strap-down navigation calculations for the state and covariancematrix at 100 Hz between GPS updates by integratingaccelerometer readings after they were transformed to NEDcoordinates. This filter performed well in the six DOFsimulations with noise characteristics taken from the sen-sor specifications. It was downloaded to the autopilot forfurther testing.

The objective of the first few flights was to log sensorreadings while the UAV was manually controlled to vali-date attitude and position estimates before using these out-puts for closed-loop guidance and control. Magnetometerreadings were taken in the field during static ground test-ing before the first flight, and they were deemed unreliable.Thus, the attitude filter with COG heading reference wasdownloaded to the autopilot in the field and used for theremainder of the flights.

Sensor readings were logged during manually controlledflight. These data were then replayed after the flightthrough the Simulink position and attitude models.Unfortunately, the accelerometers’ in-flight readings con-tained a high level of noise and the output of the EKF wasunusable. These telemetry data also showed that the GPSreadings had little noise (except for altitude drift and verylarge jumps in altitude readings) and GPS updates wereaveraging 3 Hz.

Due to the relatively clean GPS data and the fast GPSupdate rate, we replaced the EKF with complementary fil-ters for horizontal position and velocity. For the verticalchannel, barometric altitude was blended with GPS alti-tude readings to eliminate drift. The outlier changes inaltitude readings were trapped with a threshold test beforefurther processing.

The new filter design reduced the computational load onthe sensor DSC from near 100% to 40%. Using the SLUGSenvironment, the elapsed time—from beginning the

redesign to the first download to the autopilot—was lessthan 24 hours, demonstrating the flexibility and usabilityof the SLUGS for research purposes.

The final design performed extremely well, as shown bythe excellent performance of the UAV under autonomouscontrol. In addition, the attitude and position filters didextremely well when compared with more sophisticatedmodels using other flight-test data (see Figure 5).

NAVIGATION & CONTROLThe control system designed for the SLUGS is based on

the successive loop closure (or inner/outer loop) approachfor aircraft control. It is completely implemented in thecontrol DSC (see Part 1 of this series). The inner loop com-prises a set of proportional integral derivative (PID) con-trollers separated into lateral (pitch and airspeed) and longi-tudinal (roll and yaw) channels. These are designed to sta-bilize the UAV and receive mid-level commands of turnrate, altitude, and airspeed. This loop generates the directcommands to the control surfaces: ailerons, rudder, eleva-tor, and throttle. These each contain saturation limits onthe servos, and can be individually tuned in flight. Theouter loop implements a high-level, waypoint-based naviga-tion, which in turn generates commanded turn rate andaltitude for the inner loop. Airspeed is set to a constant andheld by the inner loop PID controller. Altitude is con-trolled with two cascaded PID loops, one to commandpitch based on altitude error with a strong saturation limit(set from the GCS to 15°), and a second to command theelevator to achieve the desired pitch output from the firstPID stage.

The autopilot itself has three different submodes of oper-ation. The first mode is Pilot Feedthrough, where the pilotcommands are received by the RCRx and passed directlythrough the autopilot untouched.

The second mode is Manual Direct, where the GCS sendsmid-level commands directly to the autopilot. Manualdirect can be combined with Pilot Feedthrough such that

Figure 6—Time progression of the UAV position and the computationof the L2 vector during a flight test

Figure 5—Position and attitude filter architecture

Accelerometers

Attitudeestimation

Gyros

Velocity estimateAccel biases

Attitude estimate

Rates estimate

Gyro biases

Baro altimeter

Positionestimation

GPS DataAccelerometers

Attitude estimate

Velocity estimate

Accel biases

Position estimate

2011-5-018_Lizarraga_part2_Layout 1 4/6/2011 9:48 AM Page 40

Page 6: F Reprogrammable UAV Autopilot System (Part 2)elkaim/Documents/... · Reprogrammable UAV Autopilot System ... rate gyro, and magnetometer data are ... of temperature versus sensor

Photo 3a) to make way for a wireframepod (see Photo 3b). This pod, made outof balsa wood, securely housed theautopilot and the radio modem (seePhoto 3c) in the airframe. The pod hastwo functions. One, it protects themain avionics in case of a crash. Andtwo, it provides a vibration-isolatedenvironment on which to mount theautopilot. This also makes the coreavionics trivial to mount andunmount from the UAV for lab testingand HIL simulation.

Flight tests were performed atUCSC’s east field. During the firstthree weeks of flight tests, the UAVwas flown under pilot control. Thecollected flight data were analyzedoffline to verify and validate the innerworkings of the autopilot. This initialtest period was also used to tune theaforementioned position and attitudeestimation filters. Once the positionand attitude estimation filters wereproven to work, one week of flighttests (more than nine flights) wasdevoted to tune the PID gains for theinner loop. The tuning process

www.circuitcellar.com • CIRCUIT CELLAR® 41

May

201

1 –

Issue

250

point that is on the path from the pre-vious waypoint to the next one that isahead of the UAV. The “look ahead”length, called “L2,” is scaled by groundvelocity, making the aircraft less sensi-tive in a tailwind and more sensitiveto cross-track errors in a headwind.The angle between the aircraft and theL2 point is used to command the later-al acceleration of the aircraft (conve-niently, this corresponds to a com-mand-in-roll angle for the UAV).Tighter control is achieved by short-ening the look-ahead distance, but atthe risk of oscillations about the path.Figure 6 shows data from an actualflight path of the UAV in the presenceof wind. The L2 vector is shown ingreen, the UAV’s position is in blue,and the commanded path is in red.

FLIGHT TEST RESULTSThe UAV employed for flight tests

was a modified version of an off-the-shelf Multiplex Mentor radio control(RC) aircraft. Modifying the aircraft’sfuselage involved shaving out a signif-icant section of the “cockpit”‘ (see

the safety pilot controls some of thecontrol surfaces, but the autopilotothers. This is extremely useful whentuning control gains. For instance,throttle is given to the autopilot, butall other controls remain with thesafety pilot. The airspeed control loopis tuned for decent behavior while thesafety pilot can keep the UAV withinthe bounds of the test area. Further-more, the safety pilot can induceerrors by climbing or diving the UAVand seeing if the throttle controlresponds correctly.

The third submode is Autonomous-waypoint Navigation, in which theGCS operator configures the way-points, and the navigation algorithmbecomes responsible for generating themid-level commands. All modesreceive configuration settings from theGCS operator. The inner loop gets thePID gains for each controller. Theouter loop receives the waypoints (inlatitude, longitude, and height) andadditional configurations that are nec-essary for its operation.

In our project, the UAV is tracking a

Photo 3a—External modifications to the Multiplex Mentor RC aircraft.We cut out the “cockpit’’ to mount the autopilot pod. b—The autopilotpod is mounted. c—This is a full view of the aircraft fuselage with thewings and pod mounted. (The green arrow indicates the pod.)

a) b)

c)

2011-5-018_Lizarraga_part2_Layout 1 4/6/2011 9:48 AM Page 41

Page 7: F Reprogrammable UAV Autopilot System (Part 2)elkaim/Documents/... · Reprogrammable UAV Autopilot System ... rate gyro, and magnetometer data are ... of temperature versus sensor

42 CIRCUIT CELLAR® • www.circuitcellar.com

May

201

1 –

Issue

250

involved adjusting the gainof each controller one at atime during flight whilethe safety pilot retainedpartial control of the air-craft. Any time it wasdeemed necessary, the safe-ty pilot could recover com-plete control by flipping aswitch on his transmitterthat turned the analogMUX back to pass through.Eventually, the safety pilotonly had control of the rud-der, using it to skid theUAV through turns to keepit within the test areawhile the inner loop stabi-lized airspeed, roll, alti-tude, and turn rate.

The primary objective ofexperimental testing was to validate the results correspon-dence between Simulink and HIL simulations and flighttests. The secondary objective was to validate thecomplete workflow in the SLUGS: Simulink to HIL simu-lation to flight test. The waypoint track set for the flighttest experiments consisted of four waypoints defining twoshort and two long legs (roughly defining a rectangle). Theflights were always performed within visual range of thesafety pilot, thus the short separation between waypoints.For every flight test, the UAV took off under pilot control.The pilot would do several circuits around the path tomake sure everything was working correctly, then releasethe control to the SLUGS autopilot. Figure 7 shows a typi-cal route around the preprogrammed waypoint track underautonomous control.

It is important to stress that all of the SLUGS’s controland navigation loops were implemented directly inSimulink. A high-level programming language (such as C,C++, or Java) wasn’t used. The Simulink models used forsoftware simulation, HIL simulation, and flight tests areexactly the same. These were first used in Simulink andthen compiled and downloaded to the autopilot hardware.The navigation loop was shown to perform robustly, withgood tracking results even in the presence of turbulenceand wind. The inner and outer loops performed well, andwe achieved basic flight and waypoint following.

TAKING FLIGHTWe presented a top-level view of the SLUGS, an open-

source UAV autopilot, its ground control station, and itsHIL simulator. We presented results for its sensors, tem-perature compensation, and calibration. Using Simulinkas the primary development platform, UAV researcherscan easily transition between software simulation, HILsimulation, and flight testing without the need to re-codethe algorithms in a traditional programming language.

The SLUGS was flight tested under nominal flying

conditions with successfulresults, but there is roomfor improvement. Werecently adopted the open-source GCS softwareQGroundControl. Thisproject is in active devel-opment and has a thrivingcommunity of contribu-tors; but, at its beginnings,it was mostly focused onautonomous quad-coptersupport. This has changed,and now there is a widerange of tools to supportautonomous airplanes.The SLUGS has beenslowly adopting all thesefeatures.

Also, we’re taking stepsto adapt the SLUGS’s code

base so it can work in a wider variety of autonomousvehicles. In particular, work is being done to use theSLUGS in an autonomous boat, a VTOL aircraft, and aground vehicle. Finally, the SLUGS is currently beingadopted in several educational research laboratories inthe United States and elsewhere. This will undoubtedlyhelp improve the overall quality of this project as a com-munity of users each adds their own improvements. I

Mariano Lizarraga ([email protected]) holds a BS in NavalSciences Engineering from the Mexican Naval Academy, anMSEE and Electrical Engineer’s degree from the U.S. NavalPostgraduate School, and a PhD in Computer Engineeringfrom UCSC. He has more than eight years of experiencewriting software and designing hardware for UAVs. He iscurrently a Research Fellow in the Computer EngineeringDepartment at the University of California Santa Cruz.

Renwick Curry ([email protected]) earned an AB in Physicsfrom Middlebury College, and he holds a BS, MS, and PhD inAeronautics and Astronautics from MIT. He has worked as afaculty member (MIT and Cornell), a spacecraft guidanceengineer (Mariner and Apollo projects), and an aviation safe-ty engineer for NASA. Renwick is currently an Adjunct Pro-fessor in the Computer Engineering Department at University

Figure 7—Trajectory plot of the UAV under autonomous control. Theground station was physically located at the origin.

Authors’ note: We would like to express our deepest gratitudeto Greg Horn for working so hard on the board schematics andlayout and hand-soldering the components of the first twobatches of autopilots. He was also our safety pilot, and morethan once he saved us from having to build another UAV. Wewould also like to thank Vladimir Dobrokhodov for all of hishelp in making the SLUGS happen and lending his experienceas a long-time UAV developer and end user. Finally, we wouldlike to thank Lorenz Meier at ETH Zurich for all of his help inintegrating MAVLink into the SLUGS.

2011-5-018_Lizarraga_part2_Layout 1 4/6/2011 9:48 AM Page 42

Page 8: F Reprogrammable UAV Autopilot System (Part 2)elkaim/Documents/... · Reprogrammable UAV Autopilot System ... rate gyro, and magnetometer data are ... of temperature versus sensor

www.circuitcellar.com • CIRCUIT CELLAR® 43

May

201

1 –

Issue

250

RESOURCESM. Euston, P. Coote, R. Mahony, J. Kim, and T. Hamel,“A Complementary Filter for Attitude Estimation of aFixed-Wing UAV,” Australian National University Col-lege of Engineering & Computer Science, 2008.

GitHub Inc., Source code repository, http://github.com/malife/SLUGS

L. Kerhuel, Simulink Embedded Target for PICs,www.kerhuel.eu/wiki/Index.php5, 2010.

R. Mahony, T. Hamel, and J. Pflimlin, “NonlinearComplementary Filters on the Special Orthogonal

Group,” IEEE Transactions on Automatic Control,Vol. 55, No. 5, 2008.

S. Gleason and D. Gebre-egiabher, GNSS Applicationsand Methods (GNSS Technology and Applications),Artech House Publishers, 2009.

S. Park, J. Deyst, and J. P. How, “Performance andLyapunov Stability of Non-Linear Path FollowingGuidance Method,” Journal of Guidance, Controland Dynamics, Vol. 30, 2007.

QGroundControl, MAVLink Micro Air Vehicle Com-munication Protocol, www.qgroundcontrol.org

SLUGS Project, http://slugsuav.soe.ucsc.edu

UC Santa Cruz Autonomous Systems Lab,http://asl.soe.ucsc.edu

SOURCESDSP-3000 Fiber optic gyro (FOG)KVH Industries, Inc. | www.kvh.com

dsPIC33 DSC and MCP9701 ThermistorMicrochip Technology, Inc. | www.microchip.com

of California Santa Cruz, and president of AASI (www.aasi.com), acompany specializing in aircraft flight-path optimization.

Gabriel Hugh Elkaim ([email protected]) holds a BSE inAerospace Engineering from Princeton University, as wellas an MS and PhD in Aeronautics and Astronautics fromStanford University. He is an Associate Professor in theComputer Engineering Department at University of Califor-nia Santa Cruz, where he works on autonomous vehicleguidance, navigation, and control.

2011-5-018_Lizarraga_part2_Layout 1 4/6/2011 9:48 AM Page 43