I am making an automated quadcopter: no radio transmitter-receiver and the quacopter flies on its own with pre-programmed orders.
All most all quadcopter implement PID on throttle/yaw/pitch/roll as these 4 axes are directly on an remote controller. However this is a bit inconvenient for an automated one without a controller. As an automated quadcopter without user input, velocities along x/y/z axis are of more concern, because:
keeping balance(yaw/pitch/roll=0) doesn't mean keeping still, as first, there will be some error in manufacture so it might still have some acceleration. Second, even there's no acceleration, it can have speed, causing it to drift in space. And as there's no user input, it can not fix the drift on it's own. Besides, if there's wind, it might got blow away, even it thinks it's balanced.
Orders are mostly given in "go to position(x,y)" or "keeping velocity x, fly above position(x,y) and start camera video capture." or so. These orders can't be translated into yaw/pitch/roll directly.
So basically I have two ideas:
Implement PID on yaw/pitch/roll/height and use a second PID loop to control velocity. The second PID loop take desired velocity and current velocity as input and output desired yaw/pitch/roll for first loop.
Implement PID directly on velocity. The pid loop take desired velocity and current velocity(by integrating acceleration from accelerometer) as input and output PWM width for 4 motors.
Have anyone tried idea 2? Will this work?
PID maps a measured value to a controlled value. If you can sense velocity reliably, you can use it to drive a PID. However, integrating an accelerometer won't give you a reliable enough velocity. Any sensing errors will compound through the integration and could grow your velocity estimate unbounded.
The R/P/Y PIDs on a quadcopter don't control the PWM to the motors directly, they control the roll, pitch, and yaw rates, then convert the rates to thrusts, then convert the thrust components for the various motors, then convert the thrusts into PWMs. See ArduPilor MotorMatrix code for hints.
You can put PIDs anywhere you like in the process between whatever is choosing the velocity and the motors, but you likely need some intermediate control/state variables between 'velocity' and 'PWM[1-4]' to balance things and have coordinated flight.
Related
I'm looking for a way to disable the PID.vi from running in Labview when the event case container is false.
The program controls motor position to maintain constant tension on a cable using target force and actual force as the input parameters. The output is motor position. Note that reinitialize is set to false since it needs previous instances to spool the motor.
Currently, when the event case is true the motor spools as expected and maintains the cable tension. But when the event case state is toggled the PID.vi seems to be running in the background when false causing the motor spool sporatically.
Is there a way to freeze the PID controls so that it continues from where it left off?
The PID VI does not run in the background. It only executes when you call it. That said, PID is a time-based calculation. It calculates the difference from the last time you called the VI and uses that to calculate the new values. If a lot of time passed, it will just try to fix it using that data.
If you want to freeze the value and then resume fixing smoothly, you can use the limits input on the top and set the max and min to your desired output. This will cause the PID VI to always output that value. You will probably need a feedback node or shift register to remember the last value output by the PID.
What Yair said is not entirely true - the integral and derivative terms are indeed time dependent, but the proportional is not. A great reference for understanding PIDs and how they are implemented in LabVIEW can be found here (not sure why it is archived). Also, the PID VIs are coded in G so you can simply open them to see how they operate.
If you take a closer look at the PID VI, you can see what is happening and why you might not get the response you expect. In the VI itself, dt will be either 1) what you set it to, or 2) an accumulation of time based on a tick count stored in the VI (the default). Since you have not specified a dt, the PID algorithm uses the accumulated time between calls. If you have "paused" calculation for some time, this will have an impact on the integral and derivative output.
The derivative output will kick in when there is a change in the process variable (use of the process variable prevents derivative kick). The effect of a large accumulated time between calls will be to reduce the response of this term. The time that you have paused will have a more significant impact on the integral term. Since the response of the integral portion of the controller is the proportional to the integral of the error over dt, the longer you pause the larger the response simply because because the the algorithm is performing a trapezoidal integration over dt.
My first suggestion is don't pause the controller - let the PID do what it is supposed to do. If you are using it properly, then you should not have to stop the controller action. But, if you must pause the controller action, consider re-initializing the controller. This will force the controller to reset the accumulated time term and the response in the first iteration will be purely proportional.
Hope this helps.
This is my first post on SO. I haven't already developed much code for embedded systems, but I have few problems and need help from more advanced programmers. I use following devices:
- LandTiger board with LPC1768 (Cortex M3) MCU,
- Digilent pmodACL with ADXL345 accelerometer (3 axis),
- Digilent pmodGYRO with L3G4200D gyroscope (3 axis).
I would like to get some information about device orientation, i.e. rotation angles over X, Y and Z axes. I've read that in order to achieve this I need to combine data from both accelerometer and gyroscope using Kallman filter or its simpler form i.e. complementary filter. I would like to know if it's possible to count roll, pitch and yaw from full range (0-360 degrees) using measurment data only from gyroscope and accelerometer (without magnetometer). I've also found some mathematical formulas (http://www.ewerksinc.com/refdocs/Tilt%20Sensing%20with%20LA.pdf and http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf) but they contain root squares in numerators/denominators so the information about proper quadrant of coordinate system is lost.
The question you are asking is a fairly frequent one, and is fairly complex, with many different solutions.
Although the title mentions only an accelerometer, the body of your post mentions a gyroscope, so I will assume you have both. In addition there are many steps to getting low-cost accelerometers and gyros to work, one of those is to do the voltage-to-measurement conversion. I will not cover that part.
First and foremost I will answer your question.
You asked if by 'counting' the gyro measurements you can estimate the attitude (orientation) of the device in Euler Angles.
Short answer: Yes, you could sum the scaled gyro measurements to get a very noisy estimate of the device rotation (actual radians turned, not attitude), however it would fail as soon as you rotate more than one axis. This will not suffice for most applications.
I will provide you with some general advise, specific knowledge and some example code that I have used before.
Firstly, you should not try to solve this problem by writing a program and testing with your IMU. You should start by writing a simulation using validated libraries, then validate your algorithm/program, and only then try to implement it with the IMU.
Secondly, you say you want to "count roll, pitch and yaw from full range (0-360 degrees)".
From this I assume you mean you want to be able to determine the Euler Angles that represent the attitude of the device with respect to an external stationary North-East-Down (NED) frame.
Your statement makes me think you are not familiar with representations of attitude, because as far as I know there are no Euler Angle representations with all 3 angles in the 0-360 range.
The application for which you want to use the attitude of the device will be important. If you are using Euler Angles you will not be able to accurately track the attitude of the device when large (greater than around 50 degrees) rotations are made on the roll or pitch axes, due to what is known as Gimbal Lock.
If you require the tracking of such motions then you will need to use a quaternion or Direction Cosine Matrix (DCM) representation of attitude.
Thirdly, as you have said you can use a Complimentary Filter or Kalman Filter variant (Extended Kalman Filter, Error-State Kalman Filter, Indirect Kalman Filter) to accurately track the attitude of the device by fusing the data from the accelerometer, gyro and a magnetometer. I suggest the Complimentary Filter described by Madgwick which is implemented in C, C# and MATLAB here. A Kalman Filter variant would be necessary if you wanted to track the position of the device, and had an additional sensor such as GPS.
For some example code of mine using accelerometer only to get Euler Angle pitch and roll see my answer to this other question.
I want to develop an app that detects how far the user/device is from points on a map.
Calculating the distance is easy, but when you get close to about 30meters I would need it to be as precise as possible.
Basically I want some lights on the UI to get brighter the closer you get to the target/point.
How do I achieve this if the gps position sometimes bounces around for 5-10 meters or more?
Any ideas on how to approach this?
Thanks!
In general there is the inaccuracy with the position, and indeed its meters, thus the bouncing will be there and its rather impossible to get rid of it, anyways, one suggestion would be to collect the last few (3-10 up to you and your logic really) locations and calculate average from them. Then with fast movements your position would be lagging of course, but when doing slow movements the position shown should be more stable.. Of course you could also have additional logic on determining the movement direction, and accepting the location change towards that faster etc.
You will not get a better precision than 3m to the target.
At low, speed, like walking, you will no make it better than 8-10m.
Count the distance sicne last used fix, If it exceeds 12m then use the fix, and mark it as last used.
This is a simple filter which works well for walking speeds.
At speeds higher (> 10km/h) switch off the filter.
GPS should not jump at that speed.
I am using GL_LINE_LOOP to draw a circle in C and openGL! Is it possible for me to fill the circle with colors?
If needed, this is the code I'm using:
const int circle_points=100;
const float cx=50+i, cy=50+x, r=50;
const float pi = 3.14159f;
int i = 50;
glColor3f(1, 1, 1);
glBegin(GL_LINE_LOOP);
for(i=0;i<circle_points;i++)
{
const float theta=(2*pi*i)/circle_points;
glVertex2f(cx+r*cos(theta),cy+r*sin(theta));
}
glEnd();
Lookup polygon triangulation!
I hope something here is somehow useful to someone, even though this question was asked in February. There are many answers, even though a lot of people would give none. I could witter forever, but I'll try to finish before then.
Some would even say, "You never would," or, "That's not appropriate for OpenGL," I'd like to say more than them about why. Converting polygons into the triangles that OpenGL likes so much is outside of OpenGL's job-spec, and is probably better done on the processor side anyway. Calculate that stage in advance, as few times as possible, rather than repeatedly sending such a chunky problem on every draw call.
Perhaps the original questioner drifted away from OpenGL since February, or perhaps they've become an expert. Perhaps I'll re-inspire them to look at it again, to hack away at some original 'imposters'. Or maybe they'll say it's not the tool for them after all, but that would be disappointing. Whatever graphics code you're writing, you know that OpenGL can speed it up!
Triangles for convex polygons are easy
Do you just want a circle? Make a triangle fan with the shared point at the circle's origin. GL_POLYGON was, for better or worse, deprecated then killed off entirely; it will not work with current or future implementations of OpenGL.
Triangles for concave polygons are hard
You'll want more general polygons later? Well, there are some tricks you could play with, for all manner of convex polygons, but concave ones will soon get difficult. It would be easy to start five different solutions without finishing a single one. Then it would be difficult, on finishing one, to make it quick, and nearly impossible to be sure that it's the quickest.
To achieve it in a future-proofed way you really want to stick with triangles -- so "polygon triangulation" is the subject you want to search for. OpenGL will always be great for drawing triangles. Triangle strips are popular because they reuse many vertices, and a whole mesh can be covered with only triangle strips, (perhaps including the odd lone triangle or pair of triangles). Drawing with only one primitive usually means the entire mesh can be rendered with a single draw call, which could improve performance. (Number of draw calls is one performance consideration, but not always the most important.)
Polygon triangulation gets more complex when you allow convex polygons or polygons with holes. (Finding algorithms for triangulating a general polygon, robustly yet quickly, is actually an area of ongoing research. Nonetheless, you can find some pretty good solutions out there that are probably fit for purpose.)
But is this what you want?
Is a filled polygon crucial to your final goals in OpenGL? Or did you simply choose what felt like it would be a simple early lesson?
Frustratingly, although drawing a filled polygon seems like a simple thing to do -- and indeed is one of the simplest things to do in many languages -- the solution in OpenGL is likely to be quite complicated. Of course, it can be done if we're clever enough -- but that could be a lot of effort, without being the best route to take towards your later goals.
Even in languages that implement filled polygons in a way that is simple to program with, you don't always know how much strain it puts on the CPU or GPU. If you send a sequence of vertices, to be linked and filled, once every animation frame, will it be slow? If a polygon doesn't change shape, perhaps you should do the difficult part of the calculation just once? You will be doing just that, if you triangulate a polygon once using the CPU, then repeatedly send those triangles to OpenGL for rendering.
OpenGL is very, very good at doing certain things, very quickly, taking advantage of hardware acceleration. It is worth appreciating what it is and is not optimal for, to decide your best route towards your final goals with OpenGL.
If you're looking for a simple early lesson, rotating brightly coloured tetrahedrons is ideal, and happens early in most tutorials.
If on the other hand, you're planning a project that you currently envision using filled polygons a great deal -- say, a stylized cartoon rendering engine for instance -- I still advise going to the tutorials, and even more so! Find a good one; stick with it to the end; you can then think better about OpenGL functions that are and aren't available to you. What can you take advantage of? What do you need or want to redo in software? And is it worth writing your own code for apparently simple things -- like drawing filled polygons -- that are 'missing from' (or at least inappropriate to) OpenGL?
Is there a higher level graphics library, free to use -- perhaps relying on OpenGL for rasterisation -- that can already do want you want? If so, how much freedom does it give you, to mess with the nuts and bolts of OpenGL itself?
OpenGL is very good at drawing points, lines, and triangles, and hardware accelerating certain common operations such as clipping, face culling, perspective divides, perspective texture accesses (very useful for lighting) and so on. It offers you a chance to write special programs called shaders, which operate at various stages of the rendering pipeline, maximising your chance to insert your own unique cleverness while still taking advantage of hardware acceleration.
A good tutorial is one that explains the rendering pipeline and puts you in a much better position to assess what the tool of OpenGL is best used for.
Here is one such tutorial that I found recently: Learning Modern 3D Graphics Programming
by Jason L. McKesson. It doesn't appear to be complete, but if you get far enough for that to annoy you, you'll be well placed to search for the rest.
Using imposters to fill polygons
Everything in computer graphics is an imposter, but the term often has a specialised meaning. Imposters display very different geometry from what they actually have -- only more so than usual! Of course, a 3D world is very different from the pixels representing it, but with imposters, the deception goes deeper than usual.
For instance, a rectangle that OpenGL actually constructs out of two triangles can appear to be a sphere if, in its fragment shader, you write a customised depth value to the depth coordinate, calculate your own normals for lighting and so on, and discard those fragments of the square that would fall outside the outline of the sphere. (Calculating the depth on those fragments would involve a square root of a negative number, which can be used to discard the fragment.) Imposters like that are sometimes called flat cards or billboards.
(The tutorial above includes a chapter on imposters, and examples doing just what I've described here. In fact, the rectangle itself is constructed only part way through the pipeline, from a single point. I warn that the scaling of their rectangle, to account for the way that perspective distorts a sphere into an ellipse in a wide FOV, is a non-robust fudge . The correct and robust answer is tricky to work out, using mathematics that would be slightly beyond the scope of the book. I'd say it is beyond the author's algebra skills to work it out but I could be wrong; he'd certainly understand a worked example. However, when you have the correct solution, it is computationally inexpensive; it involves only linear operations plus two square roots, to find the four limits of a horizontally- or vertically-translated sphere. To generalise that technique for other displacements requires one more square root, for a vector normalisation to find the correct rotation, and one application of that rotation matrix when you render the rectangle.)
So just to suggest an original solution that others aren't likely to provide, you could use an inequality (like x * x + y * y <= 1 for a circle or x * x - y * y <= 1 for a hyperbola) or a system of inequalities (like three straight line forms to bound a triangle) to decide how to discard a fragment. Note that if inequalities have more than linear order, they can encode perfect curves, and render them just as smoothly as your pixelated screen will allow -- with no limitation on the 'geometric detail' of the curve. You can also combine straight and curved edges in a single polygon, in this way.
For instance, a fragment shader (which would be written in GLSL) for a semi-circle might have something like this:
if (y < 0) discard;
float rSq = x * x + y * y;
if (1 < rSq) discard;
// We're inside the semi-circle; put further shader computations here
However, the polygons that are easy to draw, in this way, are very different from the ones that you're used to being easy. Converting a sequence of connected nodes into inequalities means yet more code to write, and deciding on the Boolean logic, to deal with combining those inequalities, could then get quite complex -- especially for concave polygons. Performing inequalities in a sensible order, so that some can be culled based on the results of others, is another ill-posed headache of a problem, if it needs to be general, even though it is easy to hard-code an optimal solution for a single case like a square.
I suggest using imposters mainly for its contrast with the triangulation method. Something like either one could be a route to pursue, depending on what you're hoping to achieve in the end, and the nature of your polygons.
Have fun...
P.S. have a related topic... Polygon triangulation into triangle strips for OpenGL ES
As long as the link lasts, it's a more detailed explanation of 'polygon triangulation' than mine. Those are the two words to search for if the link ever dies.
A line loop is just an outline.
To fill the middle as well, you want to use GL_POLYGON.
Not unlike a clap detector ("Clap on! clap clap Clap off! clap clap Clap on, clap off, the Clapper! clap clap ") I need to detect when a door closes. This is in a vehicle, which is easier than a room or household door:
Listen: http://ubasics.com/so/van_driver_door_closing.wav
Look:
It's sampling at 16bits 4khz, and I'd like to avoid lots of processing or storage of samples.
When you look at it in audacity or another waveform tool it's quite distinctive, and almost always clips due to the increase in sound pressure in the vehicle - even when the windows and other doors are open:
Listen: http://ubasics.com/so/van_driverdoorclosing_slidingdoorsopen_windowsopen_engineon.wav
Look:
I expect there's a relatively simple algorithm that would take readings at 4kHz, 8 bits, and keep track of the 'steady state'. When the algorithm detects a significant increase in the sound level it would mark the spot.
What are your thoughts?
How would you detect this event?
Are there code examples of sound pressure level calculations that might help?
Can I get away with less frequent sampling (1kHz or even slower?)
Update: Playing with Octave (open source numerical analysis - similar to Matlab) and seeing if the root mean square will give me what I need (which results in something very similar to the SPL)
Update2: Computing the RMS finds the door close easily in the simple case:
Now I just need to look at the difficult cases (radio on, heat/air on high, etc). The CFAR looks really interesting - I know I'm going to have to use an adaptive algorithm, and CFAR certainly fits the bill.
-Adam
Looking at the screenshots of the source audio files, one simple way to detect a change in sound level would be to do a numerical integration of the samples to find out the "energy" of the wave at a specific time.
A rough algorithm would be:
Divide the samples up into sections
Calculate the energy of each section
Take the ratio of the energies between the previous window and the current window
If the ratio exceeds some threshold, determine that there was a sudden loud noise.
Pseudocode
samples = load_audio_samples() // Array containing audio samples
WINDOW_SIZE = 1000 // Sample window of 1000 samples (example)
for (i = 0; i < samples.length; i += WINDOW_SIZE):
// Perform a numerical integration of the current window using simple
// addition of current sample to a sum.
for (j = 0; j < WINDOW_SIZE; j++):
energy += samples[i+j]
// Take ratio of energies of last window and current window, and see
// if there is a big difference in the energies. If so, there is a
// sudden loud noise.
if (energy / last_energy > THRESHOLD):
sudden_sound_detected()
last_energy = energy
energy = 0;
I should add a disclaimer that I haven't tried this.
This way should be possible to be performed without having the samples all recorded first. As long as there is buffer of some length (WINDOW_SIZE in the example), a numerical integration can be performed to calculate the energy of the section of sound. This does mean however, that there will be a delay in the processing, dependent on the length of the WINDOW_SIZE. Determining a good length for a section of sound is another concern.
How to Split into Sections
In the first audio file, it appears that the duration of the sound of the door closing is 0.25 seconds, so the window used for numerical integration should probably be at most half of that, or even more like a tenth, so the difference between the silence and sudden sound can be noticed, even if the window is overlapping between the silent section and the noise section.
For example, if the integration window was 0.5 seconds, and the first window was covering the 0.25 seconds of silence and 0.25 seconds of door closing, and the second window was covering 0.25 seconds of door closing and 0.25 seconds of silence, it may appear that the two sections of sound has the same level of noise, therefore, not triggering the sound detection. I imagine having a short window would alleviate this problem somewhat.
However, having a window that is too short will mean that the rise in the sound may not fully fit into one window, and it may apppear that there is little difference in energy between the adjacent sections, which can cause the sound to be missed.
I believe the WINDOW_SIZE and THRESHOLD are both going to have to be determined empirically for the sound which is going to be detected.
For the sake of determining how many samples that this algorithm will need to keep in memory, let's say, the WINDOW_SIZE is 1/10 of the sound of the door closing, which is about 0.025 second. At a sampling rate of 4 kHz, that is 100 samples. That seems to be not too much of a memory requirement. Using 16-bit samples that's 200 bytes.
Advantages / Disadvantages
The advantage of this method is that processing can be performed with simple integer arithmetic if the source audio is fed in as integers. The catch is, as mentioned already, that real-time processing will have a delay, depending on the size of the section that is integrated.
There are a couple of problems that I can think of to this approach:
If the background noise is too loud, the difference in energy between the background noise and the door closing will not be easily distinguished, and it may not be able to detect the door closing.
Any abrupt noise, such as a clap, could be regarded as the door is closing.
Perhaps, combining the suggestions in the other answers, such as trying to analyze the frequency signature of the door closing using Fourier analysis, which would require more processing but would make it less prone to error.
It's probably going to take some experimentation before finding a way to solve this problem.
You should tap in to the door close switches in the car.
Trying to do this with sound analysis is overengineering.
There are a lot of suggestions about different signal processing
approaches to take, but really, by the time you learn about detection
theory, build an embedded signal processing board, learn the processing
architecture for the chip you chose, attempt an algorithm, debug it, and then
tune it for the car you want to use it on (and then re-tune and re-debug
it for every other car), you will be wishing you just stickey taped a reed
switch inside the car and hotglued a magnet to the door.
Not that it's not an interesting problem to solve for the dsp experts,
but from the way you're asking this question, it's clear that sound
processing isn't the route you want to take. It will just be such a nightmare
to make it work right.
Also, the clapper is just an high pass filter fed into a threshold detector. (plus a timer to make sure 2 claps quickly enough together)
There is a lot of relevant literature on this problem in the radar world (it's called detection theory).
You might have a look at "cell averaging CFAR" (constant false alarm rate) detection. Wikipedia has a little bit here. Your idea is very similar to this, and it should work! :)
Good luck!
I would start by looking at the spectral. I did this on the two audio files you gave, and there does seem to be some similarity you could use. For example the main difference between the two seems to be around 40-50Hz. My .02.
UPDATE
I had another idea after posting this. If you can, add an accelerometer onto the device. Then correlate the vibrational and acoustic signals. This should help with cross vehicle door detection. I'm thinking it should be well correlated since the sound is vibrationally driven, wheres the stereo for example, is not. I've had a device that was able to detect my engine rpm with a windshield mount (suction cup), so the sensitivity might be there. (I make no promises this works!)
(source: charlesrcook.com)
%% Test Script (Matlab)
clear
hold all %keep plots open
dt=.001
%% Van driver door
data = wavread('van_driver_door_closing.wav');
%Frequency analysis
NFFT = 2^nextpow2(length(data));
Y = fft(data(:,2), NFFT)/length(data);
freq = (1/dt)/2*linspace(0,1,NFFT/2);
spectral = [freq' 2*abs(Y(1:NFFT/2))];
plot(spectral(:,1),spectral(:,2))
%% Repeat for van sliding door
data = wavread('van_driverdoorclosing.wav');
%Frequency analysis
NFFT = 2^nextpow2(length(data));
Y = fft(data(:,2), NFFT)/length(data);
freq = (1/dt)/2*linspace(0,1,NFFT/2);
spectral = [freq' 2*abs(Y(1:NFFT/2))];
plot(spectral(:,1),spectral(:,2))
The process for finding distinct spike in audio signals is called transient detection. Applications like Sony's Acid and Ableton Live use transient detection to find the beats in music for doing beat matching.
The distinct spike you see in the waveform above is called a transient, and there are several good algorithms for detecting it. The paper Transient detection and classification in energy matters describes 3 methods for doing this.
I would imagine that the frequency and amplitude would also vary significantly from vehicle to vehicle. Best way to determine that would be taking a sample in a Civic versus a big SUV. Perhaps you could have the user close the door in a "learning" mode to get the amplitude and frequency signature. Then you could use that to compare when in usage mode.
You could also consider using Fourier analysis to eliminate background noises that aren't associated with the door close.
Maybe you should try to detect significant instant rise in air pressure that should mark a door close. You can pair it with this waveform and sound level analysis and these all might give you a better result.
On the issue of less frequent sampling, the highest sound frequency which can be captured is half of the sampling rate. Thus, if the car door sound was strongest at 1000Hz (for example) then a sampling rate below 2000Hz would lose that sound entirely
A very simple noise gate would probably do just fine in your situation. Simply wait for the first sample whose amplitude is above a specified threshold value (to avoid triggering with background noise). You would only need to get more complicated than this if you need to distinguish between different types of noise (e.g. a door closing versus a hand clap).