[FeatureRequest] Support VISLAM with PX4 Stack on Snapdragon Platform

[rkintada]

Problem Statement

There is a VISLAM ROS example released at https://github.com/ATLFlight/ros-examples for Snapdragon Flight^TM. The examples work as standalone and does not work when PX4 stack is running. The reason being both the "Snapdragon::ImuManager" and the PX4 stack try to use the same MPU9x50.

This request is to support the VISLAM with Px4 Stack.

Proposed Solution/Steps:

The solution is to not use the Sensor::ImuManager directly but use the PX4's ORB topics to get the IMU Data. Here is a summary of the changes:

1. Create a module in PX4 for VISLAM
   1. The module must use the Snapdragon::CameraManager(https://github.com/ATLFlight/ros-examples/tree/master/src/camera) class to get the camera Frames
   2. Create a PX4 IMUOrbWrapper class/implementation to subscribe the “accel“ and “gyro“ ORB topics.
   3. For each ORB Topic the timestamp needs to be adjusted as follows:

      
      // get the adsp offset.  
      int64_t dsptime;  
      static const char qdspTimerTickPath[] = "/sys/kernel/boot_adsp/qdsp_qtimer";  
      char qdspTicksStr[20] = "";  
      static const double clockFreq = 1 / 19.2;  
      FILE * qdspClockfp = fopen( qdspTimerTickPath, "r" );  
      fread( qdspTicksStr, 16, 1, qdspClockfp );  
      uint64_t qdspTicks = strtoull( qdspTicksStr, 0, 16 ); 
      fclose( qdspClockfp );  
      dsptime = (int64_t)( qdspTicks*clockFreq*1e3 );    
      //get the apps proc timestamp;  
      int64_t appstimeInNs;  
      struct timespec t;  
      switch( clockType ) 
      {    
      case SENSOR_CLOCK_SYNC_TYPE_MONOTONIC:     
      clock_gettime( CLOCK_MONOTONIC, &t );      
      break;    
      default:     
      clock_gettime( CLOCK_REALTIME, &t );      
      break;  
      }    

uint64_t timeNanoSecMonotonic = (uint64_t)(t.tv_sec) * 1000000000ULL + t.tv_nsec;
appstimeInNs = (int64_t)timeNanoSecMonotonic;
offsetInNs = appstimeInNs – dsptime;

   1. Add the computed offset to the timestamp to the ORB topic
      1. This is needed as the timestamp of the ORB topics are based on the aDSP time.
   1. Modify the VISLAM Manager code to add an API to get the gyro and accel data into the VISLAM engine.
      1. The implementation should call the VISLAM's API mvVISLAM_AddAccel and mvVISLAM_AddGyro
      1. An example implementation is in the "Snapdragon::VislamManager::Imu_IEventListener_ProcessSamples" located in the file: https://github.com/ATLFlight/ros-examples/blob/master/src/vislam/SnapdragonVislamManager.cpp

  1. Create a Cmakelists file for the new module and add the dependency on mv and libcamera.  An example can be found in the https://github.com/ATLFlight/ros-examples/blob/master/CMakeLists.txt
  1. Compile the PX4 code
   1. Update the start up script to start the px4 module.

[mhkabir]

Sounds interesting. Have you though about using the mavros bridge (https://github.com/mavlink/mavros) instead of accessing the IMU data directly?

[rkintada]

I have heard of this, but not actively used it. I am wondering if the latency may be an issue here? Also if the VISLAM's pose information needs to be injected into Px4 stack I think it may be easy to have VISLAM as module within px4.

[mhkabir]

Latency shouldn't be a problem as it's all on the same board.

We have been doing VIO for 1.5 years + : https://youtu.be/AdPpIjlAdKg

For injecting the data back into PX4, I have an API proposed here : https://github.com/PX4/Firmware/pull/6074

[r1b4z01d]

@rkintada this is great; I am attempting this now. Have you been able to complete and test this?

[r1b4z01d]

@rkintada if the intention is to allow the VISLAM Manager to have access to PX4's IMU ORB topics then why does the PX4 module need access to the camera frames?

The module must use the Snapdragon::CameraManager(https://github.com/ATLFlight/ros-examples/tree/master/src/camera) class to get the camera Frames

[rkintada]

@r1b4z01d Bryan It is great that you are going to look into this. Please go ahead and try it out. I did not get a chance to look into this.

The VISLAM manager needs both the camera frames(Optic Flow ) and IMU. Hence the dependency is coming from VISLAM Manager and not directly from PX4.

For this to work, you will need the Machine Vision SDK for Snapdragon Flight from here: https://developer.qualcomm.com/hardware/snapdragon-flight/tools

Thanks Rama

[r1b4z01d]

I am still a little confused by what your end goal is. Are you proposing to incorporate the entire ros-example into the PX4 module or just provide the PX4 IMU data to the ros example? If the latter then how do you propose to send PX4 the computed pose?

I have incorporated MAVROS into the ros-example like @mhkabir mentioned, successfully populating vision_position_estimate. I need to decide if I should build the module or use the IMU data from MAVROS. I am currently leaning towards MAVROS.

[rkintada]

HI , The original goal is to get the VISLAM ros-example to work with PX4. Feeding the VISLAM pose back into PX4 is optional.

My original comment was based on the fact you are not using MAVROS. If you are using MAVROS, the changes to MAVROS do not need any dependency on the camera manager.

With limited knowledge of MAVROS, this is my understanding of your approach:

• Enhance MAVROS to publish IMU data( the timestamp correct still needs to be done ).
• Update the ros-example(VISLAM) to subscribe to the ROS topics that publish IMU data from MAVROS.
• Update the VILSAM Manager code to do the following: Add an API to add the Accel/Gyro Data Remove the dependency of IMU Manager in the same code. At run-time, both the nodes MAVROS and VISLAM ROS example need to run.

Thanks

[rkintada]

Hi, I just came to know that PX4 already does the time-stamp synchronization between aDSP and the apps processor. To the above pseudo code to do time-adjustment is not needed. thanks

[mhkabir]

@rkintada Yep :)

I'm all for pushing this ahead with Mavros now. We can just disable the Mavlink sync since the time bases will already be the same.

@r1b4z01d Can you please push your changes to the VI-slam example somewhere and I'll get it streamlined and working with the new API, since this has been merged : https://github.com/PX4/Firmware/pull/6074

[Seanmatthews]

@rkintada Where did you find that PX4 performs timestamp synchronization between aDSP and the apps processor?

[rkintada]

@Seanmatthews The synchronization is done when initializing muorb. It first computes the offset and invokes an fastrpc call to set the offset in adsp. Here are the relevant source files:

• offset computation: https://github.com/PX4/Firmware/blob/master/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.cpp - Line 85
• Call to adsp: https://github.com/PX4/Firmware/blob/master/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.cpp - line 243
• setting offset in adsp: https://github.com/PX4/Firmware/blob/master/src/modules/muorb/adsp/px4muorb.cpp - line 76

[Seanmatthews]

@rkintada In the approach you outlined above-- frames from camera manager, IMU from MAVROS-- I'm finding that there's still a timestamp synchronization problem between the IMU and camera frame timestamps. The discrepancy in the timestamps between the two prevent VISLAM from upgrading pose quality beyond "initializing". The timestamp on the IMU data from MAVROS reflects realtime (date +%s%N), while the camera timestamp uses monotonic time. Is this just a matter of manually synchronizing? Or should either MAVROS or VISLAM use a different clock?

[rkintada]

@Seanmatthews The DSP time is synchronized to Monotonic clock( as seen in https://github.com/PX4/Firmware/blob/master/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.cpp line 110 ). Do you know if the MavRos is overriding the IMU timestamp with ros::now()? This may explain why it is getting the real time clock. Also the camera timestamps are based off Monotonic clock and not realtime clock.

[mhkabir]

No it doesn't override anything.

Mavros syncs the IMU time to the ROS time base, which is the real-time clock.

I will add a parameter to mavros to disable the timesync and pass on the timestamp straight though. Meanwhile, you can edit the imu_pub plugin and remove the sync_stamp call for the IMU message.

[rkintada]

@mhkabir thanks for the clarification. @Seanmatthews can you please share a sample log of the raw timestamps for the IMU and the camera?

Thanks

[mhkabir]

@rkintada is it feasible to timestamp the camera data using the realtime clock? That way, we'd be maintaining ROS standards.

Related : https://github.com/mavlink/mavros/issues/661

[rkintada]

@mhkabir No. that is not an easy change and also has ripple effect as other application using the camera API rely on the monotonic based timestamp.

[mhkabir]

Ok, understood. My proposal, in that case, would be to simply add an the time offset to the camera image (so that it's time base matches the IMU time), before pushing it into VISLAM. That way we have valid ROS timestamps for IMU, and maintain standards across the board. VISLAM will also return the estimate in the ROS timebase in that case, and PX4 will sync it back to monotonic on receiving.

@Seanmatthews Here is what you need for the onboard timesync (the mode timesync_mode::ONBOARD will calculate the offset between CLOCK_REALTIME for ROS and CLOCK_MONOTONIC for PX4 and timestamp data) : https://github.com/mavlink/mavros/tree/uasys-upstreaming

[Seanmatthews]

@rkintada As per @mhkabir suggestion above, I removed the timestamp synchronization from the MAVROS imu plugin. Doing this results in camera/imu timestamps that are typically within 0.5ms of each other.

Big picture, VISLAM does output a pose, but the poseQuality goes through blips of INITIALIZATION and FAILED about every second. Also, the estimated pose varies wildly when retrieving IMU data through MAVROS (possibly due to the aforementioned blip). For equal sampling rates, the speed of IMU data through MAVROS and VISLAM appears to be somewhat equivalent to the speed of IMU data reception from imu_app, though the data does lag through ROS at higher sample rates (at 200Hz, for example, VISLAM receives/processes at 180-190Hz). Also, there's no difference in the number of samples received per callback between MAVROS and imu_app-- each sends one at a time.

Beyond guesses, I don't have a clear picture of what's wrong, but I have these questions:

• What properties of the IMU data, or its transmission, would cause mvVISLAM to transition from an initialized state to a failed (or initializing) state?
• Does MAVROS filter/process the IMU data from PX4 in any way?
• VISLAM uses IMU values for adding gyro and accelerometer data to mvVISLAM. This seems to work for running VISLAM in the previously prescribed manner. Is it essential to add gyro and accelerometer data from elsewhere when running with MAVROS (as @rkintada suggested in his latter design proposal)?

[mhkabir]

Pretty sure that IMU frame and/or units are wrong. No idea what vislam expects (NED?), but mavros follows the ROS convention of ENU. Try swapping X, Y and negating Z in the mavros gyro and accel data to get it into NED.

[mhkabir]

Then there is the matter of extrinsics and intrinsics calibration for your camera-IMU system. A quick skim of the code base didn't reveal vislam taking in that information anywhere. I don't see how it could work without an accurate calib.

There are too many variables in the VIO chain to predict what exactly goes wrong without actually using it or having more information on the vislam approach. I'll bring out my Snapdragon Flight later and see if I can get something going.

[Seanmatthews]

I wondered about that as well-- there's no mention of calibration at ATLFlight/ros-examples. Though, the VISLAM example does seem to work on its own. @rkintada can you fill us in?

I'll post links to a couple bag files of IMU + pose data from both methods to this comment shortly.

The bag file with /vislam/imu is using imu_app forIMU data, while /mavros/imu/data uses MAVROS:

https://mega.nz/#F!MR9hxRSa!XTa4UrCOs1Dna-GH89RdLA

[mhkabir]

I suspect that the default intrinsics and extrinsics are just close enough for the filter to converge.

[rkintada]

@mhkabir @Seanmatthews Sorry I could not get back to you sooner. Yes, the IMU to camera frame spatial transformation (extrinsic calibration) needs to be provided to the mvVISLAM's initialize API (the tbc and ombc parameters). I will be updating the documentation with more details soon( with some diagrams ). This may be primary cause as the ROS IMU frame is different from the IMU frame I am inputting into the mvVISLAM example code.

Thanks Rama.

[mhkabir]

@rkintada What about the intrinsics?

[Seanmatthews]

Updating the transformation yields more stable pose values. However, the FAIL/INIT blip has become more frequent. On qualcomm's advice, I tried raising the frequency with which PX4 samples the IMU to 500Hz. Though MAVROS seems to publish IMU data up to only 250Hz, despite calling /mavros/set_sample_rate. I don't believe this causes the blips, since VISLAM works fine (though less accurately) at 100Hz.

[mhkabir]

You need to increase the rate on the PX4 side using the mavlink stream command to set rate of HIGHRES_IMU message.

Also, make sure that you're using the /mavros/imu/data_raw topic, not the filtered one.

[Seanmatthews]

I had already set HIGHRES_IMU and ATTITUDE to rates of 500 (the IMU data from MAVROS seems to also be limited by the ATTITUDE rate). Switching from /mavros/imu/data to /mavros/imu/data_raw prevents mvVISLAM from ever breaching its initialization phase.

@rkintada Do you have some insight into mvVISLAM's poseQuality, or what about the IMU data would prevent mvVISLAM from initializing?

[rkintada]

@mhkabir For the camera intryincs we set them via the structure defined in the mvCameraConfiguration structure. Here is an example of where it is used in the example code: https://github.com/ATLFlight/ros-examples/blob/master/src/nodes/SnapdragonRosNodeVislam.cpp Lines: 95 - 109. You can modify them if they need to be updated.

[rkintada]

@Seanmatthews Thanks you very much for trying the integration with PX4. Here some of my comments:

• The standalone VILSAM example uses the following configuration for the MPU9x50:
  • gyro_lpf = MPU9X50_GYRO_LPF_184HZ,
  • acc_lpf = MPU9X50_ACC_LPF_184HZ,
  • gyro_fsr = MPU9X50_GYRO_FSR_2000DPS,
  • acc_fsr = MPU9X50_ACC_FSR_16G,
  • Can you please make sure that the PX4 MPU driver is doing the same. The VISLAM is tuned for the above configurations.
• The VISLAM needs to see atleast 10 feature points to initialize correctly. Wanted to check if the surface that the Camera is pointing to has the features.
• Regarding using the RAW_imu stream, wanted to check if the IMU frame is the same as the one you were using before( ie HIGHRES_IMU ) stream? Otherwise the spatial transformation may need to be re-adjusted. Also make sure the that the timestamp are based of Monotonic clock and not the Ros::time() based.
• Regarding the test setup, can you provide how you are testing to see the FAIL/INITIALIZE blips? Also for the blip you see, is this the transition you see( ie INITIALIZING->HIGH_QUALITY->FAIL ??? )
• Regarding test scenario, can you try in two phase tests.
  • Test1: Keep the board stationary and see if the VISLAM initializes properly( ie it should give pose with High Quality after initializing ). This should not go into the BLIP you are mentioning.
  • Test2: Once test 1 is successful, can you please lift the board and see if the pose is generated correctly and with no blips( I assume this is what you are trying ).
    • If blip happens, then it could be that the spatial transformation is not correct.

[Seanmatthews]

@rkintada @mhkabir

• Thanks for those values. I made the changes with PX4.
• I've been testing the camera on a flat surface, on top of a newspaper.
• I had already made adjustments to both the frame and timestamp. Both did not match up between imu_app and PX4/MAVROS. ROS uses ENU where imu_app/VISLAM uses local NED. And as you stated, VISLAM uses monotonic time.
• Yes-- INIT(x3-4)->HIGH_QUALITY(*)->FAIL(x1).
• I've only tested on a flat surface, since it never stayed initialized.

The good news is that I solved the blip issue. It was a matter of one IMU source accounting for g and the other not. When using IMU data from MAVROS with VISLAM, don't multiply by kNormG here: https://github.com/ATLFlight/ros-examples/blob/master/src/vislam/SnapdragonVislamManager.cpp#L202

[rkintada]

@Seanmatthews Thanks for the inputs. Just so that I understand, with the blip issue you solved are you still seeing that the VISLAM pose to from INIT--> HIGH_QUALITY-->FAIL?

Thanks Rama

[Seanmatthews]

Essentially yes. It would cycle through INIT for a couple seconds at the beginning, until it reached HIGH_QUALITY. Then, about every 1s, it would hit a FAIL followed by ~4 cycles of INIT, then back to HIGH_QUALITY for another ~1s. The IMU data is coming from MAVROS at 250Hz. @rkintada recommends 500Hz, but I'm currently having trouble getting MAVROS to go any higher than that.

[rkintada]

@Seanmatthews One more question, when you do the bench testing( with mavROS ), is there a fan or the props spinning? Also can you share the Imu logs for the test you ran?

The mvVILSAM has a mvSRW_Writer class( mvSRW.h ). Can you use the api's of the mvSRW_Writer class to store the accel, gyro, camera frames and camera configurations? Once you have it we can do some offline analysis internally. Make sure to call the mvSRW_Writer_Initialize() first.

[Seanmatthews]

@rkintada I have only the Snapdragon Flight board by itself. To reiterate from above, I no longer have the blip issue after I fixed the linear acceleration values. Do you still want logs? If so, do you want them from the code pre or post-fix?

[rkintada]

@Seanmatthews With the fix you have, what is the current issue you are seeing? Based on your earlier comment, I understood that you are still seeing the INIT->HighQuality( for 1s ) --> Fail after your fix to the linear acceleration values. If this is still correct, I would like to get the logs to analyze it.

[mhkabir]

The 250Hz thing isn't a mavros limit. PX4 samples the sensors at 1kHz, but the sensor_combined uORB topic (from which the HIGHRES_IMU message is generated) is published at 250 Hz.

[Seanmatthews]

@mhkabir Is that something that can be changed with a setting? I have no problem getting 500Hz using Qualcomm's imu_app.

@rkintada The remaining issue is that after flashing with image 3.1.2 and installing the system according to the PX4 wiki (with v1.6.0rc2 from PX4/Firmware), the ESCs no longer arm (as they did the the ALFlight/Firmware). Do I need to change some ESC settings or install special Qualcomm drivers? Do I need to also install the fc_addon drivers, as prescribed by ATLFlight/ATLFlightDocs?

[rkintada]

@Seanmatthews Once the 3.1.2 image is installed, there is not need to load the add-on drivers as they already part of the image. Regarding arm's the esc I am not aware of any ESC changes. However we are seeing a different issue where there are some IMU related messages on mini-dm( i guess this already reported on qdn forum ). I am suspecting this may be causing the issue. We are looking this currently and will keep you posted.

Thanks Rama.

[Seanmatthews]

@rkintada Just to be clear, you are (or were) able to load the 3.1.2 image from Intrinsyc, install the firmware from this repository, and then operate the Snapdragon Flight?

[Seanmatthews]

@rkintada https://developer.qualcomm.com/forum/qdn-forums/hardware/snapdragon-flight/34105

[Seanmatthews]

@mhkabir Can you spare any info regarding your last comment? Is there a PX4 setting that can be changed to send IMU data at 500Hz?

[potaito]

@Seanmatthews I had the same problem as you with the ESC board not arming. Maybe you have the same problem, namely an incorrect tty device mapping. I posted my findings on the qualcomm Forums (https://developer.qualcomm.com/comment/12329#comment-12329) and James Wilson already merged it to both, the px4 dev page as well as the ATLFlight docs.

Any news on making the ros-examples work with the px4 flight stack? I was about to tackle the same problem when I stumbled upon this issue here.

[Seanmatthews]

@potaito My arming problem was happening because I was following the dev.px4.io instructions for building PX4 for the Snapdragon Flight. The instructions have an error-- the build command should be make eagle_legacy_default.

You can find a version of ros-examples, which I adapted to work with PX4, here.

[mhkabir]

The instructions are not incorrect. The legacy_default target is only for building the legacy binary drivers (not open-source), such as that for the Snapdragon Flight ESC. In which case, you should be following the ATLFlight instructions.

[Seanmatthews]

@mhkabir Correct or not, the instructions listed specifically for the Snapdragon Flight on dev.px4.io do not produce a flyable unit.

EDIT: ...with the Snapdragon Flight dev kit.

[mhkabir]

They do, if you use the PWM ESCs, which is the fully open source build we support.

[potaito]

@mhkabir

Here is what you need for the onboard timesync (the mode timesync_mode::ONBOARD will calculate the offset between CLOCK_REALTIME for ROS and CLOCK_MONOTONIC for PX4 and timestamp data) : https://github.com/mavlink/mavros/tree/uasys-upstreaming

The link is no longer leading anywhere... Can you help me track down your changes? It's probably not compatible to ros-indigo anyway right?

[mhkabir]

It is already in mavros master. It will not be backported to indigo however.