Hey all,
Maybe I'm tired and the answer is staring me in the face, but I thought I'd throw this problem out to see who can solve it.
In 2.7.5 I'm looking to add linear deceleration when approaching a waypoint. Right now we decelerate at a proportion of our distance. This is ok in certain situations, but it fails horribly when you're flying faster than 6m/s. Currently the copter will rapidly decelerate and then slow down too much as it approaches the WP. What we need is a continuous and linear deceleration of a set amount (say 100cm/s/s).
If you just do this by measuring time, you'll get burned by wind and aerodynamic effects. We need something that can look at the deceleration and adjust so we don't overshoot or stop short. A PID controller could work or something simpler. I also want to keep it computationally small and close approximations are fine.
We could run a mortgage style calc every GPS read as well.
Anyone know the best approach?
Jason
Replies
i have this implemented in the sim and its working great. Thanks everyone. And faster copter flyers will thank you as well.
Robert, i've done a slightly different implementation that i'll fly tomorrow then push for 2.81's release. it still uses your equation, just handles accelerating differently.
Jason
Just a thought as I've been wanting to do this for a while too.
How about a simple approach where wpspeed is the set speed in m/s and wpdist is the distance from the waypoint in m:
If wpspeed>6 and 60<wpdist<wpspeed*10, then set wpspeed=wpdist/10.
That way for say 10m/s wpspeed, it'll start adjusting the wpspeed from 100m out. By the time it is 60m from waypoint the wpspeed will be set to 6m/s automatically.
One would ofcourse need to store what the original wpspeed setting was and revert back to it as it goes to the next waypoint. It can then gradually accelerate in the same way.
Nice. I'm stuck at work and can't look right now. Will check it out tonight or tomorrow morning.
Jason
Got it!
Working on something in my clone now.
Jason, do I understand this right, that we do fast_corner if the next WP is at less than 60° from the previous waypoint?
// Fast corner management
// ----------------------
if(tmp_index == -1) {
// there are no more commands left
}else{
// we have at least one more cmd left
Location tmp_loc = get_cmd_with_index(tmp_index);
if(tmp_loc.lat == 0) {
fast_corner = false;
}else{
int32_t temp = get_bearing_cd(&next_WP, &tmp_loc) - original_target_bearing;
temp = wrap_180(temp);
fast_corner = labs(temp) < 6000;
}
}
Jason,
So, taking your deceleration rate of 1m/s/s, you could do this:
Compute deceleration distance = 1/2*(V0*V0), V0 in meters/second.
So, suppose you are going along at 10 meters/second. Then, stopping distance is 50 meters.
When you are within 50 meters, apply deceleration of 1 m/s/s.
You could also adjust the deceleration a bit as you decelerate according to:
a = 1/2*(V*V)/d, where d is distance to waypoint, V is instantaneous velocity.
When you get close to waypoint you would probably want to switch to some other method, to avoid division by zero.
For this method to work, you compute everything in earth frame, so if there is some wind, you will have to adjust tilt appropriately in order to get the desired deceleration in earth frame.
Best regards,
Bill
Jason,
For constant deceleration, the kinematics are simple. Depending on whether you want to know where to start decelerating, or how hard to decelerate, the equations are:
d = (V0*V0)/(2*a)
or
a = (V0*V0)/(2*d)
Where V0 is initial velocity in meters/second, a is deceleration rate in meters/(sec*sec), and d is distance to stopping point in meters.
That said, you probably do not want to do constant deceleration, you might want to "feather" the deceleration rate at the end, much like when you brake a car.
So, for example, what you might want to do is rotate at a certain rate until the tilt is the value that you want, decelerate for a while, and then slowly level as you come to the point.
At any rate, if you tell me the general shape of tilt versus time that you would like to use, it would be simple matter for me to work out the math for you to tell you when to start "braking" and what tilt profile to use.
Best regards,
Bill Premerlani
PS: here is the math for constant deceleration:
V = V0 - a*t, so t = V0/a
d = V0*t - 1/2*a*t*t
substitute t into the second equation and simplify
d= 1/2*(V0*V0)/a
To get things started I'll make my small contribution which are based on formulas pulled from this wikipedia page:
ideal linear acceleration = -2 * ( orig_velocity * desired_time_period + orig_distance ) / desired_time_period^2
(in the above formula you need to put either orig_velocity or orig_position as negative)
So if you're 10m from the target (i.e. orig_distance = 10)
and travelling towards it at 15m/s (i.e orig_velocity = -15)
and you want to stop on the target in 3 seconds (desired_time_period = 3)
your ideal constant deceleration over the 3 seconds would be 7.78m/s/s
Actually this may not be quite right..if I graph it it looks like it goes past zero and comes back..