vmount: add mount and ROI implementation

MavLink spec implementation implemented vehicle_roi topic rename old gimbal to rc_gimbal little changes corrected RC Gimbal group Starting ROI implementation in commander implementation done, needs to be tested uhm.. add todo Change to float32 for x,y and z remove mission topic again, not needed change roi coordinates to lat, lon and alt adjust to float64 starting mount implementation correcting small mistakes, compiles now add todos further progress implementing parameters adjust default parameters started implementation of mavlink fix typo change to lat, lon and alt fix typo :D change to double (to represent float64) add global_position_ add mount topic commander mount implementation done cleanup almost finished little fix codestyle fixes leave pitch at 0 degrees added pitch calculation codestyle changes Undo vehicle_mount, react to updated parameters, parsing of CMD_DO_MOUNT_* in mount.cpp start implementing mode override forgot a semikolon. add debug Finish implementation of mount override and manual control. fix codestyle correct cleanup rename to vmount works now fix rebase error fix polling refactoring and custom airframe for gimbal couple changes remove warnx almost done finally What is going on? change back to actuator_controls_2 working bump parameter version number and some clarification fix submodules
2016-05-19 23:46:06 +02:00 · 2016-05-19 23:46:06 +02:00 · faebdeedcf
parent e6391189bc
commit faebdeedcf
24 changed files with 1757 additions and 843 deletions
--- a/ROMFS/px4fmu_common/init.d/10021_H4_680mm
+++ b/ROMFS/px4fmu_common/init.d/10021_H4_680mm
@ -0,0 +1,26 @@
 #!nsh
 #
 # @name H4 680mm with Z1 Tiny2 Gimbal
 #
 # @type Quadrotor x
 #
 # @maintainer Leon Mueller <thedevleon>
 #
 sh /etc/init.d/4002_quad_x_mount
 # The Z1 Tiny2 can handle up to 400Hz
 # and works with min 1020us, middle 1520us, max 2020us
 # see http://www.zhiyun-tech.com/uploadfile/datedown/instruction/Tiny2_English_instructionV1.03.pdf
 # under Gimbal Connection Instruction
 set PWM_AUX_RATE 400
 set PWM_AUX_DISARMED 1520
 set PWM_AUX_MIN 1020
 set PWM_AUX_MAX 2020
 # Start FrSky telemetry on SERIAL4 (ttyS6, designated "SERIAL4/5" on the case)
 frsky_telemetry start -d /dev/ttyS6
 # GPIO LED
 gpio_led start -p 6
--- a/ROMFS/px4fmu_common/init.d/4002_quad_x_mount
+++ b/ROMFS/px4fmu_common/init.d/4002_quad_x_mount
@ -0,0 +1,21 @@
 #!nsh
 #
 # @name Generic Quadrotor X config with mount (e.g. gimbal)
 #
 # @type Quadrotor x
 #
 #
 # @maintainer Leon Mueller <thedevleon>
 #
 sh /etc/init.d/rc.mc_defaults
 set MIXER quad_x
 set PWM_OUT 1234
 set MIXER_AUX mount
 set PWM_AUX_OUT 123456
 set PWM_AUX_RATE 50
 # Start mount driver
 vmount start
--- a/ROMFS/px4fmu_common/mixers/mount.aux.mix
+++ b/ROMFS/px4fmu_common/mixers/mount.aux.mix
@ -0,0 +1,27 @@
 # Mount Mixer (e.g. Gimbal, servo-controlled gimbal, etc...)
 # pitch
 M: 1
 O:      10000  10000      0 -10000  10000
 S: 2 0  10000  10000      0 -10000  10000
 # roll
 M: 1
 O:      10000  10000      0 -10000  10000
 S: 2 1  10000  10000      0 -10000  10000
 # yaw
 M: 1
 O:      10000  10000      0 -10000  10000
 S: 2 2  10000  10000      0 -10000  10000
 # mode
 M: 1
 O:      10000  10000      0 -10000  10000
 S: 2 3  10000  10000      0 -10000  10000
 # retracts
 M: 1
 O:      10000  10000      0 -10000  10000
 S: 2 4  10000  10000      0 -10000  10000
