ArduPilot Sensor Updates

[rajat2004]

About

Updates the sensor capabilities of the ArduPilot interface. Replaces #2920 which was quite old and had some problems with the branch

1. Adds support for Distance sensor (called Rangefinder in AP), also increases the RC channels to 12
2. Fixes some style inconsistencies, and uses unique_ptr instead of shared_ptr
3. Updates Unity to C++ 14 (required for make_unique)
4. Only send GPS data if sensor available
5. ExternalController field in Lidar, Distance sensor settings to indicate if data should be sent to external controllers such as ArduPilot or PX4 (Naming suggestions would be appreciated!)

Related AP PRs - ArduPilot/ardupilot_wiki#2948, ArduPilot/ardupilot#15007

The ExternalController field is useful if someone doesn't want AP to use the Lidar data but their own avoidance system

Any ideas on how to reduce the code duplication?

How Has This Been Tested?

Requires AP SITL setup. Distance sensor can be tested using the info provided in the AP Wiki PR, for the GPS sensor, example settings -

{
  "SettingsVersion": 1.2,
  "SimMode": "Multirotor",
  "OriginGeopoint": {
    "Latitude": -35.363261,
    "Longitude": 149.165230,
    "Altitude": 583
  },
  "Vehicles": {
    "Copter": {
      "VehicleType": "ArduCopter",
      "UseSerial": false,
      "LocalHostIp": "127.0.0.1",
      "UdpIp": "127.0.0.1",
      "UdpPort": 9003,
      "ControlPort": 9002,
      "AutoCreate": true,
      "Sensors": {
        "Imu": {
          "SensorType": 2,
          "Enabled": true
        }
      }
    }
  }
}

Earlier it would crash if GPS wasn't present.

ExternalController filed was tested using the settings in the Wiki PR in one of the sensors, and verifying using Wireshark that only a single sensor data is sent, but the sensor is active in the viewport (DrawDebugPoints)

Screenshots (if appropriate):

[rajat2004]

@zimmy87 @jonyMarino Could you have a look at this PR if possible? Thanks!

[zimmy87]

What is the benefit of controlling the sending of data via the ExternalController field? Can't we just send the sensors we have? Also we should clarify that this field is currently only implemented in Ardupilot as there is no support for lidar in PX4 yet.

[zimmy87]

Hey, I'm testing with the settings.json you have above with ArduPilot running in a WSL2 VM on my machine, and after attempting to arm the drone, I see the following output from sim_vehicle.py:

AP: PreArm: GPS is not healthy

This is preventing the drone from getting armed, and I see with w/ and w/o an explicit setting for a GPS sensor. Do you know what could be causing this?

[rajat2004]

The sensor could be disabled from the ArduPilot side as well, and anyways sending the data is fine for distance sensors, but for Lidars, it can be a very big size of data being sent 100s of times in a second completely wasted if not being used. That's why thought that having a field to control it on the AirSim side will be useful

Sorry, forgot to clarify that PX4 doesn't send Lidar currently, will do so, thanks!

[rajat2004]

Got your second comment after replying

Yeah, currently the non-GPS part in SITL needs work on AP side as well. Coincidently, I started working on this a bit today itself :). Current branches - https://github.com/rajat2004/AirSim/tree/ap-non-gps, https://github.com/rajat2004/ardupilot/tree/airsim-non-gps
Still need to confirm whether updating the position info the Airsim backend will work or some Mavlink messages are required.

[rajat2004]

Also we should clarify that this field is currently only implemented in Ardupilot as there is no support for lidar in PX4 yet.

Added, thanks for the review!

[zimmy87]

Just to clarify, I'm getting the "AP: PreArm: GPS is not healthy" error when I have the GPS settings set what you have listed in your example settings.json, so this is separate from the crash you see in the non-GPS scenario. This is preventing me from taking off and performing any flight, so I'm currently blocked from testing any further.

[rajat2004]

For that, you can disable the arming checks, (param set ARMING_CHECK 0) in the Mavproxy console and fly in Stabilize mode (the default one, or use mode stabilize) then arm throttle, rc 3 1700 and it should fly. The Pre-Arm GPS problem is actually occurring with the normal AP-Airsim SITL also (i.e with GPS), need to look into that, maybe the checks have gotten tighter now

[rajat2004]

I'm actually having a look at the GPS implementation, and the flow of data I'm seeing is -

GPS::Update <- Environment::Update which uses EarthUitls::nedToGeodetic

I have zero experience with this and am just thinking out here, my feeling is that the conversion seems to use direct trigonometry, and might be assuming that Earth is a perfect sphere. I haven't found any references for this formula to check, will update if found.

From Wikipedia, the conversion should be a 2 step process and seems much more complicated than this. Incidentally, there is a GeodeticConverter.hpp, with a similar ned2Geodetic which is following the approach and is much more complicated. This header file is unused in AirLib itself, and is only used in pd_position_controller in the ROS wrapper example.

I might be entirely wrong here, and I am just posting here to get any ideas or comments regarding this. I'll probably try to use the GeodeticConverter and see if it makes any difference with the GPS errors as well

So I tested this, but am getting some strange GPS readings with AP wherein the map is showing a different location entirely. The data sent by Airsim seems to be consistent according to the home location set in settings, I haven't yet replaced all the instances of EarthUtils::nedToGeodetic though.

BTW, here's another comment describing this - #1855 (comment)
Plus there are issues on the main repo which might be important - ethz-asl/geodetic_utils#36, ethz-asl/geodetic_utils#28

