Adventures with fast updates for guided mode (10Hz)

Recently I’ve been playing around with sending position and velocity targets in guided mode, and I’m having problems at high rates when velocity is included. Just wondering how fast I can expect to give targets and still have it work successfully.

Copter hardware is omnibusf4pro, ‘companion computer’ is a board of my own design that receives 10Hz position updates from a similar board that I’m holding while I walk around. Since I have 10Hz target updates available, I’d like to make the most of them. I’m hoping that this will be able to closely track my movement on a bicycle at about 10m/s.

Here’s a little record of what I’ve tried so far and the outcomes.

Installed 4.1.0-official, had many problems with ‘unhealthy GPS’, altitude suddenly changing by 30 meters, ‘err pos vert variance’ and flight mode changes failing due to ‘requiring position’, when GPS fix was solid.

Gave up on 4.1.0 and installed 3.6.1 and had no problems whatsoever… the only difference in setup was using sbus instead of ibus.

With 3.6.1 I tried these tests:

  • alternate between two hard-coded positions every 20 seconds to check communication, wiring etc. Works flawlessly. All subsequent tests are using a moving target position supplied by the ground tag, with an offset to place the copter due south of my location.
  • set position at 1Hz, walking only test, works flawlessly.
  • set position and velocity at 2Hz. Works ok, although I did not try for long. The copter jerks noticeably as each update arrives and it tries to match the desired velocity.
  • set position and velocity at 10Hz. Works ok for a while, albeit with a lot of jerking around as the velocity updates are commanded. Holds the position offset fairly well even at jogging speed. However, it would occasionally launch itself at full speed in one of the cardinal directions, or very near to a cardinal direction. It would typically do this after no more than 20 seconds of normal behavior. After getting into this state no further guided mode updates seemed have any effect, and I had to switch into RTL to stop it from flying away. The direction it chose was most commonly north/south, less commonly east/west, and only a couple of times it rocketed straight up. In the latter case, switching to RTL did not stop it, and I had to switch to stabilize mode to bring it down.
  • set position only at 10Hz. Works but moves so excruciatingly slow that you can hardly tell it’s moving, at least when the set point is close. Even when the target point is 50 meters away it still only moved at about 0.5m/s. I got the feeling that the frequent updates were causing it to start the movement calculation afresh every time, which somehow prevented it from picking up speed.
  • set position only, at every 2 seconds. Works well, copter can now pick up reasonable speed but the slow update rate mean lower tracking accuracy.
  • set position only, at 1Hz. Seems like the best compromise if only using position updates. Copter can keep up with me quite well at walking speed, is only 2 seconds behind me when jogging. Unfortunately the bike is way too fast.

Here’s a video of that last case, which is about the best I have for now. Since the offset position is due south and the camera gimbal angle is set to always face forward, the copter yaw is being set to zero (face north) so that we can tell when it has reached the target point (ie. when I’m in the center of the frame). As long as the bike is not too fast I’m still in the view, but with even a little bit of normal speed it quickly loses me. Of course if the yaw was being set to face me this particular video would have looked fine, but that wasn’t the point of this test.

Fortunately none of this resulted in any crashes, although there was one issue that almost lost the copter. I decided to see what would happen if I switched off the tag that was sending the position updates. It was probably not a good idea to try this for the first time with the radio being about 100 meters away, but I was confident the copter would just stop in place when the position updates were no longer being sent. Unfortunately it made a beeline due east at top speed, and before I could get back to the radio it was so far away I could no longer see or hear it, and being after twilight didn’t help. I switched into RTL and waited about 30 seconds… nothing. Then I remembered the time that RTL did not work and I had to switch to stabilize to get the guided mode to stop. So I switched into stabilize and then back to RTL. 30 more seconds… nothing. I had pretty much resigned myself to having lost the quad, but waited another minute or so anyway, straining my ears. Finally I heard a faint bzzzzz… ah, that sweet sound :slight_smile:

Anyway, the crazy behavior with fast updates when including velocity gave me the feeling that some data might be getting partially overwritten in the middle of a calculation somehow. It seems like one of the NED values used became suddenly very large, in either negative or positive direction.

After this I noticed in the release notes for Copter 4.1.0-beta6 there is a note saying “Guided mode accepts position targets at high rate” and it’s classed as a bug. This made me interested in trying 4.1.0-official again, so I did. Strangely this time I had none of the GPS problems I had before!