--- a/cmake/configs/nuttx_mindpx-v2_default.cmake
+++ b/cmake/configs/nuttx_mindpx-v2_default.cmake
@ -48,7 +48,7 @@ set(config_module_list
 	#drivers/mkblctrl
 	drivers/px4flow
 	#drivers/oreoled
-	drivers/gimbal
+	drivers/vmount
 	drivers/pwm_input
 	drivers/camera_trigger
 	drivers/bst
--- a/cmake/configs/nuttx_px4fmu-v1_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v1_default.cmake
@ -36,6 +36,7 @@ set(config_module_list
 	drivers/meas_airspeed
 	drivers/frsky_telemetry
 	modules/sensors
 	drivers/vmount
 	drivers/camera_trigger
 	drivers/mkblctrl
 	drivers/px4flow
@ -119,7 +120,7 @@ set(config_module_list
 	platforms/nuttx
 	# had to add for cmake, not sure why wasn't in original config
-	platforms/common 
+	platforms/common
 	platforms/nuttx/px4_layer
 	#
--- a/cmake/configs/nuttx_px4fmu-v2_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake
@ -42,7 +42,7 @@ set(config_module_list
 	#drivers/mkblctrl
 	drivers/px4flow
 	#drivers/oreoled
-	drivers/gimbal
+	drivers/vmount
 	drivers/pwm_input
 	drivers/camera_trigger
 	drivers/bst
@ -89,7 +89,7 @@ set(config_module_list
 	modules/load_mon
 	modules/navigator
 	modules/mavlink
-	#modules/gpio_led
+	modules/gpio_led
 	modules/uavcan
 	modules/land_detector
@ -144,7 +144,7 @@ set(config_module_list
 	platforms/nuttx
 	# had to add for cmake, not sure why wasn't in original config
-	platforms/common 
+	platforms/common
 	platforms/nuttx/px4_layer
 	#
--- a/cmake/configs/nuttx_px4fmu-v2_test.cmake
+++ b/cmake/configs/nuttx_px4fmu-v2_test.cmake
@ -42,7 +42,7 @@ set(config_module_list
 	#drivers/mkblctrl
 	drivers/px4flow
 	#drivers/oreoled
-	drivers/gimbal
+	drivers/vmount
 	drivers/pwm_input
 	drivers/camera_trigger
 	#drivers/bst
@ -144,7 +144,7 @@ set(config_module_list
 	platforms/nuttx
 	# had to add for cmake, not sure why wasn't in original config
-	platforms/common 
+	platforms/common
 	platforms/nuttx/px4_layer
 	#
--- a/cmake/configs/nuttx_px4fmu-v4_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v4_default.cmake
@ -40,7 +40,7 @@ set(config_module_list
 	drivers/mkblctrl
 	drivers/px4flow
 	drivers/oreoled
-	drivers/gimbal
+	drivers/vmount
 	drivers/pwm_input
 	drivers/camera_trigger
 	drivers/bst
--- a/cmake/configs/nuttx_tap-v1_default.cmake
+++ b/cmake/configs/nuttx_tap-v1_default.cmake
@ -22,6 +22,7 @@ set(config_module_list
 	drivers/airspeed
 	drivers/meas_airspeed
 	modules/sensors
 	drivers/vmount
 	drivers/camera_trigger
 	#
--- a/msg/CMakeLists.txt
+++ b/msg/CMakeLists.txt
@ -118,6 +118,7 @@ set(msg_file_names
 	vision_position_estimate.msg
 	vtol_vehicle_status.msg
 	wind_estimate.msg
 	vehicle_roi.msg
 	)
 # Get absolute paths
--- a/msg/vehicle_command.msg
+++ b/msg/vehicle_command.msg
@ -2,15 +2,15 @@
 uint32 VEHICLE_CMD_CUSTOM_0 = 0				# test command
 uint32 VEHICLE_CMD_CUSTOM_1 = 1				# test command
 uint32 VEHICLE_CMD_CUSTOM_2 = 2				# test command
-uint32 VEHICLE_CMD_NAV_WAYPOINT = 16			# Navigate to MISSION. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Acceptance radius in meters (if the sphere with this radius is hit, the MISSION counts as reached)| 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at MISSION (rotary wing)| Latitude| Longitude| Altitude| 
+uint32 VEHICLE_CMD_NAV_WAYPOINT = 16			# Navigate to MISSION. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Acceptance radius in meters (if the sphere with this radius is hit, the MISSION counts as reached)| 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at MISSION (rotary wing)| Latitude| Longitude| Altitude|
-uint32 VEHICLE_CMD_NAV_LOITER_UNLIM = 17		# Loiter around this MISSION an unlimited amount of time |Empty| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| 
+uint32 VEHICLE_CMD_NAV_LOITER_UNLIM = 17		# Loiter around this MISSION an unlimited amount of time |Empty| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude|
-uint32 VEHICLE_CMD_NAV_LOITER_TURNS = 18		# Loiter around this MISSION for X turns |Turns| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| 
+uint32 VEHICLE_CMD_NAV_LOITER_TURNS = 18		# Loiter around this MISSION for X turns |Turns| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude|
-uint32 VEHICLE_CMD_NAV_LOITER_TIME = 19			# Loiter around this MISSION for X seconds |Seconds (decimal)| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| 
+uint32 VEHICLE_CMD_NAV_LOITER_TIME = 19			# Loiter around this MISSION for X seconds |Seconds (decimal)| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude|
-uint32 VEHICLE_CMD_NAV_RETURN_TO_LAUNCH = 20		# Return to launch location |Empty| Empty| Empty| Empty| Empty| Empty| Empty| 
+uint32 VEHICLE_CMD_NAV_RETURN_TO_LAUNCH = 20		# Return to launch location |Empty| Empty| Empty| Empty| Empty| Empty| Empty|
-uint32 VEHICLE_CMD_NAV_LAND = 21			# Land at location |Empty| Empty| Empty| Desired yaw angle.| Latitude| Longitude| Altitude| 
+uint32 VEHICLE_CMD_NAV_LAND = 21			# Land at location |Empty| Empty| Empty| Desired yaw angle.| Latitude| Longitude| Altitude|
-uint32 VEHICLE_CMD_NAV_TAKEOFF = 22			# Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| 
+uint32 VEHICLE_CMD_NAV_TAKEOFF = 22			# Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude|
-uint32 VEHICLE_CMD_NAV_ROI = 80				# Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| 
+uint32 VEHICLE_CMD_NAV_ROI = 80				# Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of interest mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z|
-uint32 VEHICLE_CMD_NAV_PATHPLANNING = 81		# Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| 
+uint32 VEHICLE_CMD_NAV_PATHPLANNING = 81		# Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal|
 uint32 VEHICLE_CMD_NAV_VTOL_TAKEOFF = 84		# Takeoff from ground / hand and transition to fixed wing |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude|
 uint32 VEHICLE_CMD_NAV_VTOL_LAND = 85			# Transition to MC and land at location |Empty| Empty| Empty| Desired yaw angle.| Latitude| Longitude| Altitude|
 uint32 VEHICLE_CMD_NAV_GUIDED_LIMITS = 90		# set limits for external control |timeout - maximum time (in seconds) that external controller will be allowed to control vehicle. 0 means no timeout| absolute altitude min (in meters, AMSL) - if vehicle moves below this alt, the command will be aborted and the mission will continue.  0 means no lower altitude limit| absolute altitude max (in meters)- if vehicle moves above this alt, the command will be aborted and the mission will continue.  0 means no upper altitude limit| horizontal move limit (in meters, AMSL) - if vehicle moves more than this distance from it's location at the moment the command was executed, the command will be aborted and the mission will continue. 0 means no horizontal altitude limit| Empty| Empty| Empty| 
@ -21,7 +21,7 @@ uint32 VEHICLE_CMD_CONDITION_DELAY = 112		# Delay mission state machine. |Delay
 uint32 VEHICLE_CMD_CONDITION_CHANGE_ALT = 113		# Ascend/descend at rate.  Delay mission state machine until desired altitude reached. |Descent / Ascend rate (m/s)| Empty| Empty| Empty| Empty| Empty| Finish Altitude| 
 uint32 VEHICLE_CMD_CONDITION_DISTANCE = 114		# Delay mission state machine until within desired distance of next NAV point. |Distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| 
 uint32 VEHICLE_CMD_CONDITION_YAW = 115			# Reach a certain target angle. |target angle: [0-360], 0 is north| speed during yaw change:[deg per second]| direction: negative: counter clockwise, positive: clockwise [-1,1]| relative offset or absolute angle: [ 1,0]| Empty| Empty| Empty| 
-uint32 VEHICLE_CMD_CONDITION_LAST = 159			# NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| 
+uint32 VEHICLE_CMD_CONDITION_LAST = 159		# NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| 
 uint32 VEHICLE_CMD_DO_SET_MODE = 176			# Set system mode. |Mode, as defined by ENUM MAV_MODE| Empty| Empty| Empty| Empty| Empty| Empty| 
 uint32 VEHICLE_CMD_DO_JUMP = 177			# Jump to the desired command in the mission list.  Repeat this action only the specified number of times |Sequence number| Repeat count| Empty| Empty| Empty| Empty| Empty| 
 uint32 VEHICLE_CMD_DO_CHANGE_SPEED = 178		# Change speed and/or throttle set points. |Speed type (0=Airspeed, 1=Ground Speed)| Speed  (m/s, -1 indicates no change)| Throttle  ( Percent, -1 indicates no change)| Empty| Empty| Empty| Empty| 
@ -31,35 +31,36 @@ uint32 VEHICLE_CMD_DO_SET_RELAY = 181			# Set a relay to a condition. |Relay num
 uint32 VEHICLE_CMD_DO_REPEAT_RELAY = 182		# Cycle a relay on and off for a desired number of cyles with a desired period. |Relay number| Cycle count| Cycle time (seconds, decimal)| Empty| Empty| Empty| Empty| 
 uint32 VEHICLE_CMD_DO_SET_SERVO = 183			# Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| 
 uint32 VEHICLE_CMD_DO_REPEAT_SERVO = 184		# Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| 
-uint32 VEHICLE_CMD_DO_FLIGHTTERMINATION = 185	# Terminate flight immediately |Flight termination activated if > 0.5| Empty| Empty| Empty| Empty| Empty| Empty| 
+uint32 VEHICLE_CMD_DO_FLIGHTTERMINATION = 185		# Terminate flight immediately |Flight termination activated if > 0.5| Empty| Empty| Empty| Empty| Empty| Empty| 
 uint32 VEHICLE_CMD_DO_GO_AROUND = 191			# Mission command to safely abort an autonmous landing. |Altitude (meters)| Empty| Empty| Empty| Empty| Empty| Empty|  */ 
 uint32 VEHICLE_CMD_DO_REPOSITION = 192
 uint32 VEHICLE_CMD_DO_PAUSE_CONTINUE = 193
-uint32 VEHICLE_CMD_DO_CONTROL_VIDEO = 200		# Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| 
+uint32 VEHICLE_CMD_DO_CONTROL_VIDEO = 200		# Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty|
 uint32 VEHICLE_CMD_DO_SET_ROI = 201				# Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of interest mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z|
 uint32 VEHICLE_CMD_DO_DIGICAM_CONTROL=203
-uint32 VEHICLE_CMD_DO_MOUNT_CONFIGURE=204		# Mission command to configure a camera or antenna mount |Mount operation mode (see MAV_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty| 
+uint32 VEHICLE_CMD_DO_MOUNT_CONFIGURE=204		# Mission command to configure a camera or antenna mount |Mount operation mode (see MAV_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty|
-uint32 VEHICLE_CMD_DO_MOUNT_CONTROL=205			# Mission command to control a camera or antenna mount |pitch or lat in degrees, depending on mount mode.| roll or lon in degrees depending on mount mode| yaw or alt (in meters) depending on mount mode| reserved| reserved| reserved| MAV_MOUNT_MODE enum value| 
+uint32 VEHICLE_CMD_DO_MOUNT_CONTROL=205			# Mission command to control a camera or antenna mount |pitch or lat in degrees, depending on mount mode.| roll or lon in degrees depending on mount mode| yaw or alt (in meters) depending on mount mode| reserved| reserved| reserved| MAV_MOUNT_MODE enum value|
-uint32 VEHICLE_CMD_DO_SET_CAM_TRIGG_DIST=206		# Mission command to set CAM_TRIGG_DIST for this flight |Camera trigger distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| 
+uint32 VEHICLE_CMD_DO_SET_CAM_TRIGG_DIST=206		# Mission command to set CAM_TRIGG_DIST for this flight |Camera trigger distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty|
-uint32 VEHICLE_CMD_DO_FENCE_ENABLE=207			# Mission command to enable the geofence |enable? (0=disable, 1=enable)| Empty| Empty| Empty| Empty| Empty| Empty| 
+uint32 VEHICLE_CMD_DO_FENCE_ENABLE=207			# Mission command to enable the geofence |enable? (0=disable, 1=enable)| Empty| Empty| Empty| Empty| Empty| Empty|
-uint32 VEHICLE_CMD_DO_PARACHUTE=208			# Mission command to trigger a parachute |action (0=disable, 1=enable, 2=release, for some systems see PARACHUTE_ACTION enum, not in general message set.)| Empty| Empty| Empty| Empty| Empty| Empty| 
+uint32 VEHICLE_CMD_DO_PARACHUTE=208			# Mission command to trigger a parachute |action (0=disable, 1=enable, 2=release, for some systems see PARACHUTE_ACTION enum, not in general message set.)| Empty| Empty| Empty| Empty| Empty| Empty|
-uint32 VEHICLE_CMD_DO_INVERTED_FLIGHT=210		# Change to/from inverted flight |inverted (0=normal, 1=inverted)| Empty| Empty| Empty| Empty| Empty| Empty| 
+uint32 VEHICLE_CMD_DO_INVERTED_FLIGHT=210		# Change to/from inverted flight |inverted (0=normal, 1=inverted)| Empty| Empty| Empty| Empty| Empty| Empty|
-uint32 VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT=220		# Mission command to control a camera or antenna mount, using a quaternion as reference. |q1 - quaternion param #1, w (1 in null-rotation)| q2 - quaternion param #2, x (0 in null-rotation)| q3 - quaternion param #3, y (0 in null-rotation)| q4 - quaternion param #4, z (0 in null-rotation)| Empty| Empty| Empty| 
+uint32 VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT=220		# Mission command to control a camera or antenna mount, using a quaternion as reference. |q1 - quaternion param #1, w (1 in null-rotation)| q2 - quaternion param #2, x (0 in null-rotation)| q3 - quaternion param #3, y (0 in null-rotation)| q4 - quaternion param #4, z (0 in null-rotation)| Empty| Empty| Empty|
-uint32 VEHICLE_CMD_DO_GUIDED_MASTER=221			# set id of master controller |System ID| Component ID| Empty| Empty| Empty| Empty| Empty| 
+uint32 VEHICLE_CMD_DO_GUIDED_MASTER=221			# set id of master controller |System ID| Component ID| Empty| Empty| Empty| Empty| Empty|
-uint32 VEHICLE_CMD_DO_GUIDED_LIMITS=222			# set limits for external control |timeout - maximum time (in seconds) that external controller will be allowed to control vehicle. 0 means no timeout| absolute altitude min (in meters, AMSL) - if vehicle moves below this alt, the command will be aborted and the mission will continue.  0 means no lower altitude limit| absolute altitude max (in meters)- if vehicle moves above this alt, the command will be aborted and the mission will continue.  0 means no upper altitude limit| horizontal move limit (in meters, AMSL) - if vehicle moves more than this distance from it's location at the moment the command was executed, the command will be aborted and the mission will continue. 0 means no horizontal altitude limit| Empty| Empty| Empty| 
+uint32 VEHICLE_CMD_DO_GUIDED_LIMITS=222			# set limits for external control |timeout - maximum time (in seconds) that external controller will be allowed to control vehicle. 0 means no timeout| absolute altitude min (in meters, AMSL) - if vehicle moves below this alt, the command will be aborted and the mission will continue.  0 means no lower altitude limit| absolute altitude max (in meters)- if vehicle moves above this alt, the command will be aborted and the mission will continue.  0 means no upper altitude limit| horizontal move limit (in meters, AMSL) - if vehicle moves more than this distance from it's location at the moment the command was executed, the command will be aborted and the mission will continue. 0 means no horizontal altitude limit| Empty| Empty| Empty|
-uint32 VEHICLE_CMD_DO_LAST = 240			# NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| 
+uint32 VEHICLE_CMD_DO_LAST = 240			# NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty|
-uint32 VEHICLE_CMD_PREFLIGHT_CALIBRATION = 241		# Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty| 
+uint32 VEHICLE_CMD_PREFLIGHT_CALIBRATION = 241		# Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty|
-uint32 VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS = 242	# Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| 
+uint32 VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS = 242	# Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units|
-uint32 VEHICLE_CMD_PREFLIGHT_STORAGE = 245		# Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty| 
+uint32 VEHICLE_CMD_PREFLIGHT_STORAGE = 245		# Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty|
-uint32 VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN = 246	# Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty| 
+uint32 VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN = 246	# Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty|
-uint32 VEHICLE_CMD_OVERRIDE_GOTO = 252			# Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position| 
+uint32 VEHICLE_CMD_OVERRIDE_GOTO = 252			# Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position|
-uint32 VEHICLE_CMD_MISSION_START = 300			# start running a mission |first_item: the first mission item to run| last_item:  the last mission item to run (after this item is run, the mission ends)| 
+uint32 VEHICLE_CMD_MISSION_START = 300			# start running a mission |first_item: the first mission item to run| last_item:  the last mission item to run (after this item is run, the mission ends)|
-uint32 VEHICLE_CMD_COMPONENT_ARM_DISARM = 400		# Arms / Disarms a component |1 to arm, 0 to disarm| 
+uint32 VEHICLE_CMD_COMPONENT_ARM_DISARM = 400		# Arms / Disarms a component |1 to arm, 0 to disarm|
 uint32 VEHICLE_CMD_START_RX_PAIR = 500			# Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX|
 uint32 VEHICLE_CMD_DO_TRIGGER_CONTROL = 2003            # Enable or disable on-board camera triggering system
 uint32 VEHICLE_CMD_DO_VTOL_TRANSITION = 3000    # Command VTOL transition
 uint32 VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY = 30001	# Prepare a payload deployment in the flight plan
 uint32 VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY = 30002	# Control a pre-programmed payload deployment
-uint32 VEHICLE_CMD_PREFLIGHT_UAVCAN = 243		# UAVCAN configuration. If param 1 == 1 actuator mapping and direction assignment should be started 
+uint32 VEHICLE_CMD_PREFLIGHT_UAVCAN = 243		# UAVCAN configuration. If param 1 == 1 actuator mapping and direction assignment should be started
 uint32 VEHICLE_CMD_RESULT_ACCEPTED = 0			# Command ACCEPTED and EXECUTED |
 uint32 VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED = 1	# Command TEMPORARY REJECTED/DENIED |
@ -68,23 +69,30 @@ uint32 VEHICLE_CMD_RESULT_UNSUPPORTED = 3		# Command UNKNOWN/UNSUPPORTED |
 uint32 VEHICLE_CMD_RESULT_FAILED = 4			# Command executed, but failed |
 uint32 VEHICLE_CMD_RESULT_ENUM_END = 5			#
-uint32 VEHICLE_MOUNT_MODE_RETRACT = 0				# Load and keep safe position (Roll,Pitch,Yaw) from permant memory and stop stabilization |
+uint32 VEHICLE_MOUNT_MODE_RETRACT = 0				# Load and keep safe position (Roll,Pitch,Yaw) from permanent memory and stop stabilization |
 uint32 VEHICLE_MOUNT_MODE_NEUTRAL = 1				# Load and keep neutral position (Roll,Pitch,Yaw) from permanent memory. |
-uint32 VEHICLE_MOUNT_MODE_MAVLINK_TARGETING = 2			# Load neutral position and start MAVLink Roll,Pitch,Yaw control with stabilization |
+uint32 VEHICLE_MOUNT_MODE_MAVLINK_TARGETING = 2		# Load neutral position and start MAVLink Roll,Pitch,Yaw control with stabilization |
 uint32 VEHICLE_MOUNT_MODE_RC_TARGETING = 3			# Load neutral position and start RC Roll,Pitch,Yaw control with stabilization |
 uint32 VEHICLE_MOUNT_MODE_GPS_POINT = 4				# Load neutral position and start to point to Lat,Lon,Alt |
 uint32 VEHICLE_MOUNT_MODE_ENUM_END = 5				#
-float32 param1			# Parameter 1, as defined by MAVLink uint32 VEHICLE_CMD enum.  
+uint32 VEHICLE_ROI_NONE = 0                         # No region of interest |
-float32 param2			# Parameter 2, as defined by MAVLink uint32 VEHICLE_CMD enum.  
+uint32 VEHICLE_ROI_WPNEXT = 1                       # Point toward next MISSION |
-float32 param3			# Parameter 3, as defined by MAVLink uint32 VEHICLE_CMD enum.  
+uint32 VEHICLE_ROI_WPINDEX = 2                      # Point toward given MISSION |
-float32 param4			# Parameter 4, as defined by MAVLink uint32 VEHICLE_CMD enum.  
+uint32 VEHICLE_ROI_LOCATION = 3                     # Point toward fixed location |
-float64 param5			# Parameter 5, as defined by MAVLink uint32 VEHICLE_CMD enum.  
+uint32 VEHICLE_ROI_TARGET = 4                       # Point toward target
-float64 param6			# Parameter 6, as defined by MAVLink uint32 VEHICLE_CMD enum.  
+uint32 VEHICLE_ROI_ENUM_END = 5
-float32 param7			# Parameter 7, as defined by MAVLink uint32 VEHICLE_CMD enum.  
+
-uint32 command			# Command ID, as defined MAVLink by uint32 VEHICLE_CMD enum.  
+float32 param1			# Parameter 1, as defined by MAVLink uint32 VEHICLE_CMD enum.
-uint32 target_system		# System which should execute the command 
+float32 param2			# Parameter 2, as defined by MAVLink uint32 VEHICLE_CMD enum.
-uint32 target_component		# Component which should execute the command, 0 for all components 
+float32 param3			# Parameter 3, as defined by MAVLink uint32 VEHICLE_CMD enum.
-uint32 source_system		# System sending the command 
+float32 param4			# Parameter 4, as defined by MAVLink uint32 VEHICLE_CMD enum.
-uint32 source_component		# Component sending the command 
+float64 param5			# Parameter 5, as defined by MAVLink uint32 VEHICLE_CMD enum.
 float64 param6			# Parameter 6, as defined by MAVLink uint32 VEHICLE_CMD enum.
 float32 param7			# Parameter 7, as defined by MAVLink uint32 VEHICLE_CMD enum.
 uint32 command			# Command ID, as defined MAVLink by uint32 VEHICLE_CMD enum.
 uint32 target_system		# System which should execute the command
 uint32 target_component		# Component which should execute the command, 0 for all components
 uint32 source_system		# System sending the command
 uint32 source_component		# Component sending the command
 uint8 confirmation		# 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command)
--- a/msg/vehicle_roi.msg
+++ b/msg/vehicle_roi.msg
@ -0,0 +1,15 @@
 # Vehicle Region Of Interest (ROI)
 uint8 VEHICLE_ROI_NONE = 0                         # No region of interest |
 uint8 VEHICLE_ROI_WPNEXT = 1                       # Point toward next MISSION |
 uint8 VEHICLE_ROI_WPINDEX = 2                      # Point toward given MISSION |
 uint8 VEHICLE_ROI_LOCATION = 3                     # Point toward fixed location |
 uint8 VEHICLE_ROI_TARGET = 4                       # Point toward target
 uint8 VEHICLE_ROI_ENUM_END = 5
 uint8 mode          # ROI mode (see above)
 uint32 mission_seq  # mission sequence to point to
 uint32 target_seq   # target sequence to point to
 float64 lat			    # Latitude to point to
 float64 lon			    # Longitude to point to
 float32 alt			    # Altitude to point to
--- a/src/drivers/gimbal/gimbal.cpp
+++ b/src/drivers/gimbal/gimbal.cpp
@ -1,704 +0,0 @@
 /****************************************************************************
 *
 *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/
 /**
 * @file gimbal.cpp
 * @author Lorenz Meier <lm@inf.ethz.ch>
 * @author Anton Matosov <anton.matosov@gmail.com>
 * @author Andreas Antener <andreas@uaventure.com>
 *
 * Driver to control a gimbal - relies on input via telemetry or RC
 * and output via the standardized control group #2 and a mixer.
 */
 #include <px4_config.h>
 #include <sys/types.h>
 #include <stdint.h>
 #include <stdlib.h>
 #include <stdbool.h>
 #include <semaphore.h>
 #include <string.h>
 #include <fcntl.h>
 #include <poll.h>
 #include <errno.h>
 #include <stdio.h>
 #include <math.h>
 #include <unistd.h>
 #include <termios.h>
 #include <nuttx/arch.h>
 #include <nuttx/wqueue.h>
 #include <nuttx/clock.h>
 #include <systemlib/err.h>
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_range_finder.h>
 #include <drivers/device/device.h>
 #include <drivers/device/ringbuffer.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/parameter_update.h>
 #include <board_config.h>
 #include <mathlib/math/test/test.hpp>
 #include <mathlib/math/Quaternion.hpp>
 /* Configuration Constants */
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
 # undef ERROR
 #endif
 static const int ERROR = -1;
 #ifndef CONFIG_SCHED_WORKQUEUE
 # error This requires CONFIG_SCHED_WORKQUEUE.
 #endif
 #define GIMBAL_DEVICE_PATH	"/dev/gimbal"
 #define GIMBAL_UPDATE_INTERVAL (5 * 1000)
 #define GIMBALIOCATTCOMPENSATE		1
 class Gimbal : public device::CDev
 {
 public:
 	Gimbal();
 	virtual ~Gimbal();
 	virtual int 			init();
 	virtual ssize_t			read(struct file *filp, char *buffer, size_t buflen);
 	virtual int			ioctl(struct file *filp, int cmd, unsigned long arg);
 	/**
 	* Diagnostics - print some basic information about the driver.
 	*/
 	void				print_info();
 protected:
 	virtual int			probe();
 private:
 	float				_min_distance;
 	float				_max_distance;
 	work_s				_work;
 	int				_vehicle_command_sub;
 	int				_att_sub;
 	int				_vehicle_control_mode_sub;
 	int				_manual_control_sub;
 	int				_params_sub;
 	bool				_attitude_compensation_roll;
 	bool				_attitude_compensation_pitch;
 	bool				_attitude_compensation_yaw;
 	bool				_initialized;
 	bool				_control_cmd_set;
 	bool				_config_cmd_set;
 	int					_mount_mode; // see MAV_MOUNT_MODE
 	orb_advert_t			_actuator_controls_2_topic;
 	struct vehicle_command_s _control_cmd;
 	struct vehicle_command_s _config_cmd;
 	struct manual_control_setpoint_s _manual_control;
 	struct vehicle_control_mode_s _control_mode;
 	struct {
 		int aux_mnt_chn;
 		int use_mnt;
 	} _parameters;
 	struct {
 		param_t aux_mnt_chn;
 		param_t use_mnt;
 	} _params_handles;
 	/**
 	* Initialise the automatic measurement state machine and start it.
 	*
 	* @note This function is called at open and error time.  It might make sense
 	*       to make it more aggressive about resetting the bus in case of errors.
 	*/
 	void				start();
 	/**
 	* Stop the automatic measurement state machine.
 	*/
 	void				stop();
 	/**
 	* Update parameters
 	*/
 	void				update_params();
 	/**
 	* Perform a poll cycle; collect from the previous measurement
 	* and start a new one.
 	*/
 	void				cycle();
 	/**
 	* Static trampoline from the workq context; because we don't have a
 	* generic workq wrapper yet.
 	*
 	* @param arg		Instance pointer for the driver that is polling.
 	*/
 	static void			cycle_trampoline(void *arg);
 };
 /*
 * Driver 'main' command.
 */
 extern "C" __EXPORT int gimbal_main(int argc, char *argv[]);
 Gimbal::Gimbal() :
 	CDev("Gimbal", GIMBAL_DEVICE_PATH),
 	_vehicle_command_sub(-1),
 	_att_sub(-1),
 	_vehicle_control_mode_sub(-1),
 	_manual_control_sub(-1),
 	_params_sub(-1),
 	_attitude_compensation_roll(true),
 	_attitude_compensation_pitch(true),
 	_attitude_compensation_yaw(true),
 	_initialized(false),
 	_actuator_controls_2_topic(nullptr)
 {
 	// disable debug() calls
 	_debug_enabled = false;
 	_params_handles.aux_mnt_chn = param_find("GMB_AUX_MNT_CHN");
 	_params_handles.use_mnt = param_find("GMB_USE_MNT");
 	update_params();
 	// work_cancel in the dtor will explode if we don't do this...
 	memset(&_work, 0, sizeof(_work));
 }
 Gimbal::~Gimbal()
 {
 	/* make sure we are truly inactive */
 	stop();
 	::close(_vehicle_command_sub);
 }
 int
 Gimbal::init()
 {
 	int ret = ERROR;
 	/* do regular cdev init */
 	if (CDev::init() != OK) {
 		goto out;
 	}
 	start();
 	ret = OK;
 out:
 	return ret;
 }
 int
 Gimbal::probe()
 {
 	return OK;
 }
 int
 Gimbal::ioctl(struct file *filp, int cmd, unsigned long arg)
 {
 	switch (cmd) {
 	case GIMBALIOCATTCOMPENSATE:
 		_attitude_compensation_roll = (arg != 0);
 		_attitude_compensation_pitch = (arg != 0);
 		_attitude_compensation_yaw = (arg != 0);
 		return OK;
 	default:
 		/* give it to the superclass */
 		return CDev::ioctl(filp, cmd, arg);
 	}
 }
 ssize_t
 Gimbal::read(struct file *filp, char *buffer, size_t buflen)
 {
 	return 0;
 }
 void
 Gimbal::start()
 {
 	/* schedule a cycle to start things */
 	work_queue(LPWORK, &_work, (worker_t)&Gimbal::cycle_trampoline, this, 1);
 }
 void
 Gimbal::stop()
 {
 	work_cancel(LPWORK, &_work);
 }
 void
 Gimbal::cycle_trampoline(void *arg)
 {
 	Gimbal *dev = static_cast<Gimbal *>(arg);
 	dev->cycle();
 }
 void
 Gimbal::update_params()
 {
 	param_get(_params_handles.aux_mnt_chn, &_parameters.aux_mnt_chn);
 	param_get(_params_handles.use_mnt, &_parameters.use_mnt);
 }
 void
 Gimbal::cycle()
 {
 	if (!_initialized) {
 		/* get subscription handles */
 		_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command));
 		_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
 		_vehicle_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 		_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
 		_params_sub = orb_subscribe(ORB_ID(parameter_update));
 		/* get a publication handle on actuator output topic */
 		struct actuator_controls_s zero_report;
 		memset(&zero_report, 0, sizeof(zero_report));
 		zero_report.timestamp = hrt_absolute_time();
 		_actuator_controls_2_topic = orb_advertise(ORB_ID(actuator_controls_2), &zero_report);
 		if (_actuator_controls_2_topic == nullptr) {
 			warnx("advert err");
 		}
 		_mount_mode = vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT;
 		_initialized = true;
 	}
 	bool	updated = false;
 	float roll = 0.0f;
 	float pitch = 0.0f;
 	float yaw = 0.0f;
 	float out_mount_mode = 0.0f;
 	/* Parameter update */
 	bool params_updated;
 	orb_check(_params_sub, &params_updated);
 	if (params_updated) {
 		struct parameter_update_s param_update;
 		orb_copy(ORB_ID(parameter_update), _params_sub, &param_update); // XXX: is this actually necessary?
 		update_params();
 	}
 	/* Control mode update */
 	bool vehicle_control_mode_updated;
 	orb_check(_vehicle_control_mode_sub, &vehicle_control_mode_updated);
 	if (vehicle_control_mode_updated) {
 		orb_copy(ORB_ID(vehicle_control_mode), _vehicle_control_mode_sub, &_control_mode);
 	}
 	/* Check attitude */
 	struct vehicle_attitude_s att;
 	orb_copy(ORB_ID(vehicle_attitude), _att_sub, &att);
 	if (_attitude_compensation_roll) {
 		roll = 1.0f / M_PI_F * -att.roll;
 		updated = true;
 	}
 	if (_attitude_compensation_pitch) {
 		pitch = 1.0f / M_PI_F * -att.pitch;
 		updated = true;
 	}
 	if (_attitude_compensation_yaw) {
 		yaw = 1.0f / M_PI_F * att.yaw;
 		updated = true;
 	}
 	/* Check manual input */
 	bool manual_updated;
 	orb_check(_manual_control_sub, &manual_updated);
 	if (manual_updated) {
 		orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sub, &_manual_control);
 		/* only check manual input for mount mode when not in offboard and aux chan is configured */
 		if (!_control_mode.flag_control_offboard_enabled && _parameters.aux_mnt_chn > 0) {
 			float aux = 0.0f;
 			switch (_parameters.aux_mnt_chn) {
 			case 1:
 				aux = _manual_control.aux1;
 				break;
 			case 2:
 				aux = _manual_control.aux2;
 				break;
 			case 3:
 				aux = _manual_control.aux3;
 				break;
 			}
 			if (aux < 0.0f && _mount_mode != vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT) {
 				_mount_mode = vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT;
 				updated = true;
 			}
 			if (aux > 0.0f && _mount_mode != vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL) {
 				_mount_mode = vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL;
 				updated = true;
 			}
 		}
 	}
 	/* Check command input */
 	struct vehicle_command_s cmd;
 	bool cmd_updated;
 	orb_check(_vehicle_command_sub, &cmd_updated);
 	if (cmd_updated) {
 		orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd);
 		if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL
 		    || cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT) {
 			_control_cmd = cmd;
 			_control_cmd_set = true;
 		} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE) {
 			_config_cmd = cmd;
 			_config_cmd_set = true;
 		}
 	}
 	if (_config_cmd_set) {
 		_config_cmd_set = false;
 		_attitude_compensation_roll = (int)_config_cmd.param2 == 1;
 		_attitude_compensation_pitch = (int)_config_cmd.param3 == 1;
 		_attitude_compensation_yaw = (int)_config_cmd.param4 == 1;
 		/* only check commanded mount mode when in offboard */
 		if (_control_mode.flag_control_offboard_enabled &&
 		    fabsf(_config_cmd.param1 - _mount_mode) > FLT_EPSILON) {
 			_mount_mode = int(_config_cmd.param1 + 0.5f);
 			updated = true;
 		}
 	}
 	if (_control_cmd_set) {
 		unsigned mountMode = _control_cmd.param7;
 		DEVICE_DEBUG("control_cmd: %d, mountMode %d | param1: %8.4f param2: %8.4f", _control_cmd.command, mountMode,
 			     (double)_control_cmd.param1, (double)_control_cmd.param2);
 		if (_control_cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL &&
 		    mountMode == vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) {
 			/* Convert to range -1 ... 1, which corresponds to -180deg ... 180deg */
 			roll += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param1;
 			pitch += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param2;
 			yaw += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param3;
 			updated = true;
 		}
 		if (_control_cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT &&
 		    mountMode == vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) {
 			float gimbalDirectionQuat[] = {_control_cmd.param1, _control_cmd.param2, _control_cmd.param3, _control_cmd.param4};
 			math::Vector<3> gimablDirectionEuler = math::Quaternion(gimbalDirectionQuat).to_dcm().to_euler();
 			roll += gimablDirectionEuler(0);
 			pitch += gimablDirectionEuler(1);
 			yaw += gimablDirectionEuler(2);
 			updated = true;
 		}
 	}
 	/* consider mount mode if parameter is set */
 	if (_parameters.use_mnt > 0) {
 		switch (_mount_mode) {
 		case vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT:
 			out_mount_mode = -1.0f;
 			roll = 0.0f;
 			pitch = 0.0f;
 			yaw = 0.0f;
 			break;
 		case vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL:
 		case vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING:
 		case vehicle_command_s::VEHICLE_MOUNT_MODE_RC_TARGETING:
 		case vehicle_command_s::VEHICLE_MOUNT_MODE_GPS_POINT:
 			out_mount_mode = 1.0f;
 			break;
 		default:
 			out_mount_mode = -1.0f;
 		}
 	}
 	if (updated) {
 		struct actuator_controls_s controls;
 		// DEVICE_DEBUG("publishing | roll: %8.4f pitch: %8.4f yaw: %8.4f", (double)roll, (double)pitch, (double)yaw);
 		/* fill in the final control values */
 		controls.timestamp = hrt_absolute_time();
 		controls.control[0] = roll;
 		controls.control[1] = pitch;
 		controls.control[2] = yaw;
 		//controls.control[3] = ; // camera shutter
 		controls.control[4] = out_mount_mode;
 		/* publish it */
 		orb_publish(ORB_ID(actuator_controls_2), _actuator_controls_2_topic, &controls);
 	}
 	/* notify anyone waiting for data */
 	poll_notify(POLLIN);
 	/* schedule a fresh cycle call when the measurement is done */
 	work_queue(LPWORK,
 		   &_work,
 		   (worker_t)&Gimbal::cycle_trampoline,
 		   this,
 		   USEC2TICK(GIMBAL_UPDATE_INTERVAL));
 }
 void
 Gimbal::print_info()
 {
 }
 /**
 * Local functions in support of the shell command.
 */
 namespace gimbal
 {
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
 # undef ERROR
 #endif
 const int ERROR = -1;
 Gimbal	*g_dev;
 void	start();
 void	stop();
 void	test();
 void	reset();
 void	info();
 /**
 * Start the driver.
 */
 void
 start()
 {
 	if (g_dev != nullptr) {
 		errx(1, "already started");
 	}
 	/* create the driver */
 	g_dev = new Gimbal();
 	if (g_dev == nullptr) {
 		goto fail;
 	}
 	if (OK != g_dev->init()) {
 		goto fail;
 	}
 	exit(0);
 fail:
 	if (g_dev != nullptr) {
 		delete g_dev;
 		g_dev = nullptr;
 	}
 	errx(1, "driver start failed");
 }
 /**
 * Stop the driver
 */
 void stop()
 {
 	if (g_dev != nullptr) {
 		delete g_dev;
 		g_dev = nullptr;
 	} else {
 		errx(1, "driver not running");
 	}
 	exit(0);
 }
 /**
 * Perform some basic functional tests on the driver;
 * make sure we can collect data from the sensor in polled
 * and automatic modes.
 */
 void
 test()
 {
 	int fd = open(GIMBAL_DEVICE_PATH, O_RDONLY);
 	if (ioctl(fd, GIMBALIOCATTCOMPENSATE, 1) < 0) {
 		err(1, "failed enabling compensation");
 	}
 	errx(0, "PASS");
 }
 /**
 * Reset the driver.
 */
 void
 reset()
 {
 	int fd = open(GIMBAL_DEVICE_PATH, O_RDONLY);
 	if (fd < 0) {
 		err(1, "failed ");
 	}
 	// if (ioctl(fd, GIMBALIOCATTCOMPENSATE, 1) < 0) {
 	// 	err(1, "failed enabling compensation");
 	// }
 	exit(0);
 }
 /**
 * Print a little info about the driver.
 */
 void
 info()
 {
 	if (g_dev == nullptr) {
 		errx(1, "driver not running");
 	}
 	printf("state @ %p\n", g_dev);
 	g_dev->print_info();
 	exit(0);
 }
 } // namespace
 int
 gimbal_main(int argc, char *argv[])
 {
 	/*
 	 * Start/load the driver.
 	 */
 	if (!strcmp(argv[1], "start")) {
 		gimbal::start();
 	}
 	/*
 	 * Stop the driver
 	 */
 	if (!strcmp(argv[1], "stop")) {
 		gimbal::stop();
 	}
 	/*
 	 * Test the driver/device.
 	 */
 	if (!strcmp(argv[1], "test")) {
 		gimbal::test();
 	}
 	/*
 	 * Reset the driver.
 	 */
 	if (!strcmp(argv[1], "reset")) {
 		gimbal::reset();
 	}
 	/*
 	 * Print driver information.
 	 */
 	if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) {
 		gimbal::info();
 	}
 	errx(1, "unrecognized command, try 'start', 'stop', 'reset', 'test' or 'info'");
 }
--- a/src/drivers/gimbal/gimbal_params.c
+++ b/src/drivers/gimbal/gimbal_params.c
@ -1,74 +0,0 @@
 /****************************************************************************
 *
 *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * AS IS AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/
 /**
 * @file gimbal_params.c
 *
 * Parameters for the gimbal controller.
 *
 * @author Andreas Antener <andreas@uaventure.com>
 */
 #include <px4_config.h>
 #include <systemlib/param/param.h>
 /**
 * Consider mount operation mode.
 *
 * If set to 1, mount mode will be enforced.
 *
 * @boolean
 * @group Gimbal
 */
 PARAM_DEFINE_INT32(GMB_USE_MNT, 0);
 /**
 * Auxiliary switch to set mount operation mode.
 *
 * Set to 0 to disable manual mode control.
 *
 * If set to an auxiliary switch:
 * Switch off means the gimbal is put into safe/locked position.
 * Switch on means the gimbal can move freely, and landing gear
 * will be retracted if applicable.
 *
 * @value 0 Disable
 * @value 1 AUX1
 * @value 2 AUX2
 * @value 3 AUX3
 * @min 0
 * @max 3
 * @group Gimbal
 */
 PARAM_DEFINE_INT32(GMB_AUX_MNT_CHN, 0);
--- a/src/drivers/vmount/CMakeLists.txt
+++ b/src/drivers/vmount/CMakeLists.txt
@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2016 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@ -31,15 +31,18 @@
 #
 ############################################################################
 px4_add_module(
-	MODULE drivers__gimbal
+	MODULE drivers__vmount
-	MAIN gimbal
+	MAIN vmount
 	STACK_MAIN 1024
 	COMPILE_FLAGS
 		-Os
 	SRCS
-		gimbal.cpp
+        vmount.cpp
-		gimbal_params.c
+        vmount_params.c
        vmount_rc.cpp
        vmount_mavlink.cpp
        vmount_onboard.cpp
 	DEPENDS
 		platforms__common
 	)
-# vim: set noet ft=cmake fenc=utf-8 ff=unix : 
+ # vim: set noet ft=cmake fenc=utf-8 ff=unix :
--- a/src/drivers/vmount/vmount.cpp
+++ b/src/drivers/vmount/vmount.cpp
@ -0,0 +1,692 @@
 /****************************************************************************
 *
 *   Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/
 /**
 * @file vmount.cpp
 * @author Leon Müller (thedevleon)
 * MAV_MOUNT driver for controlling mavlink gimbals, rc gimbals/servors and
 * future kinds of mounts.
 *
 */
 #include <stdlib.h>
 #include <stdio.h>
 #include <stdbool.h>
 #include <string.h>
 #include <sys/types.h>
 #include <fcntl.h>
 #include <unistd.h>
 #include <systemlib/err.h>
 #include <systemlib/systemlib.h>
 #include "vmount_mavlink.h"
 #include "vmount_rc.h"
 #include "vmount_onboard.h"
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_roi.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_command_ack.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <px4_config.h>
 #include <px4_posix.h>
 #include <poll.h>
 /* thread state */
 static volatile bool thread_should_exit = false;
 static volatile bool thread_running = false;
 static volatile bool thread_should_restart = false;
 static int vmount_task;
 typedef enum { IDLE = -1, MAVLINK = 0, RC = 1, ONBOARD = 2 } vmount_state_t;
 static vmount_state_t vmount_state = IDLE;
 /* polling */
 px4_pollfd_struct_t polls[7];
 /* functions */
 static void usage(void);
 static void vmount_update_topics(void);
 static void update_params(void);
 static bool get_params(void);
 static float get_aux_value(int);
 static void ack_mount_command(uint16_t command);
 static int vmount_thread_main(int argc, char *argv[]);
 extern "C" __EXPORT int vmount_main(int argc, char *argv[]);
 /* uORB subscriptions */
 static int vehicle_roi_sub = -1;
 static int vehicle_global_position_sub = -1;
 static int vehicle_command_sub = -1;
 static int vehicle_attitude_sub = -1;
 static int position_setpoint_triplet_sub = -1;
 static int manual_control_setpoint_sub = -1;
 static int parameter_update_sub = -1;
 /* uORB publication */
 static orb_advert_t vehicle_command_ack_pub;
 static struct vehicle_roi_s vehicle_roi;
 static bool   vehicle_roi_updated;
 static struct vehicle_global_position_s vehicle_global_position;
 static bool   vehicle_global_position_updated;
 static struct vehicle_command_s vehicle_command;
 static bool   vehicle_command_updated;
 static struct vehicle_attitude_s vehicle_attitude;
 static bool   vehicle_attitude_updated;
 static struct position_setpoint_triplet_s position_setpoint_triplet;
 static bool   position_setpoint_triplet_updated;
 static struct manual_control_setpoint_s manual_control_setpoint;
 static bool   manual_control_setpoint_updated;
 static struct parameter_update_s parameter_update;
 static bool   parameter_update_updated;
 static struct vehicle_command_ack_s vehicle_command_ack;
 static struct {
 	int mnt_mode;
 	int mnt_mav_sysid;
 	int mnt_mav_compid;
 	int mnt_ob_lock_mode;
 	int mnt_ob_norm_mode;
 	int mnt_man_control;
 	int mnt_man_pitch;
 	int mnt_man_roll;
 	int mnt_man_yaw;
 	int mnt_mode_ovr;
 } params;
 static struct {
 	param_t mnt_mode;
 	param_t mnt_mav_sysid;
 	param_t mnt_mav_compid;
 	param_t mnt_ob_lock_mode;
 	param_t mnt_ob_norm_mode;
 	param_t mnt_man_control;
 	param_t mnt_man_pitch;
 	param_t mnt_man_roll;
 	param_t mnt_man_yaw;
 	param_t mnt_mode_ovr;
 } params_handels;
 static bool manual_control_desired;
 /**
 * Print command usage information
 */
 static void usage()
 {
 	fprintf(stderr,
 		"usage: vmount start\n"
 		"       vmount stop\n"
 		"       vmount status\n");
 	exit(1);
 }
 /**
 * The daemon thread.
 */
 static int vmount_thread_main(int argc, char *argv[])
 {
 	if (!get_params()) {
 		warnx("could not get mount parameters!");
 	}
 	if (params.mnt_mode == 0) { vmount_state = IDLE;}
 	else if (params.mnt_mode == 1) { vmount_state = MAVLINK;}
 	else if (params.mnt_mode == 2) { vmount_state = RC;}
 	else if (params.mnt_mode == 3) { vmount_state = ONBOARD;}
 	//TODO is this needed?
 	memset(&vehicle_roi, 0, sizeof(vehicle_roi));
 	memset(&vehicle_global_position, 0, sizeof(vehicle_global_position));
 	memset(&vehicle_command, 0, sizeof(vehicle_command));
 	memset(&vehicle_attitude, 0, sizeof(vehicle_attitude));
 	memset(&position_setpoint_triplet, 0, sizeof(position_setpoint_triplet));
 	memset(&manual_control_setpoint, 0, sizeof(manual_control_setpoint));
 	memset(&parameter_update, 0, sizeof(parameter_update));
 	memset(&vehicle_command_ack, 0, sizeof(vehicle_command_ack));
 	vehicle_roi_sub = orb_subscribe(ORB_ID(vehicle_roi));
 	vehicle_global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
 	vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command));
 	vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
 	position_setpoint_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
 	manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
 	parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
 	vehicle_command_ack_pub = orb_advertise(ORB_ID(vehicle_command_ack), &vehicle_command_ack);
 	if (!vehicle_roi_sub || !position_setpoint_triplet_sub ||
 	    !manual_control_setpoint_sub || !vehicle_global_position_sub ||
 	    !vehicle_command_sub || !parameter_update_sub || !vehicle_command_ack_pub ||
 		!vehicle_attitude_sub) {
 		err(1, "could not subscribe to uORB topics");
 	}
 	polls[0].fd = 		vehicle_roi_sub;
 	polls[0].events = 	POLLIN;
 	polls[1].fd = 		vehicle_global_position_sub;
 	polls[1].events = 	POLLIN;
 	polls[2].fd = 		vehicle_attitude_sub;
 	polls[2].events = 	POLLIN;
 	polls[3].fd = 		vehicle_command_sub;
 	polls[3].events = 	POLLIN;
 	polls[4].fd = 		position_setpoint_triplet_sub;
 	polls[4].events = 	POLLIN;
 	polls[5].fd = 		manual_control_setpoint_sub;
 	polls[5].events = 	POLLIN;
 	polls[6].fd = 		parameter_update_sub;
 	polls[6].events = 	POLLIN;
 	thread_running = true;
 	run: {
 		if (vmount_state == MAVLINK) {
 			if (!vmount_mavlink_init()) {
 				err(1, "could not initiate vmount_mavlink");
 			}
 			warnx("running mount driver in mavlink mode");
 			if (params.mnt_man_control) manual_control_desired = true;
 			while (!thread_should_exit && !thread_should_restart) {
 				vmount_update_topics();
 				if (vehicle_roi_updated) {
 					vehicle_roi_updated = false;
 					vmount_mavlink_configure(vehicle_roi.mode, (params.mnt_man_control == 1), params.mnt_mav_sysid, params.mnt_mav_compid);
 					if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_NONE) {
 						if (params.mnt_man_control) manual_control_desired = true;
 					} else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_WPNEXT) {
 						manual_control_desired = false;
 						vmount_mavlink_set_location(
 							position_setpoint_triplet.next.lat,
 							position_setpoint_triplet.next.lon,
 							position_setpoint_triplet.next.alt
 						);
 					} else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_WPINDEX) {
 						manual_control_desired = false;
 						//TODO how to do this?
 					} else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_LOCATION) {
 						manual_control_desired = false;
 						vmount_mavlink_set_location(
 							vehicle_roi.lat,
 							vehicle_roi.lon,
 							vehicle_roi.alt
 						);
 					} else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_TARGET) {
 						manual_control_desired = false;
 						//TODO is this even suported?
 					}
 				}
 				else if (manual_control_desired && manual_control_setpoint_updated)
 				{
 					manual_control_setpoint_updated = false;
 					vmount_mavlink_point_manual(get_aux_value(params.mnt_man_pitch), get_aux_value(params.mnt_man_roll), get_aux_value(params.mnt_man_yaw));
 				}
 				else if (!manual_control_desired && vehicle_global_position_updated)
 				{
 					vehicle_global_position_updated = false;
 					vmount_mavlink_point(vehicle_global_position.lat, vehicle_global_position.lon, vehicle_global_position.alt);
 				}
 			vmount_mavlink_deinit();
 			}
 		} else if (vmount_state == RC) {
 			if (!vmount_rc_init()) {
 				err(1, "could not initiate vmount_rc");
 			}
 			warnx("running mount driver in rc mode");
 			if (params.mnt_man_control) {
 				manual_control_desired = true;
 			}
 			while (!thread_should_exit && !thread_should_restart) {
 				vmount_update_topics();
 				if (vehicle_roi_updated) {
 					vehicle_roi_updated = false;
 					vmount_rc_configure(vehicle_roi.mode, (params.mnt_man_control == 1), params.mnt_ob_norm_mode, params.mnt_ob_lock_mode);
 					if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_NONE) {
 						if (params.mnt_man_control) manual_control_desired = true;
 					} else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_WPNEXT) {
 						manual_control_desired = false;
 						vmount_rc_set_location(
 							position_setpoint_triplet.next.lat,
 							position_setpoint_triplet.next.lon,
 							position_setpoint_triplet.next.alt
 						);
 					} else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_WPINDEX) {
 						manual_control_desired = false;
 						//TODO how to do this?
 					} else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_LOCATION) {
 						manual_control_desired = false;
 						vmount_rc_set_location(
 							vehicle_roi.lat,
 							vehicle_roi.lon,
 							vehicle_roi.alt
 						);
 					} else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_TARGET) {
 						manual_control_desired = false;
 						//TODO is this even suported?
 					}
 				}
 				else if (manual_control_desired && manual_control_setpoint_updated) {
 					manual_control_setpoint_updated = false;
 					vmount_rc_point_manual(get_aux_value(params.mnt_man_pitch), get_aux_value(params.mnt_man_roll), get_aux_value(params.mnt_man_yaw));
 				}
 				else if (!manual_control_desired && vehicle_global_position_updated)
 				{
 					vehicle_global_position_updated = false;
 					vmount_rc_point(vehicle_global_position.lat, vehicle_global_position.lon, vehicle_global_position.alt);
 				}
 			}
 			vmount_rc_deinit();
 		} else if (vmount_state == ONBOARD) {
 			if (!vmount_onboard_init()) {
 				err(1, "could not initiate vmount_onboard");
 			}
 			warnx("running mount driver in onboard mode");
 			if (params.mnt_man_control) manual_control_desired = true;
 			while (!thread_should_exit && !thread_should_restart) {
 				vmount_update_topics();
 				if(vehicle_attitude_updated)
 				{
 					vmount_onboard_update_attitude(vehicle_attitude);
 					vehicle_attitude_updated = false;
 				}
 				if (params.mnt_mode_ovr && manual_control_setpoint_updated) {
 					manual_control_setpoint_updated = false;
 					float ovr_value = get_aux_value(params.mnt_mode_ovr);
 					if(ovr_value < 0.0f)
 					{
 						vmount_onboard_set_mode(vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT);
 					}
 					else if (ovr_value > 0.0f)
 					{
 						vmount_onboard_set_mode(vehicle_command_s::VEHICLE_MOUNT_MODE_RC_TARGETING);
 					}
 				}
 				else if (vehicle_command_updated) {
 					vehicle_command_updated = false;
 					if(vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL)
 					{
 						switch ((int)vehicle_command.param7) {
 						case vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT:
 						case vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL:
 							manual_control_desired = false;
 							break;
 						case vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING:
 							manual_control_desired = false;
 							vmount_onboard_set_mode(vehicle_command.param7);
 							vmount_onboard_set_manual(vehicle_command.param1,
 										 vehicle_command.param2, vehicle_command.param3);
 						case vehicle_command_s::VEHICLE_MOUNT_MODE_RC_TARGETING:
 							if (params.mnt_man_control) {
 								manual_control_desired = true;
 								vmount_onboard_set_mode(vehicle_command.param7);
 							}
 						case vehicle_command_s::VEHICLE_MOUNT_MODE_GPS_POINT:
 							manual_control_desired = false;
 							vmount_onboard_set_mode(vehicle_command.param7);
 							vmount_onboard_set_location(
 								vehicle_command.param1,
 								vehicle_command.param2,
 								vehicle_command.param3);
 						default:
 							manual_control_desired = false;
 							break;
 						}
 						ack_mount_command(vehicle_command.command);
 					}
 					else if(vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE)
 					{
 						vmount_onboard_configure(vehicle_command.param1,
 									((uint8_t) vehicle_command.param2 == 1), ((uint8_t) vehicle_command.param3 == 1), ((uint8_t) vehicle_command.param4 == 1));
 						ack_mount_command(vehicle_command.command);
 					}
 				}
 				else if (manual_control_desired && manual_control_setpoint_updated)
 				{
 					manual_control_setpoint_updated = false;
 					vmount_onboard_point_manual(get_aux_value(params.mnt_man_pitch), get_aux_value(params.mnt_man_roll), get_aux_value(params.mnt_man_yaw));
 				}
 				else if (!manual_control_desired && vehicle_global_position_updated)
 				{
 					vehicle_global_position_updated = false;
 					vmount_onboard_point(vehicle_global_position.lat, vehicle_global_position.lon, vehicle_global_position.alt);
 				}
 			}
 		} else if (vmount_state == IDLE)
 		{
 			warnx("running mount driver in idle mode");
 			while (!thread_should_exit && !thread_should_restart) {
 				vmount_update_topics();
 			}
 		}
 	}
 	if (thread_should_restart)
 	{
 		thread_should_restart = false;
 		warnx("parameter update, restarting mount driver!");
 		goto run;
 	}
 	thread_running = false;
 	return 0;
 }
 /**
 * The main command function.
 * Processes command line arguments and starts the daemon.
 */
 int vmount_main(int argc, char *argv[])
 {
 	if (argc < 1) {
 		warnx("missing command");
 		usage();
 	}
 	if (!strcmp(argv[1], "start")) {
 		/* this is not an error */
 		if (thread_running) {
 			errx(0, "mount driver already running");
 		}
 		thread_should_exit = false;
 		vmount_task = px4_task_spawn_cmd("vmount",
 						SCHED_DEFAULT,
 						SCHED_PRIORITY_DEFAULT + 40, //TODO we might want a higher priority?
 						1100,
 						vmount_thread_main,
 						(char *const *)argv);
 		while (!thread_running) {
 			usleep(200);
 		}
 		exit(0);
 	}
 	if (!strcmp(argv[1], "stop")) {
 		/* this is not an error */
 		if (!thread_running) {
 			errx(0, "mount driver already stopped");
 		}
 		thread_should_exit = true;
 		while (thread_running) {
 			usleep(1000000);
 			warnx(".");
 		}
 		warnx("terminated.");
 		exit(0);
 	}
 	if (!strcmp(argv[1], "status")) {
 		if (thread_running) {
 			switch (vmount_state) {
 			case IDLE:
 				errx(0, "running: IDLE");
 				break;
 			case MAVLINK:
 				errx(0, "running: MAVLINK - Manual Control? %d", manual_control_desired);
 				break;
 			case RC:
 				errx(0, "running: RC - Manual Control? %d", manual_control_desired);
 				break;
 			case ONBOARD:
 				errx(0, "running: ONBOARD");
 				break;
 			}
 		} else {
 			errx(1, "not running");
 		}
 	}
 	warnx("unrecognized command");
 	usage();
 	/* not getting here */
 	return 0;
 }
 /* Update oURB topics */
 void vmount_update_topics()
 {
 	/*
 	polls = {
 		{ .fd = vehicle_roi_sub,   .events = POLLIN },
 		{ .fd = vehicle_global_position_sub,   .events = POLLIN },
 		{ .fd = vehicle_attitude_sub,   .events = POLLIN },
 		{ .fd = vehicle_command_sub,   .events = POLLIN },
 		{ .fd = position_setpoint_triplet_sub,   .events = POLLIN },
 		{ .fd = manual_control_setpoint_sub,   .events = POLLIN },
 		{ .fd = parameter_update_sub,   .events = POLLIN },
 	};
 	*/
 	/* wait for sensor update of 7 file descriptors for 100 ms */
 	int poll_ret = px4_poll(polls, 7, 100);
 	//Nothing updated.
 	if(poll_ret == 0) return;
 	if (polls[0].revents & POLLIN) {
 		orb_copy(ORB_ID(vehicle_roi), vehicle_roi_sub, &vehicle_roi);
 		vehicle_roi_updated = true;
 	}
 	if (polls[1].revents & POLLIN) {
 		orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &vehicle_global_position);
 		vehicle_global_position_updated = true;
 	}
 	if (polls[2].revents & POLLIN) {
 		orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &vehicle_attitude);
 		vehicle_attitude_updated = true;
 	}
 	if (polls[3].revents & POLLIN) {
 		orb_copy(ORB_ID(vehicle_command), vehicle_command_sub, &vehicle_command);
 		vehicle_command_updated = true;
 	}
 	if (polls[4].revents & POLLIN) {
 		orb_copy(ORB_ID(position_setpoint_triplet), position_setpoint_triplet_sub, &position_setpoint_triplet);
 		position_setpoint_triplet_updated = true;
 	}
 	if (polls[5].revents & POLLIN) {
 		orb_copy(ORB_ID(manual_control_setpoint), manual_control_setpoint_sub, &manual_control_setpoint);
 		manual_control_setpoint_updated = true;
 	}
 	if (polls[6].revents & POLLIN) {
 		orb_copy(ORB_ID(parameter_update), parameter_update_sub, &parameter_update);
 		parameter_update_updated = true;
 		update_params();
 	}
 }
 void update_params()
 {
 	param_get(params_handels.mnt_mode, &params.mnt_mode);
 	param_get(params_handels.mnt_mav_sysid, &params.mnt_mav_sysid);
 	param_get(params_handels.mnt_mav_compid, &params.mnt_mav_compid);
 	param_get(params_handels.mnt_ob_lock_mode, &params.mnt_ob_lock_mode);
 	param_get(params_handels.mnt_ob_norm_mode, &params.mnt_ob_norm_mode);
 	param_get(params_handels.mnt_man_control, &params.mnt_man_control);
 	param_get(params_handels.mnt_man_pitch, &params.mnt_man_pitch);
 	param_get(params_handels.mnt_man_roll, &params.mnt_man_roll);
 	param_get(params_handels.mnt_man_yaw, &params.mnt_man_yaw);
 	param_get(params_handels.mnt_mode_ovr, &params.mnt_mode_ovr);
 	if (vmount_state != params.mnt_mode)
 	{
 		thread_should_restart = true;
 	}
 	else if (vmount_state == MAVLINK)
 	{
 		vmount_mavlink_configure(vehicle_roi.mode, (params.mnt_man_control == 1), params.mnt_mav_sysid, params.mnt_mav_compid);
 	}
 	else if (vmount_state == RC)
 	{
 		vmount_rc_configure(vehicle_roi.mode, (params.mnt_man_control == 1), params.mnt_ob_norm_mode, params.mnt_ob_lock_mode);
 	}
 	else if(vmount_state == ONBOARD)
 	{
 		//None of the parameter changes require a reconfiguration of the onboard mount.
 	}
 }
 bool get_params()
 {
 	params_handels.mnt_mode = param_find("MNT_MODE");
 	params_handels.mnt_mav_sysid = param_find("MNT_MAV_SYSID");
 	params_handels.mnt_mav_compid = param_find("MNT_MAV_COMPID");
 	params_handels.mnt_ob_lock_mode = param_find("MNT_OB_LOCK_MODE");
 	params_handels.mnt_ob_norm_mode = param_find("MNT_OB_NORM_MODE");
 	params_handels.mnt_man_control = param_find("MNT_MAN_CONTROL");
 	params_handels.mnt_man_pitch = param_find("MNT_MAN_PITCH");
 	params_handels.mnt_man_roll = param_find("MNT_MAN_ROLL");
 	params_handels.mnt_man_yaw = param_find("MNT_MAN_YAW");
 	params_handels.mnt_mode_ovr = param_find("MNT_MODE_OVR");
 	if (param_get(params_handels.mnt_mode, &params.mnt_mode) ||
 	    param_get(params_handels.mnt_mav_sysid, &params.mnt_mav_sysid) ||
 	    param_get(params_handels.mnt_mav_compid, &params.mnt_mav_compid) ||
 		param_get(params_handels.mnt_ob_lock_mode, &params.mnt_ob_lock_mode) ||
 		param_get(params_handels.mnt_ob_norm_mode, &params.mnt_ob_norm_mode) ||
 	    param_get(params_handels.mnt_man_control, &params.mnt_man_control) ||
 	    param_get(params_handels.mnt_man_pitch, &params.mnt_man_pitch) ||
 		param_get(params_handels.mnt_man_roll, &params.mnt_man_roll) ||
 	    param_get(params_handels.mnt_man_yaw, &params.mnt_man_yaw) ||
 		param_get(params_handels.mnt_mode_ovr, &params.mnt_mode_ovr)) {
 		return false;
 	}
 	return true;
 }
 void ack_mount_command(uint16_t command)
 {
 	vehicle_command_ack.command = command;
 	vehicle_command_ack.result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
 	orb_publish(ORB_ID(vehicle_command_ack), vehicle_command_ack_pub, &vehicle_command_ack);
 }
 float get_aux_value(int aux)
 {
 	switch (aux)
 	{
 		case 0:
 			return 0.0f;
 		case 1:
 			return manual_control_setpoint.aux1;
 		case 2:
 			return manual_control_setpoint.aux2;
 		case 3:
 			return manual_control_setpoint.aux3;
 		case 4:
 			return manual_control_setpoint.aux4;
 		case 5:
 			return manual_control_setpoint.aux5;
 		default:
 			return 0.0f;
 	}
 }
