Activity Log

Day 1

Focus: Simulation Setup

Installation of ROS Kinetic + Gazebo was already done (apt **-full**)

Guide for PX4 SITL + Gazebo

1. We used this script https://dev.px4.io/en/setup/dev_env_linux.html#gazebo-with-ros, paying attention on not redoing stuff already done and changing the catkin workspace to our project directory.
2. QtCreator: we used https://dev.px4.io/en/setup/building_px4.html#qt-creator-on-linux that checkout needed gazebo sitl files
3. make posix_sitl_default gazebo executed in ~/src/Firmware launches gazebo and px4 sitl with the default iris model
4. Init script for px4 console which configures also mavlink, is found under %Firmware%/posix-configs/SITL/init/ekf2/iris

Gazebo World

Followed the following tutorial to get a simple gazebo world with 2D ground plane image. http://gazebosim.org/tutorials?tut=static_map_plugin&cat=build_world

Day 2

Focus: Route Custom Messages from ground station through Pixhawk (SITL) to MavROS Node

Useful links to start from

• Create new message XML and source code
• Google groups discussion on passing through MAVlink packages
• custom message from stream (may be useful)
• MAVlink tutorial for absolute dummies

Daily Log

1. Wrote two python scripts for MAVlink UDP debugging, i.e. send(Custom)Message and listenForMessages
   • Python interface turned out to be not very much customizable with outgoing / ingoing port (at least not as MAVros does)
2. Tried routing by exploiting system_id and target_id
3. Using python script setup, we debugged the mavlink module running in PX4 SITL
   • Figured out that the forwarding option -f does not work as expected
   • The code base is somehow messed up (over 2500 loc in mavlink_main.cpp) and we could not find the routing fault
   • Seems that only some messages carry the component id and system_id properly
4. By using -f on both instances of mavlink the routing is possible in both directions --> decided to take the easy way
5. Commented out the sending of an unsuccessful ACK when the target system and component is not matched --> leaving to mavros to respond
6. Installation of QtCreator with ROS plugin
   • Make build from QtCreator in order to get the proper project setup
7. Fighting with catkin_ws build setup and QtCreator using BambiSaver Report (wstool)
8. Reached goal of the day: successfully published on topic after custom message has been received

Results

• QGroundControl Custom Widget to send command

Day 3

1. Setup ROS development environment using Eclipse Oxygen
   • catkin config -G"Eclipse CDT4 - Unix Makefiles" -DCMAKE_BUILD_TYPE=Release -DCMAKE_CXX_COMPILER_ARG1=-std=c++11 -D__cplusplus=201103L -D__GXX_EXPERIMENTAL_CXX0X__=1
   • bash -i -c "eclipse"
2. Discussed messages and nodes design
3. Switched back to QtCreator for new bambi ros package, because we noticed the catkin build tool support (not catkin_make)
4. Refactored bambi repository!
   • Created submodules for mavlink and mavros
   • Checked it in as whole catkin workspace
   • PROVIDED developer setup shell script (!)

LESSONS LEARNED

