Servers by jDrones

Injecting fake GPS into APM (from Motion Capture System)

(Quentin) #1

Hello,

I’m currently working on a project with an Erle-Copter to make it fly indoor with the help of a Motion Capture System (MCS). I am using the Erle-Brain 3 with the latest July “frambuesa” update (ArduCopter 3.5-rc9).
I already implemented a ROS node that retrieves the position estimation of the MCS and publishes it in a dedicated topic. The challenge I’m facing now is to inject this position estimation into the APM flight controller.

My idea was to fake a GPS signal, but I am still not familiar with every aspect of APM and I am unsure of how to proceed. I have made a lot of research on the subject, but I am getting more and more confused… Found topic are from 2015 or 2016, and apparently there have been some changes and new implementations in the code since then.

My main source of inspiration is the following topic:
http://forum.erlerobotics.com/t/gps-input-message-not-working/2050

Once my (X,Y,Z) position is transformed into adequate latitude and longitude, is it sufficient to publish it on “/mavros/fake_gps/fix” ?

I have also found interesting MAVLINK messages (http://mavlink.org/messages/common): HIL_GPS (#113) and GPS_INPUT (#232).
What is the difference between them and could they be of any help ?

Here are the implementations of “AP_GPS_MAV.cpp” and “fake_gps.cpp” of mavros:
https://github.com/ArduPilot/ardupilot/blob/c75f30eb9b6c1d331347e029652d4bde546218e7/libraries/AP_GPS/AP_GPS_MAV.cpp
https://github.com/mavlink/mavros/blob/9494c26adaebd4e1a69c12b12922f1a640f1c72a/mavros_extras/src/plugins/fake_gps.cpp

Could anyone lead me on the right way to proceed ?
Many thanks !

1 Like
Fake GPS via mavlink HIL_GPS message
How can I correct the LOCAL_POSITION_NED from an external vision system
(Fine D) #2

Hi @12332111,
I’m also try to control my drone in an indoor environment using a fake gps (feed from vive tracker)
I’m using the HIL_GPS message but I changed the code in AP_GPS_MAV to act like the arducopter receive GPS_INPUT message. I also created a new ROS node that send the HIL_GPS message (I’m not using the mavros\fake_gps.cpp - I think there are some errors in the code…)
I’m still cannot control the drone with this method - somehow the EKF doesn’t like the altitude I sent to it.
working on it…

(Quentin) #3

Hi @JustFineD,
Thanks for sharing your experience !

What version of ArduCopter and Mavros are you using ?
I did not really understand what are the changes you made in AP_GPS_MAV… From my understanding, it can process both HIL_GPS and GPS_INPUT messages, right ?
Also, on which topic is your ROS node sending the messages ?
Where you able to have at least a GPS fix with your method ?

The Erle-Copter I have works with ArduCopter 3.5-rc9 and Mavros 0.18.3. What I did in my experiment was simply to relay the topic on which my MCS data are published to “/mavros/mocap/pose” (geometry_msgs/PoseStamped type), but I couldn’t get any GPS signal recognized… After having a look at the code of the fake GPS plugin, it seems that this former version of Mavros_extra uses the ATT_POS_MOCAP message (#138), which I think is not supported by APM. I am now considering updating both APM and Mavros since significant changes and improvements have been made in the more recent versions regarding the support of MAV GPS.

Now about the configuration of parameters in APM, did you change anything else than setting GPS_TYPE to 14 (MAV) ? I recall reading somewhere that other parameters could be tuned to adjust the confidence level the EKF gives to GPS, or something similar (I will try to find it back).

Best

(Fine D) #4

Hi

  1. I’m using arducopter 3.5.4 and the mavros 0.21.3
  2. You can see that in AP_GPS_MAV::handle_mag() there is a different code for handling the GPS_INPUT messages and the HIL_GPS message. I copied most of the code from GPS_INPUT section into the HIL_GPS handling code. In this way I can set the gps type to 3d fix type.
  3. You’re right about ATT_POS_MOCAP - sadly it is only implemented in PX4 firmware.
  4. I set the following parameters:
    GPS_TYPE = 14
    AHRS_EKF = 3
    EK3_ENABLE = 1 (EK2_ENABLE=0)
    EK3_ALT_SOURCE = 1 (Range finder)
    EK3_GPS_TYPE = 0
    EK3_POSNE_M_NSE = 0.1
    I also found some parameters to adjust the confidence level of GPS but I have not yet played with them.
(Quentin) #5

Hi @JustFineD,

  • Both messages seem very similar to me, except that GPS_INPUT uses float values and has additional fields to set the accuracy. Is this the main reason to use it rather than HIL_GPS ?

  • Also, how do you send your GPS message to APM ? Are you also publishing it to a particular Mavros topic ?

  • How far are you in the control of your drone ? Where you able to takeoff and hover, or is the EKF causing critical errors since beginning ?

I will make new experiments later this week when I have more time. From what I read, many people are/were trying to achieve offboard control using fake GPS, I really hope we will eventually succeed one day !

Best

(Fine D) #6

HI @12332111
I managed to fly my drone in an indoor environment using the HIL_GPS message.
I am using MAVROS that already implement the HIL_GPS message.
I wrote a new ros node that receives the drone position from the Vive system and then it publishes the fake gps to the FCU.

I able to hover perfectly in the same position (I believe no more then 10 cm movement and I believe I can improve that by changing the PID values)
I made a few changes in the ArduCopter code:

  1. Improve HIL_GPS handle function in GPS_MAV.cpp
  2. Override the selectHeightForFusion() function in AP_NavEKF3_PosVelFusion.cpp

You should also set EK3_POSNE_M_NSE to minimum (0.1m)

BR

(Quentin) #7

Hi @JustFineD,

Just by curiosity, what hardware are you using ?

I am still struggling to update Mavros on my system, so I don’t have anything new since last time…
If Mavros experts are passing by, maybe they can have a look at this topic and help me solve my issues ?

Best and Merry Christmas !

(ppoirier) #8

@JustFineD are you planning on a PR with these changes to Arducopter code ?
That would be a great contribution :wink:

(Fine D) #9

Hi @ppoirier,
I’m newbie in the ardupilot community - I’m not sure how can I contribute my code and if my code is good enough for the community.
I changed the code in a “fast and dirty” manner for my purposes (only for an Indoor environment), I just removed some functions from the original code (for example - I do not read from the barometer any data and the altitude is taken directly from the HIL_GPS message) and I added some few lines of my own in the right places for fake range-finder.
The only reason I used HIL_GPS message is that this message is implemented in MAVROS (GPS_INPUT is much better choice). Now I’m trying to bypass the magnetometer measurements.

I think it is not hard for an expert programmer with a good experience with arducopter code to implement a good support for a VIVE tracker device - He only needs to override the (x,y,z, yaw) measurements in the right places in the EKF code.

BR

(Fine D) #10

Merry Christmas,
I am using intel AERO RTF
You do not need to change anything in MAVROS - only run it (roslaunch mavros apm.launch …).
You should only write a new ROS node of your own that publish HIL_GPS message with the motion capture measurements.

BR

(Janne Vaattari) #11

Hello @JustFineD,
How is your drone heading/yaw controlled? GPS_INPUT and HIL_GPS does not have any yaw data.
If your system is working maybe you could share details?
I’m stuck and waiting for this: https://github.com/ArduPilot/ardupilot/pull/7215

BR