Position not getting locked in PX4 when using vision pose in mavros

[mmsarode]

*I am trying to use motion capture data by qualysis system for position estimate of the PX4 in hexacopter.

-Run mavros -Push motion capture data to mavros/vision_pose/pose using a publisher (there is a message saying "VISION estimate valid" when I run the publisher. There is no solid green color (expected in case of position lock) on PX4 main LED. When I echo to vision_pose/pose and local_position/local in mavros, I get the correct motion capture data (x, y, z) which means that it should lock position. Am I missing something?

I am using the PX4 firmware from this location-" https://github.com/PX4/Firmware.git". It seems position_valid is not getting flagged.

@mhkabir Can you point out what should I do to sort this?

[mhkabir]

I will try to debug this tomorrow, but meanwhile, have a look here : https://github.com/PX4/Firmware/tree/master/src/modules/position_estimator_inav

Also, are your inav weights for vision set properly?

Some printf debugging in inav should solve this. Start with these : https://github.com/PX4/Firmware/blob/master/src/modules/position_estimator_inav/position_estimator_inav_main.c#L913

[TSC21]

@mmsarode one thing you can do is directly use the mocap plugin of mavros. inav already supports mocap data fusion (through att_pos_mocap topic). In a first look into the current code, I can't seem to find any problem. Are you using current master?

[TSC21]

Also, are you getting any timeouts?

[mmsarode]

@TSC21 I am using the master downloaded on June 10. I do not get any timeouts. Do we need to start position_estimator_ianv manually or is it used by default?

[TSC21]

Well that question may be the answer. Take a look at: https://github.com/PX4/Firmware/blob/master/ROMFS/px4fmu_common/init.d/rc.mc_apps#L7-L27. But well, since you have INAV debugging msgs, that means inav has started.

[mmsarode]

@mhkabir About vision weights, I have changed these weights (rest have default values) INAV_W_XY_GPS_P 0 INAV_W_XY_GPS_V 0 INAV_W_XY_VIS_P 8 INAV_W_XY_VIS_V 2 INAV_W_XY_GPS_Z 0

[mmsarode]

@TSC21 I looked up in parameters and INAV_ENABLED is set to 1 (inav is started).

[mmsarode]

@TSC21 Correction in the previous post. I get timeouts. Till now I waited for the GPS: No fix messages and then started publishing to vision_pose/pose after which I did not see any timeout messages. I just tried starting both mavros and publisher at the same time and I see timeout messages. There is also a message saying your FCU don't support to AUTOPILOT_VERSION, swtiched to default capabilities. Here is a screenshot -

Previous case screenshot when I started publisher after "GPS: no fix" messages can be found here - https://github.com/mavlink/mavros/issues/349

[mhkabir]

@mmsarode OK, couple of things :

1. It is highly likely that your node might not be sending proper timestamps with the messages. Please re-check.
2. As I've been repeatedly saying, the "GPS: no fix" message doesn't affect anything, it's simply mavros telling you that no GPS data is coming through.
3. Please sync to latest master for Firmware and mavros both. We're moving pretty fast with development, and June 10 is pretty old.
4. The WP / PR timeouts generally indicates bad wiring. Please re-check thoroughly. The VER timeouts indicate you're using old Firmware.
5. There was some target component ID changes, recently, so make sure you're using latest mavros + latest Firmware + roslaunch px4.launch

[mmsarode]

@mhkabir Thank you Kabir for the elaborate comments. I will sync master for Firmware and mavros and check the wiring.

• About the timestamps, can you tell me the correct way to send it with the messages? I did not understand how to send it to the pose topic. Right now, I am using "ros::Time::now();".
• I am using a Laptop as companion computer to pixhawk. I will later switch to Beaglebone black board. Both of these have ubuntu 14.04 with mavros installed. Does this have anything to do with timestamps?
• Earlier, I was getting "Clock Skew error" so I disabled the sync by changing timesync_rate to 0.0 in px4_config.yaml file in mavros. Is this correct?

[mhkabir]

You might not have properly disabled timesync. Please restore the setting. The clock skew warning should just appear once in the beginning. How are you connecting to the Pixhawk? The companion link?

[mmsarode]

I am using the USB port (ttyACM0) to connect to Pixhawk.

[mmsarode]

@mhkabir I updated the mavros and mavros extras package (using the default timesync). There are no VER or WP/PR timeouts now. -I also wrote a publisher for sending setpoints to setpoint_position/local topic. Do we need to send any other setpoint data? I was able to switch to offboard mode (using a switch on RC). I gave continuous stream of setpoints with z value of about 1.5 meters. However, activating the offboard mode (with propellers connected) did not generate thrust to lift the robot from ground. I can navigate the robot fine in Manual mode. Is there anything I need to do? -I can see the vision position published in local_position/local topic in mavros which means it takes vision estimate for position but still I can not get solid green LED on PX4.

[mhkabir]

@mmsarode Can you switch to POSCTL mode while feeding the visual data?

[mmsarode]

@mhkabir When I try to switch to POSCTL mode it toggles between POSCTL and AUTO mode. ( Says POSCTL mode on and immediately says reject AUTO mission). I think I need to calibrate RC switch for this. Without vision estimates, I can go to only Manual, ALTCTL and AUTO mode ( which gets rejected). On Jul 28, 2015 7:58 AM, "Kabir Mohammed" notifications@github.com wrote:

@mmsarode https://github.com/mmsarode Can you switch to POSCTL mode while feeding the visual data?

— Reply to this email directly or view it on GitHub https://github.com/PX4/Firmware/issues/2606#issuecomment-125574273.

[mmsarode]

@TSC21 I tried using mocap plugin. I get Mocap Estimate valid message but I do not solid green LED on PX4. Is there any way to figure out if position lock is set? Can you tell me how to update mavros master? Earlier I used "sudo apt-get install ros-indigo-mavros command" in the ubuntu terminal. However, this is not the master version of mavros I guess.

[mhkabir]

@mmsarode If you can switch to POSCTL after calibration and mode setup on RC, you're good to go :)

Refer to mavros README if you want to install from source (i.e master)

[mmsarode]

@mhkabir Thanks Kabir. I updated mavros, mavlink and PX4 firmware from source. After setup on RC, I can not switch to POSCTL mode using a dedicated POSCTL RC switch. When I use mode switch on RC, it goes from Manual to ALTCTL then to POSCTL and after sometime (about 5 seconds) without changing the switch level, it says Reject Auto Mission. I still get the message VISION Estimate Valid. Let me know what I can do to get position lock.

[mhkabir]

@mmsarode Your switch mapping is still incorrect. Please use latest QGC and follow this page : https://pixhawk.org/users/system_modes

[mmsarode]

@mhkabir I will re-do the switch mapping. Do I need to tune PID controllers before setting the hexacopter in offboard/POSCTL mode?

[mhkabir]

@mmsarode Any updates? Issue fixed?

[mmsarode]

@mhkabir -I am able to switch it to offboard mode. However, with motion capture data, the hexacopter does not take off. It tries to drag on floor in specific direction. Without any positioning source, offboard mode make it take off. Do I need to tune PID controller?
-I should send co-ordinate to mocap/pose topic in ENU world frame, right? I am confused about the co-ordinate frames. Right now, I am sending the x, y, z position as it is received from motion capture system without any change (its default co-ordinate system). For roll, pitch and yaw, does pixhawk rely on its onboard sensors? Special switch for position control does not work, however I think switching to offboard mode when sending the motion capture data means that hexacopter is getting position feedback.

[Jaeyoung-Lim]

Is the issue fixed?

@mmsarode What setpoints are you sending for the hexacopter?

[mhkabir]

This was never an issue if you follow the appropriate steps. I recently tested offboard and it works just fine.

[Jaeyoung-Lim]

@mhkabir I believe you are right, I guess I did something wrong but cannot find out what...

I am sending the /mavros/setpoint_position/local using rqt message publisher, could that be an issue?

[mhkabir]

The publishing may be timing out. Also, check the connection using rostopic. This reminds me that we should make rejection more verbose. I'll have a look in any case.

[Jaeyoung-Lim]

@mhkabir Thank you for you help!

I don't know if this is a dull question but how do you solve the timing out problem?

Do I need to make another publisher node to publish setpoints?

[mhkabir]

That would be good, yes. Start here : https://pixhawk.org/dev/offboard_control

[Jaeyoung-Lim]

@mhkabir Thanks, I will try using a publisher node!

Though I think maybe giving basic instructions in how to 'stream setpoints' in the wiki will give a lot of help helping out people like me who are not familiar with ROS!

[mhkabir]

Well, its there if you actually click the links to the next pages : https://pixhawk.org/dev/offboard_control/testing

[mmsarode]

@gokoreas An example code for streaming setpoints using MAVROS which might be helpful for you: https://github.com/mmsarode/ROS-Hexacopter/blob/master/Github/ROS-Hexacopter/src/beginner_tutorials/src/data_to_setpoint_4.cpp I am away from the university lab from last 2 months and will update soon as soon as I get a chance. I think I did not account for the frame conversions while sending position feedback from motion capture to PX4., but the setpoints streaming and offboard mode works fine.

[rohankishore21]

Hi Guys, I have a question, What Iam currently planning to do is:

1. To use the Offboard mode indoors, and send local set points (X,Y,Z) at a particular frequency
2. Then parallelly send the vision pose local (X,Y,Z) from an indoor positioning system which sends x, y and z which is the current position of the drone through vision pose local .

So basically with this , say for ex. if I give local setpoints (0,1,2) and if I send the vision pose data from the indoor positioning then the drone should go to position (0,1,2) and hover right?

If anyone can help me will be great :)

[LorenzMeier]

Stale, closing

[Seanmatthews]

This still seems to be an issue. I see the exact same problems. Here's a video of running the mavros offboard example with PX4 master. Here's a video of running the same using global_to_local branch. Using a Snapdragon Flight with VIO only.

[m1577]

Hello @Seanmatthews maybe we are have similar issue, my laboratory have six optitrack cameras, I am not using companion computer, I have a ubuntu pc conect to pixhawk with usb. Today I did my first test sending mocap data to pixhawk and I got a similar reaction to your first video, I am using the offboard example and I want to my 3dr iris to get an altitude of 0.5m.

Did you find the solution to your problem?

I attach some images as my rqt_graph and my terminals linux.

http://imgur.com/yqJkvAy http://imgur.com/qApjOUy http://imgur.com/8QRTvOj http://imgur.com/u5tLp0X http://imgur.com/4RCYZlk

Regards!