sim vehicle location drift at a second-time takeoff

[Sheng-Cheng]

I'm running into the following issue in the master branch.
The sim vehicle's ENU coordinates are drifted upon takeoff for a second time after it finishes one round of takeoff-mission-land.

I put below the MATLAB printout of the ENU north and east which are obtained from subscribing to '/MACE/UPDATE_LOCAL_POSITION'. Before the vehicle takes off for the first time, the following are the reported locations:
ENU_North = 0.001276, ENU_East = 0.002638
ENU_North = 0.004587, ENU_East = 0.003392
ENU_North = 0.005394, ENU_East = 0.003458
ENU_North = 0.004766, ENU_East = 0.003255
ENU_North = 0.003782, ENU_East = 0.003029

Then the arm command was sent to the vehicle and the ENU coordinates did not have much change:
ENU_North = 0.014094, ENU_East = 0.002904
ENU_North = 0.013144, ENU_East = 0.002715
ENU_North = 0.013066, ENU_East = 0.002769
ENU_North = 0.012647, ENU_East = 0.002728

Now, after the vehicle completed some waypoint missions and landed, below indicates the vehicle's reported location.
ENU_North = -7.076199, ENU_East = -0.305027
ENU_North = -7.076190, ENU_East = -0.304971
ENU_North = -7.076227, ENU_East = -0.304978
ENU_North = -7.076325, ENU_East = -0.305013
ENU_North = -7.076257, ENU_East = -0.306208

What happened next is problematic. After the arm command was sent, it seemed the ENU coordinates were doubled:
ENU_North = -14.148325, ENU_East = -0.608212
ENU_North = -14.148348, ENU_East = -0.608546
ENU_North = -14.148302, ENU_East = -0.608466
ENU_North = -14.148212, ENU_East = -0.608508

---

My .xml file for running OpenMACE is attached although it has a .txt suffix (.xml not allowed as an attachment).
Default_ROS_test.txt

And the sim vehicle was initiated through
$ sim_vehicle.py --instance=1 --sysid=1 --console --out=udp:127.0.0.1:14551 --custom-location=38.973699,-76.921897,0,240

[kkroeger007]

@Sheng-Cheng Within the xml file set the DuplicateToHome flag as true.

[Sheng-Cheng]

@Sheng-Cheng Within the xml file set the DuplicateToHome flag as true.

I changed it to true but the issue remains.

[patnolan33]

@Sheng-Cheng it looks like you need to wait a little longer for the vehicle to completely boot up before you call your sendDatumAndWaitForGPS command. I ran a few tests where I waited for the aircraft to report that APM: EKF2 IMU1 is using GPS, and then set the datum/origin before taking off. After setting the origin, I was not able to recreate the issue you are seeing above. Basically, what seems to be happening is you are launching the simulation and taking off before the vehicle has completely set up and set its home and EKF origins. Therefore, the next time it arms, the origin is set to whatever position it currently is sitting at, which then creates an incorrect coordinate transformation for what you are trying to do after that ARM (the transformation is within MACE).

My suggestion would be to put a "pause" in your launch script before your sendDatumAndWaitForGPS method call. It can either be a timed pause, which should probably be something like 15 seconds or greater, or you can put the operator in charge of when to start the test. If the operator is in control, he/she would just need to wait for APM: EKF2 IMU1 is using GPS message to appear, as that is the final setup message.