mavlink / qgroundcontrol

Cross-platform ground control station for drones (Android, iOS, Mac OS, Linux, Windows)
http://qgroundcontrol.io
3.29k stars 3.62k forks source link

Support of ArduPilot Copter/Rover Follow mode #7811

Closed rmackay9 closed 1 month ago

rmackay9 commented 5 years ago

ArduPilot Copter and Rover have a Follow flight mode which is implemented on the flight controller. There are pros and cons of implementing the logic in the vehicles vs the ground station but in any case, this enhancement request is to make use of the flight controller based Follow mode for regular "FollowMe" where the vehicle follows the person holding the ground station computer.

To make this work, the ground station must publish it's position at 1hz (or faster if possible) to the vehicle using the GLOBAL_POSITION_INT message. Beyond this it's possible that the GCS user could modify the position of the vehicle etc with one of the existing mavlink parameters but I haven't looked into this much.

FYI we have a somewhat related issue on the AP side here to support QGC's GCS based follow-mode (i.e. the logic is done on the ground station computer) but I think we should support both methods.

Some videos of the mode in action:

DonLakeFlyer commented 5 years ago

Should it publish GLOBAL_POSITION_INT all the time to any ArduPilot vehicle? Or just when the flight mode is follow me?

rmackay9 commented 5 years ago

@DonLakeFlyer, the GCS only needs to send the position when the vehicle is in Follow mode. The vehicle will stop and hold position if it's not getting position updates.

DonLakeFlyer commented 5 years ago

@rmackay9 I assume GLOBAL_POSITION_INT.relative_alt should always be 0?

DonLakeFlyer commented 5 years ago

@rmackay9 Also ok to set GLOBAL_POSITION_INT.hdg = UINT16_MAX or do you need that?

rmackay9 commented 5 years ago

@DonLakeFlyer, for ground vehicles it won't matter but for flying vehicles it can. On the AP side we have a parameter (FOLL_ALT_TYPE) which allows the user to decide if it will use the relative or absolute altitude from the lead vehicle/GCS. By default is uses the relative alt.

I guess it depends upon whether we want the feature to be useful on ski hills or not.

DonLakeFlyer commented 5 years ago

What is a relative alt for a GCS Position? Relative to what? The first ever gcs position gotten? Kind of an odd concept.

rmackay9 commented 5 years ago

@DonLakeFlyer, the relative altitude in the GLOBAL_POSITION_INT message is relative to the senders home. In AP's current implementation we assume that all the vehicles have very similar home altitudes which is probably true in most cases. This is why we allow using the absolute altitude though as well in case this isn't true. In practice though, because GPSs altitudes are often off using absolute altitudes doesn't always work very well.

We also have some logic in how the position offset is calculate that may paper over some altitude issues. In particular if the FOLL_OFS_X/Y/Z values are left at zero then they're initialised when the following vehicle first sees the target. So even if the altitudes were apparently off by something ridiculous like 200m, the follow may still work because it will try and maintain that 200m difference.

Re the heading, sending a heading of UINT16_MAX will require a change on the AP side. Right now the follow fails in some cases at least if it doesn't get a proper heading. for example the offsets can be setup to be relative to the lead vehicle so in these cases at least it will fail. This is not a really difficult change for us to make though. Alternatively I guess you could make the GCS's heading come from it's ground course... or maybe some tablets know which way they're facing?

DonLakeFlyer commented 5 years ago

Ok no problem, I can get you relative alt and heading.

DonLakeFlyer commented 5 years ago

Another issue is that depending on the device you are running on you may not be able to get things like heading and/or velocity information. I could try to calcuate that from locations but I"m not sure that's going to provide quality data good enough for follow. Should I just bail on sending in that case?

rmackay9 commented 5 years ago

@DonLakeFlyer, I'm not sure. we current depend upon all three being sent (pos, vel, heading).. we can make changes on the AP side to handle them not being present of course or we can just say those devices aren't support for now. I'm not sure

DonLakeFlyer commented 5 years ago

I'm going to not send if the data is missing then. In the current GLOBAL_POSITION_INT as speced there is no way to send with missing bits of information.

This case is a reason I like the FOLLOW_TARGET message better for this. It tells the vehicle which pieces of information are available and then the vehicle can decide if it has enough information to work from. As opposed to coding this into the ground station side. In reality that is the major difference between the two messages. Other than that they work exactly the same and achieve the same result. There isn't anything special with regard to how QGC uses FOLLOW_TARGET. The QGC code is exactly the same up to the point where it sends a mavlink message to the vehicle. In PX4 it sends FOLLOW_TARGET, in ArduPilot it sends GLOBAL_POSITION_INT.