--- a/src/drivers/vmount/vmount_mavlink.cpp
+++ b/src/drivers/vmount/vmount_mavlink.cpp
@ -0,0 +1,178 @@
 /****************************************************************************
 *
 *   Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/
 /**
 * @file vmount_mavlink.cpp
 * @author Leon Müller (thedevleon)
 *
 */
 #include "vmount_mavlink.h"
 #include <stdlib.h>
 #include <stdio.h>
 #include <string.h>
 #include <arch/math.h>
 #include <geo/geo.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_roi.h>
 #include <px4_posix.h>
 /* uORB advertising */
 static struct vehicle_command_s *vehicle_command;
 static orb_advert_t vehicle_command_pub;
 static int sys_id;
 static int comp_id;
 static double lat;
 static double lon;
 static float alt;
 bool vmount_mavlink_init()
 {
 	memset(&vehicle_command, 0, sizeof(vehicle_command));
 	vehicle_command_pub = orb_advertise(ORB_ID(vehicle_command), &vehicle_command);
 	if (!vehicle_command_pub) { return false; }
 	return true;
 }
 void vmount_mavlink_deinit()
 {
 	orb_unadvertise(vehicle_command_pub);
 	free(vehicle_command);
 }
 /*
 * MAV_CMD_DO_MOUNT_CONFIGURE (#204)
 *   param1 = mount_mode (1 = MAV_MOUNT_MODE_NEUTRAL recenters camera)
 */
 void vmount_mavlink_configure(int roi_mode, bool man_control, int sysid, int compid)
 {
 	sys_id = sysid;
 	comp_id = compid;
 	vehicle_command->command = vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE;
 	vehicle_command->target_system = sys_id;
 	vehicle_command->target_component = comp_id;
 	switch (roi_mode) {
 	case vehicle_roi_s::VEHICLE_ROI_NONE:
 		if (man_control) { vehicle_command->param1 = vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING; }
 		else { vehicle_command->param1 = vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL; }
 		break;
 	case vehicle_roi_s::VEHICLE_ROI_WPNEXT:
 		vehicle_command->param1 = vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING;
 		break;
 	case vehicle_roi_s::VEHICLE_ROI_WPINDEX:
 		vehicle_command->param1 = vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING;
 		break;
 	case vehicle_roi_s::VEHICLE_ROI_LOCATION:
 		vehicle_command->param1 = vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING;
 		break;
 	case vehicle_roi_s::VEHICLE_ROI_TARGET:
 		vehicle_command->param1 = vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING;
 		break;
 	default:
 		vehicle_command->param1 = vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL;
 		break;
 	}
 	orb_publish(ORB_ID(vehicle_command), vehicle_command_pub, vehicle_command);
 }
 /* MAV_CMD_DO_MOUNT_CONTROL (#205)
 *  param1 = pitch, angle in degree or pwm input
 *  param2 = roll, angle in degree or pwm input
 *  param3 = yaw, angle in degree or pwm input
 *  param6 = typeflags (uint16_t)
 *   0x0001: pitch, 0: unlimited, 1: limited, see CMD_SETANGLE
 *   0x0002: roll, 0: unlimited, 1: limited, see CMD_SETANGLE
 *   0x0004: yaw, 0: unlimited, 1: limited, see CMD_SETANGLE
 *   0x0100: input type, 0: angle input (see CMD_SETANGLE), 1: pwm input (see CMD_SETPITCHROLLYAW)
 */
 void vmount_mavlink_set_location(double lat_new, double lon_new, float alt_new)
 {
 	lat = lat_new;
 	lon = lon_new;
 	alt = alt_new;
 }
 void vmount_mavlink_point(double global_lat, double global_lon, float global_alt)
 {
 	float pitch = vmount_mavlink_calculate_pitch(global_lat, global_lon, global_alt);
 	float roll = 0.0f; // We want a level horizon, so leave roll at 0 degrees.
 	float yaw = get_bearing_to_next_waypoint(global_lat, global_lon, lat, lon) * (float)M_RAD_TO_DEG;
 	vmount_mavlink_point_manual(pitch, roll, yaw);
 }
 void vmount_mavlink_point_manual(float pitch_new, float roll_new, float yaw_new)
 {
 	vehicle_command->command = vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL;
 	vehicle_command->target_system = sys_id;
 	vehicle_command->target_component = comp_id;
 	vehicle_command->param1 = pitch_new;
 	vehicle_command->param2 = roll_new;
 	vehicle_command->param3 = yaw_new;
 	orb_publish(ORB_ID(vehicle_command), vehicle_command_pub, vehicle_command);
 }
 float vmount_mavlink_calculate_pitch(double global_lat, double global_lon, float global_alt)
 {
 	float x = (lon - global_lon) * cos(M_DEG_TO_RAD * ((global_lat + lat) * 0.00000005)) * 0.01113195;
 	float y = (lat - global_lat) * 0.01113195;
 	float z = (alt - global_alt);
 	float target_distance = sqrtf(powf(x, 2) + powf(y, 2));
 	return atan2(z, target_distance) * M_RAD_TO_DEG;
 }
