REJECT POSITION CONTROL, EKF2 STATUS Local Position: Invalid

[gyronikeleg]

I created my own motion capture system, am I able to position the drone and send posa data via Matlab ROS integration and get the correct output when I echo the vision_pose topic in MAVROS from the Onboard Computer.

When I switch to Position Control on my Taranis I get a "Reject Position Control" result from QGC

Mavlink Console also says the following for EKF2 Status:

Here is my flight log: https://logs.px4.io/plot_app?log=c33ff996-c4ae-4558-b844-6205f4c976e8

I am only able to send position data at ~5 hz, is this the cause of the problem?

[julianoes]

@kamilritz do you see what is going on?

[kamilritz]

@gyronikeleg As far as I know, the vision_pose topic is only consumed by the Local Position Estimator (LPE) and not by the EKF2 module. So I would argue that your pose information never reached the estimator and therefore you had no local position.

Is your motion capture system publishing a 6DOF pose? I never worked with Matlab ROS integration. Are you using MAVROS to communicate with the vehicle?

[gyronikeleg]

@gyronikeleg As far as I know, the vision_pose topic is only consumed by the Local Position Estimator (LPE) and not by the EKF2 module. So I would argue that your pose information never reached the estimator and therefore you had no local position.

Is there a way I can confirm this?

Is your motion capture system publishing a 6DOF pose? I never worked with Matlab ROS integration. Are you using MAVROS to communicate with the vehicle?

Yes, I'm using MAVROS and publishing the 6DOF pose from MATLAB. When I echo the vision pose topic in MAVROS, I get the correct pose information.

[kamilritz]

@gyronikeleg I would use the ODOMETRY plugin of MAVROS. So publishing on the /mavros/odometry/out topic. Is your MOCAP system also computing velocities? If yes, this can help the drone to achieve higher flight performance. If no, you can set the velocity components of the nav_msgs/Odometry probably to NAN or zero and tell the FCU not to use it. A way to check if the odometry is received then on the Firmware side is to temporally set MAV_ODOM_LP=1 and MAV_0_MODE to "external vision min". After setting this, you can check the incoming and from MAVROS transformed MOCAP data under the ODOMETRY topic with the MAVLink inspector of QGroundcontrol. On the Firmware side you should set the EV_POS, EV_YAW (and EV_VEL) bits of the EKF2_AID_MASK parameter to true and EKF2_HGT_MODE to vision in order to fuse the information in the EKF.

[gyronikeleg]

@kamilritz

@gyronikeleg I would use the ODOMETRY plugin of MAVROS. So publishing on the /mavros/odometry/out topic.

Done. in matlab I published as follows:

rosinit('192.168.1.14');
robotVision = rospublisher('/mavros/odometry/out','geometry_msgs/PoseStamped');
posemsg = rosmessage(robotVision);

tic
mRate = 50;
while true
    elapsedTime = toc;
    if(elapsedTime>=(1/mRate))
        posemsg.Pose.Position.X = 0;
        posemsg.Pose.Position.Y = 0;
        posemsg.Pose.Position.Z = 0;
        posemsg.Pose.Orientation.X = 0;
        posemsg.Pose.Orientation.Y = 0;
        posemsg.Pose.Orientation.Z = 0;
        posemsg.Pose.Orientation.W = 1;
        posemsg.Header.Stamp = rostime("now");
        posemsg.Header.FrameId = 'World';
        send(robotVision,posemsg);
        tic
    end
end

Is your MOCAP system also computing velocities? If yes, this can help the drone to achieve higher flight performance. If no, you can set the velocity components of the nav_msgs/Odometry probably to NAN or zero and tell the FCU not to use it.

My MOCAP systems does not compute velocities. How do I set the velocities to zero? (sorry a bit new to this)

A way to check if the odometry is received then on the Firmware side is to temporally set MAV_ODOM_LP=1 and MAV_0_MODE to "external vision min". After setting this, you can check the incoming and from MAVROS transformed MOCAP data under the ODOMETRY topic with the MAVLink inspector of QGroundcontrol.

I attached a picture of the mavlink inspector. The numbers in mavlink inspector don't match what I am publishing.

I am publishing exact numbers: x=0,y=0,z=0 x=0,y=0,z=0,w=1 ^these are shown as exact in Mavros

On the Firmware side you should set the EV_POS, EV_YAW (and EV_VEL) bits of the EKF2_AID_MASK parameter to true and EKF2_HGT_MODE to vision in order to fuse the information in the EKF.

Done, I set EKF2_AID_MASK to 24 in QGC

Thanks very much for your help, by the way!

