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 
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);
}
)
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).