Let me just say that the new behavior of guided mode when interrupting a previous fly-to command is really sweet. Since I had not seen this before I almost wasted half a battery just telling it to go somewhere (with Mission Planner ‘fly to’) and then while it was on the way there, changing the target just to watch the smooth way it turns onto the new track. That’s really awesome!

I don’t recall seeing that smooth response to target changes before, so this gave me hope that all would be good with the fast updates too. But unfortunately, the affinity for cardinal directions remains with 4.1.0-official, when pos+vel targets are given at 10Hz.

I just discovered that “Follow Mode” might be more appropriate for this so I will try that next, but the awful New Zealand weather means it could be a while yet. I still like the finer control I have with setting the target position directly, which allows me to adjust the follower offset on the fly, and enforce a no-go bubble around myself. On the other hand, Follow Mode would allow me to use fences which would be nice.

Eventually I’d also like to use DO_SET_ROI_LOCATION so that a gimbal could be better controlled than just setting copter yaw the way Follow Mode would. Unfortunately all my attempts with DO_SET_ROI_LOCATION have been ignored so far, but I’ll figure that out later (I wonder if 10Hz updates will also be a problem there?)

fwiw here’s the code I’m using on the ‘companion computer’:

uint8_t buffer[MAVLINK_MAX_PACKET_LEN];

void set_posvel(bool useVelocity, int32_t lat, int32_t lon, float alt, float vx, float vy, float vz) {

    uint16_t mask = useVelocity ? 0b0000100111000000 : 0b0000100111111000;

    mavlink_message_t msgOut;
    mavlink_msg_set_position_target_global_int_pack(
                mavlink_system.sysid,  // 1
                mavlink_system.compid, // 1
                &msgOut,
                app_timer_cnt_get(), // basically getMillis
                1,
                MAV_COMP_ID_MISSIONPLANNER, // ??? saw this in a forum somewhere
                MAV_FRAME_GLOBAL_INT,
                mask, // field ignore flags
                lat, // lat
                lon, // lon
                alt, // alt MSL
                vx, // vx m/s
                vy, // vy m/s
                vz, // vz m/s
                0, // ax
                0, // ay
                0, // az
                0, // yaw (rad) 0 = face north
                1 // yaw rate (rad/s) set to non-zero if you actually want yaw to change
    );

    uint16_t messageLength = mavlink_msg_to_send_buffer(buffer, &msgOut);
    uart_write(buffer, messageLength);
}

void set_roi( int32_t lat, int32_t lon, float hMSL )
{
    float flat = lat / 10000000.f;
    float flon = lon / 10000000.f;

    mavlink_command_long_t com = { 0 };
    com.target_system    = mavlink_system.sysid;  // 1
    com.target_component = mavlink_system.compid; // 1
    com.command          = MAV_CMD_DO_SET_ROI_LOCATION;
    com.confirmation     = true;
    com.param5           = flat;
    com.param6           = flon;
    com.param7           = hMSL;

    int companion_id = 0; // ??? from mavlink uart example

    mavlink_message_t msgOut;
    mavlink_msg_command_long_encode(mavlink_system.sysid, companion_id, &msgOut, &com);

    uint16_t messageLength = mavlink_msg_to_send_buffer(buffer, &msgOut);
    uart_write(buffer, messageLength);
}
1 Like

Small update, I tried MAV_CMD_DO_SET_ROI instead of MAV_CMD_DO_SET_ROI_LOCATION and that works ok. With the useless weather we have here I had to test this in the rain, but thankfully nothing got damaged. Change to the above code is like:

    com.command          = MAV_CMD_DO_SET_ROI;
    com.param1           = MAV_ROI_LOCATION;

… and also change the bitmask so that yaw is not used when setting the position target.

While it’s an interesting idea, I can’t imagine that guided mode target updates at 10Hz are actually a good idea. Especially at slower (bicycle-type) speeds where the Copter is likely to always be on the cusp of achieving the target, it could perpetually be accelerating and decelerating, killing battery longevity and making the follow speed erratic.

In the video, however, the Copter seems extremely slow to accelerate and almost incapable of following you at speed, possibly suffering from a poor tune or extremely conservative pitch/roll angle limits (possibly masking the accel/decel problem from above). If the Copter’s performance really is that sluggish all the time, then the 10Hz updates do you no favors since it’s incapable of responding at that rate.

