ESC Servo Code PASM Problem
Hey Guys,
This is one of the first PASM programs I wrote, and it has been quite awhile since I wrote it. Also I have not written a PASM program since. It is for my quad project! The program is designed to run 8 ESC's at a 400hz rate. I thought the program was running fine, I set it up using my O Scope. But I noticed a problem while flying my quad, every once in awhile all 4 motors will glitch out for just a fraction of a second. Sometimes it won't happen at all during a 10 minute flight and sometimes it will happen 4 or 5 times. I think since it happens to all 4 motors that it is in my servo code.
Here is the code, its not real long but it is in PASM I am hoping that someone that is good with PASM can take a look at it with out wasting a bunch of their time. The program is basically my interpretation of the Servo 32 code. I tore it apart and tried to slim it down for just 8 ESC's. The sync portion is probably where the problem is, I figured out how many clock cycles it takes to run at 400hz. Then I add it to CNT and if it throws up an overflow flag I run the sync loop again. It looks like it should work to me but I am not sure. I suppose it is possible that my ESC's cannot run at 400hz but even if that is the problem I don't see that causing all four ESC's glitching out at the same time. I will try changing the wavelength tomorrow.
Thanks
Shawn
This is one of the first PASM programs I wrote, and it has been quite awhile since I wrote it. Also I have not written a PASM program since. It is for my quad project! The program is designed to run 8 ESC's at a 400hz rate. I thought the program was running fine, I set it up using my O Scope. But I noticed a problem while flying my quad, every once in awhile all 4 motors will glitch out for just a fraction of a second. Sometimes it won't happen at all during a 10 minute flight and sometimes it will happen 4 or 5 times. I think since it happens to all 4 motors that it is in my servo code.
Here is the code, its not real long but it is in PASM I am hoping that someone that is good with PASM can take a look at it with out wasting a bunch of their time. The program is basically my interpretation of the Servo 32 code. I tore it apart and tried to slim it down for just 8 ESC's. The sync portion is probably where the problem is, I figured out how many clock cycles it takes to run at 400hz. Then I add it to CNT and if it throws up an overflow flag I run the sync loop again. It looks like it should work to me but I am not sure. I suppose it is possible that my ESC's cannot run at 400hz but even if that is the problem I don't see that causing all four ESC's glitching out at the same time. I will try changing the wavelength tomorrow.
CON
_clkmode = xtal1 + pll16x
_xinfreq = 5_000_000
MHZ = 80
UpDate = 2_500
NoGlitch = 3_000
_1uS = 1_000_000
VAR
Long ServoOutput[8]
OBJ
PUB Start
'ServoOutPut[0] := 80_000 '1.0mS
'ServoOutPut[1] := 80_000 '1.0mS
'ServoOutPut[2] := 80_000 '1.0ms
'ServoOutPut[3] := 80_000 '1.0ms
'ServoOutPut[4] := 80_000 '2.0ms
'ServoOutPut[5] := 80_000 '1.75mS
'ServoOutPut[6] := 80_000 '1.5mS
'ServoOutPut[7] := 80_000 '1.25ms
cognew(@ServoOut, @ServoOutput)'Launch new cog
Pub Set(ServoIndex,Width)
ServoOutput[ServoIndex] := constant(80000000 /1_000_000) * Width 'calculate # of clocks for a specific Pulse Width
DAT
ORG 0
ServoOut
mov Address, PAR
mov DIRA, Servo_Pin_Mask
Sync
mov Temp, cnt
mov WaveTemp, Wave_Length
add WaveTemp, cnt wc
if_c jmp #Sync
LoadServoWidth
rdlong ServoWidth1, Address
add ServoWidth1, Temp
add Address, #4
rdlong ServoWidth2, Address
add ServoWidth2, Temp
add Address, #4
rdlong ServoWidth3, Address
add ServoWidth3, Temp
add Address, #4
rdlong ServoWidth4, Address
add ServoWidth4, Temp
add Address, #4
rdlong ServoWidth5, Address
add ServoWidth5, Temp
add Address, #4
rdlong ServoWidth6, Address
add ServoWidth6, Temp
add Address, #4
rdlong ServoWidth7, Address
add ServoWidth7, Temp
add Address, #4
rdlong ServoWidth8, Address
add ServoWidth8, Temp
sub Address, #28
OutPutLoop
cmpsub ServoWidth1, cnt nr,wc
muxc ServoByte, ServoOut1
cmpsub ServoWidth2, cnt nr,wc
muxc ServoByte, ServoOut2
cmpsub ServoWidth3, cnt nr,wc
muxc ServoByte, ServoOut3
cmpsub ServoWidth4, cnt nr,wc
muxc ServoByte, ServoOut4
cmpsub ServoWidth5, cnt nr,wc
muxc ServoByte, ServoOut5
cmpsub ServoWidth6, cnt nr,wc
muxc ServoByte, ServoOut6
cmpsub ServoWidth7, cnt nr,wc
muxc ServoByte, ServoOut7
cmpsub ServoWidth8, cnt nr,wc
muxc ServoByte, ServoOut8
mov OUTA, ServoByte
cmp WaveTemp, cnt nr,wc
if_NC jmp #OutPutLoop
jmp #Sync
Address Long 00_0000_0000_0000_0000_0000_0000_0000
Servo_Pin_Mask Long 00_0000_0000_0000_1111_1111_0000_0000
Wave_Length Long 200_000' = 400hz 320_000 = 250hz
ServoOut1 Long 00_0000_0000_0000_1000_0000_0000_0000
ServoOut2 Long 00_0000_0000_0000_0100_0000_0000_0000
ServoOut3 Long 00_0000_0000_0000_0010_0000_0000_0000
ServoOut4 Long 00_0000_0000_0000_0001_0000_0000_0000
ServoOut5 Long 00_0000_0000_0000_0000_1000_0000_0000
ServoOut6 Long 00_0000_0000_0000_0000_0100_0000_0000
ServoOut7 Long 00_0000_0000_0000_0000_0010_0000_0000
ServoOut8 Long 00_0000_0000_0000_0000_0001_0000_0000
ServoByte Long 00_0000_0000_0000_0000_0000_0000_0000
ServoWidth1 Long 00_0000_0000_0000_0000_0000_0000_0000
ServoWidth2 Long 00_0000_0000_0000_0000_0000_0000_0000
ServoWidth3 Long 00_0000_0000_0000_0000_0000_0000_0000
ServoWidth4 Long 00_0000_0000_0000_0000_0000_0000_0000
ServoWidth5 Long 00_0000_0000_0000_0000_0000_0000_0000
ServoWidth6 Long 00_0000_0000_0000_0000_0000_0000_0000
ServoWidth7 Long 00_0000_0000_0000_0000_0000_0000_0000
ServoWidth8 Long 00_0000_0000_0000_0000_0000_0000_0000
Temp Long 00_0000_0000_0000_0000_0000_0000_0000
WaveTemp Long 00_0000_0000_0000_0000_0000_0000_0000
Fit 80
'.000_000_012_5nS per clock cycle
'1_600_000 clock cycles = 20mS or 50hz
'200_000 clock cycles = 2.5mS or 400hz
Thanks
Shawn

