Autoresume mission when RTK Fix is recovered

I was testing alot with RTK automission yesterday. We had a blue sky and RTK Fix most of the day. Sometimes the rover stopped, at a waypoint, and refused to start unless I changed mode to manual and then back to auto.
My skid-steering rover starts in auto and requires no arming switch but requires RTK Fix. The Piksi RTK is the only GPS enabled as i have to rely on precise navigation

think I have been through all parameters and my rover made many, many rounds yesterday but for the project to be reliably it should resume when RTK fix is back.

Many thanks in advance
Mr Anders
Sweden

Maybe can use lua to check gps status and instruct the rover consequently.

Corrado

Then the transmitter has to be connected all the time. Currently I’m using a Graupner receiver (no Lua script) but I will probably shift to FrSky later.
Still I think this should be done in the Rover firmware. You don’t want to write i Lua script to make your car running after a red light stop :slight_smile:

Anders

I never used lua scripts in ardurover, but as far as I know, they run on the flightcontroller.
Have a look here:

https://ardupilot.org/rover/docs/common-lua-scripts.html

I needed this feature and ended up implementing it on my companion computer. I wrote a python program that talks to mavproxy (also running on the companion computer) and monitors the GPS status… and sets mode to HOLD if GPS status changes to a non-RTK status, and resumes (by setting mode back to AUTO) when the GPS is stable again.

This is an obvious feature that I imagine could be useful to many… I implemented this using ROS on a companion computer. We use our own local base stations and high gain GNSS antennas. We seem to get very reliable RTK solutions. The only time I have observed our rovers stopping due to lack of RTK fix solution is when something has happened to the communication to the base station.

Make sure you understand the argument before making fun of who suggests something. I was obviously talking of on board lua scripting.

All three Pixhawks I own are 1M type. No Lua script possible.
For me it seems that the best solution is when the Rover firmware can handle this.

I still think of the Rover an AI-vehicle. You give orders/missions and the Rover fulfills them. There may be obstacles and signal losses but when the signal is back the Rover continues.

I assume it is no big deal to implement this in the awesome Rover code. It will also be very helpful in copter when you fly RTK missions.

Thanks
Anders

Ok than guess you have to sit and wait for the dev to add it when/if they’ll do it .

For the most basic functionality you can achieve what you want with a few lines of code in the firmware. To stop the vehicle in auto mode when there is not an RTK Fix, you could add the following in mode_auto.cpp - void ModeAuto::update():