--- a/src/drivers/vmount/vmount_mavlink.h
+++ b/src/drivers/vmount/vmount_mavlink.h
@ -0,0 +1,56 @@
 /****************************************************************************
 *
 *   Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/
 /**
 * @file vmount_mavlink.h
 * @author Leon Müller (thedevleon)
 *
 */
 #ifndef _VMOUNT_MAVLINK_H
 #define _VMOUNT_MAVLINK_H
 #include <sys/types.h>
 #include <stdbool.h>
 // Public functions
 bool vmount_mavlink_init();
 void vmount_mavlink_deinit(void);
 void vmount_mavlink_configure(int roi_mode, bool man_control, int sysid, int compid);
 void vmount_mavlink_set_location(double lat_new, double lon_new, float alt_new);
 void vmount_mavlink_point(double global_lat, double global_lon, float global_alt);
 void vmount_mavlink_point_manual(float pitch_new, float roll_new, float yaw_new);
 float vmount_mavlink_calculate_pitch(double global_lat, double global_lon, float global_alt);
 #endif /* _VMOUNT_MAVLINK_H */
--- a/src/drivers/vmount/vmount_onboard.cpp
+++ b/src/drivers/vmount/vmount_onboard.cpp
@ -0,0 +1,193 @@
 /****************************************************************************
 *
 *   Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/
 /**
 * @file vmount_onboard.cpp
 * @author Leon Müller (thedevleon)
 *
 */
 #include "vmount_onboard.h"
 #include <stdlib.h>
 #include <stdio.h>
 #include <string.h>
 #include <arch/math.h>
 #include <geo/geo.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_command.h>
 /* uORB topics */
 static struct actuator_controls_s actuator_controls;
 static orb_advert_t actuator_controls_pub;
 static struct vehicle_attitude_s vehicle_attitude;
 static int mount_mode;
 static float pitch;
 static float roll;
 static float yaw;
 static float retracts;
 static int stab_pitch;
 static int stab_roll;
 static int stab_yaw;
 static double lat;
 static double lon;
 static float alt;
 bool vmount_onboard_init()
 {
 	memset(&actuator_controls, 0, sizeof(actuator_controls));
 	memset(&vehicle_attitude, 0, sizeof(vehicle_attitude));
 	actuator_controls_pub = orb_advertise(ORB_ID(actuator_controls_2), &actuator_controls);
 	if (!actuator_controls_pub) { return false; }
 	vmount_onboard_configure(vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT, 0.0f, 0.0f, 0.0f);
 	return true;
 }
 void vmount_onboard_deinit()
 {
 	orb_unadvertise(actuator_controls_pub);
 	//free(actuator_controls);
 	//free(vehicle_attitude);
 }
 void vmount_onboard_configure(int new_mount_mode, bool new_stab_roll, bool new_stab_pitch, bool new_stab_yaw)
 {
 	mount_mode = new_mount_mode;
 	stab_roll = new_stab_roll;
 	stab_pitch = new_stab_pitch;
 	stab_yaw = new_stab_yaw;
 	switch (mount_mode) {
 	case vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT:
 		retracts = -1.0f;
 		vmount_onboard_set_manual(0.0f, 0.0f, 0.0f);
 		break;
 	case vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL:
 		retracts = 0.0f;
 		vmount_onboard_set_manual(0.0f, 0.0f, 0.0f);
 		break;
 	case vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING:
 	case vehicle_command_s::VEHICLE_MOUNT_MODE_RC_TARGETING:
 	case vehicle_command_s::VEHICLE_MOUNT_MODE_GPS_POINT:
 		retracts = 0.0f;
 	default:
 		break;
 	}
 }
 void vmount_onboard_set_location(double lat_new, double lon_new, float alt_new)
 {
 	lat = lat_new;
 	lon = lon_new;
 	alt = alt_new;
 }
 void vmount_onboard_set_manual(double pitch_new, double roll_new, float yaw_new)
 {
 	pitch = pitch_new;
 	roll = roll_new;
 	yaw = yaw_new;
 }
 void vmount_onboard_set_mode(int new_mount_mode)
 {
 	vmount_onboard_configure(new_mount_mode, stab_roll, stab_pitch, stab_yaw);
 }
 void vmount_onboard_point(double global_lat, double global_lon, float global_alt)
 {
 	if (mount_mode == vehicle_command_s::VEHICLE_MOUNT_MODE_GPS_POINT) {
 		pitch = vmount_onboard_calculate_pitch(global_lat, global_lon, global_alt);
 		roll = 0.0f; // We want a level horizon, so leave roll at 0 degrees.
 		yaw = get_bearing_to_next_waypoint(global_lat, global_lon, lat, lon) * (float)M_RAD_TO_DEG;
 	}
 	vmount_onboard_point_manual(pitch, roll, yaw);
 }
 void vmount_onboard_point_manual(float pitch_target, float roll_target, float yaw_target)
 {
 	float pitch_new = pitch_target;
 	float roll_new = roll_target;
 	float yaw_new = yaw_target;
 	if (mount_mode != vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT &&
 	    mount_mode != vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL) {
 		if (stab_roll) {
 			roll_new += 1.0f / M_PI_F * -vehicle_attitude.roll;
 		}
 		if (stab_pitch) {
 			pitch_new += 1.0f / M_PI_F * -vehicle_attitude.pitch;
 		}
 		if (stab_yaw) {
 			yaw_new += 1.0f / M_PI_F * vehicle_attitude.yaw;
 		}
 	}
 	actuator_controls.timestamp = hrt_absolute_time();
 	actuator_controls.control[0] = pitch_new;
 	actuator_controls.control[1] = roll_new;
 	actuator_controls.control[2] = yaw_new;
 	actuator_controls.control[4] = retracts;
 	orb_publish(ORB_ID(actuator_controls_2), actuator_controls_pub, &actuator_controls);
 }
 void vmount_onboard_update_attitude(vehicle_attitude_s vehicle_attitude_new)
 {
 	vehicle_attitude = vehicle_attitude_new;
 }
 float vmount_onboard_calculate_pitch(double global_lat, double global_lon, float global_alt)
 {
 	float x = (lon - global_lon) * cos(M_DEG_TO_RAD * ((global_lat + lat) * 0.00000005)) * 0.01113195;
 	float y = (lat - global_lat) * 0.01113195;
 	float z = (alt - global_alt);
 	float target_distance = sqrtf(powf(x, 2) + powf(y, 2));
 	return atan2(z, target_distance) * M_RAD_TO_DEG;
 }
