This is a work in progress. I've tried to address some fine tuning and performance issues.

Denny R. had made a comment about the inertia of bigger props causing issues. I've added a low pass filter to smooth the positive acceleration of the props to see if we can get at this issue. It may require tuning up Rate_P for a few folks, but I saw little issue in multiple flights.

Crosstrack had a small math error that decreased the resolution of it. I've fixed that and upped the default gains to get better tracking. 

Made WP hit radius 1 by default. even 3m is too much for quads. (If you pass a WP you will move on to the next)

The Loiter method is tuned a little better by default, and now uses GPS offsets when flying less than 1.5m/s. Code experimentation will continue on this front. Thanks to Emile and afernan for their help!

A fix in the Z Accel startup was added to get an averaged result.

Added the ability to enter Loiter with Optflow enabled. - still a work in progress, not for everyday use just yet.

This alpha is on GIT now and is for user's who want to test code. As always, you need to use the "relaxpatch" version of Arduino you can find in the downloads section

Update: We've found and patched some small type bugs in the latest and updated some GPS drivers in the Library. Be sure to pull the latest code and check for the status of each build run against the SIL sim server.

Update r3:

I pushed a version on to GIT that addresses a number of issues.

- the low speed GPS XY calculations were incorrect and have been fixed

- Nav_Rate I term has been removed in loiter control - it's too easy to get two iterms working out of phase

- A second derivative has been added to Roll and Pitch. I found it removed wobbles nicely - Can be adjusted in the planner as STAB_D with a default of .25 (just enough)

- Smoothing has been applied to the motor commands in a way that really quiets down the alt hold pulsing without much effect on latency

- Yaw now has a dynamic constraint and I've upped the yaw gain.

- The motors output now have an LP filter on them so that the accelerate just a tad slower than the deceleration. This is a test to see if it helps big Octo's and Hexa's

- The Rate_I term is now zero'd for first five seconds after takeoff to keep balance.

- Loiter gains are lightened up a bit

- Nav_Rate_P is lower to remove back and forth sped related hunting in loiter

- Compass is enabled by default

- New I2C Library now included which should solve I2C related lockouts

- optflow is still a work in progress.

Update R4:

This update is based on flights today in a very windy environment.

It occurred to me that we're handling the WP nav I terms incorrectly and I reworked the WP navigation to share the same I terms from the Loiter. Even though they use different error input, etc, they turn out to both deal with wind in the same way. I have not Flown WPs with this new code, but heavily tested it in the sim and It's really rocking in hard wind. Transitions from Loiter flight and Nav flight are very smooth. Please let me know if you have luck.

Update R5:

This is a quick patch based on a bad crash Marco had. My theory was an I term that built up during wind that needed to be reset, but wasn't. It's a corner case but It bit Marco pretty bad. Please re-pull if you have R4 running to go to R5. And please, please be careful. This is alpha code not for general testing, but for development. Don't fly it on anything you would feel bad about crashing. 


Update R6:

Includes fix for Acro mode reset bug.
I went through all of the global variables and gave them lengthy descriptions. It's worth a read if you want to learn more how the internals work. I'll be doing some more organization like this as we move forward with an architectural redesign for 3.0 \
This version is mostly clean-up, but does include 1 interesting performance enhancement. It's disabled by default because It's fairly untested. The idea is that the wind compensation created by the Iterms for lat and lon in Loiter and Navigation are carried over into Stabilize. This unifies all of the modes so that when you switch modes, you don't get a small, but fast change in pitch or roll. This is only noticeable in high wind environment. 
To get this to work, I look for near zero velocity and start moving the pitch and roll into the wind control items. This takes a  transformation from copter frame to world frame, but the result is the copter will hold the position against the wind with the sticks in the center of the controller. If you fly around the wind compensation will bleed off slowly - about 30 seconds.
Again, this is off by default and needs significant testing, preferably in the HIL sim first.JNL has already started and flown this version for real.
The feature may be off by default for another version or two, but just letting you know it's there.
I think it will go a long way to making the mode changes feel seamless.

Update R7:

Added an auto-land timer for RTL. If you don't change modes for 20s after the copter arrives at home, it will begin to auto-land. If you have failsafe and no GPS, you will immediately begin auto-landing.
Failsafe RTL now goes 10M up as it RTLs to avoid obstructions.
Added SIL test for failsafe. 

Update R8:

Minor tweaks and cleanup

Update R9:

Made climb rate controller for landing universal for all altitude changes

Update R10:

Updated Loiter controller - Works great in the sim, thanks Afernan.



Views: 42945

Reply to This

Replies to This Discussion

