Issue guided mode mavlink velocity command

[avncalst]

Bug report

Issue details

Using ROS-SITL, I detected using dronekit to set the velocity in the _vehicle.message_factory.set_position_target_local_nedencode, that the drone starts rotating if the y velocity component, v_y, is non-zero. With the v_y=0 and v_x or v_z non-zero the drone behaves normal. In Arducopter version 4.0.7 this does not happen. Is this a bug in Copter 4.1 or am I missing something?

Version ArduCopter V4.2.0-dev

Platform [ ] All [ ] AntennaTracker [ x ] Copter [ ] Plane [ ] Rover [ ] Submarine

Airframe type What type of airframe (flying wing, glider, hex, Y6, octa etc)

Hardware type What autopilot hardware was used? (Pixhawk, Cube, Pixracer, Navio2, etc)

Logs Please provide a link to any relevant logs that show the issue

[amilcarlucas]

it could be a dronekit bug. can you test sending the mavlink the messages directly?

[avncalst]

the code works correct with Copter v 4.0.7

[hfujikawa77]

I have the same problem. Simulator(stable) launched in MP is OK, but SITL built from latest master source code doesn`t work.

The script I ran https://github.com/tajisoft/droneschool/blob/master/dronekit_scripts/209_position.py

[hendjoshsr71]

I have the same problem. Simulator(stable) launched in MP is OK, but SITL built from latest master source code doesn`t work.

The script I ran https://github.com/tajisoft/droneschool/blob/master/dronekit_scripts/209_position.py

@hfujikawa77 I have submitted this PR https://github.com/ArduPilot/ardupilot/pull/19475 to loosen a safety restriction I added now that we accept acceleration inputs. The problem with that code is that the TYPE_MASK sets force_set true. That would mean that we need to interpret acceleration commands as force which could easily lead to a bad situation. (We don't currently have a good way to interpret acceleration represented by force commands.)

Your code is not trying to send acceleration commands as it has set acceleration_ignore for each acceleration field. Thus rejecting force_set commands in that case is too restrictive.

[hendjoshsr71]

the code works correct with Copter v 4.0.7

@avncalst That statement doesn't help us debug the issue very far. Would you please tell us the commands you are sending: type_mask, mav_frame, and values. See my example below or the code @hfujikawa77 gave as an example.

I could not replicate this on 4.1.2 using any of the following messages. I even went outside of your described scope and checked yaw and yaw rate commands.

GUIDED> message SET_POSITION_TARGET_LOCAL_NED 0 1 0 1 4039 0 0 0 2 -2 1 0 0 0 0 0
GUIDED> message SET_POSITION_TARGET_LOCAL_NED 0 1 0 1 4039 0 0 0 2 -2 -1 0 0 0 0 0
GUIDED> message SET_POSITION_TARGET_LOCAL_NED 0 1 0 1 4039 0 0 0 2 -2 -1 0 0 0 0 0
GUIDED> message SET_POSITION_TARGET_LOCAL_NED 0 1 0 1 4039 0 0 0 2 -2 -1 0 0 0 0 0
GUIDED> message SET_POSITION_TARGET_LOCAL_NED 0 1 0 1 4039 0 0 0 2 -2 -1 0 0 0 0 0
GUIDED> message SET_POSITION_TARGET_LOCAL_NED 0 1 0 1 4039 0 0 0 2 -2 -1 0 0 0 0 1
GUIDED> message SET_POSITION_TARGET_LOCAL_NED 0 1 0 1 967 0 0 0 2 -2 -1 0 0 0 0 1
GUIDED> message SET_POSITION_TARGET_LOCAL_NED 0 1 0 1 1991 0 0 0 2 -2 -1 0 0 0 0 1
GUIDED> message SET_POSITION_TARGET_LOCAL_NED 0 1 0 1 1991 0 0 0 0 -2 0 0 0 0 0 1
GUIDED> message SET_POSITION_TARGET_LOCAL_NED 0 1 0 1 4039 0 0 0 0 -2 0 0 0 0 0 0
GUIDED> message SET_POSITION_TARGET_LOCAL_NED 0 1 0 1 4039 0 0 0 0 -2 0 0 0 0 0 0

[avncalst]

