Feasability of waypoint navigating autopilot with PIC16

[kodel]

I was thinking that it must be possible to implement a full waypoint navigating autopilot using something like a PIC16F627:

component list:
- 1 PIC
- 1 cheap NMEA GPS
- 4 thermophiles
- 4 resistors (1 voltage reference, 1 rs232 voltage limiter)
- 5 header pins and 5 header sockets

Design strategy:
- use the thermophiles in a 2 pair XY arrangement to control aileron/elevator for attitude stabilisation (copying the FMA copilot, but with less parts)
- use trottle to control GPS height (requiring positive engine pitch moment)
- use rudder to navigate to GPS waypoint

Implementation stategy:

- I would link the thermophiles in a classical front to back pair and feed each pair to a voltage comparator input of the PIC. Of course i need to connect the other end of the pair to a voltage divider created by the two resitors (between Vss and Vdd). I would configure the comparators to reference against the PIC's internal voltage reference. In the main software loop of the PIC I would then iterate through the 16 steps that the internal voltage reference can be scaled to, essentially creating a very slow (0.16 ms so resolution about 6 KHz, so certainly still fast enough for flight control) 4-bit sloping A/D convertor. I think that for simple stabilisation the resolution of 4 bits might be enough. Possibly I need to average out over time (PI control) instead of just giving more servo output when more unbalance is found (P control).
- I would read out the NMEA GPS using the UART of the PIC, by just waiting byte by byte for the heigth data bytes to pass along. Then when they have all arrived I would make the trottle servo calculation by comparing with the desired heigth of the next waypoint and increase throttle to max if we still need to go a lot higher. Of course when height is almost reached, trottle is backed of progressively. When needing to descend, I would bring trottle to zero. Again, I think an averaging over time is needed for the non-extreme values to avoid hunting around the target height.
- Same thing for the direction control, although calculation is a bit more difficult here. I would just use simple triangulation as a real world approximation to determine the direction we need to go in. Also here integration is needed for the smaller values of rudder deflection. (note: actually only using the small deflection values to average out until a big defelection comes along turns the PI into some rude PID if I remember my theory correctly)
- The waypoints would be stored in the eeprom of the pic at programming time. So no in the field update, unless I do in circuit programming with a batch file connecting a google-earth frontend to an automated source editor/compiler/programmer.
- The outputs to the servo's would be implemented with 2 simple timers and a couple of status registers: 4 registers hold the target on time (0=1 ms, 255=2 ms), another register hold the channel number we're currently outputting. If I put timer 1 on the normal frame value of the servo pulse cycle, I just need to pull RAx high on each timer expiration interrupt of timer1, with x determined by the register that holds the current channel. Then I would look up the register holding the target time for that channel and start timer2 with that value. When that timer expires I just need to pull the RAx line down, increase the regsiter holding the channel number (or put on 0 when it is already 4) and exit the interrupt. When timer1 expires the next time, it will start the pulse for the next servo's channel and so on.
- since I have enought inputs to spare (even on the 18 pin chip), I still can take 5 inputs of the RC receiver: 1 channel to use as a switch between auto/manual mode and the other 4 channels to pass transparently to the output when in manual mode.
- from a mechanical point of view, I would make the unit into a "bloc" that simply connects between the receiver and the servo cables. The only other connector is the one going to the nmea gps. (which of course best sits on top of the plane, with the thermophiles integrated in the package)


I'm looking for feedback of people experienced in PIC microcontrollers. I only di 1 project so far (didn't even bothered to learn about timers so implemented my own timing stuff in asm counting CPU cycles etc. Still in daily use as a domotics controller).

I know most projects involving stabilisation/waypoint generation involve much more complex hardware, but I don't see why a 2$ PIC16F627 would not be up to the job. The only thing it can't do is read waypoint data from an external source, unless in circuit programming is used .

Am I missing something or should I really try to get this project started? Sounds fun for a total cost of maybe about 10 dollar ! (GPS is available for 40$ on ebay)

[zik]

You might want to add a barometric pressure sensor for better altitude control but yes, I'd say it's do-able. For a simple control loop autopilot a PIC is adequate. With these sensors it won't be as stable as an autopilot with gyros etc but it'll work as long as your airframe is stable enough.

Incidentally thermopile sensors aren't cheap. If you buy them from digikey they'll cost you $60 just to start with. It's cheaper to buy a copilot and rip the sensors out but the conformal coating (which is there for weatherproofing) makes it a messy process.

Good luck with your project!

[_helitron_]

Hi kodel,

perhaps here you can buy some cheap thermopiles at the moment

http://www.rcgroups.com/forums/showpost.php?p=8202227&postcount=180

Cheers,

//Erwin

[kodel]