replace:

            if (!g2.wp_nav.reached_destination()) {
            // update navigation controller
            navigate_to_waypoint();

with:

    if (!g2.wp_nav.reached_destination()) {
        if(rover.gps.status() != GPS::GPS_OK_FIX_3D_RTK_FIXED){
            stop_vehicle();
        } else {
            // update navigation controller
            navigate_to_waypoint();
        }

This will keep the vehicle in AUTO but when the GPS status is not RTK fixed, will stop the vehicle and not attempt to navigate until the RTK fix returns. There is no feedback or means to disable this functionality.

Thank you very much for your help!
I have downloaded the code from github and I’m trying to understand what makes the rover to stop and refusing to continue. Obviously not this piece of code as far as I can understand.
When I look at the log the mode changes to HOLD

The log is too large. I have put it on my WEB-Server: http://fuzzyfool.no-ip.com/Rover/00000133.BIN

As you can see the rover has made quite a large number of rounds successfully but sometimes it stops. GPS_MIN_DGPS is set to 100 so only RTK Fix should do

Many thanks in advance
Anders

Obviously not this piece of code as far as I can understand.

Which piece of code are you referring to? The code change I have given above should accomplish what you want - the vehicle will stop and pause navigation when the GNSS reports anything other than a RTK fix and will resume automatically when the RTK fix returns.`

I believe that GPS_MIN_DGPS is a configuration setting for your GNSS module that is used when GPS_AUTO_CONFIG is enabled. I do not believe that this parameter directly effects AUTO mode.

Anders, Looking at your log file, it is apparent that your GPS is not happy. You have repeated status drops to 1:


I would look at your GNSS antenna connection/placement and make sure everything is secure and that the antenna has a clear view of the sky and that the antenna and module are both away from any source of interference.
I can also see that the number of satellites in use at any given time is typically 6-7 with as few as 4. I see that you are using the Swift Piksi. We have a couple of the Pkisk Multi’s as well as the Duro Inertial. I presume that your Piksi is limited to the GPS constellation only? Even with only one constellation, you should be getting a better result than what you are getting. Also, What correction source are you using and how are your corrections being sent to the rover? The SBPH entry in the log indicates that there are many CRC errors in the raw data injection messages. This indicates that the correction data is only intermittently reaching the GPS.

Despite your very intermittent GPS results, the EKF seems to do OK most of the time however, there are a number of critical errors reported in the log file. These will correspond with your vehicle stopping during AUTO missions. The vehicle did not stop due to lack of RTK fix (as this happened literally 100+ times in this log…) The failsafe errors seem to have occurred due to the EKF INAV not being happy. The errors seem to correspond with the periods of large yaw innovations.

I haven’t been concerned with vibration levels since I was using Arducopter more than 5 years ago, so I’m not sure what vibrations levels are acceptable on rover. Unfortunately I do not have access to any log files at work to check, but at first glance, your vibration levels look quite high. This may be contributing to the EKF issues.

I am not sure what your use case is or how close to the path you need your rover to stay, but if you want the rover to stay closer to the path after turning then I can give you some tips.

Thank you very, very much for our help!

I have a Garmin antenna mounted around 3 dm above the Pixhawk. It should be a very good antenna and the best I have tested so far. Do I need to separate the Piksi from the Pixhawk?

For RTK communication I have 3DR 433MHz SIK Radios but yesterday I got better antennas.

I will carefully read all above

Best regards
Mr Anders W

This is how my rover currently looks. The 433MHz antenna is new and wasn’t mounted in the test. RunCam and video Tx was unpowered.

The DC-motors and drivers are located under the metal sheet which is connected to battery minus. The 3DR radio, in the front, had a simple 1/4-wave antenna. For MP telemetry the standard antenna has been enough as I control the rover with my radio.

Any tip would be helpful

Anders

Your 433MHz antenna certainly looks impressive - but I would be concerned that it will reduce the visibility of the GNSS antenna… Where are your corrections coming from? Are you certain that you are only transmitting the data that the Piksi requires for RTK and no more? What sort of data rates are you seeing? I have not used his type of radio to transmit corrections for some time but I did have some reasonable success (single constellation/ single frequency) with reasonable line of site range using a setup not too dissimilar. I did play around with the SIK radio settings and made sure it was only transmitting what I needed so that I could keep the data rate down (slower radio transmission rates will give you better range/reception). If you are using SBP output from your own base station, make sure you have only the required messages enabled and that the rate is appropriate (~ 1 sec - the corrections do not change fast over time and the rover can use 1 second correction updates to produce rtk fixes faster than this) .
Looking at the steering and speed/throttle response, it seems that your rover is suffering from poor tuning. The steering output has significant oscillation. The throttle output seems to struggle to meet target too. Take a look here: https://ardupilot.org/rover/docs/rover-tuning-steering-rate.html and here: https://ardupilot.org/rover/docs/rover-tuning-throttle-and-speed.html. Spending time getting this part right will make a big difference!
Some of the apparent tuning problem may also be due to the significant vibration present. You seem to have ~+/-5g on all three axes. I took a look at some of our logs today and we typically have less than +/-1g. Do you have any form of vibration isolation on your Pixhawk mount?
As far as keeping your vehicle as close as possible to the path, there are a few things we have done to achieve this. First of all, you have to understand how the L1 controller works and that it reacts to reduce the heading error with respect to the ‘L1’ point - which is always some distance along the WP path in front of it, depending on parameters you set and the current velocity. It is worth while looking over this page: https://ardupilot.org/dev/docs/rover-L1.html. To get the most accurate path following, we tune our vehicle to get the lowest possible NAVL1_PERIOD possible without inducing wild oscillations. We set NAVL1_XTRACK_I to zero as we have found that where you have good traction, this I term induces oscillation backwards and forwards across the track and does not help (I can see a case for using it where you might encounter loss of traction) We set the WP_OVERSHOOT param to zero which has the effect of forcing the vehicle to attempt to slow almost to a stop at each WP. Depending on what sort of turn you are after, you can enforce a minimum speed using the WP_SPEED_MIN param. Slowing down for turns makes a big difference to accuracy! To get the rover to react sooner to a corner, you can increase the WP_RADIUS so that it thinks it has achieved the WP before it actually gets there and turns to focus on the next WP. These params can also limit how hard your rover attempts to turn: MAX_TURN_G, ATC_STR_RAT_MAX. Another factor in WP course accuracy is having a reliable heading. Check that at all times your heading matches your GPS course. You can alter the weighting that the EKF places on the magnetometer using the EK2_YAW_M_NSE param (or EK3_YAW_M_NSE if you use EKF3). Increasing this param will make the EKF rely more on the GPS course.

Thank you very, very much for this helpful information.
I will carefully read those pages

Anders