@hendjoshsr71 Thanks for looking into my problem. This is the mavlink message I use def slide_velocity(vel_x,vel_y,vel_z): msg = vehicle.message_factory.set_position_target_local_ned_encode( 0, # time_boot_ms (not used) 0, 0, # target system, target component mavutil.mavlink.MAV_FRAME_BODY_OFFSET_NED, # frame Needs to be MAV_FRAME_BODY_NED for forward/back left/right control. 0b110111000111, # type_mask, x:LSB & yaw_rate: MSB 0, 0, 0, # x, y, z positions vel_x, vel_y, vel_z, # m/s 0, 0, 0, # x, y, z acceleration 0.0, 0) return msg Sending this message from a python script with vel_x=vel_z=0 and vel_y=0.8 makes the copter rotate in guided mode.

[hendjoshsr71]

Hint for embedding code, :) ```python your code here ```

https://stackoverflow.com/questions/10280902/how-to-embed-code-from-repo-in-github-markdown

[hendjoshsr71]

I just tried the following in MAVProxy to attempt replication and (I think) I did.

git checkout Copter-4.1.2 /Tools/autotest/sim_vehicle.py -v Copter --console -D --speedup 1 --map

In MAVProxy mode guided arm throttle takeoff 25 module load message message SET_POSITION_TARGET_LOCAL_NED 0 1 0 9 3527 0 0 0 0 0.8 0 0 0 0 0 0

The vehicle does rotate to move in the body y-axis direction MAV_FRAME_BODY_OFFSET_NED. This doesn't seem like the desired behavior. BUT a BIG BUT the MAVLINK definitions for these Frames are a bit weird and counter to some normal conventions at times. I will recheck tomorrow.

Just playing with position that seem correct.
message SET_POSITION_TARGET_LOCAL_NED 0 1 0 9 3520 0 0.8 0 0 0 0 0 0 0 0 0

But yeah when using that message the copter rotates into the y-direction and then moves. I would think it should just move in the y-direction. I wonder if this is because of the auto-yaw state

[avncalst]

In my application I want the copter to move along the y-axis without any rotation. Thx for looking into the problem

[avncalst]

