Copter: add support for VISION_POSITION_ESTIMATE message

[Ahrovan]

I read diydrones And #4498 (& #103,#102) for Indoor navigation with ArduCopter and see this:
three possible solutions:

1. implement a angular controller in Guided mode to allow the companion computer to more directly control the vehicle's lean angles. The companion computer would be responsible for estimating the vehicle's velocity and position and converting the (velocity and position) errors into angle requests.

2. feed an estimated position into the EKF as a substitute for a GPS position. The EKF could then work as i does not calculating position, velocity, acceleration by combining this data with the other sensors. There's a GLOBAL_VISION_POSITION_ESTIMATE message but we haven't implemented the Copter side of it (which could be complex).

3. attach an optical flow sensor to the vehicle. The EKF already knows how to use this sensor to estimate position and velocity. The companion computer could then control the vehicle with either the velocity or position controllers.

We how to start work on it for support VISION_POSITION_ESTIMATE just like px4 pro flight stack ?

[MasterRos]

hmm. good job. we wait for developers for help about and mention tips for start this work
GLOBAL_VISION_POSITION_ESTIMATE
VISION_POSITION_ESTIMATE
and ..
`struct vision_position_estimate {
unsigned id; /**< ID of the estimator, commonly the component ID of the incoming message */

uint64_t timestamp_boot;		/**< time of this estimate, in microseconds since system start */
uint64_t timestamp_computer;		/**< timestamp provided by the companion computer, in us */

float x;				/**< X position in meters in NED earth-fixed frame */
float y;				/**< Y position in meters in NED earth-fixed frame */
float z;				/**< Z position in meters in NED earth-fixed frame (negative altitude) */

float vx;				/**< X velocity in meters per second in NED earth-fixed frame */
float vy;				/**< Y velocity in meters per second in NED earth-fixed frame */
float vz;				/**< Z velocity in meters per second in NED earth-fixed frame */

float q[4];				/**< Estimated attitude as quaternion */

};`
and vision_position_estimate

[rmackay9]

We've been talking about this a bit in the ardupilot vision-projects gitter channel recently. https://gitter.im/ArduPilot/ardupilot/VisionProjects. On the ArduPilot side we need to add some support in the EKF to accept those messages and that's not terribly hard. Do you have a system to produce those messages? Can you tell me a bit about it?

[Ahrovan]

For vision, the mavlink message used to send the pose data is VISION_POSITION_ESTIMATE and the message for all motion capture systems is ATT_POS_MOCAP messages.
[1-Ros ref] vision_pose_estimate
[2-Mavlink ref] VISION_POSITION_ESTIMATE
[3-Mavlink ref] ATT_POS_MOCAP
[4-PX4 ref] vision
[5-gitter ref] VisionProjects

I check this msg's for do work and replay soon. but how to use this msg's with arducopter please send me some Ref ..

[arjunbj]

@rmackay9 Any updates on this? Expected time when this will be available?
Also, is there any way to insert heading ( from external source like slam) currently in ArduCopter?

[HumamHelfawi]

Hi, I was wondering what benefit will VISION_POSITION_ESTIMATE bring since Arducopter already can process VISION_POSITION_DELTA ?
Thank you very much!

[rmackay9]

@HumamHelfawi, the vision-position-delta provides the change in position while the vision-position-estimate provides the absolute position. Having the absolute position is better in cases where there is a position reset provided by the external system.

[HumamHelfawi]

@rmackay9 Thank you very much for clarification! I am sorry but it is still a bit fuzzy for me.

where there is a position reset provided by the external system.

Do you mean something like resetting the position when a loop closure optimization happens or did I misunderstood your sentence?

[rmackay9]

@HumamHelfawi, I'm not exactly sure what "loop closure optimization" means but to give a practical example, say a vehicle was driving around in a maze and after 30min the vehicle arrived back at the entrance. At that moment, the external position estimate system might recognise the entrance and suddenly shift the position estimate to the position estimate it had when it entered the maze. Passing this shift into ardupilot would only be possible using the vision-position-estimate message. The vision-delta message wouldn't work well because arduilot's EKF would likely reject the massive shift passed in from the external position estimate system because it doesn't match the accelerometers.

[HumamHelfawi]

@rmackay9 I thank you very much! Your example illustrates exactly what I meant by "loop closure optimization" (when a loop is detected at the entrance of the maze).
I am really interested in VISION_POSITION_ESTIMATE getting implemented since no real SLAM system could work without it. I will see if I can help with my very basic experience in this field.

[SubMishMar]

@rmackay9 I really want to know how to make the vision_position_delta work?? I am willing to do the extra work of working out rotations and transformation to send the dp and dR msgs but I want to make this work. I am using mavros and made some modifications in the mavros/mavros_extras/src/plugins/vision_pose_estimate.cpp . I changed mavlink::common::msg::VISION_POSITION_ESTIMATE vp to mavlink::ardupilotmega::msg::VISION_POSITION_DELTA vp{} in the aforementioned code and tried sending some random variables to vp just to test the input output rostopics. I want to know in which rostopic can the output be seen. For example if one uses LPE in PX4 firmware, we send data via a topic named /mavros/vision_pose/pose and get an output at /mavros/local_position/pose. Here I am sending the vision pose via /mavros/vision_pose/pose but I don't know where I will get the output. Sure, I should not expect an output at /mavros/local_position/pose because APM has got no LPE. Any help will be gratefully acknowledged!

