QuadCopter (Tri/Quad/Hex/Octa) - propeller chip code discussion
Cluso99
Posts: 18,069
I have changed the title because it has evolved more into help from Jason to get me running.
A number of us are playing with QuadCopters (and similar).
I have found that (myself included) many objects vary in the data range that is being passed between objects, requiring unnecessary code conversions.
For example, the servo inputs can be 0+-500 (-500..+500); 0..1000; 1000..2000.
Now, the 1000..2000 is because this is the ms range of each servo; 0..1000 is just the value normalised; and -500..+500 is centred on 0 obviously.
Similar problems for driving the ESCs where ultimately the are converted to 1000..2000 for output (or something else if using I2C ESCs).
Then, we have the various sensors that can give various results.
For those involved with programming their QUads, do you think a standard range value would make sense???
A number of us are playing with QuadCopters (and similar).
I have found that (myself included) many objects vary in the data range that is being passed between objects, requiring unnecessary code conversions.
For example, the servo inputs can be 0+-500 (-500..+500); 0..1000; 1000..2000.
Now, the 1000..2000 is because this is the ms range of each servo; 0..1000 is just the value normalised; and -500..+500 is centred on 0 obviously.
Similar problems for driving the ESCs where ultimately the are converted to 1000..2000 for output (or something else if using I2C ESCs).
Then, we have the various sensors that can give various results.
For those involved with programming their QUads, do you think a standard range value would make sense???
Comments
I have modified/re-done the ESC & RC drivers (so I could understand how they worked). I have a Wii Motion Plus driver (from Pat Daderko & John Abshier & Jo
I tend to use values of +/- 500 everywhere in my code that deals with ESC / servo values simply because it's easier to do things like scaled mixing, addition, etc. As an example, I read the remote input and it says "go left" (-100). I take that number, multiply it by a scale factor, then add it to the left ESC value and subtract it from the right one. If the value I got from the remote was a PWM length, it'd range from 1000 to 2000, so the math wouldn't work nearly as cleanly.
At the very last moment before I set the outputs in the ESC/servo code, I add the base 1500 uSec value and clamp them. In fact I'm not sure I even do that - I think because I base everything off the "throttle" value as my starting point for addition / subtraction, I add the 1500 to the throttle value, which has the effect of adding it to all the others. I still have to clamp them individually.
To clarify, I used to do this: (and I'm ignoring the rudder here)
but it's more efficient (less code) to do this:
All the remote control inputs are centered at 0 with a range of +/- 500, roughly. It tends to be less than that because of the settings in the remote transmitter - Usually about +/- 400 or a little better.
Regarding standard ranges for things, it might make some things simpler, and others harder. I try to work within the ranges that I get from my sensors for the PIDs, for example. The PIDs just take a measured value and a desired one, and produce an output from that and some constants. If the constants are set properly, the range conversion happens as a side effect, so it doesn't need to be done elsewhere.
Most quad code I've seen, for example, converts the gyro readings into degrees per second values by scaling them from "gyro units" to degrees, and then scaling by the time step between readings. This makes the numbers much easier for people to digest, but the quad doesn't care if it's degrees or some other units *unless* you're using standard library functions like sin, cos, etc where it becomes necessary. Atan2, the function used most of the time, doesn't care what the inputs are - they're vector coordinates.
The other caveat of converting to a standard range is that you may lose precision. My accumulated gyro readings end up being about 320,000 units per 90 degrees, or a little over 1.2 million units per full revolution. If you choose a lower range as a standard, you may sacrifice some of the control nuance you get by having so much precision kept internally. I can't say for sure that would *actually* happen because I haven't tried using lower ranges - I figured keeping the most precision possible would ultimately be a good thing, so I stuck with that. It's certainly possible that I don't need anywhere near the precision I have.
I have been playing with the gyro and accel outputs today from the Wii. I wasn't certain that I was getting anything valid so I started a crude graph using PST and dashes, and scaling it. I think I may be getting correct and usable values now???
If I take the pcb, and roll it 45 degrees it seems the gyros settle quite quickly (few seconds). Is this mormal? Maybe because I am displaying my outputs between reads I am losing what I expected to see.
In order to get an estimate of your orientation, you need to accumulate the samples and display the sum over time. If your gyro rotates left, then right, the sum of those rotations should be zero. You'll have a little drift and sample imprecision in there too, but it should be close to a net zero.
The accelerometer produces a value that, on average, will represent gravity, so those shouldn't settle back to zero like the gyro does, but they'll be much noisier.
If you want, you can download my GroundStation program - The packet format is -really- simple, and only 8 bytes total:
$7F7F = gyro packet
WORD = GX
WORD = GY
WORD = GZ
The word values are 16 bit, signed, raw gyro readings, and it will graph them for you, complete with min, max, average, and variance numbers for the current set of displayed values. It auto-ranges the graph, too. The 7F7F value is rare enough that it allows me to re-sync mid stream. It runs at 115,200 baud and grabs the first valid serial port past COM4 it can find, assuming that's your PropPlug. There's no reason you couldn't make your program send out accelerometer values (or any other three values) instead - it'll just as happily graph those.
Because the sent data is only 8 bytes, and it's raw, you won't fill the TX buffer so the code runs pretty quick. On the prop side I use:
'Send gyro readings at the end of frame dbg.tx( $7f ) dbg.tx( $7f ) dbg.tx( gx >> 8 ) dbg.tx( gx ) dbg.tx( gy >> 8 ) dbg.tx( gy ) dbg.tx( gz >> 8 ) dbg.tx( gz )Jason
So maybe I can get this off the ground tomorrow now
Someone posted in the DCM thread that I'm missing a read termination call in the PASM code (he details the changes). I haven't fixed it on my side yet, so that's likely what's causing your issue with all the reads enabled.
Thought today was going to be the day...but a friend from NZ is coming so guess I will not be attempting my flying code today.
You may also want to check the temperature drift by pointing a hair dryer at your board - if the gyro numbers change quite a bit as the temp goes up you might need to alter my formula to compensate for it.
PUB UpdateLoop | iGx, iGy, iGz, ErrorAxis[3] 'Convert the gyro readings into proper range for the DCM inputs. Also, swap the axis 'around and adjust signs to coincide with my frame of reference iGz := -Gx / 5 iGx := Gy / 5 iGy := -Gz / 5How far off am I?I swap Y & Z from the gyro/accel because I think of Y as down and Z as forward (standard convention in games) whereas the gyro/accel combo treats Z as down and Y as forward. You may or may not need to leave the negations in there.
After some fiddling around... The Wii alternates between the gyro and the accel results, so I have to check and reissue the read command if its not the right one (because I am ignoring the accel for now, but it is wired). The other factor is that the gyro results can be scaled by *2000/440 if a fast move is detected. So, this is close enough (for now anyways) to 4.5 so I just *4 (shl 2) and then do a /2 (shr) and add them together... nice and quick maths.
Anyway, here is my code - you may recognise some of it
PRI FlightLoop ''calculate and update controls every 200Hz 'initialise Gyro_FindZero 'find GX0/GY0/GZ0 over 1s DisplayGyroZero 'setup PID scales RollPID.Init ( 65, 0, 10 ) PitchPID.Init( 65, 0, 10 ) YawPID.Init ( 65, 0, 10 ) loopfreq := clkfreq / 200 'precalc 200Hz loop value to speedup code lastcnt := cnt ''========== Flight Loop ========== repeat {---------- 'read RC Receiver Thro := rcin.get(0) 'read throttle Aile := rcin.get(1) 'read aileron Elev := rcin.get(2) 'read elevator Rudd := rcin.get(3) 'read rudder ----------} Thro := 1500 '<=== fix values Aile := 1500 Elev := 1500 Rudd := 1500 'now limit throttle/aileron/elevator/rudder Thro := 1000 #> Thro <# 2000 'limit between 1ms..2ms Aile := 1000 #> Aile <# 2000 'limit between 1ms..2ms Elev := 1000 #> Elev <# 2000 'limit between 1ms..2ms Rudd := 1000 #> Rudd <# 2000 'limit between 1ms..2ms 'make code friendly +/-0 Thro -=1500 Aile -=1500 Elev -=1500 Rudd -=1500 'calc desired throttle mix ThroMix := (Thro + 400) ~> 2 'approx 0-256 ThroMix := 0 #> ThroMix #> 64 'clamp throttlemix to 1/4 max 'accum integral for Aile/Elev/Rudd iAile += Aile iElev += Elev iRudd += Rudd 'read gyro x,y&z repeat gyro.readMotionPlus 'read data from MotionPlus while gyro.datatype <> GX := gyro.gyrox - GX0 'adj for zero state GY := gyro.gyroy - GY0 GZ := gyro.gyroz - GZ0 'accum integral for gyro X/Y/Z iGX += GX iGY += GY iGZ += GZ 'calc desired Roll/Pitch/Yaw DesiredRoll := iAile << 1 DesiredPitch := iElev << 1 DesiredYaw := iRudd << 1 'calc PID values RollOut := RollPID.calculate ( DesiredRoll, iGX ) PitchOut := PitchPID.calculate( DesiredPitch, iGY ) YawOut := YawPID.calculate ( DesiredYaw, iGZ ) 'calculate ESC output values ThroMix += 1500 'make "servo friendly" again eFront := Thro + ((-PitchOut + YawOut) * ThroMix) ~> 6 eRight := Thro + (( RollOut - YawOut) * ThroMix) ~> 6 eBack := Thro + (( PitchOut + YawOut) * ThroMix) ~> 6 eLeft := Thro + ((-RollOut - YawOut) * ThroMix) ~> 6 {{ 'and limit values eFront := 1000 #> eFront <# 2000 eRight := 1000 #> eRight <# 2000 eBack := 1000 #> eBack <# 2000 eLeft := 1000 #> eLeft <# 2000 }} {{ 'output throttle to all 4 motors (ESCs) rcout.out(eFront,0) ' & output to Esc0 rcout.out(eRight,1) ' & output to Esc1 rcout.out(eBack, 2) ' & output to Esc2 rcout.out(eLeft, 3) ' & output to Esc3 }} DisplayEsc 'display the values ' waitcnt(loopfreq + lastcnt) ' 200Hz loop waitcnt(clkfreq/10 + cnt) lastcnt := cnt 'save for next loop timePostedit: The GX, GY & GZ values displayed have 1500 added to each (keeps display better organised)!
I am using your older PID routine. However, I am not getting what I expect.
I am seeing gyro values 1500 +- 30 for x,y&z but the escs are setting 50us, -250us, 325us, -150us all +- 25us or 50us.
My gyro pcb is sitting on the bench fairly flat and definately still. I have fixed RC inputs of 1500 ea (in code).
Do you have any pointers on what is wrong??? I am just about to display the other readings and calcs.
Here is my debug output
ratronic: This output may help you too. The GX, GY & GZ are the values read from the gyro after the zero values are subtracted.
Looks much better now
PRI FlightLoop ''calculate and update controls every 200Hz 'initialise Gyro_FindZero 'find GX0/GY0/GZ0 over 1s DisplayGyroZero 'setup PID scales RollPID.Init ( 65, 0, 10 ) PitchPID.Init( 65, 0, 10 ) YawPID.Init ( 65, 0, 10 ) loopfreq := clkfreq / 200 'precalc 200Hz loop value to speedup code lastcnt := cnt ''========== Flight Loop ========== repeat {---------- 'read RC Receiver Thro := rcin.get(0) 'read throttle Aile := rcin.get(1) 'read aileron Elev := rcin.get(2) 'read elevator Rudd := rcin.get(3) 'read rudder ----------} Thro := 1500 '<=== fix values Aile := 1500 Elev := 1500 Rudd := 1500 'now limit throttle/aileron/elevator/rudder Thro := 1000 #> Thro <# 2000 'limit between 1ms..2ms Aile := 1000 #> Aile <# 2000 'limit between 1ms..2ms Elev := 1000 #> Elev <# 2000 'limit between 1ms..2ms Rudd := 1000 #> Rudd <# 2000 'limit between 1ms..2ms 'make code friendly +/-0 Thro -=1500 Aile -=1500 Elev -=1500 Rudd -=1500 'calc desired throttle mix ThroMix := (Thro + 400) ~> 2 'approx 0-256 ThroMix := 0 #> ThroMix #> 64 'clamp throttlemix to 1/4 max 'accum integral for Aile/Elev/Rudd iAile += Aile iElev += Elev iRudd += Rudd 'read gyro x,y&z repeat gyro.readMotionPlus 'read data from MotionPlus while gyro.datatype <> %10 GX := gyro.gyrox - GX0 'adj for zero state GY := gyro.gyroy - GY0 GZ := gyro.gyroz - GZ0 'accum integral for gyro X/Y/Z iGX += GX iGY += GY iGZ += GZ 'calc desired Roll/Pitch/Yaw DesiredRoll := iAile << 1 DesiredPitch := iElev << 1 DesiredYaw := iRudd << 1 'calc PID values RollOut := RollPID.calculate ( DesiredRoll, iGX ) PitchOut := PitchPID.calculate( DesiredPitch, iGY ) YawOut := YawPID.calculate ( DesiredYaw, iGZ ) 'calculate ESC output values Thro += 1500 'make "servo friendly" again eFront := Thro + ((-PitchOut + YawOut) * ThroMix) ~> 6 eRight := Thro + (( RollOut - YawOut) * ThroMix) ~> 6 eBack := Thro + (( PitchOut + YawOut) * ThroMix) ~> 6 eLeft := Thro + ((-RollOut - YawOut) * ThroMix) ~> 6 {{ 'and limit values eFront := 1000 #> eFront <# 2000 eRight := 1000 #> eRight <# 2000 eBack := 1000 #> eBack <# 2000 eLeft := 1000 #> eLeft <# 2000 }} {{ 'output throttle to all 4 motors (ESCs) rcout.out(eFront,0) ' & output to Esc0 rcout.out(eRight,1) ' & output to Esc1 rcout.out(eBack, 2) ' & output to Esc2 rcout.out(eLeft, 3) ' & output to Esc3 }} DisplayEsc 'display the values ' waitcnt(loopfreq + lastcnt) ' 200Hz loop waitcnt(clkfreq/10 + cnt) lastcnt := cnt 'save for next loop timeIn the following outputs, the gyros and Escs are converted to 1000-2000 range with 1500 being the center.
In case it's not clear, that ThroMix value is just used a scale to apply to the mixing code so that a non-throttle stick input when it's at rest won't start the motors spinning. I smoothly mix the balance code back in over a very short throttle range that wouldn't be useful in flight anyway. It'd be easy enough to make it just switch on/off instead, but this seemed cleaner.
I like your limiting code - I was pretty sure those could be done on one line, but wasn't sure how. I may have to adopt that.
IE:
eFront := 1000 #> eFront <# 2000
I'll time them to see which is faster, but yours looks like it'd be fewer ops.
Now to pluck up the courage to give it a real try...
The code in 1 line should be quite a bit quicker. The maths is where I improved the faster spin interpreter quite substantially, so maybe I should give that a run later. Actually, I hate using those types of shortcuts ( ~>, #>) and even +=, -= but unless the compiler is smart, the code will not be reduced as much as it could be.
Off to have a New Years Eve drink and then try out the code. For Ken... I will try and avoid the cars!
Once you've gotten past the "at arms length" tests, then go try it outside. I've had a few cases where I did the latter first and wound up with an upside down quad and a busted prop because I did something silly in the code.
I could only get 3 motors to spin (its not the motor or esc because I could reverse them. So yes there is something wrong in the code even though it looked reasonable on the bench. Back to the bench and remove the props to sort it out. At least here I can see the output on the pc.
But this is actually good because its the first time I have used the gyro data with the motors and there is feedback because I can move the stick and see the responses.
Suppose I best do the right thing and give this a miss till tomorrow. Its 1645 here on 31Dec. New Years only 7 1/4 hrs away
Here are two photos of my setup and electronics (sorry they are a bit out of focus)
I am also working on quadcopter control quite while.
Previously, I have used IDG500 and IXZ500 gyros and ADXL335 Accelerometer. And I had some success http://vimeo.com/23909597
But it was not stable as much as I want.
After I saw Jasons quad, (I think its a great success), I am planning to use ITG3200 and ADXL345 (http://www.sparkfun.com/products/10724)
@Jason
I am planning to use your driver for ITG3200 and ADXL345. You have included CORDIC Atan function, what is the format for ARX and ARY ?
Please help
@Cluso
Is there any news from your flight test ?
Aydin
I think they are 32 bit full range values, meaning 1^32 = 360 degrees. I never ended up using those values anyway - My DCM version of the flight code uses the gyro and accelerometer values directly, then uses ATan2 and ASin from a modified Float32 object to compute angles from the orientation matrix.
If you want something that is self-leveling, I have code that works, but I crashed while doing some testing (I did something stupid). I need to build another setup to finish doing the tuning, but it shouldn't take very long - possibly another week or two. It uses the same ITG/ADXL object, so if you're using those chips it will likely drop right in.
Edit: I have been playing with your Kalman manager and an IMU that has both of these chips although I really don't have a clue what I'm messing with. But I have experimented with the QAngle, QGryo, and RAngle values and wonder how you even go about calculating those.