Update: There seems to be some very strange GPS Altitude jumps when directly ascending, from 50m (rel. to ground), it goes directly to 0, and repeats. This is occurring with the GeodeticConverter as well as the EarthUtils. And it doesn't occur with the 1.3.0 release
The below image is the direct log of the raw GPS altitude rather than AP's estimated altitude.

Apart from the jumps, the values should actually start from 583 m rather than 80, the settings -

{
  "SettingsVersion": 1.2,
  "LogMessagesVisible": true,
  "SimMode": "Multirotor",
  "OriginGeopoint": {
    "Latitude": -35.363261,
    "Longitude": 149.165230,
    "Altitude": 583
  },
  "Vehicles": {
    "Copter": {
      "VehicleType": "ArduCopter",
      "UseSerial": false,
      "LocalHostIp": "127.0.0.1",
      "UdpIp": "127.0.0.1",
      "UdpPort": 9003,
      "ControlPort": 9002
    }
  }
}

[zimmy87]

Interesting, thank you for investigating the GPS implementation. I followed your steps for disabling the arming check and flying in stabilize mode. I'm now seeing the drone continually rise in the air and takeoff and land commands return a FAILED COMMAND_ACK each time I run them. Is that to be expected/do those commands only work in guided mode? Also is there a way to hover at a set altitude w/o the GPS?

[rajat2004]

Yup, the takeoff command only works in Guided, Auto modes. The Copter SITL Tutorial page has more details. To test in Guided mode, you could disable the GPS arming check, (another way arm uncheck gps) and then use takeoff 40 or something, and then the position jump occurs.
ArduPilot supports several non-GPS implementations, Vicon is easy to test in SITL - https://ardupilot.org/dev/docs/using-sitl-for-ardupilot-testing.html#testing-vicon-aka-vision-positioning. GPS isn't necessary, but a good position estimate is required to hover as you would expect, which is currently not the case with latest Airsim master. Trying to do that just causes the drone to keep rising and move erratically, somewhat similar to #3415 but that flight looks quite a bit extreme :)

I built a binary of the latest master to confirm that the GPS jump isn't occurring only in Editor (remote possibility but just to be sure), and that's not the case, still present in the binary as well.
A normal flight with 1.3.0 has the following graph -

I think I'll try a few commits since the 1.3.0 release and see if it can be tracked down to a specific commit, but currently no such PR comes to my mind

[lovettchris]

I'm not seeing this with PX4. But your assumptions about EarthUtils being relatively simplistic is true. It models the earth as an ellipsoid with x_rad and r_yrad, it also uses a simplifying assumption that local coordinates are in a flat plane, as it assumes your drone will never fly more than a distance where the curvature of the earth becomes a factor. So if you plan to fly around the planet you will run into trouble with local coordinates. You can see that nedToGeodetic is a smooth function , I see no reason in that code for any discontinuities in altitude.

There is a known bug in magnetometer when you use a home location other than Seattle. I don't know if that bug was ever fixed. This is why the pix4 sitl docs recommend using "LPE_LAT": 47.641468, "LPE_LON": -122.140165, so you could try that too and see if that helps.

[rajat2004]

@lovettchris Thanks for the info and testing this! Yeah, even I couldn't see any reason as to why the altitude is jumping, and there aren't any recent commits in the relevant files as well. Re the magnetometer, it's not actually being used with ArduPilot, the magnetometer data is generated by AP SITL itself and not sent by AirSim. Tried out the Seattle location, but the jump is still present.

Since there doesn't seem to be anything in the code which could cause a jump of altitude, I did a Wireshark capture of a flight to see if the data being sent is correct. And the mystery continues, the data sent by Airsim seems to be correct -

The low values are due to my very limited parser code, the text export from Wireshark sometimes has the value split across the lines, which causes missing of some digits. The altitude is increasing continuously from 580 to 800. All the files are in https://drive.google.com/drive/folders/1aLrrHsL7hUDEFc6fLiaK7TSDTWaoh9-H?usp=sharing

I'll check the parser on AP side, though another strange thing is that the exact same AP code is being used to test the 1.3.0 release and the latest master.

[rajat2004]

It was a very very stupid mistake, The ArduPilot parser assumes a space after each key, e.g. "alt": , which I had forgotten to add in the new code. This caused the first character in the data to be skipped, and so whenever the data went from 580 -> 600, the parsed value went from 80 -> 00. This came to my mind after I wrote the parser for the Wireshark data and something similar was happening.
Just went too deep into figuring out a simple bug, really really sorry for disturbing everyone!

[lovettchris]

Thanks for closing the loop - great to hear you found the root cause.

[zimmy87]

Tested latest changes with ArduPilot and was able to take off, fly to inputted locations in the map UI, and land. Confirmed that distance and lidar sensor data was sent to ArduPilot. Tested distance sensors in PX4 w/ and w/o "ExternalController" parameter set. Also confirmed Unity version builds and runs okay. Didn't see any regressions so I'm moving ahead with merging this. Thank you for the contribution @rajat2004!

[rajat2004]

@zimmy87 Thanks a lot for the detailed testing and fixing the merge conflict! I'll try to provide more testing evidence myself as well in the future

[lovettchris]

I think it would be safer to make these "dynamic_casts" instead of static casts. See https://docs.microsoft.com/en-us/cpp/cpp/static-cast-operator?view=msvc-160

[lovettchris]

In fact, I think we need to sweep the code base for this and use dynamic cast in a lot of places. But this one is specifically a downcast from SensorBase* to DistanceSimple* and I'm surprised that even works as a static cast.

[rajat2004]

Wasn't aware of this, thanks for the info! Yeah, I guess it worked since the object was actually of Derived type and base class had virtual methods. But makes sense to use dynamic_cast instead to be safe