Rather, for smooth control, I think you’d want the slowest reasonable target update rate rather than the fastest possible one. Have you tried 1-2Hz? Was performance better or worse?

I definitely agree that the native FOLLOW flight mode should yield better results. You’re already crafting MAVLink messages on the companion, so it shouldn’t be too difficult to have it report a heartbeat message and position (which I think are what is required for FOLLOW mode - you’ll want to research that a bit more thoroughly).

And make sure the copter is good PID tuned and running at least ArduCopter 4.1.1-rc1

@iforce2d
lots of work and testing … and much of it very much reminds me of my own experiences with this :slight_smile:
I can speak only for copter 3.x and 4.0, haven’t really checked out how 4.1 would do, thing is that I eventually gave up on all this guided-with-mavlink stuff
when you look at the source code you’ll find that guided mode is extremely burdened with “history” and 1000ends of conditions exist for each case to work
I too observed that you can’t really guide it with fast position updates (so don’t waste your time with thinking it is because of your copter isn’t well tuned, set up, etc, it’s not this ;)), so I eventually only used velocity wrapped into a position P controller
note that this is (in variations) also what good ole Solo did for its smart shots, is what the demo lua scripts of Randy do, and also follow is largely doing this (so, yes, chances are that you like follow better :wink: )
as said I mostly gave up on guided by mavlink (I really use it only for quick demos), and started using rather on-board Ardupilot lua script, and I think that’s really the way to go
I do have two videos on this on my yt channel, and I believe I should have also posted somewhere the ardupilot lua script, but am not sure LOL
just some of my experiences in case it might help, and also so you not think that you are the only one with these observations
:slight_smile:

1 Like

I guess @Leonardthall could certainly offer some guidance here ? :wink:

On discord while talking about his latest PR on full Support MAVLINK_MSG_ID_SET_ATTITUDE_TARGET, he wrote this:
I bet you could feed the 24.7 Hz trajectory straight into guided mode using the NED message and do just as well.

Yes, I have been meaning to get around to rewriting guided mode for years and finally got started on it for 4.1. Previous to 4.1 I would say you only had the most basic guided mode support. But I am very happy with 4.1 and have tested full accel / vel / pos input commands in a VICON style chamber and it works very nicely, and is fast!

4.1 adds almost full support for the guided position / velocity / acceleration commands with the only thing I should add is absolute raw input (but unless you are running at 100 Hz you will be able to do better with what we have now).

4.2 will have the same capabilities for the attitude commands and we will have the extra guided Mavlink messages added.

So the first step is getting 4.1 up and running.

1 Like

Thanks for the feedback. I’m pretty sure the tune is fine.

It occurred to me that I had not tried the “position only 10Hz updates” case with 4.1.0 so I tried that, and it’s way better. It seems much more capable of maintaining the current speed while the target is being constantly updated, instead of moving super slow like 3.6 did.

Here’s another try with these settings:

  • still using guided mode
  • 5Hz updates for the position target, no velocity used
  • 10Hz updates for SET_ROI to change yaw
  • ground tag smooths the velocity before transmitting, to remove jitter. The velocity jitter is mostly noticeable when standing still, when the GPS can give sudden changes in direction from one reading to the next, not as useful for a ‘look-ahead’
  • position target is actually the current position plus a little of the current velocity. I was already doing this a little before but the ‘look-ahead’ factor is now much larger

There is no sound because I’m using a different camera this time and the sound was completely unusable. The gimbal pitch is locked so it’s not supposed to be doing any vertical following, gimbal yaw is set to follow the copter nose.

At first comparison this video looks better simply because the yaw is following me, but even without that it would still have been a good improvement on the previous one. The quad is able to move much faster and it matches my bike speed even at full throttle (well, at least with the bike on an almost empty battery). The position offset still takes a fair while to catch up in some cases though. There are a couple of strange behaviors, obviously one being the fly-away at the end when position updates were stopped, I have not tried this much but it seems to happen about 50% of the time, usually at a much faster speed. This time it was a slow drift upward, almost perfectly southward, while facing east. The other strange thing is altitude seems to drift more than before.

@olliw42 I don’t know much about scripts but I’m guessing the cable cam script is able to access RC input values in the flight controller for the moment-to-moment ‘fast’ updates to slide along the cable, so there is no external component involved while it is actually running. In my case I need to transmit at least 3 values (preferably many more) from the ground tag and feed them into the flight controller somehow, which I think will require a companion computer either way. Is there some way I could provide for example, 8 floating point values to a script at 10Hz via UART?