--- a/src/drivers/vmount/vmount_onboard.h
+++ b/src/drivers/vmount/vmount_onboard.h
@ -0,0 +1,59 @@
 /****************************************************************************
 *
 *   Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/
 /**
 * @file vmount_onboard
 * @author Leon Müller (thedevleon)
 *
 */
 #ifndef _VMOUNT_ONBOARD_H
 #define _VMOUNT_ONBOARD_H
 #include <sys/types.h>
 #include <stdbool.h>
 #include <uORB/topics/vehicle_attitude.h>
 // Public functions
 bool vmount_onboard_init(void);
 void vmount_onboard_deinit(void);
 void vmount_onboard_configure(int new_mount_mode, bool new_stab_roll, bool new_stab_pitch, bool new_stab_yaw);
 void vmount_onboard_set_location(double lat, double lon, float alt);
 void vmount_onboard_set_manual(double pitch_new, double roll_new, float yaw_new);
 void vmount_onboard_set_mode(int new_mount_mode);
 void vmount_onboard_point(double global_lat, double global_lon, float global_alt);
 void vmount_onboard_point_manual(float pitch_target, float roll_target, float yaw_target);
 void vmount_onboard_update_attitude(vehicle_attitude_s vehicle_attitude_new);
 float vmount_onboard_calculate_pitch(double global_lat, double global_lon, float global_alt);
 #endif /* _VMOUNT_ONBOARD_H */
