Copter moving in circle in POSCTL mode (external position estimate)

Hello Everyone,

Problem: My copter is moving in small circles which gets bigger over time when flying in POSCTL mode (position control mode)

I am using the following hardware:

  • IRIS+ drone with PX4 v1.9.2 (I changed the flight controller Pixhawk 2.1)
  • One Marvelmind mobile beacon
  • Odroid
  • A down facing camera (detects the tag and corrects the orientation of the pose send to the pixhawk by marvelmind)
  • Teraranger (for height estimation)

Log
https://review.px4.io/plot_app?log=5155edca-516d-4a87-be9a-9b6bfff66fef

Data Flow

  1. Marvelmind is connected to the odroid via USB. It gives the position of the copter in ENU (after correcting the yaw with tag, so there is no drift) and is publishing a message of type ‘geometry_msgs/PoseStamped.msg’ on the topic ‘vision_pose/pose’.
  2. Odroid is connected to the pixhawk over MAVROS, and I am able to visualize the data in mavlink analyser.
  3. I am also able to see the data on ‘local_position/pose’.

Parameters

  • EKF2_AID_MASK = 24 (Fuse vision position and vision yaw)
  • EKF2_HGT_MODE = ranger (using teraranger)
  • EKF2_EV_DELAY = 50ms

NOTE:
The copter was aligned along the x axis of the world (Marvelmind frame).

My Observations

  1. On ROS,
    • When I moved the copter forward along the x axis of the world frame, the x value increased. (Both vision_pose/pose and local_position/pose where checked).
    • When I moved the copter left along the y axis of the world frame, the y value increased. (Both vision_pose/pose and local_position/pose where checked).
    • When I moved the copter up along the z axis of the world frame, the z value increased. (Both vision_pose/pose and local_position/pose where checked).
    • When I rotated the copter counter-clockwise around the z axis of the world frame, the yaw value increased. (Both vision_pose/pose and local_position/pose where checked).

Hope this says, my readings from marvelmind are in ENU. So does the reading on local_position/pose.

(Image to show xy of vision/pose and local_position/pose is same )

  1. On Mavlink analyser,
    • When I moved the copter forward along the x axis of the world frame, the y value increased on the local_position_ned on mavlink analyser.
    • When I moved the copter left along the y axis of the world frame, the x value increased on the local_position_ned on mavlink analyser.
    • When I moved the copter up along the z axis of the world frame, the z value decreased on the local_position ned.
    • When I rotated the copter clockwise around the z axis of the world frame, the yaw value increased.

So it looks like x and y are exchanged. I was expecting the x to increase when the I moved the copter forward along the x axis of the world frame and y to decrease when I moved the copter left along the y axis of the world frame as in NED frame or FRD. Now it looks more like LFD. Please correct me if I’m wrong.

However, following https://github.com/mavlink/mavros/issues/523, user @andrea-nisti suggested here Vision Estimates and/or setpoints not rotated properly (forwarded quaternions are wrong) · Issue #523 · mavlink/mavros · GitHub, quoting…

What I do is the following, allign the robot X with X ENU and create the rigid body then start streaming its position. You will see a PI/2 yaw in the mavlink vision topic from px4 but don’t worry, it is due to the rotations. X and Y axis will be swapped in NED and Z will be negative as it should be, just place your ENU frame as convenient for you and it is fine.

And it seems things are working fine for him.

  1. On Mavlink analyser, there is an offset of 90degree in the yaw (in the image below, the copter is now aligned zero to x axis of world frame).

Hope someone can help.

Ashwin Sushil

1 Like

Hello Everyone, just want to tell all of you that I have posted the same issue on slack.

Here is a link for it:
https://px4.slack.com/archives/C0W2KUFFT/p1565704050120200

Please feel free to join the conversation there.

Regards,
Ashwin Sushil