Now that's what I'm talkin' about. 7 minutes in the air and didn't have to correct loiter once. Sure, I've got no wind right now, but this will rival a human's ability to stay on point- especially when it comes to the patience of flying over one spot ;)  Nice detective work guys. Approved. (Revision: 570c17968edd)

I posted a revision that tests well in the SIM and the values look good.


I made simple fixes according to the things I mention above and test flew. Nice and smooth though the loiter did wander around a bit in alt and xy.

I still had the gains set low in my attempts to get 2.1.1 loitering yesterday. I didn't get to a second flight since my Tx ran out of battery.

static void calc_XY_velocity(){
    // offset calculation of GPS speed:
    // used for estimations below 1.5m/s
    // our GPS is about 1m per
    static long last_longitude = 0;
    static long last_latutude  = 0;

    // this speed is ~ in cm because we are using 10^7 numbers from GPS
    x_GPS_speed        = (last_longitude - g_gps->longitude) / dTnav;
    y_GPS_speed        = (last_latutude  - g_gps->latitude) / dTnav;
    last_longitude     = g_gps->longitude;
    last_latutude     = g_gps->latitude;

    if(g_gps->ground_speed > 150){
        // Derive X/Y speed from GPS
        // this is far more accurate when traveling about 1.5m/s
        float temp        = g_gps->ground_course * RADX100;
        x_GPS_speed     = sin(temp) * (float)g_gps->ground_speed;
        y_GPS_speed     = cos(temp) * (float)g_gps->ground_speed;
    //Serial.printf("GS: %d  \tx:%d \ty:%d\n", g_gps->ground_speed, x_GPS_speed, y_GPS_speed);

3DR quad, 4S, APM2


I made some recent improvements. Please check them out. They should help you hold a better position.



     You may know this already but looking at AP_GPS_MTK_Common.h it looks like we have SBAS_ON/OFF and WAAS_ON/OFF defined so I suspect we can just add a couple of lines into the init function in AP_GPS_MTK.cpp and AP_GPS_MTK16.cpp like what's below and see if it helps or not.

// turn on SBAS and WAAS

     One issue in testing is that I'm in Japan and I think WAAS only works in the West and I'm unsure about SBAS.

     How about I switch it on in the git version (or perhaps just in the example sketch) and then some people send back the example sketch output (or logs) after leaving their vehicle in place for 5~10 minutes and then we can check how much deviation we see in the gps values over that time.

     Depending upon the results we could decide whether it's already on and/or whether we should add a parameter to let people turn it on/off.

    What do you think?


Phil: Nicely done! Please PM me with your email address if you'd like to be added to the dev team maillist, so you can work directly with Jason et al on integrating your changes. 

(PS--the same goes for anyone else who is comfortable enough with the programming side to contribute. Just PM me and I'll add you to to the list) 

Jason, I was looking through the new code.

Your speed calc for the <1.5 m/s case seems to just take change in coords without dividing by dTnav.

Was this intentional? Without the division, the speed feedback will be a factor of ~10 slow, so there will be a gain change when the speed passes the 1.5 m/s threshold. Also, it seems like the speed error would only approach zero when the speed itself approaches zero.

Jason thanks for your efforts. we are on the right track. 

Thanks Jason, I have updated the firmware with your latest version 2.1.1 from today and also I have updated the GPS library with the latest fix about the MTK16 from Randy about the validation of the SBAS and WAAS, I shall keep you informed about the soon tests flights on LOITER...

Regards, Jean-Louis

Test it today with the same, also with the validation of the SBAS and WAAS in the GPS library, stay tuned... :-)

Hello Jason and Randy,

Woow, very GOOD News, I have just done a LOITER test flight with your latest major update of the ArduCopter v2.1.1 (570c17968edd) and also updated the GPS library with a 10 Hz sampling rate and add the Randy fix so ato set the SBAS/WASS ON. This is a major improvement about the LOITER mode !!! The position accuracy is now greatly improved (1-2 m), see the GPS log below:

and also my tested PIDs parameters:

My tested version 2.1.1 (with the GPS fix) can be dowloaded HERE

Good work Jason and Randy,

Regards, Jean-Louis

Good news JL!
I am very pleased with the results that you achieved in the short go and try it on my heavy octo, where the situation is bound to be more critical given the size, but they are very confident.
Well, good, very good! :-)

You went to
try out at dawn?
But at this time in France is not freezing cold? :P
Here 0° now...

Reply to Discussion



Season Two of the Trust Time Trial (T3) Contest 
A list of all T3 contests is here. The current round, the Vertical Horizontal one, is here

© 2019   Created by Chris Anderson.   Powered by

Badges  |  Report an Issue  |  Terms of Service