I'm still getting the reject position error, do you know of something else I can try?

[kamilritz]

Done. in matlab I published as follows:

You are not sending the the pose information to MAVROS correctly. MAVROS expects a message of type nav_msgs/Odometry. You can check if it is received by listening to the /mavros/odometry/out topic. If you are not that familiar with ROS, this should help you with checking that you are sending your data on the right topic.

How do you launch MAVROS? And are you sure the connection to the FCU is detected?

[gyronikeleg]

I updated the odometry to use the correct message type:

rosshutdown
rosinit('192.168.1.14');
robotVision = rospublisher('/mavros/odometry/out','nav_msgs/Odometry');
posemsg = rosmessage(robotVision);

tic
mRate = 50;
while true
    elapsedTime = toc;
    if(elapsedTime>=(1/mRate))
        posemsg.Pose.Pose.Position.X = 0;
        posemsg.Pose.Pose.Position.Y = 0;
        posemsg.Pose.Pose.Position.Z = 0;
        posemsg.Pose.Pose.Orientation.X = 0;
        posemsg.Pose.Pose.Orientation.Y = 0;
        posemsg.Pose.Pose.Orientation.Z = 0;

        posemsg.Header.Stamp = rostime("now");
        posemsg.Header.FrameId = 'odom';
        send(robotVision,posemsg);
        tic
    end
end

I tried to do the rqt_graph but I get this error. The plugin is installed, so not sure why I get the error

rqt_graph: cannot connect to X server

Here are the diagnostics output, I do get a heartbeat I launch mavros from putty terminal with this command: roslaunch mavros px4.launch fcu_url:=serial:///dev/ttyUSB0:921600 gcs_url:=udp://@192.168.1.2:14550