Comments
cmp WaveTemp, cnt wc if_NC jmp #OutPutLoop jmp #SyncIf the wavetemp + CNT roll over $FFFFFFFF the wc flag should be set and it will loop back to sync. The most I would lose is 1 maybe 2 wavelengths. Which at 250hz that would only be 8mS. I don't think that would be noticeable. Or am I missing something else?
Thanks
Shawn
To see the effect just add these 3 lines before your sync loop (don't connect anything expensive to the outputs in case it can't cope).
neg cnt, #14 sub cnt, Wave_Length waitcnt cnt, #0This will prime WaveTemp to be -1 when leaving the sync loop.Thanks
Shawn
Thanks
Shawn
Servo8Fast.spin
CON _clkmode = xtal1 + pll16x 'Standard clock mode * crystal frequency = 80 MHz _xinfreq = 5_000_000 VAR OBJ ESC : "Servo8Fast" PUB Main ESC.Init 'Initiate ESC Program. ESC.AddFastPin(15) 'Actual Pin number on Prop. ESC.AddFastPin(14) 'Actual Pin number on Prop. ESC.AddFastPin(13) 'Actual Pin number on Prop. ESC.AddFastPin(12) 'Actual Pin number on Prop. ESC.Set(15, 1000) 'Set ESC value before start up. ESC.Set(14, 1000) 'Set ESC value before start up. ESC.Set(13, 1000) 'Set ESC value before start up. ESC.Set(12, 1000) 'Set ESC value before start up. ESC.Start 'Start ESC COG.Thanks
Shawn
''Note that the INDEX passed here is [COLOR="#FF0000"]in the range 0 to 7[/COLOR], and does NOT ''specify a pin number like the normal Servo32 object PUB Set(ServoIndex, Width) 'Set Servo value ServoData[ServoIndex] := constant(80000000 / _1uS) * Width 'calculate # of clocks for a specific Pulse WidthAlso, the Servo8Fast object is currently hardwired for pins 0..3 and 28..31 (see ZoneShift# table). Adjusting Set() parameters and the table should get you there. The AddFastPin() parameters should stay the same.Thanks for the reply
Shawn
I got the code to work with the pins stated above, I thought I had got a hold of the code Jason Dorie modified so any pin can be defined, I guess not.
Shawn
https://drive.google.com/folderview?id=0B0DJmXrvrE-IemxSdktUTmZHd2s&usp=sharing
History:
Version 1 - initial concept
Version 2 - (03-08-2006) Beta release
Version 3 - (11-04-2007) Improved servo resolution to 1uS
Version 4 - (05-03-2009) Ability to disable a servo channel
and remove channel preset requirement
Version 5 - (05-08-2009) Added ramping ability
Version 6 - (07-18-2009) Fixed slight timing skew in ZoneLoop and
fixed overhead timing latency in ZoneCore
Version 7 - (08-18-2009) Fixed servo jitter in ramping function when
servo reached it's target position
Version 8 - (12-20-2010) Added PUB method to retrieve servo position
Version 9 - (04-05-2013) cnt rollover issue corrected with ZonePeriod
by setting up Counter A and using the Phase
accumulator instead of the cnt
Note: This also eliminates the need to setup
a 'NoGlitch' variable
I got my board rebuilt and the Servo 8 Fast code running. I am still having glitching or twitching problems with all 4 motors every once in awhile. It can happen once a minute or 2 or three times in the matter of a couple of seconds. I do not think it is the servo 8 fast code, has anyone else had problems using the servo 8 Fast object, I have it running at 250 Hz. It is version 1.3 I believe. I have beating my head over this for awhile, I am going to change out my receiver and see if that is the problem.
Shawn
Shawn
waitcnt( constant(80_000_000 / 250) + LastTime) LastTime += constant(80_000_000 / 250)This is my main flight loop.
'>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> PUB Flight LastTime := cnt Repeat DbgCountLow := cnt 'Used to time Flight loop. Check_Armed_Status Get_RC Get_GYRO Convert_GYRO_2_Angle Convert_ACC_2_Angle LowPass_Filter_ACC Complimentary_Filter Change_RC_2_Angle If ARMED == TRUE DoInt := True Roll_PID Pitch_PID Yaw_PID Motor_Output_Calc Write_to_ESC Else IErrorRoll := 0 IErrorPitch := 0 DoInt := False Check_Gestures DbgCountHigh := cnt Debug[34] := DbgCountLow Debug[35] := DbgCountHigh Debug[36] := DbgCountHigh - DbgCountLow waitcnt( constant(80_000_000 / 250) + LastTime) LastTime += constant(80_000_000 / 250) '>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>Thanks
Shawn
t := cnt repeat ' loop code waitcnt(t += constant(80_000_000 / 250))Have you started analyzing time required by your calls? One of many great things about the Propeller is that it can time itself:
Finally... move your debug elements to another cog. You don't want to be putting code is tight loop that does not belong there in production.
After the waitcnt(t += constant(80_000_000 / 250))wouldn't I still have to have a t:= cnt? Also and I appreciate the help, but other than a waste of code can you see a problem with running my loop like that. I haven't fully worked my head around your example yet.
I tried a few more things last night and I am still having problems. I am assuming all 4 motors(esc's) are glitching or twitching at the same time. When it happens the whole quad jerks up about a inch or two. If it was just one motor I do not think it would behave this way I think it would veer off in a random direction depending on which motor glitched. Unless it is only one motor(esc) and the PID loops are compensating for it and trying to maintain attitude.
Another thought that has occurred to me and I hope this is not the case because I don't understand it and I don't see it done that often in the code I look at. In my flight loop I have a routine that writes the motor outputs. My flight loop runs in 1 cog and my Servo8Fast code runs in another cog. Is it possible that Write_to_ESC routine is calling the object while the Servo8Fast object is trying to update its PMW value. Is a person supposed to use locks when 2 Cogs are using the same variable. Below is how I am setting the PMW values from my main flight loop, very simple.
Thanks
Shawn
Wow there are just so many things that are involved with a project like this, and when there is something wrong it is very hard to pinpoint a problem.
I read somewhere that a cap needs to be placed across the signal wire to the ESC's if the length of the wire exceeded a certain length, I do not remember the length of the wire or the value of the cap. Could this be my problem, I think my leads are about 6 to 9 inches long if you include the jumper wire I used on my proto-board?
The quad is very fun to fly and I don't even notice it while I am flying. I only notice it when I am hovering and trying to have it maintain attitde. It even maintains attitude pretty well by itself, but it scares me when it glitches out by itself.
Thanks
Shawn
No. If you look again you'll see it right before the repeat. It's standard practice to initialize the timer variable immediately before dropping into the synchronized loop. This structure ensures that the loop always consumes the same amount of time. If you reset the timer variable in the loop, you're adding to that time. So... initialize the timer variable right before the loop and update it in the waitcnt() call.
Doing what you're doing is NOT a problem UNLESS the code in your loop consumes too much time and cnt goes past the waitcnt() target. It's tedious, but testing each call individually may be necessary to analyze where a possible bottleneck is located.
I cannot really quantify the conditions that would cause this, because I am not sure what is going on. The ESC's are motor controllers, and there are PID loops that are constanly updating the PWM width being sent to the Servo8Fast object so that the quad stays at a desired attitude, with no stick input the quad should just hover parallel to the ground, which it does. So if one only applies throttle the quad should lift straight up into the sky and if one would decrease throttle the quad would fall from the sky.
My thought was originally that it was my servo code because I wrote it by modifying the servo32 code. I thought that maybe my counter was off and that every once in awhile it was not switching the high time of my PWM signal off fast enough which would cause an increase in RPM's of my motors.
I don't know, anyways because of your guy's posts and my ramblings I have come up with a few more things to check, thank you all.
Jonny, what is the -544? Is that the amount of clock cycles it takes to execute those 2 linesof code? Are you thinking maybe I am missing my cnt and my loop cycles are getting messed up? If I missed a handful of loops in any given second that could cause erratic behavor. I thought I had plenty of overhead but I will investigate further.
Shawn
The other flight control board I have I was told has a 400hz refresh rate for the ESC's. This other Flight Controller is the one I used these ESC's with before. I am going to get the scope out and verify that my other FC is actually refreshing that fast. It is possible that it is not, and my ESC's won't handle 400hz or even 250hz.
Thanks for the input Heater
Shawn
I just scoped the output pin.
However I now have to ask what is the motivation for wanting a faster than 50Hz rate on a PWM signal driving motor speed? I'm rather surprised at the idea of being able to control the speed of a motor, with it's inertia plus that of the propeller, at such a rate.
It would be a nice experiment to measure the acceleration and decceleration achievable with various PWM frequencies. My gut, which is often wrong, tells me there might not be much to be gained with increasing frequency past 50Hz.
That would be an interesting experiment Heater. I think the theory behind the faster refresh rate is........... the faster the motors can be updated the smoother and more responsive the machine will fly. Interestingly enough when I just switched to the new servo code to try, my motors were refreshing at 50hz. I did not notice much of a difference in its flight characteristics. To be honest though, I did not really fly it around. I got the quad of the ground for about ten seconds and it glitched twice so I set it back down.
Time to dig into my code.
Shawn
I am still brain storming because I can't find a problem with my code. I am going to reprogram my ESC's and switch batteries.
How is the supply to the Prop itself?
As for PWM rate, yes I guess faster is better but at some point no improvement will be noticeable. As I say you have all the enertial of all those big heavy mechanical parts, motor and propeller slowing things down. When it comes to responsiveness of the entire machine you have the inertial of the whole copter to overcome as well.
http://www.parallax.com/Store/Microcontrollers/PropellerDevelopmentBoards/tabid/514/CategoryID/73/List/0/SortField/0/catpageindex/2/Level/a/ProductID/878/Default.aspx
The BEC supplies 5V to the Vin on this board. The Vin says it will handle 5-16V. I suppose if I am pulling a lot of current from the battery the output of my BEC may drop below 5v but if it does it can't be much. I think the battery pack fully charged is only 12 or 13 volts. I suppose I could hook it directly to the input of my proto board. I figured if I let the BEC from my ESC drop the voltage down to 5v then the regulator on my proto board would not have to dissipate so much heat.
There is more than likely a problem with my code, but I have yet to find it.
I just reprogrammed my ESC's and was really excited, I hovered the quad for about 3 minutes with no glitches, then it twitched and glitched. I continued to hover it for another couple minutes and it glitched one more time. When I say glitch or twitch, its like I goosed the throttle for just a fraction of a second. Everything is flying smooth then goose, and then everything is fine again. Its really weird. I was really bummed the first time it happened and I actually fried a Prop trying to trouble shoot the outputs.
Being able to bounce ideas off you guys is awesome.
Shawn
Later
Thanks for listening to me ramble, and offering me suggestions of what to look for guys.
Shawn