--- a/src/drivers/vmount/vmount_params.c
+++ b/src/drivers/vmount/vmount_params.c
@ -0,0 +1,164 @@
 /****************************************************************************
 *
 *   Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/
 /**
 * @file vmount_params.c
 * @author Leon Müller (thedevleon)
 *
 */
 #include <px4_config.h>
 #include <systemlib/param/param.h>
 /**
 * Mount operation mode
 * MAVLINK and RC use the ROI (or RC input if enabled and no ROI set) to control a mount.
 * ONBOARD uses MAV_CMD_DO_MOUNT_CONFIGURE and MAV_CMD_DO_MOUNT_CONTROL MavLink messages
 * to control a mount.
 * @value 0 DISABLE
 * @value 1 MAVLINK
 * @value 2 RC
 * @value 3 ONBOARD
 * @min 0
 * @max 3
 * @group Mount
 */
 PARAM_DEFINE_INT32(MNT_MODE, 0);
 /**
 * Auxiliary channel to override current mount mode (only in ONBOARD mode)
 * if <0.0f then MODE_RETRACT (retracts not retracted)
 * if =0.0f then don't override
 * if >0.0f then MODE_RC_TARGETING (retracts retracted)
 * @value 0 Disable
 * @value 1 AUX1
 * @value 2 AUX2
 * @value 3 AUX3
 * @value 4 AUX4
 * @value 5 AUX5
 * @min 0
 * @max 5
 * @group Mount
 */
 PARAM_DEFINE_INT32(MNT_MODE_OVR, 0);
 /**
 * Mavlink System ID
 *
 * @group Mount
 */
 PARAM_DEFINE_INT32(MNT_MAV_SYSID, 71);
 /**
 * Mavlink Component ID
 *
 * @group Mount
 */
 PARAM_DEFINE_INT32(MNT_MAV_COMPID, 67);
 /**
 * Mixer value for selecting normal mode
 * if required by the gimbal (only in RC mode)
 * @min -1.0
 * @max 1.0
 * @decimal 3
 * @group Mount
 */
 PARAM_DEFINE_FLOAT(MNT_OB_NORM_MODE, -1.0f);
 /**
 * Mixer value for selecting a locking mode
 * if required for the gimbal (only in RC mode)
 * @min -1.0
 * @max 1.0
 * @decimal 3
 * @group Mount
 */
 PARAM_DEFINE_FLOAT(MNT_OB_LOCK_MODE, 0.0f);
 /**
 * This enables the mount to be controlled when no ROI is set.
 *
 * If set to 1, the mount will be controlled by the AUX channels below
 * when no ROI is set.
 *
 * @boolean
 * @group Mount
 */
 PARAM_DEFINE_INT32(MNT_MAN_CONTROL, 0);
 /**
 * Auxiliary channel to control roll.
 *
 * @value 0 Disable
 * @value 1 AUX1
 * @value 2 AUX2
 * @value 3 AUX3
 * @value 4 AUX4
 * @value 5 AUX5
 * @min 0
 * @max 5
 * @group Mount
 */
 PARAM_DEFINE_INT32(MNT_MAN_ROLL, 0);
 /**
 * Auxiliary channel to control pitch.
 *
 * @value 0 Disable
 * @value 1 AUX1
 * @value 2 AUX2
 * @value 3 AUX3
 * @value 4 AUX4
 * @value 5 AUX5
 * @min 0
 * @max 5
 * @group Mount
 */
 PARAM_DEFINE_INT32(MNT_MAN_PITCH, 0);
 /**
 * Auxiliary channel to control yaw.
 *
 * @value 0 Disable
 * @value 1 AUX1
 * @value 2 AUX2
 * @value 3 AUX3
 * @value 4 AUX4
 * @value 5 AUX5
 * @min 0
 * @max 5
 * @group Mount
 */
 PARAM_DEFINE_INT32(MNT_MAN_YAW, 0);