In a submodule we are initially detached from the original repository, i.e. git push will not push to the originating remote repository (which makes actually sense, since we are in a submodule and won't necessary change the original repository (we are formally not working on them). So any commit in the submodule will result in a HEAD DETACHED state.

If we anyhow want to commit to the main repository of the submodule we have to checkout first:

1. git checkout <original-branch> e.g. release/kinetic/mavlink

To fix head detached state:

1. git checkout <original-branch> e.g. release/kinetic/mavlink --> Previous HEAD position was fdea82a...
2. git merge <your-temporary-branch-number-from-head-detached> with head position number from point 1
3. git push

Day 4

1. First implementation of /missioncontroller ROS node (it should publish a take off mavros_msg/sendtext on /mavros/sendtext/send topic)
2. Find out that the starting position of PX4 simulation is hard-coded in /Tools/sitl_gazebo/include/gazebo_gps_plugin.h
   • Add this to your .bashrc:
     1. export PX4_HOME_LAT=46.452895
     2. export PX4_HOME_LON=11.490920
     3. export PX4_HOME_ALT=1415
3. New repository config:
   • Created repository mavlink-library-v2 as a fork in BambiSaver repo to use it as a submodule in PX4 Firmware
     • Updatet it with current mavlink msg header build output
     • N.B Every time .xml messsage definition is updated we should update the forked repository pushing the content ofbambi/build/mavlink/include/v2.0. Then it is required to update submodules of px4-firmware-bambi to make sure it is up-to-date
4. Push on bambi repo a primitive missioncontroller node which arm and lets the quad to take off. Up to now we use /mavros/cmd/takeoff service which only sends MAV_CMD_NAV_TAKEOFF forcing us to set an absolute ground altitude. It would be convenient to use a relative altitude using MAV_CMD_NAV_TAKEOFF_LOCAL message. This can be done using /mavros/cmd/command service which allow to send any COMMAND_LONG MAVlink messages.

Day 5

1. Sketch the ROS node, topic and messages layout and start implementing node skeleton.
2. MAV_CMD_NAV_TAKEOFF_LOCAL is not support in px4 (command 24), so we are forced to use the one without _LOCAL. For this reason we are force to send absolute altitude which turns out to be tricky:
   MAVlink uses WGS84 convention for altitude and latitude in global position messages (i.e GPS_RAW_INT the one used by mavros to gather information about current GPS fix) while it use AMSL convention for altitude. On the other hand, mavros global_position plugin, publish messages with altitude converted in WGS84 using the GeographicLib library (operation which requires 24 MB to be loaded on ram). In this way we would have to reconvert altitude back before adding the require offset to obtain the absolute altitude used in TAKEOFF mavlink message.
   We choose to modify mavros global_position plugin so that it does not convert altitude. Now we have an exact mapping between MAVLink and mavros global position messages and the problem of having different conventions no more occurs.

KML Parser Library for Python

To have some sample data easily available and customizable, it is convenient to use the .kml format. To be able to handle it in the python boundary border, we may use pyKml. To install we need:

1. sudo apt-get install libxml2-dev libxslt-dev python-dev
2. sudo pip install lxml
3. sudo pip install pykml

The KML file has been generated using https://www.doogal.co.uk/polylines.php and can be checked with https://www.doogal.co.uk/KmlViewer.php

Bambi Package Design sketch

Day 6

State Machine Design

Implementation

Day 7

Log

1. Use geodesy package for GSM84 --> UTM coordinate transformation
   • sudo apt install ros-kinetic-geodesy see http://wiki.ros.org/geodesy
   • UTM Coordinate frame more or less squares of 180km
2. Modified bambi gazebo world and model path. Now they are placed under "bambi_ws"/gazebo. REMEMBER to add the following in .bashrc:
   • export GAZEBO_RESOURCE_PATH=$GAZEBO_RESOURCE_PATH:~%BAMBI-GIT-ROOT%/gazebo
   • export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:~%BAMBI-GIT-ROOT%/gazebo/models
3. Successfully implemented WP navigation to orthophoto target position

Coordinate Reference System

In UTM (Northing, Easting, Down) reference system is positive toward north pole (toward map top) and toward east (toward the map right)

Wave Front

Visualized on Google Maps it becomes:

Day 8

1. Reorganize px4-firmware repo:
   • Forked sitl-Gazebo and added as submodule, in order to create bambi world and models.
2. Set up simulation to develop and test obstacle avoidance:
   • Add iris_rplidar model in bambi world. N.B to make the lidar works properly we had to change rayGPU plugin to the standard ray plugin which instead uses CPU. Moreove we notice strange outputs when the drone is in flight (random short distances,less then 35cm, outputs) so we increase the minimum detected distance to 40cm which is a bit more than the iris frame dimension.
   • Disabled down-facing lidar as suggested in https://github.com/PX4/Firmware/issues/9156 otherwise Gazebo crashes reporting segmentation fault.
3. Added first non-working implementation of Repulsive Force Field Obstacle Avoidance in px4 mc_pose_control_main.cpp:
   • Subscribed to uORB topic obstacle_distance and injected obstacle repulsive thrust vector summing it with setpoint thrust computed by velocity controller. The resulting thrust is then sent to mc_attitude_controller.

Day 9

Fixed Obstacle Avoidance!!

LOG

1. Main error was the angle: to obtain a mean angle from the vector of obstacles, we cannot make a mean value from the angle values:
   • if there are some at 5° and some at 355° the mean value should be zero, not 180°
   • --> fixed using mean values of reaction forces, and then calculating the angle using arctan2
2. Evaluated if to use a PID controller --> desired distance from obstacle 5m, measured distance given
   • Decided to DO NOT so, because integral wouldn't discharge
   • We only have limited range measurable, which means we don't see the actual distance if it is larger
   • Furthermore we do not want to have a fixed obstacle distance of 5m, but of AT LEAST 5m, so controller wouldn't make any sens
3. Decided to keep NON-LINEAR control action, i.e. governed by the inverse of the obstacle distance
4. By smoothing repulsive force action with a Low-Pass (cut-off freq = 1Mhz) we obtained satisfying results FOR OUR USE CASE
   • Decided to NOT investigate further papers and NOT implement a more sophisticated obstacle avoidance algorithm, because it is not our use case.
   • i.e. obstacle should be an exceptional case during our mission, where a rudimentary O.A. behavior is good enough

Day 10

LOG

1. Looked for BSpline implementation, decided to fork [https://github.com/chen0040/cpp-spline]
2. Faced problems with too many points: rostopic echo did not work any more (message size probably too large)
   • Found workaround by using rosbag record -O path.bag /bambi/coverage_path_planner/path together with rostopic echo -b path.bag /bambi/coverage_path_planner/path > data.txt, but sublime was not able to handle multiple cursor usage for that filesize and regex on command line were to cumbersome
   • Finally decided to implement python ros node that outputs KML format --> /bambi/kml_generator was born
3. Obtained better results with BSpline that with CatmullRom
4. Optimized first part of CoveragePathPlanning code
5. Introduced $BAMBI_OWNCLOUD_HOME environment variable

Results

Day 11

1. Discovered that PX4 does not implement SET_POSITION_TARGET_GLOBAL_INT and mavros mess up converting from global setpoint to local one => Our approach is now:
   • trajectory_generator node now publish local setpoints in Trajectory message. This has been done referring all the points (UTM) to home position (UTM) (we have just needed to subtract easter and northen of the home position as our local reference system has the y axis pointing to north).
   • Send the obtained trajectory with flight_controller node publishing on mavros setpoint_raw/local topic. The "raw" version has been chosen because it allows to send position, velocity and acceleration all in the same message. The mavros plugin then send SET_POSITION_TARGET_LOCAL_NED mavlink messages.
   • N.B Mavros itself handle the conversion from ENU (East North UP) to NED (North East Down) so the setpoint we send are in ENU coordinate system.
2. mc_pos_control_main.cpp:540 uses the paramter MPC_ALT_MODE, to forward the rangefinder senser velocity to the internal z_v state, to enable terrain following
3. We need to ensure in FlightController, by subscribing the altitude topic and eventually doing some calculations, that the information in the setpoint, which INTERALLY we treat it as RELATIVE TO GROUND, in the trajectory with the list of setpoints, is CONVERTED IN TO A PX4 compatible ALTITUDE
   • Note altitude (from altitude topic) .relative means relative to home position (!)
   • TODO: Check if altitude.terrain gets meaningful values when mounting an onboard rangefinder sensor (it should)
4. Found out that PX4 does not handle MAV_FRAME_GLOBAL_TERRAIN_ALT in MISSIO_ITEM command so we decided to use absolute altitude for the orthophoto waypoint adopting MAV_FRAME_GLOBAL.

Day 12

1. Fixed path sampling algorithm in trajectory generator
2. Implemented Velocity and Yaw generation
3. Statemachine LOGGING
4. Implemented MISSION timer
   • Notice that the behaviour is predictable, but the version we did not prefer, but predictable (makes first climb to ortophoto altitude and then to the position)
5. Prepared altitude in coverage path planner node
   • For this reason we created a new node in python terraind_data_provider which for now asks google earth (without caching) for elevation data
   • The node is implemented as a service, and if not available, the coverage path planner works anyhow

Day 13

1. Made drone jesus smaller of about 8cm each arm
2. Gimbal mount to mosquito
3. Loss of motivation in the afternoon

Day 14

1. Made corpus of jesus
2. Motor failure because of missing phase due to broken banana plug (FIXED)
3. Went out for the first time for dinner :-)

Day 15

1. Scary test flight of jesus
   • Brought home safely, analyzed LOG
   • Swell of magnetic field sensor (compass) and coupling after min 7:55, may be EMI
   • PID Tuning needed

Day 16

1. Successful test flight of jesus using Geofence and other safety measures, trimming PID gains
   • The crucial gains (too high) were MC_PITCH_P and MC_ROLL_P, i.e. propotional angle error compensation, NOT rate PID controller gains.
   • Last test flight was quite satisfying
   • Set-UP of Radio Controller with 6th channel working in 3 states
2. Moskito flight, filming a badminton match, all good
3. Set-UP of BAMBI onto raspberry, using moskito
   • Compile time of more than 2h (!)
   • Connection between FU and Raspberry working nearly at the first try, very happy
   • Implementation of Camera connection through WIFI

Day 17

(only in the morning)

1. Implementation of shutter timer in the orthophoto node
   • Taking PIC and saving GPS infos into .csv file every 8sec
2. NOTES:
   • YI CAM got hot during test flights and had problems handling the WIFI requests (got stuck once in a while)
   • moskito had problems with RADIO link (RTL triggered)