There was a lot of random discussion around this on the ArduPilot issue which headed of in a different more complicated direction. But in reality it should be a few minutes of coding to support FOLLOW_TARGET in ArduPilot. The data is the same, just a different message.

Anyway... I have it all coded. It's going to make me a while to get set up to this this though.

DonLakeFlyer commented 5 years ago

@rmackay9 Can I test this using the current Copter stable?

DonLakeFlyer commented 5 years ago

Tested on Copter master and working

rmackay9 commented 5 years ago

Wow, great!

DonLakeFlyer commented 5 years ago

There is still a problem here. Since HOME_POSITION isnt streamed QGC will only get home position if you boot the vehicle after you start QGC,

DonLakeFlyer commented 5 years ago

This last pull should have everything needed now

Hwurzburg commented 5 years ago

@DonLakeFlyer Don, so to test this with the QGC nightly build, I would open QGC, and try to connect to the telem connection to my Rover (I use a BT based repeater to connect to the phone from the 915 receiver), then boot the Rover, and put it into FOLLOW mode....correct?

rmackay9 commented 5 years ago

@Hwurzburg do you think you could test this?

DonLakeFlyer commented 5 years ago

@Hwurzburg There is a follow parameter you need to enable as well I believe.

DonLakeFlyer commented 5 years ago

@Hwurzburg I just merged in the home position streaming change to QGC. So a new daily build should be available in about 30 minutes.

DonLakeFlyer commented 5 years ago

Don, so to test this with the QGC nightly build, I would open QGC, and try to connect to the telem connection to my Rover (I use a BT based repeater to connect to the phone from the 915 receiver), then boot the Rover, and put it into FOLLOW mode....correct?

Correct. Other than I think it's FOLLOW_ENABLE parameter or something like that. Don't exactly remember from my testing. I tested with Copter and not Rover. Hopefully shouldn't be a problem.

Hwurzburg commented 5 years ago

Unfortunately, the daily build apparently is no longer being loaded on my Android Phone every day...and cant get it from play store, says its installed(its not anymore on my phone), but then says all my phones are incompatible...says my Razr is incompatible (stable works fine on it)...

DonLakeFlyer commented 5 years ago

Hmm, the Daily store is probably screwed up with the latest requirements Google just imposed. YOu can install it manually from here: https://qgroundcontrol.s3-us-west-2.amazonaws.com/builds/master/QGroundControl.apk

Hwurzburg commented 5 years ago

got it...but shows as 8/28 version on General screen....hope its the right one....will try tomorrow

DonLakeFlyer commented 5 years ago

Crap sorry. Wrong link. There are 32 and 64 bit versions now: https://qgroundcontrol.s3-us-west-2.amazonaws.com/builds/master/QGroundControl32.apk

DonLakeFlyer commented 5 years ago

@Hwurzburg There is still a problem with sending the heading correctly to the vehicle. I'm working on a pull to fix that as well as create a setup page to make it easier to setup gcs follow me usage. I'll let you know when I've got that. Than you can test the whole thing together.

DonLakeFlyer commented 4 years ago

So this is about as good as it's going to get for now. I still can't get quality heading information from a tablet, but that will require Qt updates.

DonLakeFlyer commented 4 years ago

@rmackay9 So I implemented all of this and from a purely technical standpoint it's been done for a while. But from a usability standpoint I think it varies from unusable to dangerous. The reason for that are the quality of data available at the groundstation level. The altitude you get from a devices gps is pretty much useless and the quality of a heading from a gps is low unless you are moving above a certain speed.

There is pretty much no way to fix the device altitude problem. Which in turn means that you can only use absolute mode for heights. This means an end user needs to enter an AMSL altitude for the height they want to fly at. To me, requiring the user the correct AMSL value to enter there is not usable. It also means that follow me params must be tweaked for every different location you fly at. My guess is most people will not understand this, not pay attention to even any warnings I can put in there are fly into the ground due to bad settings.

The second problem is with heading, this is a somewhat fixable problem. But once again the quality varies device to device. You can switch to using the device compass, but that is affected by the calibration state. I think I can make this better but still concering.