--- a/src/drivers/vmount/vmount_rc.cpp
+++ b/src/drivers/vmount/vmount_rc.cpp
@ -0,0 +1,155 @@
 /****************************************************************************
 *
 *   Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/
 /**
 * @file vmount_rc.c
 * @author Leon Müller (thedevleon)
 *
 */
 #include "vmount_rc.h"
 #include <stdlib.h>
 #include <stdio.h>
 #include <string.h>
 #include <arch/math.h>
 #include <geo/geo.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_roi.h>
 #include <uORB/topics/actuator_controls.h>
 /* uORB advertising */
 static struct actuator_controls_s actuator_controls;
 static orb_advert_t actuator_controls_pub;
 static double lon;
 static double lat;
 static float alt;
 static float normal_mode;
 static float locked_mode;
 static bool  locked;
 bool vmount_rc_init()
 {
 	memset(&actuator_controls, 0, sizeof(actuator_controls));
 	actuator_controls_pub = orb_advertise(ORB_ID(actuator_controls_2), &actuator_controls);
 	if (!actuator_controls_pub) { return false; }
 	locked = false;
 	normal_mode = -1.0f;
 	locked_mode = 0.0f;
 	return true;
 }
 void vmount_rc_deinit()
 {
 	orb_unadvertise(actuator_controls_pub);
 	//free(&actuator_controls);
 }
 void vmount_rc_configure(int roi_mode, bool man_control, int normal_mode_new, int locked_mode_new)
 {
 	normal_mode = normal_mode_new;
 	locked_mode = locked_mode_new;
 	switch (roi_mode) {
 	case vehicle_roi_s::VEHICLE_ROI_NONE:
 		locked = false;
 		if (!man_control) {vmount_rc_point_manual(0.0f, 0.0f, 0.0f);}
 		break;
 	case vehicle_roi_s::VEHICLE_ROI_WPNEXT:
 	case vehicle_roi_s::VEHICLE_ROI_WPINDEX:
 	case vehicle_roi_s::VEHICLE_ROI_LOCATION:
 	case vehicle_roi_s::VEHICLE_ROI_TARGET:
 		locked = true;
 		break;
 	default:
 		locked = false;
 		vmount_rc_point_manual(0.0f, 0.0f, 0.0f);
 		break;
 	}
 }
 void vmount_rc_set_location(double lat_new, double lon_new, float alt_new)
 {
 	lat = lat_new;
 	lon = lon_new;
 	alt = alt_new;
 }
 void vmount_rc_point(double global_lat, double global_lon, float global_alt)
 {
 	float pitch = vmount_rc_calculate_pitch(global_lat, global_lon, global_alt);
 	float roll = 0.0f; // We want a level horizon, so leave roll at 0 degrees.
 	float yaw = get_bearing_to_next_waypoint(global_lat, global_lon, lat, lon) * (float)M_RAD_TO_DEG;
 	vmount_rc_point_manual(pitch, roll, yaw);
 }
 void vmount_rc_point_manual(float pitch_new, float roll_new, float yaw_new)
 {
 	actuator_controls.timestamp = hrt_absolute_time();
 	actuator_controls.control[0] = pitch_new;
 	actuator_controls.control[1] = roll_new;
 	actuator_controls.control[2] = yaw_new;
 	actuator_controls.control[3] = (locked) ? locked_mode : normal_mode;
 	/** for debugging purposes
 	warnx("actuator_controls_2 values:\t%8.4f\t%8.4f\t%8.4f\t%8.4f\t%8.4f",
 		 (double)actuator_controls.control[0],
 		 (double)actuator_controls.control[1],
 		 (double)actuator_controls.control[2],
 	     (double)actuator_controls.control[3],
 	     (double)actuator_controls.control[4]);
 	**/
 	orb_publish(ORB_ID(actuator_controls_2), actuator_controls_pub, &actuator_controls);
 }
 float vmount_rc_calculate_pitch(double global_lat, double global_lon, float global_alt)
 {
 	float x = (lon - global_lon) * cos(M_DEG_TO_RAD * ((global_lat + lat) * 0.00000005)) * 0.01113195;
 	float y = (lat - global_lat) * 0.01113195;
 	float z = (alt - global_alt);
 	float target_distance = sqrtf(powf(x, 2) + powf(y, 2));
 	return atan2(z, target_distance) * M_RAD_TO_DEG;
 }