Overall it seems like without some direct velocity manipulation the outcome will never be quite what I was hoping for. Next I will try Follow Mode and see how that turns out. The thing about velocity wrapped into a position P controller is I would need to read position info from the flight controller which adds another point of failure. It would probably work pretty well, but I will persist with write-only methods a bit more first.

Can you provide the log?

This is absolutely not a problem with 4.1. Just send it where you want it and what velocity you want it to be doing. It will close the position error while doing your requested velocity.

@Leonardthall
I guess I should retry with 4.1
I have no doubt that you much improved the guided controllers (even though I already noted that the altitude issue hasn’t become any better)
I however wasn’t exactly saying it is the guided controllers. To me it seems the issue is (was?) largely the interface, in terms of mavlink messages, in terms of parameters usage, in terms of magic settings, etc…
(please bear in mind that I can’t say I truly understand all this)(but that’s to some extend the point, you sure can get great results, but this doesn’t mean we poor guys can ;))
My impression is that guided mode just has to serve way too many not always compatible purposes…

I really wish there would be a new mode which is dedicated exclusively to remote operation, so to say a robotics mode

I may put your statements to test :smiley:
what mavlink message(s) should I use? (with the copter set to guided)
I would be interested in pos, posvel, and vel control

@iforce2d
you may underestimate the script capabilities
I have not tried reading an uart, but I believe you can do exactly this (for examples see ardupilot/libraries/AP_Scripting/examples at master · ArduPilot/ardupilot · GitHub, see e.g. serial_dump)

you are sure correct that the script I did won’t serve you directly (no, I don’t read sticks), but as much as I understand what you are doing you are using the companion “only” to translate the data from the gps unit you carry to some mavlink. This seems to me allows for plenty of different approaches of where to do what. E.g. you could directly send mavlink from the gps unit. With the scripts it might well be that the first guess approach of how one “traditionally” would do it won’t work, but it offers so many options that there will be a way. The good thing is that once you have your thing in the script, you can do more advanced stuff so more easily … like “Overall it seems like without some direct velocity manipulation the outcome will never be quite what I was hoping for” :wink:

actually, just to have said this, I do it by attaching a GPS to my TX16S and have the TX16S sending the mavlink messages to the copter (using MAVLink for OpenTx firmware) … hardware-wise it hardly can be easier

even with a companion the P controller thing is not really a point of failure, your companion will receive it just fine and reliably. It is sure more code work to do. If you do the P controller in the script “point of failure” thoughts should just evaporate … since you get the position directly from the flight controller … at no cost and in no time :wink: it’s nearly as good as if you would do it in C code

I guess the sole point of my many words is to say that IMHO scripts is the way to go if the canned options do not already give what you want :slight_smile:

@Leonardthall Since I do testing at home I don’t really want my logs out on the net, but if you let me know where to email I’ll get some to you.
Yeah, I think the “send it where you want at whatever velocity” behavior you’re talking about is all I need, but as mentioned in my first post it’s just not working well at high update rates, because the copter shoots off in one of the cardinal directions after a while when velocity is included. If not for that I probably wouldn’t have found myself making this thread.

@olliw42 Yes, currently the companion is basically just relaying GPS values to mavlink. But I was thinking it would do more in future, such as altering the position offset based on the current location to avoid obstacles and get better camera framing. Although I hadn’t thought too deeply about it, I was thinking to have pre-defined zones within which a specific offset would be used. But actually I might be better off restricting the movement to linear segments like a cable cam, so that there is a known safe route and a well-defined method for getting from one place to another. The ground tag transmitter would then be used to find the closest point along the cable and for the ROI. Would be very cool if it worked out and much safer than a position offset for many reasons, not least of which being that the altitude could be home-relative rather than trying to coordinate two different GPS readings.
In any case it sounds like I need to look into what scripts could do here, I have been putting if off for long enough already. btw I hope it’s ok if every time I read one of your posts I’m hearing your accent in my head :slight_smile:

1 Like

Sorry, I am not aware of the problem you are talking about here. Where were you describing / discussing this? I have a bee in my bonnet about getting Guided mode going properly right now.

Yeh, I still have some work to get the Mavlink messages sorted out and meeting specification. Some of them are underdefined though.

