Flight mode switching modifications

      It turns out that commander is the app to control flight mode switching, and it publishes vehicle_control_mode to determine current flight mode in mc_position control.

     POSCTL requires local_position.timestamp, local_position.xy_valid and  local_eph_good, all three variables in commander, to be valid.

     Local_position.timestamp and local_position.xy_valid come from topic vehicle_local_position, and local_eph_good indicates the gps horizontal accuracy.

Time stamp modification

      Time stamp is valid when current px4 time t that t < timestamp + timeout and t > timeout are valid.

      Time stamp comes from mavlink message #32. Since ROS and PX4 have their own clock time, time stamp data needs to be synchronized before being sent to PX4.

 
     The above diagram shows time stamp synchronization built between ROS and PX4. A publisher in roscopter was built to get px4 time from mavlink message #30, and publish the time to roscopter. In roscopter, processing time is added to px4 time, and finally the corrected time is sent back to PX4 with local position through mavlink message #32.
   
     The other two variables were also modified in last week. Finally, flight mode switching was achieved.


Abused mavlink message #89

    Mc_position_control requires data from topic vehicle_local_position more than mavlink message #32 could provide, and those data are all calculated in position_estimator_inav. As a result, the idea of disabling position_estimator_inav was given up. Instead, data from pose_estimator would be passed into position_estimator_inav.

    To avoid overriding topic vehicle_local_position (which is correspond to #32),  mavlink message #89 was abused to send local position data from roscopter to PX4 instead of #32.

     This data from last week shows that the local position data were sent to PX4 with #89. The left window shows that the data were sent in roscopter, and the right front window shows that PX4 display the received data with a display subscriber. However, x was 0 instead of 1, y was 1 instead of 2, and etc.. It was due to type casting error in decoding.

     This week, type casting error was fixed. Finally, it is sure that #89 can be sent to PX4.

Pose_estimator - roscopter bridge

Pose_estimator test

                      

     Pose_estimator ros node was used for testing video from camera. The testing data was shown below:

     On the upper right, there was the image from camera, and on the bottom right , there was the detected landing pattern.

    On the left, there was position data output from camera. From left to right were, x, y, z (in millimetre) and yaw (in radian). The result indicates that x and y have an error of +/- 1cm while z has an error of +/- 1mm for still camera.

    The coordinate is build with origin of the centre of landing patter (i.e. #3), 3 ->2 for positive y axis and 3 -> 4 for positive x axis. It indicates the position of the centre of camera image.

Pose_estimator - roscopter bridge



   After the communication bridge between pose_estimator and roscopter node was built in ROS, a test was made to confirm that the data reaches roscopter successfully from pose_estimator.

   The window on the right shows the detection from pose_estimator node. It prints "Tick." if the landing pattern was detected by the camera. The window on the left shows the received messages in roscopter. As the above picture shows, roscopter node did receive message from pose_estimator node.



   The picture above shows that messages in roscopter node were sent to the radio (window on the left) as landing pattern was detected (window on the right).

    Up till August 5th, the whole communication bridge from camera to PX4 was built.

Desired flight mode control flow

Mavlink receiver

     By July 21th, roscopter could send only mavlink message #76 to PX4. Few days after July 21, the reason of failing to send other mavlink messages was found. The problem was that there are no handle functions for other mavlink messages in current PX4 firmware version. In mavlink_reciever app as shown above, only mavlink message #76, #100, #11, #104, #61, #109, #69, #0, #66 and #131 handle functions were implemented.

Mavlink message #32 was implemented successfully, and it was displayed in QGC 

      QGC shows that local position data (right window from roscopter) were successfully sent to PX4 (shows in QGC on the left window).

   As a result, to enable the communication between ROS and PX4, subscriber functions of missing mavlink messages need to be implemented in roscopter node, and handle functions and topics of missing mavlink messages need to be implemented in mavlink_receiver app. In other words, PX4 firmware was hacked to enable the communication.

 
Desired flight mode control flow

   After the communication problem between roscopter and PX4 was solved, further firmware hacking was planned for modifying the original PX4 flight mode control to achieve automated landing.

     The diagram below shows the desired flight mode control:

     Originally, POSCTL (position control mode) and MANUAL (manual mode) are two separated modes, and they needed to be switched by a controller into each other.

    In the desired control, the switching between POSCTL and MANUAL would become autonomous. In other words, as soon as the camera detect the landing pattern, MANUAL would be switched into POSCTL, and the quadcopter hovers autonomously above the landing pattern in POSCTL.

    In this flow, mavlink message #32 was used to send the local position (x, y, z, yaw) from roscopter to PX4. Receiving #32 indicates the camera detects the landing pattern, so #32 is the key data to switch PX4 into POSCTL.


Position control diagram

     To implement the desired flight mode control , a clear diagram of the original position control flow software chain was made. The current version (v1.0) of diagram is shown below:

      This diagram indicates that mc_pos_control app is the beginning of the control software chain, and it subscribe the topic, vehicle_local_position, which is from position_estimator_inav app, is correspond to mavlink massega #32.

     As a result, position_estimator_inav should be disabled publishing vehicle_local_position, and local position from #32 (from camera) should be published to vehicle_local_position instead in POSCTL. Also, modifications should be done for autonomous switching from MANUAL to POSCTL, and it will be discussed in following posts.

Flight mode and Hil simulation presentation 7-21

Flight mode

     In PX4, the flight mode of the system is controlled by switches on the remote control, the ground control station or via onboard parameters which set default values. If the remote control is configured to control a particular flight mode, it will override a conflicting request by the ground control station.

     The diagram below shows the main flight modes:

  
     There are four main mode in PX4 autopilot: MANUAL(manual mode), ATLCTL(attitude control), POSCTL(position control) and AUTO(auto mode). Since RC was calibrated, drones were able to be controlled through radio in both manual and attitude control mode. However, to switch into POSCTL and AUTO, GPS signal or PX4 optical flow input is required.

     This picture shows a UAV for indoor tests:

Simulation

     Roscopter was a ros node (software) used for communication between APM autopilot and computers. As it was not designed for PX4, the program was modified for adapting.

     Due to the reason that roscopter is not designed for PX4, until July, 21st, only Mavlink message #76, COMMAND_LONG, can be used to send commands to PX4. COMMAND_LONG is the mavlink message that allows to send high level commands, and most of the commands are required AUTO to be executed. As a result, in order to test commands from roscopter node to a UAV, HIL (hardware-in-loop) simulation was used for tests.  

     

     Plane X 10 and FlightGear were used for PX4 HIL simulation as recommended. After researches and installation, it turns out that plane X 10 could not be launched due some graphic problems. FlightGear was used for testing arming/disarming commands from roscopter, it turned out that arming/disarming work after fixing the mavlink library. Also, PX4 was successfully switched into POSCTL and AUTO without sensors connected. However, FlightGear turned out that it is not a good simulator for further testing. Because FlightGear does not have the right model for those three UAVs assembled in the lab, it crashes as long as launching the UAV in simulation.

    As a result, for further testing, jMAVSim, a light-weight simulator, was used. As recommended, jMAVSim can work with the autopilot via serial port or UDP connection, directly using the MAVLink protocol, without any wrappers or ground control stations. Optional secondary port (e.g. UDP) can be configured for ground station connection. 

    Due to that jMAVSim is an open source software,  users can modify or make their codes according to their needs. The view can be changed to FPV, gimbal camera view or a fixed point. After fixing some coding problems and compiling, waypoint control was tested in AUTO:

(official simulation video)

Summary

      For further research, Hardware-in-loop simulation would be a very good tool for the following benefits:

 

     1. It helps to test and debug software/hardware interfacing of sensors such as optical flow.

     2. For safety reasons, it could be used for testing custom controllers. Behaviours of controllers could be observed from simulation before testing on multirotors to avoid unexpected accidents.

 

     3. Waypoint control test could observed in simulation before out-door testing. It can avoid losing UAVs due to lost connections or software errors

     4. Software required Auto mode could be tested in simulation, and it can lower the cost for software testing.

Software chain abstract

     The first goal to this project is to have a UAV automated landing on a landing pad. To achieve this goal,  a camera was used to detect the landing pattern. In other words, video was the input and thrusts were the output of this system.

     Picture above is a software chain diagram for the system. 

     Pose_estimator node is used for processing video input data, and it sent local position x, y, z and yaw to roscopter node. Roscopter subscribed the data, and encode them to mavlink message to transmit through the radio. Mavlink message #32, LOCAL_POSITION_NED, is used for sending estimated local position from camera.

     In PX4 autopilot, when the mavlink app receive the mavlink message from radio, it publishes the data in PX4, and the position control flow software chain subscribes the data and other sensor data for controlling the position and attitude of a UAV. 

Useful fundamental researches

      The diagram below shows the flight mode state machine and the different main modes the PX4 system can be in. As the graph shows it covers simple manual / FPV assisted flight as well as full autonomous flight and ensures sane transitions between the modes.

 

      A RGB LED is installed in PX4 to indicate state transitions and errors in different states. Basically, the LED has four colors, red, amber, green and blue, and four different flashing mode: "on", "fast", "breathe" and "blink_normal". These colors and flashing modes contribute to PX4 LED indication. According to PX4 firmware of ArduCopter-3.1.5 release, a table of PX4 LED indication was made in July as presented below:

                         

	Arming state	RED	AMBER	GREEN	BLUE
RGBLED_MODE _ON	ARMING_STATE _ARMED	NONE	VEHICLE_BATTERY _WARNING_LOW || status_local->failsafe	condition_local _position_valid	condition_local _position_invalid
RGBLED_MODE _BLINK_FAST	ARMING_STATE _ARMED_ERROR	ARMING_STATE _ARMED_ERROR	NONE	NONE	NONE
RGBLED_MODE _BREATHE	ARMING_STATE _STANDBY	NONE	VEHICLE_BATTERY _WARNING_LOW || status_local->failsafe	condition_local _position_valid	condition_local _position_invalid
RGBLED_MODE _BLINK_NORMAL	STANDBY_ERROR and other states	STANDBY_ERROR and other states	NONE	NONE	NONE

      3DR telemetry was used for radio control in this research. Since three UAVs were tested in the same time, to avoid interference from each other, telemetries need to be change their Net IDs, and receivers and transmitters should be matched up to the same channel in pairs. After research, it turns out that APM Planner 2 was a good software to flash 3DR telemetry firmware:

Assembly and flight test 6-28

    Three UAVs, named Crush, Pumba and Hathi were assembled for this research in June. Main hardware specs was: PX4 autopilot, 3DR ublox GPS, 3DR telemetry and Quad Frame Kit.

     QGRoundControl and APM Planner 2 were mainly used for sensor and radio calibration.

     After two busy weeks of soldering, assembling, studying Ubuntu operating system, ROS Hydro Medusa, and PX4 autopilot firmware programming, all three drones were ready to fly! A team of five people, Gita, King, Casey, Patrick and me took the flight test on on a soccer filed in UC Berkeley.

      Casey's turn for controlling:
                                      

      Last but not least! :)

     To summary, quadrotors are not easy to control for beginners since acceleration is controlled instead of position. Plus, the time delay or the disturbance of wind turbulence would make it even worse. However, after practising for a certain time, controlling becomes much easier.