To me follow me needs to work based on a relative height from home position altitude. That is the only safe/simple setup. But that would required an additional mode added to FOLL_OFS_TYPE. As it stands now I'm thinking I"m going to remove this feature from QGC because I think it will cause more problems than it solves.

DonLakeFlyer commented 4 years ago

As it stands now I'm thinking I"m going to remove this feature from QGC because I think it will cause more problems than it solves.

@rmackay9 I guess ultimately that call is yours. I can fix it up for absolute heights only.

rmackay9 commented 4 years ago

@DonLakeFlyer,

I'd like the follow feature to stay. re the FOLL_OFS_TYPE, AP's follow mode already supports both relative and absolute altitudes and because they both arrive in the same message, the user can specify which one AP should use by setting the FOLL_ALT_TYPE to 0 for absolute or 1 for relative. Relative is the default for now. Maybe QGC needs an option to just lock the altitude? On the AP side, if the user leaves the FOLL_OFS_X/Y/Z params at the default of zero then the vehicle should just maintain it's current altitude as it follows the user around.

Re the yaw, AP's FOLL_YAW_BEHAVE parameter defaults to 1:Face Lead Vehicle so by default the ground station's heading isn't too important.

One note is that beyond Copters, AP Rover/boat also supports follow mode and with these vehicles neither altitude nor heading is used.

DonLakeFlyer commented 4 years ago

One note is that beyond Copters, AP Rover/boat also supports follow mode and with these vehicles neither altitude nor heading is used.

Yup, wasn't thinking of that.

Maybe QGC needs an option to just lock the altitude?

Ah, I think that's the answer. I can lock the altitude to home position alt. And then it turns follow me into an alt above home follow feature which I think is workable.

DonLakeFlyer commented 4 years ago

This is hopefully all done now!

rmackay9 commented 4 years ago

@DonLakeFlyer, awesome, thanks very much! I'll try and find someone to test it out. Most likely @Hwurzburg or perhaps someone on the Rover or Copter forums.

Hwurzburg commented 4 years ago

I will try this weekend or early next week...

DonLakeFlyer commented 4 years ago

@Hwurzburg FYI: This new way of doing it still isn't any different with respect to the heading coming from the GCS is dependent on the GCS gps moving in a direction above a certain before you get a good heading.

Hwurzburg commented 4 years ago

@DonLakeFlyer @rmackay9 Well I tried it on the boat today and it didn't go well.....I think the problem is that the phone running QGC is not giving good enough heading info so its very erratic and unreliable in tracking...also loiter when GCS is motionless is problematic....sometimes it tries to run back to the other side of me (which is land, not water!).....I found myself really missing the old relative offset, gps only follow me the last time I had used it....at least I could walk around the lake with it, even if I had to toggle in and out of the mode momentarily when I changed direction 60-90 degs and repositioned myself with respect to the boat in order to reset the relative offsets as I progressed around the lake...

DonLakeFlyer commented 4 years ago

The fundamental issue is that heading from the gps is extremely device dependant. My Samsung Galaxy tab seems decent even when you stop but some other look like to a random number generator like Henry's. I really wonder whether this can be made to work well ever with using the compass on a device which is a whole other ball of hair.

.I found myself really missing the old relative offset, gps only follow me the last time I had used it

Not sure what you mean by that. Any relative offset is going to need some sort of heading the determine what it is relative to.

Hwurzburg commented 4 years ago

its been so long ago, I cant remember exactly....maybe it was relative and I was walking when I initially switched to the mode......I know I had to have the set offsets at 0...

I just looked back at my PRs...now I remember...not relative, NED mode, but automatic offset calibration at mode entry if offsets are zero to your actual current NED offset.....then you can walk along the shore, either direction....if you come to position that would require offset reset, you can walk to the new position and re-enter the mode (like its in front to the left and you walk up to a left turn, if you dont drop out of follow ,make the corner, and re-enter follow, you would run into the shore)....if you have the vehicle abeam, every 90degs you would need to do this if its a little ways out in the water...a little clumsy, but very accurate...just needs GCS broadcasting GPS location, not heading

@rmackay9 did this every go into copter? if so, I need to update the wiki for copter also

DonLakeFlyer commented 4 years ago

Hmm, understood. Problem is I don't think that is what people expect from Follow Me support. That will work with QGC is you set the parameters up manually for NED mode. You won't be able to set it up from the ui.