I also discovered the incredible high price of the thermophiles. I'm sure that the manufacturers making e.g. in-ear thermometers are not paying more then 2 dollar a piece for them, but that doesn't help me, does it.

Thanks for the link to the guy selling of his thermophiles, but this is not intended as a one-off. The design goal is to do this with cheap widely available parts, so I'm thinking of moving to IR receivers instead of thermophiles. I'll have to do some research on spectral response and angle of view here.

An added benefit would be that I could write a routine to program the waypoints using the built in infrared of a laptop. This would give you wireless programmability ! Of course I would need to add a LED somewhere to signal positive reception of the new waypoint list.

This project might become dormant for a while (busy job, kids, wife, etc) but by year-end I'll definately move this beyond the idea stage.

I also came up with an algorithm for a self calibrating control loop. This algorithm is simpler than a PID controller and doesn't require you to go through a training phase to detect midpoint input, extreme input and control response amplitude. It basically is continiously calibrating. I hope to confirm with flight tests what is already working in my theory :)

[dmgoedde]

If I were you, I would start out making the simplest system that satisfies the most basic goals you have, then you can tune and make more sophisticated over time. You could cut out thermopiloes and barmoeter... just pick a dynamically stable airframe, and use GPS altitude. It will work fine +/- 20 or 30 meters...just fly the plane high enough so it doesn't matter.

The key things to accomplish first you already listed - feeding servos steady stream of pulses, how to steer to the waypoints, how to control altitude (can do throttle control for a dynamically stable plane, leaving elevator locked in a good trim position, and just fly when the wind is relatively calm). Practice on how to implement control theory to hit steering and altitude targets. As a very rough start, you can just use P term of PID (proportional control)...assuming the trim of steering is perfect. Just using P for throttle control of altitude will work also to some degree, though you will never reach the exact target, but there will be some offset... but who cares if your plane always settles 20 meters too high or too low from the target?,.. it will be more than a good start.

You need to solve the basic guidance problems of navigating to a target location. You mentioned triangulation... I'm not sure what you mean in regards to this issue of steering to target. Perhaps you meant using some triangle trigonometry equations.... pathagorean theorem for distance to waypoint and required angle between plane and target. A key here is GPS will report the current heading direction (0 is North, 90 is East, so it goes opposite direction from Cartesian coordinate system and is 90 degrees offset... heading = 90 - Cartesian angle), and you can calculate the required heading... then compute the delta between actual and required heading, and create steering response based on that. GPS will also report current location. You get the target location from on-board memory (eeprom you mentioned) and calculate required heading to be flying to fly right at the target by Arcsine(DeltaLat/DeltaLon). You may be able to implement a 8 bit lookup table in your software to handle the trigonometry to sufficient degree of accuracy. There are some complications, like Longitude angles have different size than latitude angles depending how far away you are from equator.... so you have to correct DeltaLon before using it in the equation I showed.

This is a complex project to accomplish even the most basic working autopilot... some more advice is make a good outline of all the critical things that have to take place.... make flowcharts of how the program needs to flow. A big thing is to debug the code in functional blocks.... get GPS parsing to work (converting NMEA ascii text into numeric version), get the trigonometry worked out in all 4 quadrants, put the autopilot on just the steering servo of a radio-control truck and just control the throttle yourself by R/C.... Keep working your way up. Later after it is all integrated, then add in the I term of PID for self-trimming to hit the targets more accurately and remove persistent offset or error... then add the D term so that you can crank up P without inducing oscillations... later try some other control schemes besides PID.

I thought I could use a Basic STAMP to make my first autopilot... it actually required two working together. Reason being that it was too much to handle both feeding servos steady stream of pulses, and waiting for GPS asnchronous data. Perhaps your pic scheme could use some servo driver chip to handle that load.

[kodel]

Thanks for the tips dmgoedde. I know it will be a hell of a ride, but these are the kind of challenges I always seek out. (currenlty managing/architecting a SW project with +500.000 lines of code already...)
There are indead a number of sub-project to get working before something usefull comes out. But that's the fun of it. The nice things is that you need to go to the 4 quadrants of the world to check out all the LAT/LONG positive and negative coordinates, gets you some travel experience:)

My interest comes mainly from aerial photorgaphy. I envision a system that lets me click the targets to be photographed (mostly houses), then it lets me enter the distances I want to photograph it at (e.g. 40 and 60 meter) and then the plane takes off, makes a circle at a distance of 40 meter from the target measured at 45 degrees elevation, then does the same thing at 60 meters and then moves on to the next target. At the same time the camera is continiously shooting at a 45 degree angle, so the house is always in the center of the frame as long as the wings are level.

An alternative is to steer the plane manually and control the camera on it's 3 axes with GPS (independently of the plane) to always keep the target in the center of the frame. That would be a big seller I think for full scale pilots who want to do some aerial photography.