weird: if I use the mask 2503 the copter moves along the y-axis without rotation. message SET_POSITION_TARGET_LOCAL_NED 0 1 0 9 2503 0 0 0 0 -0.8 0 0 0 0 0 0 or: def slide_velocity(velocity_x, velocity_y, velocity_z): msg = vehicle.message_factory.set_position_target_local_ned_encode( 0, # time_boot_ms (not used) 0, 0, # target system, target component mavutil.mavlink.MAV_FRAME_BODY_OFFSET_NED, # frame 0b100111000111, # type_mask (only speeds enabled), x:LSB & yaw_rate: MSB 0, 0, 0, # x, y, z positions (not used) velocity_x, velocity_y, velocity_z, # x, y, z velocity in m/s 0, 0, 0, # x, y, z acceleration (not supported yet, ignored in GCS_Mavlink) 0, 0) # yaw, yaw_rate (not supported yet, ignored in GCS_Mavlink)

return msg

[avncalst]

@hendjoshsr71 , @amilcarlucas
I have debugged the SITL-copter code using GDB and discovered that the masks POSITION_TARGET_TYPEMASK_YAW_IGNORE and POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE have wrong values (1024, 2048). This yields an incorrect _yawignore=TRUE flag. Replacing the values by 512 (=POSITION_TARGET_TYPEMASK_FORCE_SET) and 1024 yields the correct _yawignore=FALSE flag. This results in a correct sliding of the copter (no rotation).

[avncalst]

@hfujikawa77 I solved my problem see my remark above.

[lthall]

So you are saying there is no problem in ArudCopter right?

[hendjoshsr71]

There is an issue. We shouldn't be rotating before moving when given only a velocity command.

@avncalst You are incorrect on the type_masks. The POSITION_TARGET_TYPEMASKs are taken directly from the generated mavlink headers. The correct values are given here, https://mavlink.io/en/messages/common.html#POSITION_TARGET_TYPEMASK And they are the same values as you state.

I think the reason why this solves your problems is that it is shifting the initial condition for auto_yaw control.

@lthall If you see this comment to reproduce in sitl. https://github.com/ArduPilot/ardupilot/issues/19462#issuecomment-990788261

When I was in the debugger it was that the autoyaw is set to LOOK_AT_NEXT_WP and turns to look at the new target before moving.

[rmackay9]

@hendjoshsr71,

Thanks for the clarification. So now I see what we're talking about and it is a change in behaviour from Copter-4.0. Basically in 4.1 if you give a command to fly to the right the vehicle will also turn to the right. So if you keep giving it "fly to the right" commands it will actually fly in a circle. In Copter-4.0 it would have maintained the current heading and just flown right.

So to get the same behaviour as Copter-4.0 you need to specify the velocity and also a yaw-rate of 0.

[hendjoshsr71]

Yes, you are correct. (Assuming these are velocity to the right commands in the BODY_NED or BODY_OFFSET_FRAME)

[rmackay9]

Ok, so @lthall and I actually discussed this change in behaviour as part of Copter's SCurve project and Leonard's opinion was that without the automatic yaw, companion computers (and lua scripts) would never be able to achieve the nice coordinated turns that we see in Auto mode. He also argued that the way it is now users can simply provide a zero yaw and get the old behaviour. So what we have in 4.1 allows either behaviour.. if we change it back to how it was in 4.0 then the users won't be able to achieved coordinated turns.

.. so I generally agree the new behaviour is unexpected but maybe we should leave it as-is and try and resolve the problem through documentation. I have coincidentally been improving our MAVlink Interface wiki pages recently.

[lthall]

I agree.

[lthall]

Now I think about this more. I think we should explicitly declare that if yaw not commanded then we use automatic yaw as defined by WP_Nav.

That would let us do what ever we what with yaw then when we are finished move back to an automatic coordinated turn system for yaw.

So we would need to add another case for if (!use_yaw && !relative_angle) here: https://github.com/ArduPilot/ardupilot/blob/e41cad8bd7d21102e59b89c0ded766d60a736d7f/ArduCopter/mode_guided.cpp#L1016

[avncalst]

@hendjoshsr71 @lthall @rmackay9 If one calculates _bool yawignore with type_mask =3527 and POSITION_TARGET_TYPEMASK_YAW_IGNORE=1024 in line 1159 of GCS_Mavlink.cpp, one obtains _yawignore=TRUE. This leads in _ModeGuided::setvelaccel _useyaw=false and _relativeyaw=false, which does not result in a fixed yaw. I want a similar behavior to version 4.0.7, because I am sending multiple velocity commands in my python avoidance code. So I came up with 2 solutions: change the type_mask to 2503 yielding _velignore and _yawignore FALSE. The other solution is changing the POSITION_TARGET_TYPEMASK_YAW_IGNORE to the non-official value 512 in common.h. Both solutions give a behavior similar to version 4.0.7

[avncalst]

@hendjoshsr71 @lthall @rmackay9 Normally using Dronekit, I was not aware that you do not use bit 10, which explains why my type mask 2503 works. Your clarification on the WiKi pages helped a lot. Thx.

[hendjoshsr71]

I think this one should still be open. You have found an issue IMO and though there are work arounds they are not ideal. Nor do I think they achieve the strict compliance with the specification that we want to achieve?

Sending a target_velocity command, alone, in body frame should not result in a yaw change.

Thoughts @rmackay9 & @lthall?

[lthall]

At this stage I think if yaw is ignored then yaw behaviour has not been defined by the command and we are free to handle yaw how we see fit.

As I said above I would like to extend this idea to change back to yaw behaviour defined by WP_YAW_BEHAVIOR in the event that the guided command does not specify the yaw behaviour. I think that is the only thing that makes sense and is 100% consistent while providing full functionality and flexibility.

So currently the code behaves as intended and this issue does not describe a bug or feature request. It is more a support thread where the problem is in the message used, not the code.

[lthall]

I have put what I suggested together here:

https://github.com/ArduPilot/ardupilot/pull/19592

[hendjoshsr71]

TLDR I think there is documentation work to be done on these messages.

I think a user who reads the MAVLINK message description could just as reasonably come away with the impression the yaw should not change.

If we want to keep this behavior I think we need to bring it to MAVLINK upstream for discussion and add to the message description? By adding something along the lines, "an ignored parameter means the system is free to change the parameter is order to achieve the commanded targets."

---

I'm still somewhat on the fence about the assumption that when an input is ignored it is left for the system to decide. For some things that intuitively makes sense: for accelerations when commanded only via velocity. But for yaw it is not kinematically clear why it necessary and reasonable to think the system will NOT hold the yaw.

However, there is a parameter a user can you use to change this behavior: from the default LOOK_AT_NEXT_WP to AUTO_YAW_HOLD. Or as you suggest the user can change the type mask from 3527 (velocity only) to 2503 (velocity & yaw only) and input a yaw of 0 degrees.