This is heavily dependent on your requirements. If you want a replacement for a more dynamic auto mode then you can use position commands with velocity and acceleration ignored. This will create straight lines to that position (if the position is far enough away) with nice corners after an update. It also won’t time out like it does if you have not set velocity to be ignored.

If you want fast response you want to start commanding velocity for really fast response time acceleration too. But if you just define position and zero velocity but don’t ignore velocity the aircraft will attempt to get to the position in the fastest way possible but not necessarily a straight line.

  • So Pos with vel and accel ignored will change height gradually over a long distance climb.
  • But Pos with vel and/or accel zero but not ignored will climb immediately to the target height as fast as possible and travel the rest of the way at the target height.

If you know your position and velocity at all times then using position and velocity inputs with acceleration ignored or set to zero works well. Just keep in mind that you may want to set your PSC_JERK_XY and WPNAV_ACCEL parameter based on how often you are giving updates and how aggressively it makes adjustments to follow your commands.

If you are generating complex pos/vel/accel defined paths with a higher update rate (anything above 10 Hz works well) then just feed all three in and let the position controller correct for the integration errors due to the difference in update rates.

What are you using it for?

1 Like

Thanks to some help from Leonard, we found that my companion board was causing both the flyaways and the sudden shooting-off in cardinal directions, so there was no problem with Ardupilot. Data coming over my radio link was being corrupted every 30 seconds or so due to the packets not being CRC checked by the hardware layer. After adding my own software CRC check to discard corrupted packets, I was able to use 10Hz position and velocity updates without any issues.

Here’s a couple of videos with these changes:

  • 10Hz for both position and velocity
  • ground tag now sends a moving average of the past 1 second for velocity instead of a complementary filter that was slower to change
  • position target look-ahead is increased a bit

This is with fixed yaw, to get an idea of how well the quad tracks me (I will be in center of frame horizontally when it’s at the setpoint)

This is with ROI set to my location:

The result is getting pretty decent. I think the ROI target needs to use a look-ahead too, since it seems to take a while to reach the desired angle.

By “look-ahead” I mean that instead of using the current position as the position target, we use the current position plus current velocity * x.

The velocity target is just the moving average of current velocity as is.

Right now my biggest problem is that the altitude difference between the two GPS modules keeps changing over time. Sometimes I need to change the target altitude offset within about 10 minutes of it having been just fine before. At the moment I have to do this by re-uploading to the companion board which is a pain in the ass.

My original plan was that the ground tag could be given some kind of input to adjust the altitude offset, but I’ve started to daydream more about the cable cam style pre-defined routes that I mentioned in my previous post. It would be more reliable if the altitude could be defined as relative to the take-off point…

1 Like

I suspect your moving average is why you need the look ahead.

I would try passing your raw position and velocity directly to the aircraft.

Guided mode is basically using a jerk, acceleration, velocity limited complementary filter to generate the path.

That look ahead issue is why I suggested a slower update rate. Would a slower rate help, or am I off base there?

Lower update rate shouldn’t change anything. So I think you are off base with that one.

1 Like

I think I underestimated the importance of the velocity term in the target messages.

1 Like

Yes, the velocity terms in the target message update the position target until the next message is received.

1 Like

Okay, I tried passing raw position and velocity directly. The movement seemed a little odd and ended up doing an impromptu close inspection of the roof of my house, but my good luck is holding well and it didn’t crash. Watching the video afterward it’s now painfully obvious that I have the north and east axes of the velocity swapped :confused: The usage of ‘X’ and ‘Y’ is kinda vague and I did wonder about it but there was no mention of which was which in the mavlink code comments, and also on a mavlink reference page that I checked. So I went with positive X being east. It seemed to be working ok but that was only because my look-ahead offset added to the position target was masking the problem (my look-ahead used the correct axes).

Anyway here is a video of that. It’s not that useful since we already know it’s using incorrect settings, but it does showcase how well Arupilot handles the crappy data it is being given. The section of road in front of the cattle pens is aligned exactly 45 degrees north-east, so for about 40 meters the incorrect axes problem doesn’t matter, and the result is really good, best I’ve seen so far. The last run heading to the left is about as fast as I feel comfortable riding on gravel, and the quad is keeping up pretty well. I’m looking forward to trying tomorrow with the proper velocity axes, weather permitting.

4 Likes

Thanks Chris for all the effort !!
Beta Tester are SOOO important for our community , kinda like the modern days gladiators

image