diff --git a/ROMFS/px4fmu_common/init.d/10021_H4_680mm b/ROMFS/px4fmu_common/init.d/10021_H4_680mm
new file mode 100644
index 0000000000000000000000000000000000000000..759381923a2c84d0ae8aebc1bfcf5054663bed6a
--- /dev/null
+++ 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
diff --git a/ROMFS/px4fmu_common/init.d/4002_quad_x_mount b/ROMFS/px4fmu_common/init.d/4002_quad_x_mount
new file mode 100644
index 0000000000000000000000000000000000000000..201fb6cd4ec6b758e8e8769c4a8802b315f1da72
--- /dev/null
+++ 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
diff --git a/ROMFS/px4fmu_common/mixers/mount.aux.mix b/ROMFS/px4fmu_common/mixers/mount.aux.mix
new file mode 100644
index 0000000000000000000000000000000000000000..4d6d9fd0895fce831d20091a65cf18671bfc8474
--- /dev/null
+++ 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
diff --git a/cmake/configs/nuttx_mindpx-v2_default.cmake b/cmake/configs/nuttx_mindpx-v2_default.cmake
index ab29762ccfb4739520fa6332070a77facc46a79c..48ab8ff9a8234e6f36daf844af3eefed5f03d566 100644
--- 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
diff --git a/cmake/configs/nuttx_px4fmu-v1_default.cmake b/cmake/configs/nuttx_px4fmu-v1_default.cmake
index f174ed469192e86a48f4d26e676f9ce02be93c7e..6cbe281eab036b2b6926cea5914988a075a88008 100644
--- 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
#
diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake
index 5f495cdf7d3280072196d5804b48e6617710664c..939450d5c16a822d14cd54a75d9f0e6af9b6286e 100644
--- 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
#
diff --git a/cmake/configs/nuttx_px4fmu-v2_test.cmake b/cmake/configs/nuttx_px4fmu-v2_test.cmake
index 3f34faf8826d191cd7eb92e451b4f4ba2f72d78a..f6b72d9555906d9523a9efa04826e95ce408b8d1 100644
--- 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
#
diff --git a/cmake/configs/nuttx_px4fmu-v4_default.cmake b/cmake/configs/nuttx_px4fmu-v4_default.cmake
index 6dd6833119681ea2e35c91d4a49efcf5a6aae0da..7cddcbc0333f4747f0ed6c2e9c5ad42aeb56ee57 100644
--- 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
diff --git a/cmake/configs/nuttx_tap-v1_default.cmake b/cmake/configs/nuttx_tap-v1_default.cmake
index 28fe2d0c1525e34f5886f80a6b993d1a7624dd1c..4de03735d4d3a8fb2baa024f65be0d7d62c66867 100644
--- 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
#
diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt
index a07680ea001de4830e67e3aebb4e44f666e48435..a0eb89858698b50f4984c8ce5b4c92eb0a246358 100644
--- 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
diff --git a/msg/vehicle_command.msg b/msg/vehicle_command.msg
index 22915380446bd3f530b91f7631f2a4473bd6aae3..49f6443f67d60c66b54b39cf743b06db0d4bda01 100644
--- 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_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_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_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_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_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_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_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_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_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 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_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_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_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_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_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_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_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_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_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_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_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_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_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_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_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_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_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_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.
-float32 param2 # Parameter 2, as defined by MAVLink uint32 VEHICLE_CMD enum.
-float32 param3 # Parameter 3, as defined by MAVLink uint32 VEHICLE_CMD enum.
-float32 param4 # Parameter 4, as defined by MAVLink uint32 VEHICLE_CMD enum.
-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
+uint32 VEHICLE_ROI_NONE = 0 # No region of interest |
+uint32 VEHICLE_ROI_WPNEXT = 1 # Point toward next MISSION |
+uint32 VEHICLE_ROI_WPINDEX = 2 # Point toward given MISSION |
+uint32 VEHICLE_ROI_LOCATION = 3 # Point toward fixed location |
+uint32 VEHICLE_ROI_TARGET = 4 # Point toward target
+uint32 VEHICLE_ROI_ENUM_END = 5
+
+float32 param1 # Parameter 1, as defined by MAVLink uint32 VEHICLE_CMD enum.
+float32 param2 # Parameter 2, as defined by MAVLink uint32 VEHICLE_CMD enum.
+float32 param3 # Parameter 3, as defined by MAVLink uint32 VEHICLE_CMD enum.
+float32 param4 # Parameter 4, as defined by MAVLink uint32 VEHICLE_CMD enum.
+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)
diff --git a/msg/vehicle_roi.msg b/msg/vehicle_roi.msg
new file mode 100644
index 0000000000000000000000000000000000000000..715dc46b87dbb70a41e520bed4d94cad42f1fcad
--- /dev/null
+++ 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
diff --git a/src/drivers/gimbal/gimbal.cpp b/src/drivers/gimbal/gimbal.cpp
deleted file mode 100644
index 1864766dd715531df1c227f4d6bacbb998c1791b..0000000000000000000000000000000000000000
--- a/src/drivers/gimbal/gimbal.cpp
+++ /dev/null
@@ -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, ¶ms_updated);
-
- if (params_updated) {
- struct parameter_update_s param_update;
- orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_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'");
-}
diff --git a/src/drivers/gimbal/gimbal_params.c b/src/drivers/gimbal/gimbal_params.c
deleted file mode 100644
index 6b19814f61f7be6294f56c3eace42192f9db3b32..0000000000000000000000000000000000000000
--- a/src/drivers/gimbal/gimbal_params.c
+++ /dev/null
@@ -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);
diff --git a/src/drivers/gimbal/CMakeLists.txt b/src/drivers/vmount/CMakeLists.txt
similarity index 86%
rename from src/drivers/gimbal/CMakeLists.txt
rename to src/drivers/vmount/CMakeLists.txt
index c52d83915f2950964e520adc5b3dba0906b735f7..ca6ad367953165efe04532e90dfc96715d54164c 100644
--- a/src/drivers/gimbal/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
- MAIN gimbal
+ MODULE drivers__vmount
+ MAIN vmount
STACK_MAIN 1024
COMPILE_FLAGS
-Os
SRCS
- gimbal.cpp
- gimbal_params.c
+ vmount.cpp
+ 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 :
diff --git a/src/drivers/vmount/vmount.cpp b/src/drivers/vmount/vmount.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..566b7c2b205d3ac9aef7dfed6553da0fead03b02
--- /dev/null
+++ 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(¶meter_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, ¶meter_update);
+ parameter_update_updated = true;
+ update_params();
+ }
+}
+
+void update_params()
+{
+ param_get(params_handels.mnt_mode, ¶ms.mnt_mode);
+ param_get(params_handels.mnt_mav_sysid, ¶ms.mnt_mav_sysid);
+ param_get(params_handels.mnt_mav_compid, ¶ms.mnt_mav_compid);
+ param_get(params_handels.mnt_ob_lock_mode, ¶ms.mnt_ob_lock_mode);
+ param_get(params_handels.mnt_ob_norm_mode, ¶ms.mnt_ob_norm_mode);
+ param_get(params_handels.mnt_man_control, ¶ms.mnt_man_control);
+ param_get(params_handels.mnt_man_pitch, ¶ms.mnt_man_pitch);
+ param_get(params_handels.mnt_man_roll, ¶ms.mnt_man_roll);
+ param_get(params_handels.mnt_man_yaw, ¶ms.mnt_man_yaw);
+ param_get(params_handels.mnt_mode_ovr, ¶ms.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, ¶ms.mnt_mode) ||
+ param_get(params_handels.mnt_mav_sysid, ¶ms.mnt_mav_sysid) ||
+ param_get(params_handels.mnt_mav_compid, ¶ms.mnt_mav_compid) ||
+ param_get(params_handels.mnt_ob_lock_mode, ¶ms.mnt_ob_lock_mode) ||
+ param_get(params_handels.mnt_ob_norm_mode, ¶ms.mnt_ob_norm_mode) ||
+ param_get(params_handels.mnt_man_control, ¶ms.mnt_man_control) ||
+ param_get(params_handels.mnt_man_pitch, ¶ms.mnt_man_pitch) ||
+ param_get(params_handels.mnt_man_roll, ¶ms.mnt_man_roll) ||
+ param_get(params_handels.mnt_man_yaw, ¶ms.mnt_man_yaw) ||
+ param_get(params_handels.mnt_mode_ovr, ¶ms.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;
+ }
+}
diff --git a/src/drivers/vmount/vmount_mavlink.cpp b/src/drivers/vmount/vmount_mavlink.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6ed0be2a8cc40086e40393929abfa6b05d288272
--- /dev/null
+++ 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;
+}
diff --git a/src/drivers/vmount/vmount_mavlink.h b/src/drivers/vmount/vmount_mavlink.h
new file mode 100644
index 0000000000000000000000000000000000000000..1a1daefa711c0d76eac2302c6777b0a0d6a55def
--- /dev/null
+++ 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 */
diff --git a/src/drivers/vmount/vmount_onboard.cpp b/src/drivers/vmount/vmount_onboard.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2451dfb4d8a1d2a8116485cda0ceebe491e94fc2
--- /dev/null
+++ 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;
+}
diff --git a/src/drivers/vmount/vmount_onboard.h b/src/drivers/vmount/vmount_onboard.h
new file mode 100644
index 0000000000000000000000000000000000000000..3f525ba98fe9b9aac24b437d7cbc2d5e11fda8e8
--- /dev/null
+++ 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 */
diff --git a/src/drivers/vmount/vmount_params.c b/src/drivers/vmount/vmount_params.c
new file mode 100644
index 0000000000000000000000000000000000000000..f79634948dae70fc1d2943939536a4ca2e1836e0
--- /dev/null
+++ 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);
diff --git a/src/drivers/vmount/vmount_rc.cpp b/src/drivers/vmount/vmount_rc.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0d174f20e7922fc3659aeac2c05f44b3dc92b2ea
--- /dev/null
+++ 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;
+}
diff --git a/src/drivers/vmount/vmount_rc.h b/src/drivers/vmount/vmount_rc.h
new file mode 100644
index 0000000000000000000000000000000000000000..c8affedbbd87c21256d55a6ad5d023e1882b68eb
--- /dev/null
+++ 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 */
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 63768eb3269fedc7dd543ef5555127bb5900e22d..6bd915db5baaaca416f8400cdf85bfc80f55c175 100644
--- 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;
}
}