I tried looking at old Tower code to see what they did. Couldn't figure out if they used a device compass heading or a gps heading.

Hwurzburg commented 4 years ago

maybe have both follow and follow me as modes in dropdown....right now in beta I didnt see a way to select plain follow mode...at least it didnt seem available.....and GPS broadcast of position is always on now?

DonLakeFlyer commented 4 years ago

GPS broadcast of position is always on now?

That only happens when the flight mode is Follow.

Not sure what to do at this point. I'm concerned that leaving this in is simply unsafe in its current state. I either need to filter the heading from the gps somehow and head true compass based device headings woring. I'm leaning towards pulling it out of the stable which is going out now. And continuing to fight with it after that in daily builds.

DonLakeFlyer commented 4 years ago

I pulled this from Stable. It's just not there yet. I'm going to try to do some filtering on the gps heading to make it work. If I can do that I'll get that into a Stable point release. If that doesn't work I"m going to need to use device compass information which is a much harder thing given I've tried and failed on that multiple times already.

Hwurzburg commented 4 years ago

is plain Follow with gps only still an option? and I was thinking the same thing this morning re filtering heading for a relative mode....maybe forget the GPS heading and just compute it in the GCS...maybe have a threshold on minimum GPS movement (2-3m?) to add the new GPS position into the heading calculator and then filter it...I dont think that precision in terms of vehicle position relative, nor speed is an issue...this is for slowly following peds or bikes, I would imagine...even if it was a vehicle, filter lag shouldnt be a big issue I would think...

DonLakeFlyer commented 4 years ago

I don't see how any mere mortals will ever understand NED positioning. That said I also don't see it as a useful end user feature. It's not at all what people expect or want. What is the use case?

Hwurzburg commented 4 years ago

my comment above....I was using it to have the boat follow me around the lake easily....although I had to re-init the offsets (ie go to loiter mode...walk around the corner of the lake to be able to continue walking 90 deg original direction ,turn it back on to FOLLOW, it captures the new offset automatically, and if you dont change direction radically(which walking a path or shoreline you dont usually) it follows in the same general relative position...if you have to make a 180 deg, it just reverses and follows.......if a reliable relative heading follow me ever works, it would be better of course...but follow NED with automatic offset capture on mode entry works for boats and rovers well enough...better than nothing

the most re-inits occur on a circular lake....depending on how far off shore you set it up, you have to do it at least 4, more like 6 times around the whole lake...but just up and down a long shoreline not often

DonLakeFlyer commented 4 years ago

but follow NED with automatic offset capture on mode entry works for boats and rovers well enough...better than nothing

That is just way below my quality bar for something I'd want to support in QGC.

Hwurzburg commented 4 years ago

so what is the issue with just putting FOLLOW in the mode list and broadcasting GPS position? no widgets or dialogs? I dont know what support you would have to provide...

DonLakeFlyer commented 4 years ago

so what is the issue with just putting FOLLOW in the mode list and broadcasting GPS position? no widgets or dialogs?

Because it's way too easy to get a multi-rotor flying straight at your head by doing that. Or crashing into something when it doesn't work like you expected it to. To me it's a basic safety issue.

DonLakeFlyer commented 4 years ago

The key is that I'm not trying to just expose all functionality of a firmware with QGC. I'm trying to provide a usable and safe experience for QGC users. No matter what feature they may be using.

rmackay9 commented 4 years ago

I suspect that QGC will need to ignore the compass and instead heavily filter and "gate" the GPS heading. So for example, maintain a separate heading that is only updated if the GPS ground speed is over some lower limit (like 1m/s). Some extra heuristics or filtering might also be required to ensure that it doesn't flip back and forth too much.

Another approach we could take is to add a new simpler follow behaviour to ArduPilot in addition to our existing two methods (NED offset and relative offset). This new method would simply try to maintain a distance and ignore the vehicle's heading. So it would just try to move to the edge of a circle surrounding the position provided by the GCS.

Perhaps it's not necessary but I'd like to point out that AP's follow mode is pretty awesome. It's level of performance is far better than existing methods of follow and here are a couple of videos demonstrating this.

Quad following a plane: https://www.youtube.com/watch?v=0s1ZBSB0tvM Boats following each other: https://www.youtube.com/watch?v=ZSzI7XddT4A

The issue that we're facing is not a shortcoming of AP's follow mode but adapting follow mode to deal with poor inputs. i.e. "garbage in garbage out".