[rmackay9]

The various vision-position mavlink messages are now supported in master.
One note of caution is the EKF2 must be used. The EKF2 is the default EKF. support within the EKF3 will be coming soon.
Any and all feedback would be very welcome!

[SubMishMar]

@rmackay9 So we dont have to convert to deltas?

[rmackay9]

@SubMishMar, that's right. No need to convert to deltas anymore.

By the way, the EKF still needs to be told where it's origin is if no GPS is connected. That can be done using the mission planner as described here on the wiki or if you have some other way of sending in mavlink messages, it's the SET_GPS_GLOBAL_ORIGIN message that should be sent. There are more people doing this kind of thing with ROS on ArduPilot Rovers and boat here.

[HumamHelfawi]

@rmackay9 Thank you very much! and for this great community!

[SubMishMar]

@rmackay9 will this work for skyviper? I tried writing the latest ardupilot firmare(available on ardupilot's github, not skyviper's ) on skyviper, it doesn't respond after that. It doesnt respond when I burn the new firmware by configuring the board to px4-v3 and when I configure the board as skyviper-v2450, the firmware is uploaded successfully but the controller doesn't pair up with the drone and lights on both the drone and the controller keep beeping forever.

@rmackay9 Here is some more information
EDIT1: So, now the controller and drone are paired but the controller is making strange sounds and doesn't arm when i move the throttle up, I also add a screenshot from my mission planner, why does it say "BAD VISION POSITION"? Here is the video to show you what's happening, https://drive.google.com/open?id=1PJfybU0a9JWjipEi9C4E39Nmls8pDJRh

Please copy and paste that link in your browser, it may not work if you just click on it.

EDIT2: I disabled prearm checks and it is able to fly but the strange beeps from the controller still persist.

[rmackay9]

@SubMishMar,

I guess you're using the pre-built firmware from ardupilot.org? That would be the best firmware to use to be sure it's not a compile issue. Instructions are here on the wiki.

I think it should be possible to use the vision-position-estimate messages with the skyviper firmware. I guess you've got some external vision system setup which knows where the vehicle is?

[SubMishMar]

@rmackay9 I think you have given the wrong link here.

No, I am not using a prebuilt firmware. I pulled the master branch of ardupilot and build the code by configuring it for skyviper-2450. I did not get any compilation issue. I have aruco markers that provide the vehicle pose in the topic /mavros/vision_pose/pose

[rmackay9]

@SubMishMar, sorry, updated the link above to this one.

[SubMishMar]

Thanks, let me check it out. I was looking into this http://firmware.ardupilot.org/Copter/2018-05/2018-05-07-22:05/skyviper-v2450/

I think they are the same.

[SubMishMar]

@rmackay9 there is no change in behavior on uploading the firmware you pointed me to, the controller is still beeping in a strange way. I still have to check the stuff with vision_position_estimate. Is there any documentation on how to do it?

Note: The skyviper I own is a old one, bought in October 2017 does that have anything to do with the type of firmware completely compatible with the board on skyviper?

[rmackay9]

@SubMishMar, if you have a dataflash log we can look for the VISP message to see if the external vision position estimate messages are getting through or not. If they don't appear then there is likely a communication issue between the external vision system being used (and I don't know what you're using of course). If the messages do appear then the issue will likely be the times being off between the system sending the external position estimates and the flight controller.

[SubMishMar]

Okay, I will get you the data flash log. I am using aruco markers to estimate the drone's pose and this is published in /mavros/vision_pose/pose. I have vision and local white listed in my apm_pluginlist. I have also added the following lines in vision_position_estimate.cpp
mavlink::common::msg::SET_GPS_GLOBAL_ORIGIN gpo;
//mavlink::common::msg::SET_HOME_POSITION gpo;
gpo.target_system = m_uas->get_tgt_system();
// gpo.time_boot_ms = stamp.toNSec() / 1000; #TODO: requires Mavlink msg update
gpo.latitude = 30.6197674;
gpo.longitude = -96.3414215 ;
gpo.altitude = 99.9674829152;
UAS_FCU(m_uas)->send_message_ignore_drop(gpo);
ROS_INFO("[vpe:]SENT_GPS_GLOBAL_ORIGIN!");

[SubMishMar]

No logs are being recorded even if I have an SD card onboard my skyviper. It's an 8 GB SD card.

[rmackay9]

@SubMishMar, the issue is probably the LOG_DISARMED parameter needs to be set to "1" to enable logging even while the vehicle is disarmed.
Txs for the info on the setup. I strongly suspect it's a time-sync issue between the companion computer and the flight controller. To resolve this, we need this PR from @peterbarker to make it into master.

[SubMishMar]

@rmackay9 here I upload a dataflash log file and it does show relevant values in the field VISP. I think the frames are different(may be a NED ENU stuff) as what is +Z has been logged as -Z in VISP and X and Y are swapped.

Here is the .bin file https://drive.google.com/open?id=1-xSTD3CtJSHw2KvgdzxUirFSXsKaUSrD

[rmackay9]

@SubMishMar, so as expected the issue is that the times are mismatched between the flight controller and the source of the vision position messages. We really need the PR from @peterbarker linked above to go into properly resolve this.

[SubMishMar]

@rmackay9 thanks for the help! Any idea when this will be available in master?