Posted by Andrus Kangro on September 3, 2009 at 7:00am
I received request to publish more information about my "custom" autopilot, used for TTT contest, so here it is:My autopilot current version is 4. It all started when I received Welleman pic programmer & experiment board as Christmas present (to be honest, it was selected by myself). My initial interest was to make my sailplane turns more efficient and for that I needed to hold wings always crossed with gravity vector.Initial autopilot (V1) was built around 16f627 pic controler and the only thing it did was to use Parallax dual-axis accelerometer for controlling ailerons; rudder and everything else was under manual control. V2 autopilot used pic16f870 chip and added airspeed measurement (using mpxv5004dp sensor) and attempted to regulate sailplane speed by controlling elevator (it also had MPX4115 sensor for altitude measurement, but it was not used at this stage) . When I upgraded my RC-equipment to 2.4G and receiver inputs started to come in parallel groups, then 8-bit PIC did not manage any more and I upgraded autopilot core to dsPIC30f4013 (autopilot v3), adding also GPS to it. As I struggled to understand, what was actually going on inside autopilot during flights, then I attempted to add SD-card logging into autopilot, but faced memory shortage issues on 30f4013. So I upgraded autopilot core to 30f6012 and current version of autopilot (v4) was born.V4 hardware has 3 revisions, I'm using currently revA, but there is also one board of revB ready and its clear by now that there will also be revC. revA has 10 digital change notification inputs (8 for receiver and 2 for accelerometer, all used), 8 analog inputs (gyros, battery voltage, current sensor, 2 pressure sensors - 6 used currently) and 8 servo outputs(5 used), serial port for GPS/2x16 display and SPI port for SD-card. RevB has 14 digital inputs (change notification or input capture), 8 analog inputs and 8 servo outputs, 2 serial ports (for gps and terminal/radio modem), SPI for SD-card and i2c for (future)electronic compass + 3 led's. RevC will change location of some inputs/outputs. I attached Eagle schematics for revision B board here:apv4.sch.Is my autopilot inspired by UAV devboard - no, its parallel development. However, I would probably use UAV devboard, if it would have more powerful chip, much more inputs/outputs and SD-card on board. I'm also looking forward to implement cosine matrix transform in my software like in Matrixnav (waiting for gyros to arrive at the moment). I sometimes browse through Paparazzi and Matrixnav and other firmwares in order to get ideas and avoid inventing bicycle - however, I find it much easier to understand what others have been doing, if I have been working on topic myself also at least to some extent. So, I will keep inventing my own wheel also.As my goal was never to hold wings level with horizon, then there are no IR sensors, although they can probably be connected to vacant analog port if needed.What I've done with my pilot so far: the progress has been very guick since I got SD-card logging working. Once gyroscopes got involved, then direction control started to work reliably as well. My target was to make my sailplane to hunt for thermals and come home automatically and I succeeded on that: current autopilot can control plane speed, altitude and direction and can bring it home if I order it to. It also detects thermals and circles in them, attempting to shift circling center closer to stronger lift area, while making sure that it does not drift too far from "home". During best flight it used engine only for climbing into altitude and the plane stayed up thermalling for one hour without needing to turn on engine again (then I had to leave and took plane down). Thermalling is problematic in my area due to closure of the airport, so I can not do any altitude records where I usually fly. But, if there is suitable weather during some weekend and no family activities, then I plan to drive to G1 airspace area (this is reserved for sailplane flying) and see, how high it really goes....TTT competition is good stimulus for improving autopilot software and algorithms, I'm looking forward for next round. At the end of it I will probably have wp navigation mode with altitude control done via elevator and speed via motor, also navigation accuracy should be greatly improved. Better control over plane (especially when cosine matrix transform is implemented) will help me to make better thermal turns also, so its all very positive what is going on here.
Hi, the code above is missing bit near if((ground pressure but it should be obvious to correct it.
What I wanted to do there is to update ground_pressure with p if gps does not (yet) have fix (so my altitude is always zero or positive, even though negative numbers would work also).
As for sensors, if you do not plan to go above 6000...7000 feet, then HCA0811ARG8 or BSDX0811BARO would give more prescision.
Hi, you can use pickit2 or pickit3 to program & debug dspic, they are very good value for money.
I use ADC autoscan feature to read MPX4115 value and ADC interrupt to write it into variable altinp. Conversion to altitude happens with below procedure, called before calculations loop.
pow btw is very expensive function, it takes about 1ms
void calc_altitude(void){//calculate altitude (in m) based on pressure //
//mpx2125A output voltage formula is vout=vs*(0.009*pressure(kpa)-0.095)
//we are using 12-bit ADC, maximum ADC value is 2^12-1
//metric altitude=(1-(pressure/groundpressure)^0.190263)*44308
//input is altinp which contains pressure sensor ADC value
//output is in variable altitude.
//ground pressure is set to 101325 or to maximum measured pressure
double p;
unsigned int d1;
extern unsigned int state_flag;
extern int altinp, altitude;//, gps_z;
extern double ground_pressure;
extern unsigned int d;
#ifdef __DEBUG
altinp=0x0cfe;
#endif
d1=TMR1;//this code is used for timing below code segment
p=1000000.0/9.0/4096.0*altinp+95000.0/9.0;
if((ground_pressure
<<15)))ground_pressure=p; //ground pressure update ok until gps fix obtained
//altitude=gps_z;//lets skip barometric stuff and use gps
altitude=(int)(44308.0*(1-pow(p/ground_pressure, .190263)));
d=TMR1-d1;//this code is used for timing above code segment
state_flag &= ~(1<<11); //bclr _state_flag,#11
asm("nop");
return;
}
I did search for mpx4165 and mpx6145 and nothing came up...?
hi,
yeah that's cool project ,i made one autopilot as well pic based no stabilisation at all just return to home ..
i haven't got the tools to program DSpic so i jsut gave up for ardupilot ...
i would like to now if your altimeter with the mpx4165 works now,
i would like to add an altimeter using the mpx6145 that i've got (pretty much same as 4165) to get an accurate altimeter variometer...
if you've got something running 'id be very interrested about it!!
Thanks. My code is not secret, who asks can get it, but I do not feel that it has enough quality to make it permanently visible. This may change, as I'm cooperating with a guy for his drone project and one of the topics is rearranging code in a way which enables both of us to focus on specific functionality and still have same product.
As far as hunting thermals, its actually very simple. Please note, even though I use pressure sensor for altitude measurement, its sensitivity is about 2 meters and its noise is around 4 bits, so I cannot trust it to find lift. So I use GPS for thermal detection in spite of its 3 sec latency! Its far from optimal or advanced, but sufficient for having flights like this:
Code is here:
(calculations loop, entered every 20 ms. At the end of it, servos will be outputted)
energyg=9.8*gps_z+((airspeed)*(airspeed))/2;//pot + kin energy wo massenergygsum=(99*energygsum+energyg)/100;
if((ch[7] > 1750)&&((altitude-home_alt) < 350)&&((altitude-home_alt)*4>dist_home)){//hunt for thermals!
if(state_flag&(1<<13)){//new gps altitude received
if((gps_z > gps_z_old+1)&&(energyg > energygsum)){//if both altitude and energy are increasing, then enter/continue turn
liftcounter=10;//if lift is lost, then circle 10 sec more
if(gps_lat >0){//northern or southern hemisphere dictates circling direction
turndir=TRIGHT;//turn right
} else {
turndir=TLEFT;
}
} else {
if(liftcounter>0){
liftcounter-=1;
if(gps_lat >0){
turndir=TFRIGHT;//we are out of lift; turn right faster
} else {
turndir=TFLEFT;
}
} else {
turndir=dir_set;
}
}
}
} else {
liftcounter=0;
turndir=dir_set;
}
Its the code which did fly the plane like in above pic. I've now modified the code not to only detect presence/absence of lift but also react on lift strength, but its not tested yet.
As you see, the code is very simple and sensors are very inadvanced, but it works.
Thank's for this description, your autopilot is very advanced, specially on thermal.
Do you plan to share your works (code/opensource ) ?
Very good work Andrus
Comments
Nice work !
How you are detecting thermals ?
Can you post the code and schematic ?
regards
Rana
What I wanted to do there is to update ground_pressure with p if gps does not (yet) have fix (so my altitude is always zero or positive, even though negative numbers would work also).
As for sensors, if you do not plan to go above 6000...7000 feet, then HCA0811ARG8 or BSDX0811BARO would give more prescision.
sorry for the bad sensor ref , i was mistaken
i was thinking MPX4115 and 6115
fefenin
I use ADC autoscan feature to read MPX4115 value and ADC interrupt to write it into variable altinp. Conversion to altitude happens with below procedure, called before calculations loop.
pow btw is very expensive function, it takes about 1ms
void calc_altitude(void){//calculate altitude (in m) based on pressure //
//mpx2125A output voltage formula is vout=vs*(0.009*pressure(kpa)-0.095)
//we are using 12-bit ADC, maximum ADC value is 2^12-1
//metric altitude=(1-(pressure/groundpressure)^0.190263)*44308
//input is altinp which contains pressure sensor ADC value
//output is in variable altitude.
//ground pressure is set to 101325 or to maximum measured pressure
double p;
unsigned int d1;
extern unsigned int state_flag;
extern int altinp, altitude;//, gps_z;
extern double ground_pressure;
extern unsigned int d;
#ifdef __DEBUG
altinp=0x0cfe;
#endif
d1=TMR1;//this code is used for timing below code segment
p=1000000.0/9.0/4096.0*altinp+95000.0/9.0;
if((ground_pressure
<<15)))ground_pressure=p; //ground pressure update ok until gps fix obtained
//altitude=gps_z;//lets skip barometric stuff and use gps
altitude=(int)(44308.0*(1-pow(p/ground_pressure, .190263)));
d=TMR1-d1;//this code is used for timing above code segment
state_flag &= ~(1<<11); //bclr _state_flag,#11
asm("nop");
return;
}
I did search for mpx4165 and mpx6145 and nothing came up...?
yeah that's cool project ,i made one autopilot as well pic based no stabilisation at all just return to home ..
i haven't got the tools to program DSpic so i jsut gave up for ardupilot ...
i would like to now if your altimeter with the mpx4165 works now,
i would like to add an altimeter using the mpx6145 that i've got (pretty much same as 4165) to get an accurate altimeter variometer...
if you've got something running 'id be very interrested about it!!
regards
fefenin
As far as hunting thermals, its actually very simple. Please note, even though I use pressure sensor for altitude measurement, its sensitivity is about 2 meters and its noise is around 4 bits, so I cannot trust it to find lift. So I use GPS for thermal detection in spite of its 3 sec latency! Its far from optimal or advanced, but sufficient for having flights like this:
Code is here:
(calculations loop, entered every 20 ms. At the end of it, servos will be outputted)
Its the code which did fly the plane like in above pic. I've now modified the code not to only detect presence/absence of lift but also react on lift strength, but its not tested yet.
As you see, the code is very simple and sensors are very inadvanced, but it works.
Do you plan to share your works (code/opensource ) ?
Very good work Andrus
Like that...-
1
-
2
-
3
of 3 Next