To sort out my control algortihms (which will be the hardest part, programming is just logic) I am thinking of using my Pocket PC. I already did some NMEA decoding on that one. At least it provides an environment which is very easy and fast to program, has plenty of space for detailed log files and can even be remotely debugged over a serial connection. (putting a breakpoint on an airborn flight controller might not be such a good idea :))

But anyway, I'll start with a simple steer-rudder-to-get-back-to-launch-site algorithm, that is the first milestone to reach.

[kodel]

with triangulation I meant calculate the target heading by applying the Pythagoras theorem on the LAT/LONG coordinates by assuming they are in a flat plane with a linear coordinate system. I suppose it will be close enough as long as waypoints are not too far apart. Since our models can't do really long distance flights, it doesn't matter too much.

[kodel]

Just to let you know that I found a possible alternative for the expensive thermopiles:

I'm going to try to use an infrared sensor that's used in motion detectors for alarm systems. The problem is that they have multiple substrates connected front to back in order to avoid false alarm when e.g. the light is switched on in the room. But I'll see how we deal with that.

The part number I'l lbe using is IRA-E700ST0 from Murata, 3 Euro per piece.

[Tom Harper]

kodel,

It's a great do it yourself adventure, and well worth the time.

I did it with the TI MPS430f2012. It's a bit more sophisticated than the PIC line but still cheap. It is a 16 bit processor, 2K program memory, A/D and 'in circuit' programming. The development software and hardware costs $20.

My system uses integer math and a compaction scheme for tables. Also used a flat earth method for navigation. I set a reference point then calculate everything from there in feet or meters. Makes it easy. I can post it here if you are interested. Would be glad to ship you the code as a starting point. Here's the blow by blow saga:

http://www.rcgroups.com/forums/showthread.php?t=546671

Tom

[AdamKt1]

My experience with PICs was not very good.
My algo,for turning, was based on getting the lat/lon from the GPS and then calculating the direction by Haversine formula. Later I tried Law of cosines also. The problem was the decimal accuracy of Pics, although I used better PIC then the one you have selected.
Currently I am working on 32 bit dsPIC microcontroller(30F4011).

A better approach would be to use the APB sentence of GPS. This will be very useful and this will eliminate the calculations for heading.

I would like to know about your success in wing levelling using thermopile, I hope the best for you in your project.

[dmgoedde]

with triangulation I meant calculate the target heading by applying the Pythagoras theorem on the LAT/LONG coordinates by assuming they are in a flat plane with a linear coordinate system. I suppose it will be close enough as long as waypoints are not too far apart. Since our models can't do really long distance flights, it doesn't matter too much.

I do understand what you are saying about keeping it simple with flat plane assumption - which is what I always do. Consider this however:

In Brussels you are at 50.8N degrees latitude... which means that far North, 1 unit of longitude is only 63% as big (in distance like meters) of 1 unit latitude (longitude lines compress together as you approach the North or South pole). Before doing arcTan of (deltaLat/deltaLon) to get required angle to fly, you just correct Delta-Lon by multiplying it by 0.63 (which is the Cos of latitude, 50.8degrees). Without this (small) correction, the airplane will still reach target, but will 'curve' into it... not point right at target when far away, and only curve into it at very close distance, almost like the curve of a Nautilus shell.

For sake of simple math, you could choose to approximate this correction very crudely, say just assume for now all of your flights will be near 50 degrees latitude, so just always multiply Delta-Lon by 2 then divide by 3 (if doing integer math) which is very close to multiply by 0.63. Then your ArcTan calculation will give very accurate required angle of flight to intersect waypoint.

I brought this idea up because whe I started, I was scratching my head for a while because steering wasn't quite right (even at just 33 degrees latitude)... this correction wasn't intuitive to me from the start.

[kodel]

@Tom: Love your thread, definately a lot of experience in there. Specially the way you work with tables. I can learn a lot from this. Als the TI looks interesting, although I am going through for the first experiments with PIC since I'm familiar with the architecture and opcodes now.

@Adam: I am using the $gpgga and $gprmc sentences for direction and lat/long. The only mathematical challenge is the squareroot problem, I'll have to read up on how to do that in pic asm.

@dmgoedde: Completly understood. But I would feel bad of writing code that only works in my part of the world. Kind of limits the fame and fortune you can achieve :) A possibility would be to calculate the correction factor for the center of the flight in the PC based waypoint editor software. Then send this correction factor along with the waypoints to the pic

[kodel]

Since I'm not inventing anything new with the GPS stuff, I'll leave that for phase2 of the project.
I'll start of by making an equivalent of the FMA copilot but with cheap PIR detector sensors. If it works out, I only need 2 sensors to stabilise both roll and pitch axis:

