ESC Servo Code PASM Problem
Shawna
Posts: 508
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
If 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). This will prime WaveTemp to be -1 when leaving the sync loop.
Thanks
Shawn
Thanks
Shawn
Servo8Fast.spin
Thanks
Shawn
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
This is my main flight loop.
Thanks
Shawn
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