add dji2mav-0.3.0 package for 3.1

dji_sdk_dji2mav/CMakeLists.txt

@@ -0,0 +1,197 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(dji_sdk_dji2mav)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  dji_sdk
+  geometry_msgs
+  roscpp
+  rospy
+  std_msgs
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}")
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a run_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   geometry_msgs#   std_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a run_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES dji_sdk_dji2mav
+  CATKIN_DEPENDS dji_sdk geometry_msgs roscpp rospy std_msgs
+  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+#  include_directories(include)
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(dji_sdk_dji2mav
+#   src/${PROJECT_NAME}/dji_sdk_dji2mav.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(dji_sdk_dji2mav ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+add_executable(minimal src/minimal.cpp)
+add_executable(dji2mav_bringup src/dji2mav_bringup.cpp)
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+add_dependencies(minimal ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(dji2mav_bringup ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(minimal
+  ${catkin_LIBRARIES}
+)
+target_link_libraries(dji2mav_bringup
+  ${catkin_LIBRARIES}
+)
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS dji_sdk_dji2mav dji_sdk_dji2mav_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_dji_sdk_dji2mav.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

dji_sdk_dji2mav/README.md

@@ -0,0 +1,171 @@
+# DJI SDK DJI2MAV 0.2.1
+##### A package for connecting onboard computer with ground control station using mavlink protocal
+
+---
+Version: 0.2.1
+
+Author: Chris Liu
+
+Updated Date: 2015/11/30
+
+---
+
+
+## Introduction
+This package is designed as a library and can be included in various platforms. It is implemented in C++ and depended on mavlink library. A simple ROS node is provided to bringup the dji2mav package.
+
+Dji2mav connects the onboard computer with ground control station using UDP. And it is required to call functions to send heartbeat and sensors data. All the mavlink status and message encode/decode will be handled inside the dji2mav package. So far it is tested on Ubuntu 14.0 with ROS indigo.
+
+
+## Quick Start
+Since a ROS node is provided inside this package, it is easy to put the dji2mav to use.
+
+#### 1. Start Simulator
+It is recommanded to do sufficient tests on simulator first.
+Please turn on your drone, connect your drone to the PC and properly set the *API Control* to *Enable* on N1-Assistant. Remember to switch to "P" mode on RC. Then open DJISimulator, click *Start Simulation* and *Display simulator*. You should see M100 on the ground in simulator.
+
+#### 2. Connection
+Connect onboard computer with your ground control station. Any ground control station using mavlink protocol is fitted. Please do sufficient tests on simulator before a real flight. We will be appreciated if you put forward any bug.
+
+Make sure the onboard computer and ground control station are in the same LAN and can successfully ping each other.
+
+#### 3. Launch
+Firstly, fire up dji sdk main launch:
+```
+roslaunch dji_sdk sdk_manifold.launch
+```
+Record the IP and port of ground control station, modified the corresponding param in *dji2mav_bringup.launch*.
+Then run:
+```
+roslaunch dji_sdk_dji2mav dji2mav_bringup.launch
+```
+In the terminal that launched sdk_manifold, you should see the log information that the drone has been successfully activated and the control of the drone has got, as the picture shows below.
+
+![Activation and Request Control](/dji_sdk_dji2mav/doc/img/activation_and_request_control.png?raw=true)
+
+In the terminal that launched dji2mav_bringup, you should see it notifies that the connection to specific IP and port succeeds and all the modules is launched, as the picture shows below.
+
+![Bringup Dji2mav](/dji_sdk_dji2mav/doc/img/bringup_dji2mav.png?raw=true)
+
+If the output of these ROS nodes prompt that every thing goes okay, please move to next step.
+
+#### 4. Setup Ground Control Station
+Now please turn on your mavlink-protocol-adapted ground control station. Let's take QGround Control v2.7.1 for win32 as an example. Before clicking *Connect*, please check the connection settings. The *Link Type* should be UDP, and the *Listening Port* should be consistent with the port number that is set on the launch file which is mentioned on step 2. A UDP setting example is shown below.
+
+![UDP Settings](/dji_sdk_dji2mav/doc/img/udp_settings.png?raw=true)
+
+Then click *Connect*. If everything is ready, you can see your ground control station receives the heartbeat. On *Analyze* tag some sensors data from the vehicle are also displayed.
+
+Now you can do some simple waypoint test on simulator. 
+
+#### 5. Waypoint Test
+Before test waypoint module, please unlock the drone and takeoff using RC. On *Plan* tag inside QGround Station, you should see a marker on the map to show the current position of drone(You can set the initial location in DJISimulator).
+
+Double click on the map to lay markers of the waypoint. On the dashboard, please set the type of waypoint to *Global/Abs. Alt*(global position with absolute altitude). And choose *NAV: Waypoint* to be the navigation type.
+
+Check *lat*, *lon* and *alt* data of every waypoint. Be careful that there is a default value of *alt*. Make sure it is what you expect. You can also set desired yaw angle and stay time for every waypoint. An example of creating waypoint task is shown below.
+
+![Create Waypoint Task](/dji_sdk_dji2mav/doc/img/create_waypoint_task.png?raw=true)
+
+After the confirmation of the waypoint data in dashboard, you should click *Set* to send the whole mission to onboard computer. And a downloading process will be automatically execute after the uploading. You should double check the waypoint data which are downloaded from onboard device. A double check is necessary because there is no "Stop" buttom on the QGround Station. Repeated uploading process is allowed and a newer mission will cover the old one.
+
+To start the mission, click the first ckeck box of the waypoint list. This action will send a "set current target" command to the onboard computer. The vehicle will start from the current target to the end of the waypoint list. The picture shown below displays the results of downloading waypoint task from onboard computer. The "set current target" check box is on the right of every point.
+
+![Set Current Target](/dji_sdk_dji2mav/doc/img/set_current_target.png?raw=true)
+
+
+## Functional Specification
+In waypoint function module, users can set latitude, longitude, altitude, heading and staytime for the waypoint. Users can upload waypoint list to the vehicle. The vehicle will wait till a current target is set by the users in the ground control station. After that, the vehicle will directly go to that target waypoint and automatically carry out the rest of the waypoint list mission, as the picture shown below.
+
+![Auto Execution](/dji_sdk_dji2mav/doc/img/auto_execution.png?raw=true)
+
+
+## Code Architecture
+The dji2mav is designed to meet multi ground control stations and easy expansibility demands. Some classes are designed as lazy-mode singleton.
+
+![dji2mav architecture](/dji_sdk_dji2mav/doc/img/arch.png?raw=true)
+
+#### 1. Setup
+It is easy to access dji2mav interface by getting instance of Config. There are two important methods in config:
+```
+/**
+ * @brief  Set the mavlink system id and the size of GCS list
+ * @param  mavSysid : The system id of this vehicle
+ * @param  num      : The number of Ground Control Stations
+ * @return True if succeed or false if fail
+ */
+bool setup(uint8_t mavSysid, uint16_t num);
+
+/**
+ * @brief  Establish the connection of specific GCS
+ * @param  gcsIdx         : The index of the GCS. Begin from 0
+ * @param  gcsIP          : The IP of GCS
+ * @param  gcsPort        : Connection port of GCS
+ * @param  locPort        : Localhost port
+ * @param  senderListSize : Default 256
+ * @param  sendBufSize    : Default 1024
+ * @param  recvBufSize    : Default 4096
+ * @return True if succeed or false if fail
+ */
+bool start(uint16_t gcsIdx, std::string gcsIP, 
+        uint16_t gcsPort, uint16_t locPort, 
+        uint16_t senderListSize = DEFAULT_SENDER_LIST_SIZE, 
+        uint16_t sendBufSize = DEFAULT_SEND_BUF_SIZE, 
+        uint16_t recvBufSize = DEFAULT_RECV_BUF_SIZE);
+```
+These two methods must be called **BEFORE** any data transporting between ground control station and onboard computer. They can be called like this:
+```
+/* Set the ID of system "1". There is only one ground control system so the number of GCS is also "1" */
+dji2mav::Config::getInstance()->setup(1, 1);
+/* The index of first GCS is "0". Set the first GCS IP and port */
+dji2mav::Config::getInstance()->start(0, targetIp1, (uint16_t)targetPort1, (uint16_t)srcPort);
+```
+
+#### 2. Send Heartbeat
+To send heartbeat to all GCS(ground control station), use:
+```
+dji2mav::MavHeartbeat::getInstance()->sendHeartbeat();
+```
+To send heartbeat to specific GCS, use the corresponding GCS index:
+```
+dji2mav::MavHeartbeat::getInstance()->sendHeartbeat(gcsIdx);
+```
+The send buffer of heartbeat module is particular. So it is safe to be called in a thread that periodically send heartbeat and it is recommanded to do so.
+
+#### 3. Update Sensors Data
+The sending process of sensors data is similar to the heartbeat module. To update the data, please use setters. All the data are stored inside the relative sensor class.
+
+Take ROS platform as an example. Since all the sensors data are published to specific topics, we can update data in the callback function:
+```
+void locPosCB(const dji_sdk::LocalPosition &msg) {
+    dji2mav::MavSensors::getInstance()->setLocalPosition(&msg.ts, &msg.x, 
+            &msg.y, &msg.z);
+}
+
+void velCB(const dji_sdk::Velocity &msg) {
+    dji2mav::MavSensors::getInstance()->setVelocity(&msg.ts, &msg.vx, 
+            &msg.vy, &msg.vz);
+}
+
+void attCB(const dji_sdk::AttitudeQuaternion &msg) {
+    dji2mav::MavSensors::getInstance()->setAttitudeQuaternion(&msg.ts, 
+            &msg.q0, &msg.q1, &msg.q2, &msg.q3, &msg.wx, &msg.wy, &msg.wz);
+}
+
+void gloPosCB(const dji_sdk::GlobalPosition &msg) {
+    dji2mav::MavSensors::getInstance()->setGlobalPosition(&msg.ts, 
+            &msg.latitude, &msg.longitude, &msg.altitude, &msg.height);
+}
+```
+
+#### 4. Run Waypoint Module
+Once waypoint module has been launched, it is ready to receive command from ground control station and reply automatically. All you need to do is keeping the distribution process running(an example of ROS platform):
+```
+while( ros::ok() ) {
+    dji2mav::MavDistributor::getInstance()->distribute();
+    ros::Duration(0.1).sleep();
+    ros::spinOnce(); // callback function spin
+}
+```
+
+