`header:
  seq: 407
  stamp:
    secs: 1574379486
    nsecs: 304382671
  frame_id: ''
status:
  -
    level: 0
    name: mavros: FCU connection
    message: connected
    hardware_id: serial:///dev/ttyUSB0:921600
    values:
      -
        key: Received packets:
        value: 45872
      -
        key: Dropped packets:
        value: 0
      -
        key: Buffer overruns:
        value: 0
      -
        key: Parse errors:
        value: 0
      -
        key: Rx sequence number:
        value: 125
      -
        key: Tx sequence number:
        value: 57
      -
        key: Rx total bytes:
        value: 7258161
      -
        key: Tx total bytes:
        value: 272492
      -
        key: Rx speed:
        value: 14560.000000
      -
        key: Tx speed:
        value: 517.000000
  -
    level: 0
    name: mavros: GCS bridge
    message: connected
    hardware_id: serial:///dev/ttyUSB0:921600
    values:
      -
        key: Received packets:
        value: 20
      -
        key: Dropped packets:
        value: 0
      -
        key: Buffer overruns:
        value: 0
      -
        key: Parse errors:
        value: 0
      -
        key: Rx sequence number:
        value: 19
      -
        key: Tx sequence number:
        value: 0
      -
        key: Rx total bytes:
        value: 24541
      -
        key: Tx total bytes:
        value: 7257875
      -
        key: Rx speed:
        value: 47.000000
      -
        key: Tx speed:
        value: 14526.000000
  -
    level: 2
    name: mavros: GPS
    message: No satellites
    hardware_id: serial:///dev/ttyUSB0:921600
    values:
      -
        key: Satellites visible
        value: 0
      -
        key: Fix type
        value: 0
      -
        key: EPH (m)
        value: 99.99
      -
        key: EPV (m)
        value: 99.99
  -
    level: 0
    name: mavros: Heartbeat
    message: Normal
    hardware_id: serial:///dev/ttyUSB0:921600
    values:
      -
        key: Heartbeats since startup
        value: 526
      -
        key: Frequency (Hz)
        value: 1.036986
      -
        key: Vehicle type
        value: Quadrotor
      -
        key: Autopilot type
        value: PX4
      -
        key: Mode
        value: ALTCTL
      -
        key: System status
        value: Standby
  -
    level: 0
    name: mavros: System
    message: Normal
    hardware_id: serial:///dev/ttyUSB0:921600
    values:
      -
        key: Sensor present
        value: 0x002F002F
      -
        key: Sensor enabled
        value: 0x0021000F
      -
        key: Sensor helth
        value: 0x002F000F
      -
        key: Sensor 3D Gyro
        value: Ok
      -
        key: Sensor 3D Accel
        value: Ok
      -
        key: Sensor 3D Mag
        value: Ok
      -
        key: Sensor Abs Pressure
        value: Ok
      -
        key: Sensor RC Receiver
        value: Ok
      -
        key: AHRS health
        value: Ok
      -
        key: CPU Load (%)
        value: 52.1
      -
        key: Drop rate (%)
        value: 0.0
      -
        key: Errors comm
        value: 0
      -
        key: Errors count #1
        value: 0
      -
        key: Errors count #2
        value: 0
      -
        key: Errors count #3
        value: 0
      -
        key: Errors count #4
        value: 0
  -
    level: 0
    name: mavros: Battery
    message: Normal
    hardware_id: serial:///dev/ttyUSB0:921600
    values:
      -
        key: Voltage
        value: 10.94
      -
        key: Current
        value: 0.3
      -
        key: Remaining
        value: 28.0
  -
    level: 0
    name: mavros: Time Sync
    message: Normal
    hardware_id: serial:///dev/ttyUSB0:921600
    values:
      -
        key: Timesyncs since startup
        value: 850
      -
        key: Frequency (Hz)
        value: 9.999511
      -
        key: Last dt (ms)
        value: 0.175010
      -
        key: Mean dt (ms)
        value: 0.001993
      -
        key: Last system time (s)
        value: 988.079634000
      -
        key: Time offset (s)
        value: 1574378498.134831667

[kamilritz]

Great, You have already the connection to the vehicle. If you look for the message definition of nav_msgs/odometry you can see that you also can define the child_frame_id and if you want also the twist (velocities). Are you using MAVROS 0.33 version? I would recommend to do so, but you also need to specify the transform between your parent frame and the parent frame of the fcu, and your child frame and the drones body frame. So you have to include something like this in your launch file:

  <!-- Launch static transform publishers -->
  <node pkg="tf" type="static_transform_publisher" name="tf_baseLink_cameraPose"
        args="0 0 0 0 1.5708 0 base_link camera_pose_frame 1000"/>

  <!-- Launch static transform publishers -->
  <node pkg="tf" type="static_transform_publisher" name="tf_odom_cameraOdom"
        args="0 0 0 0 0 0 odom camera_odom_frame 1000"/>

Of course you need to adapt roll, pitch, yaw to according to your choice of coordinate frames.

[gyronikeleg]

If you look for the message definition of nav_msgs/odometry you can see that you also can define the child_frame_id and if you want also the twist (velocities).

Done.

rosshutdown
rosinit('192.168.1.14');
robotVision = rospublisher('/mavros/odometry/out','nav_msgs/Odometry');
posemsg = rosmessage(robotVision);

tic
mRate = 10;
while true
    elapsedTime = toc;
    if(elapsedTime>=(1/mRate))
        posemsg.Pose.Pose.Position.X = 0;
        posemsg.Pose.Pose.Position.Y = 0;
        posemsg.Pose.Pose.Position.Z = 0;
        posemsg.Pose.Pose.Orientation.X = 0;
        posemsg.Pose.Pose.Orientation.Y = 0;
        posemsg.Pose.Pose.Orientation.Z = 0;

        posemsg.Twist.Twist.Linear.X = 0;
        posemsg.Twist.Twist.Linear.Y = 0;
        posemsg.Twist.Twist.Linear.Z = 0;

        posemsg.Twist.Twist.Angular.X = 0;
        posemsg.Twist.Twist.Angular.Y = 0;
        posemsg.Twist.Twist.Angular.Z = 0;

        posemsg.Header.Stamp = rostime("now");
        posemsg.Header.FrameId = 'odom';
        posemsg.ChildFrameId = 'base_link';

        send(robotVision,posemsg);
        tic
    end
end

Are you using MAVROS 0.33 version?

Is this necessary? I currently have 0.17.5 installed, not sure how to change to 0.33

So you have to include something like this in your launch file:

I added that section to the file

Mavlink Inspector -> Odometry is still not reflecing the correct values. Do you know why this would be?

[kamilritz]

Is this necessary? I currently have 0.17.5 installed, not sure how to change to 0.33

The odom plugin was changed with 0.33 to make it easier to use. In order to install do a checkout the version here. Meaning do a git checkout 247cde879c5a9a3d1a88e38ca2b77b868cb1b4ab. You also need to upgrade to a recent version of MAVLINK and Firmware. I suggest you try MAVLink master and this Firmware version.

I added that section to the file

Without changing anything? If yes that is probably wrong. Is the z axis of your reference frame pointing up? Then that

<!-- Launch static transform publishers -->
  <node pkg="tf" type="static_transform_publisher" name="tf_odom_cameraOdom"
        args="0 0 0 0 0 0 odom camera_odom_frame 1000"/>

is ok. How is the body frame of your mocap system attached to the drone?

Mavlink Inspector -> Odometry is still not reflecing the correct values. Do you know why this would be?

Have you changed MAV_ODOM_LP to 1?

[gyronikeleg]

I installed the 0.33.0, but now I get an error after launching mavros: ERROR: cannot launch node of type [mavros/mavros_node} can't locate node [mavros_node] in package mavros

I've sourced the setup.bash files: source /opt/ros/kinetic/setup.bash source ~/catkin_ws/devel/setup.bash

Been trying to figure it out all day, it is probably something simple, but I cannot resolve it. Do you know the cause of this error?

[kamilritz]

Hard to tell what is causing that. Sourcing devel/setup.bash should usually be enough.

[gyronikeleg]

ok, got it working. I am using: PX4 1.10.0beta Mavros 0.33.0 Mavlink Master

I've tried publishing to odometry/out and vision_pose/pose with messages from Matlab (specific to each topic). When I published to vision_pose/pose, I did see the mavlink inspector "Local_Position_NED" output the correct results (or close to them). Still get the reject position control error. Here are my parameters, mavros terminal output, echo of odometry/out terminal output:

params_ekf2_26NOV.txt mavros_output_26NOV.txt odometry_out_topic_output_26NOV.txt

Here is my log: https://logs.px4.io/plot_app?log=89606854-59ae-42f8-b9a7-637fc9eecb5d

I don't have any props on my motors and one of my motors is disconnected, but I believe that shouldn't be the issue. Not sure what else to try, do you know of a next step?

[kamilritz]

Great job, you seem to very close now. You seem to receive the odometry msg on the Firmware side. If you look into the log with a program such as flightplot, you can see that the topic visual_odometry is published. Can you try to send reasonable data to the FCU? Do you compute or can you estimate the covariances? Have you adopted the tf transforms according to your coordinate frame choices?

I don't have any props on my motors and one of my motors is disconnected, but I believe that shouldn't be the issue. Not sure what else to try, do you know of a next step?

Having the props removed or/and the ESCs disconnected is very good practice. That is definitely not causing your issue.

[gyronikeleg]

For the tf transforms, I have some confusion, here is my frames.pdf output, it shows 3 children that I don't know why they appear ("map_ned", "odom_ned", "base_link_frd"). Shouldn't these not exist and it should just be map-> odom->base_link? frames.pdf

Here is what I put in the px4.launch file for static publishers:

    <node pkg="tf" type="static_transform_publisher" name="tf_odom" args="0 0 0 0 0 0 map odom 100"/>

    <!-- Launch static transform publishers -->
    <node pkg="tf" type="static_transform_publisher" name="tf_baselink" args="0 0 0 0 0 0 odom base_link 100"/>

I do receive data in mavlink, however, the odometry and IMU quaternions are different (i am publishing (0, 0, 0, 1) to odometry topic, so I dont know why it says 0.7071, 0, 0, 0.7071:

Also, I still get the "[ INFO] [1574963525.804813497]: HP: requesting home position" warning and reject position control

[kamilritz]

This should help you with it: https://github.com/PX4/Devguide/pull/876/files I would advise to not publish a tf transform between odom and base_link*, this can cause issues with the tf tree.

[stale[bot]]

This issue has been automatically marked as stale because it has not had recent activity. Thank you for your contributions.

[bresch]

I'm closing as there are no further activities in this issue and that it isn't directly related to PX4. @gyronikeleg please post an update if the issue is solved.

[Pedro-Roque]

@robertosundin still has issues with this. I will let him detail his troubles here

[robertosundin]

@julianoes, @bresch, @kamilritz I am currently experiencing the same issues with mocap forwarding on the Pixhawk 1 and 2 (Cube) models. I use a Jetson TX2 as companion computer connected through USB and forward mocap odometry to /mavros/odometry/out topic with frames and child frame id:s as specified in the PX4 user guide (I have also tested using /mavros/vision_pose/pose with no luck).

The issue seems to be related to the Pixhawks specifically since mocap forwarding works when replacing these with a Pixracer.

Expected behaviour: The EKF2 status should switch from local postion invalid to local position valid when publishing mocap data to mavros/odometry/out

Actual behaviour: The EKF2 local position remains invalid when publishing mocap data to mavros/odometry/out

flight log: https://logs.px4.io/plot_app?log=14e49a0e-02b3-43c4-9843-c31e3a8265d5

I am using mavros version 1.4.0 and mavlink release/kinetic/mavlink/2020.9.10-1.

[bresch]

If there's still someone with a similar issue and a working setup, let's sync on Slack to fix this for good