--- a/src/drivers/vmount/vmount_rc.h
+++ b/src/drivers/vmount/vmount_rc.h
@ -0,0 +1,55 @@
 /****************************************************************************
 *
 *   Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/
 /**
 * @file vmount_rc
 * @author Leon Müller (thedevleon)
 *
 */
 #ifndef _VMOUNT_RC_H
 #define _VMOUNT_RC_H
 #include <sys/types.h>
 #include <stdbool.h>
 // Public functions
 bool vmount_rc_init(void);
 void vmount_rc_deinit(void);
 void vmount_rc_configure(int roi_mode, bool man_control, int normal_mode_new, int locked_mode_new);
 void vmount_rc_set_location(double lat_new, double lon_new, float alt_new);
 void vmount_rc_point(double global_lat, double global_lon, float global_alt);
 void vmount_rc_point_manual(float pitch_new, float roll_new, float yaw_new);
 float vmount_rc_calculate_pitch(double global_lat, double global_lon, float global_alt);
 #endif /* _VMOUNT_RC_H */
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@ -89,8 +89,9 @@
 #include <uORB/topics/mission.h>
 #include <uORB/topics/mission_result.h>
 #include <uORB/topics/offboard_control_mode.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_roi.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/safety.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/subsystem_info.h>
@ -175,6 +176,7 @@ static float eph_threshold = 5.0f;
 static float epv_threshold = 10.0f;
 static struct vehicle_status_s status = {};
 static struct vehicle_roi_s _roi = {};
 static struct battery_status_s battery = {};
 static struct actuator_armed_s armed = {};
 static struct safety_s safety = {};
@ -228,7 +230,8 @@ void usage(const char *reason);
 bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd,
 		    struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos,
 		    struct vehicle_local_position_s *local_pos, struct vehicle_attitude_s *attitude, orb_advert_t *home_pub,
-		    orb_advert_t *command_ack_pub, struct vehicle_command_ack_s *command_ack);
+		    orb_advert_t *command_ack_pub, struct vehicle_command_ack_s *command_ack, struct vehicle_roi_s *roi,
 			orb_advert_t *roi_pub);
 /**
 * Mainloop of commander.
@ -656,7 +659,8 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 		    struct vehicle_command_s *cmd, struct actuator_armed_s *armed_local,
 		    struct home_position_s *home, struct vehicle_global_position_s *global_pos,
 		    struct vehicle_local_position_s *local_pos, struct vehicle_attitude_s *attitude, orb_advert_t *home_pub,
-		    orb_advert_t *command_ack_pub, struct vehicle_command_ack_s *command_ack)
+		    orb_advert_t *command_ack_pub, struct vehicle_command_ack_s *command_ack,
 			struct vehicle_roi_s *roi, orb_advert_t *roi_pub)
 {
 	/* only handle commands that are meant to be handled by this system and component */
 	if (cmd->target_system != status_local->system_id || ((cmd->target_component != status_local->component_id)
@ -1066,9 +1070,41 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 		}
 		break;
 	case vehicle_command_s::VEHICLE_CMD_NAV_ROI:
 	case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI: {
 		roi->mode = cmd->param1;
 		if (roi->mode == vehicle_roi_s::VEHICLE_ROI_WPINDEX) {
 			roi->mission_seq =  cmd->param2;
 		}
 		else if (roi->mode == vehicle_roi_s::VEHICLE_ROI_LOCATION) {
 			roi->lat = cmd->param5;
 			roi->lon = cmd->param6;
 			roi->alt = cmd->param7;
 		}
 		else if (roi->mode == vehicle_roi_s::VEHICLE_ROI_TARGET) {
 			roi->target_seq = cmd->param2;
 		}
 		if (*roi_pub != nullptr) {
 			orb_publish(ORB_ID(vehicle_roi), *roi_pub, roi);
 		} else {
 			*roi_pub = orb_advertise(ORB_ID(vehicle_roi), roi);
 		}
 		cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
 		break;
 	}
 	case vehicle_command_s::VEHICLE_CMD_CUSTOM_0:
 	case vehicle_command_s::VEHICLE_CMD_CUSTOM_1:
 	case vehicle_command_s::VEHICLE_CMD_CUSTOM_2:
 	case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL:
 	case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE:
 	case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT:
 	case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
 	case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION:
 	case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS:
@ -1079,10 +1115,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 	case vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL:
 	case vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION:
 	case vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL:
 	case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE:
 	case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL:
 	case vehicle_command_s::VEHICLE_CMD_DO_SET_CAM_TRIGG_DIST:
 	case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT:
 	case vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED:
 	case vehicle_command_s::VEHICLE_CMD_DO_GO_AROUND:
 	case vehicle_command_s::VEHICLE_CMD_START_RX_PAIR:
@ -1330,6 +1363,10 @@ int commander_thread_main(int argc, char *argv[])
 	orb_advert_t home_pub = nullptr;
 	memset(&_home, 0, sizeof(_home));
 	/* region of interest */
 	orb_advert_t roi_pub = nullptr;
 	memset(&_roi, 0, sizeof(_roi));
 	/* command ack */
 	orb_advert_t command_ack_pub = nullptr;
 	struct vehicle_command_ack_s command_ack;
@ -2656,7 +2693,7 @@ int commander_thread_main(int argc, char *argv[])
 			/* handle it */
 			if (handle_command(&status, &safety, &cmd, &armed, &_home, &global_position, &local_position,
-					&attitude, &home_pub, &command_ack_pub, &command_ack)) {
+					&attitude, &home_pub, &command_ack_pub, &command_ack, &_roi, &roi_pub)) {
 				status_changed = true;
 			}
 		}