Make VISLAM work with PX4

CHANGELOG.md

@@ -0,0 +1,22 @@
+# Change Log
+All notable changes to this project will be documented in this file.
+
+The format is based on [Keep a Changelog](http://keepachangelog.com/)
+and this project adheres to [Semantic Versioning](http://semver.org/).
+
+## [Unreleased]
+### Changed
+
+## 0.0.2 - 2017-06-6
+### Changed
+- Replaced original ATLFlight README with a link.
+- EKF2 now using vision as main source for height estimation instead of the barometer. Height estimate is now as good as `X` and `Y` estimations.
+- Added px4 configuration files to run LPE estimator instead of EKF2. Seems to work better than EKF2 as anticipated in the [official documentation](https://dev.px4.io/en/ros/external_position_estimation.html)
+
+## 0.0.1 - 2017-06-1
+### Changed
+- VISLAM running with IMU measurements coming from mavros
+
+### Added
+- EKF2 sensor fusion of VISLAM odometry on px4. `Z` is a bit doing whatever it wants (I guess baro) and occasionally the ekf2 output jumps to zero for one time step before it recovers. 
+- px4 configuration files for snapdragon added to `./px4_confs`.

CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 2.8.3)
-project(snap_ros_examples)
+project(snapdragon_mavros_vislam)
 
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
@@ -128,7 +128,6 @@ add_library(snap_vislam
    src/camera/SnapdragonCameraManager.cpp
    src/common/SnapdragonDebugPrint.cpp
    src/camera/SnapdragonCameraUtil.cpp
-   src/imu/SnapdragonImuManager.cpp
    src/vislam/SnapdragonVislamManager.cpp
 )
 
@@ -149,14 +148,15 @@ target_include_directories( snap_vislam PUBLIC
 add_library(mv1 SHARED IMPORTED)
 set_property(TARGET mv1 PROPERTY IMPORTED_LOCATION $ENV{MV_SDK}/usr/lib/libmv1.so)
 
-add_executable( test_snap_cam_manager test/test_SnapdragonCameraManager.cpp )
-target_link_libraries( test_snap_cam_manager snap_vislam sensor_imu mv1 pthread camera  )
-
-add_executable( test_imu_manager test/test_SnapdragonImuManager.cpp )
-target_link_libraries( test_imu_manager snap_vislam sensor_imu mv1 pthread camera  )
-
-add_executable( test_vislam test/test_SnapdragonVislamManager.cpp )
-target_link_libraries( test_vislam snap_vislam sensor_imu mv1 pthread camera  )
+# Tests
+# add_executable( test_snap_cam_manager test/test_SnapdragonCameraManager.cpp )
+# target_link_libraries( test_snap_cam_manager snap_vislam sensor_imu mv1 pthread camera  )
+# 
+# add_executable( test_imu_manager test/test_SnapdragonImuManager.cpp )
+# target_link_libraries( test_imu_manager snap_vislam sensor_imu mv1 pthread camera  )
+# 
+# add_executable( test_vislam test/test_SnapdragonVislamManager.cpp )
+# target_link_libraries( test_vislam snap_vislam sensor_imu mv1 pthread camera  )
 
 ## Declare a C++ executable
 add_executable(snap_vislam_node src/nodes/SnapdragonRosNodeVislam.cpp src/nodes/SnapdragonRosNodeVislam_main.cpp )

launch/mavros_vislam.launch

@@ -0,0 +1,30 @@
+<launch>
+  <!-- MAVROS -->
+  <!-- See the correct UDP port from px4's mainapp.config (HIGHRES_IMU stream) -->
+	<arg name="fcu_url" default="udp://127.0.0.1:46300@127.0.0.1:14555" />
+	<arg name="gcs_url" default="" />
+	<arg name="tgt_system" default="1" />
+	<arg name="tgt_component" default="1" />
+	<arg name="log_output" default="screen" />
+
+	<include file="$(find mavros)/launch/node.launch">
+		<arg name="pluginlists_yaml" value="$(find mavros)/launch/px4_pluginlists.yaml" />
+		<arg name="config_yaml" value="$(find mavros)/launch/px4_config.yaml" />
+
+		<arg name="fcu_url" value="$(arg fcu_url)" />
+		<arg name="gcs_url" value="$(arg gcs_url)" />
+		<arg name="tgt_system" value="$(arg tgt_system)" />
+		<arg name="tgt_component" value="$(arg tgt_component)" />
+		<arg name="log_output" value="$(arg log_output)" />
+	</include>
+
+	<node pkg="tf" type="static_transform_publisher" name="vislam_mavros_broadcaster_ned" args="0 0 0 0 0 3.14159265359 vislam local_origin_ned 10" />
+	<node pkg="tf" type="static_transform_publisher" name="vislam_mavros_broadcaster_fcu" args="0 0 0 0 0 3.14159265359 vislam fcu_frd 10" />
+
+  <node pkg="snapdragon_mavros_vislam" type="snap_vislam_node" name="snap_vislam_node" output="screen">
+      <!-- <rosparam file="$(find rpg_qualcomm_vislam)/parameters/default.yaml"/> -->
+  	<remap from="vislam/odometry" to="mavros/odometry/odom"/>
+	<remap from="vislam/pose" to="mavros/vision_pose/pose"/>
+	</node>
+
+</launch>

package.xml

@@ -1,13 +1,15 @@
 <?xml version="1.0"?>
 <package>
-  <name>snap_ros_examples</name>
+  <name>snapdragon_mavros_vislam</name>
   <version>0.1.1</version>
-  <description>This package provides example code as ros nodes for select features of snapdragon flight platfrom(TM)</description>
+  <description>
+    Fork of the Qualcomm Vislam example code from ATLFlight (https://github.com/ATLFlight/ros-examples) modified for use with the px4 flight stack.
+  </description>
 
   <!-- One maintainer tag required, multiple allowed, one person per tag --> 
   <!-- Example:  -->
   <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
-  <maintainer email="rkintada@gmail.com">rkintada</maintainer>
+  <maintainer email="potaito@TODO.com">potaito</maintainer>
 
 
   <!-- One license tag required, multiple allowed, one license per tag -->
@@ -47,6 +49,7 @@
   <run_depend>roscpp</run_depend>
   <run_depend>rospy</run_depend>
   <run_depend>std_msgs</run_depend>
+  <run_depend>mavros</run_depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->

px4_configs/ekf2/mainapp.conf

@@ -0,0 +1,34 @@
+uorb start
+muorb start
+logger start -t -b 200
+# Wait 1s before setting parameters for muorb to initialize
+sleep 1
+param set MAV_BROADCAST 1
+param set SDLOG_PRIO_BOOST 3
+# EKF2: fuse external vision pose estimate
+param set EKF2_AID_MASK 24
+# EKF2L Switch from barometer to vision as main source for height estimation
+param set EKF2_HGT_MODE 3
+param set EKF2_EVP_NOISE 0.08
+param set EKF2_EVA_NOISE 0.08
+param set EKF2_EV_DELAY 40
+param set EKF2_EV_GATE 20
+dataman start
+navigator start
+mavlink start -u 14556 -r 1000000
+sleep 1
+mavlink stream -u 14556 -s HIGHRES_IMU -r 10
+mavlink stream -u 14556 -s ATTITUDE -r 10
+mavlink stream -u 14556 -s RC_CHANNELS -r 10
+mavlink stream -u 14556 -s LOCAL_POSITION_NED -r 10
+mavlink stream -u 14556 -s VISION_POSITION_ESTIMATE -r 10
+#
+# mavlink instance for vislam
+#
+mavlink start -u 14555 -r 100000 -m magic
+sleep 1
+mavlink stream -u 14555 -s SCALED_IMU -r 250
+mavlink stream -u 14555 -s LOCAL_POSITION_NED -r 50
+mavlink stream -u 14555 -s ATTITUDE -r 50
+mavlink stream -u 14555 -s ATTITUDE_QUATERNION -r 50
+mavlink boot_complete

px4_configs/lpe/mainapp.conf

@@ -0,0 +1,34 @@
+uorb start
+muorb start
+logger start -t -b 200
+# Wait 1s before setting parameters for muorb to initialize
+sleep 1
+param set MAV_BROADCAST 1
+param set SDLOG_PRIO_BOOST 3
+dataman start
+navigator start
+mavlink start -u 14556 -r 1000000
+sleep 1
+mavlink stream -u 14556 -s HIGHRES_IMU -r 50
+mavlink stream -u 14556 -s ATTITUDE -r 50
+mavlink stream -u 14556 -s RC_CHANNELS -r 20
+#
+# mavlink instance for vislam
+#
+mavlink start -u 14555 -r 100000 -m magic
+sleep 1
+mavlink stream -u 14555 -s SCALED_IMU -r 250
+mavlink stream -u 14555 -s LOCAL_POSITION_NED -r 50
+mavlink stream -u 14555 -s ATTITUDE -r 50
+mavlink stream -u 14555 -s ATTITUDE_QUATERNION -r 50
+mavlink boot_complete
+#
+sleep 1
+attitude_estimator_q start
+sleep 1
+## Use LPE for Vision fusion
+param set LPE_FUSION 12
+param set ATT_EXT_HDG_M 1
+param set LPE_VIS_XY 0.005
+param set LPE_VIS_Z 0.005
+param set LPE_VIS_DELAY 0.40

px4_configs/lpe/px4.config

@@ -0,0 +1,73 @@
+uorb start
+qshell start
+param set SYS_AUTOSTART 4001
+param set MAV_TYPE 2
+param set MC_YAW_P 12
+param set MC_YAWRATE_P 0.08
+param set MC_YAWRATE_I 0.0
+param set MC_YAWRATE_D 0
+param set MC_PITCH_P 7.0
+param set MC_PITCHRATE_P 0.08
+param set MC_PITCHRATE_I 0.0
+param set MC_PITCHRATE_D 0.001
+param set MC_ROLL_P 7.0
+param set MC_ROLLRATE_P 0.08
+param set MC_ROLLRATE_I 0.0
+param set MC_ROLLRATE_D 0.001
+param set RC_MAP_THROTTLE 1
+param set RC_MAP_ROLL 2
+param set RC_MAP_PITCH 3
+param set RC_MAP_YAW 4
+param set RC_MAP_MODE_SW 5
+param set RC_MAP_POSCTL_SW 6
+param set RC1_MAX 1900
+param set RC1_MIN 1099
+param set RC1_TRIM 1099
+param set RC1_REV 1
+param set RC2_MAX 1900
+param set RC2_MIN 1099
+param set RC2_TRIM 1500
+param set RC2_REV -1
+param set RC3_MAX 1900
+param set RC3_MIN 1099
+param set RC3_TRIM 1500
+param set RC3_REV 1
+param set RC4_MAX 1900
+param set RC4_MIN 1099
+param set RC4_TRIM 1500
+param set RC4_REV -1
+param set RC5_MAX 1900
+param set RC5_MIN 1099
+param set RC5_TRIM 1500
+param set RC5_REV 1
+param set RC6_MAX 1900
+param set RC6_MIN 1099
+param set RC6_TRIM 1099
+param set RC6_REV 1
+param set ATT_W_MAG 0.00
+param set SENS_BOARD_ROT 0
+param set RC_RECEIVER_TYPE 1
+param set UART_ESC_MODEL 2
+param set UART_ESC_BAUDRTE 250000
+param set UART_ESC_MOTOR1 4
+param set UART_ESC_MOTOR2 2
+param set UART_ESC_MOTOR3 1
+param set UART_ESC_MOTOR4 3
+# param set SYS_MC_EST_GROUP 2
+param set SYS_MC_EST_GROUP 1
+sleep 1
+df_mpu9250_wrapper start
+df_bmp280_wrapper start
+sensors start
+commander start
+local_position_estimator start
+land_detector start multicopter
+mc_pos_control start
+mc_att_control start
+uart_esc start -D /dev/tty-2
+rc_receiver start -D /dev/tty-1
+sleep 1
+list_devices
+list_files
+list_tasks
+list_topics

src/camera/SnapdragonCameraManager.cpp

@@ -139,8 +139,8 @@ int32_t Snapdragon::CameraManager::Initialize(){
       }
     }
 
-    snap_camera_param_ptr_->mv_cpa_config.startExposure = camera_config_ptr_->exposure;
-    snap_camera_param_ptr_->mv_cpa_config.startGain = camera_config_ptr_->gain;
+    snap_camera_param_ptr_->mv_cpa_config.legacyCost.startExposure = camera_config_ptr_->exposure;
+    snap_camera_param_ptr_->mv_cpa_config.legacyCost.startGain = camera_config_ptr_->gain;
 
     if (snap_camera_param_ptr_->enable_cpa) {
       mvCPA_ptr_ = mvCPA_Initialize(&snap_camera_param_ptr_->mv_cpa_config);