Each sensor already contains 2 elements wired in series, but with opposite polarity. Exactly what is used for copilot-like stabilisation. The only problem is that both elements are facing the same direction (this is used to detect motion from an IR emitting body: if 1 elements changes with respect to the other, you know there is motion, at least if you combine it with a fresnel lens).

My solution is to let a single sensor point upwards instead of sideways. I would mount the sensor with one element to the left side and the other element to the right. Then I would attach a small mirror on top of the sensor at an angle of 45 degrees from the horizon. The mirror is attached to the center of the sensor, between the two elements. A second mirror is also attached at this same point, but facing the other direction (e.g. right instead of left). For roll stabilisation, it would look like a small V tail.

In ASCII art a view from behind would look something like this:

\ /
\/
======
| |
I ordered the parts today, I'll let you know if it will work.

[vector_vortex]

Hi kodel,

I bought some of those sensors from farnell as I was too cheap to buy "proper" thermopile sensors. My proposed theory of operation was something like this:

||_____
---|| | <- sky visible through this side, ground obscured
|| |----- <- horizontally oriented thin split
---||_|___ <- ground visible through this side, sky obscured
||


This way the top should see sky only and the bottom should see ground only. It sounded good in theory. use the built in differential nature of it to your advantage

My second idea was to place the sensor on an axially rotating platform and use an encoder to determine the horizon,

None of these have been tested maybe sometime in the future

Have Fun

Joel

[kodel]

If you want to go with your first approach, I think it would be easier to just block the radiation to one of the elements and just a sensor both on the let and the right. Then you end up with a normal thermophile situation again.

The problem with your approach is that you need to go for the maximum value, so you need to implement a delta-voltage detector an always keep oscilating around the peek voltage.
What I see as very difficult to solve in you method is that if the wing drops, the voltage goed down. But if the wing rises, the voltage also goes down. Looks like it's going to be dynamically unstable.

That's why I'm going for the V-tail like approach, I'll let you know if it works.

[kodel]

For those of you who have an operational autopilot, maybe you can try this very simpel control mechanism:

1. read out location from GPS
2. calculate the needed heading to destination
3. calculate correction factor:
correction factor = correction factor calculted during previous iteration + heading calculated in (2) - heading calculated in (2) during previous iteration
4. set rudder value = current rudder value - correction factor

This is essentialy only doing the "I" part of a PID controller. The behaviour is very simple: the more the plane is off-course, the faster the change in rudder deflection to get it back on course will be. I guess with an update speed of once per second and a 10% rudder deflection change for every 45 degrees out of course this should work pretty OK on a trainer type of plane. Of course I would limit the deflection of the rudder to about 50% or so.

Advantages:
- It automatically compensates for drift (e.g. when you are flying with a sidewind)
- no need to know your current heading
- no need to know/calibrate what value is needed to put rudder in the center position

Disadvantage:
- might oscilate more then a P controller
- oscilations might not converge in all situations (depends on how aggresive rudder is adjusted). I just don't know how much this is needed in practise.
- when abrupt course changes are needed it might take some more time then with a P controller

It will be a couple of months before I can test this out, so if anyone wants to have a go, let us know the results !

[Tom Harper]

Kodel,

Actually the value you calculate is direction and distance of 'course made good over the ground'. If there is no wind this will be close to Heading. If there is wind the model will drift to windward allowing the Bearing to target to change. The result will be that the airplane continues to change it's Heading until it has passed the target and is able to approach it directly up-wind. The path looks like a fish hook. Several UAV'ers have reported that behavior on this forum.

One cure is to use a number of closely spaced waypoints. Another is to calculate two bearing errors. One to the next waypoint and another to the last waypoint. If they are not 180 apart you are drifting off course.

Be very careful of control deflection. That was my major error. Start with an eentsie beentsie (technical navigation term).

Tom

[dmgoedde]

@dmgoedde: Completly understood. But I would feel bad of writing code that only works in my part of the world. Kind of limits the fame and fortune you can achieve :) A possibility would be to calculate the correction factor for the center of the flight in the PC based waypoint editor software. Then send this correction factor along with the waypoints to the pic

I only meant that hard-code your part of world as a start - you will have plenty of other challenges to juggle and figure out besides this one. After initial development in your region, then make the code generic - just a suggestion.

[kodel]

@Tom: I disagree.

The fishhook thing is exactly what you get when you try to move in a certain heading: You calculate the heading, you point the plane in that direction and surprise: it ends up somewhere else.
To avoid this, I would use the scheme mentioned above. Even with a sidewind, the plane will move in the direction of the target. The direction that the nose is pointing to will be a different one, to compensate for the weather.

I think that if you read it carefully you see the difference with the conventional control loop. Or am I wrong somewhere?