dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

vmount: add mount and ROI implementation 

MavLink spec implementation

implemented vehicle_roi topic

rename old gimbal to rc_gimbal

little changes

corrected RC Gimbal group

Starting ROI implementation in commander

implementation done, needs to be tested

uhm..

add todo

Change to float32 for x,y and z

remove mission topic again, not needed

change roi coordinates to lat, lon and alt

adjust to float64

starting mount implementation

correcting small mistakes, compiles now

add todos

further progress

implementing parameters

adjust default parameters

started implementation of mavlink

fix typo

change to lat, lon and alt

fix typo :D

change to double (to represent float64)

add global_position_

add mount topic

commander mount implementation done

cleanup

almost finished

little fix

codestyle fixes

leave pitch at 0 degrees

added pitch calculation

codestyle changes

Undo vehicle_mount, react to updated parameters, parsing of CMD_DO_MOUNT_* in mount.cpp

start implementing mode override

forgot a semikolon.

add debug

Finish implementation of mount override and manual control.

fix codestyle

correct cleanup

rename to vmount

works now

fix rebase error

fix polling

refactoring and custom airframe for gimbal

couple changes

remove warnx

almost done

finally

What is going on?

change back to actuator_controls_2

working

bump parameter version number and some clarification

fix submodules
This commit is contained in:
Leon 2016-05-19 23:46:06 +02:00 committed by Beat Küng
parent e6391189bc
commit faebdeedcf
24 changed files with 1757 additions and 843 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
ROMFS/px4fmu_common/init.d/10021_H4_680mm Normal file
View File

@ -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

21
ROMFS/px4fmu_common/init.d/4002_quad_x_mount Normal file
View File

@ -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

27
ROMFS/px4fmu_common/mixers/mount.aux.mix Normal file
View File

@ -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

2
cmake/configs/nuttx_mindpx-v2_default.cmake
View File

@ -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

3
cmake/configs/nuttx_px4fmu-v1_default.cmake
View File

@ -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
#

6
cmake/configs/nuttx_px4fmu-v2_default.cmake
View File

@ -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
#

4
cmake/configs/nuttx_px4fmu-v2_test.cmake
View File

@ -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
#

2
cmake/configs/nuttx_px4fmu-v4_default.cmake
View File

@ -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

1
cmake/configs/nuttx_tap-v1_default.cmake
View File

@ -22,6 +22,7 @@ set(config_module_list
drivers/airspeed
drivers/meas_airspeed
modules/sensors
drivers/vmount
drivers/camera_trigger
#

1
msg/CMakeLists.txt
View File

@ -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

96
msg/vehicle_command.msg
View File

@ -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)

15
msg/vehicle_roi.msg Normal file
View File

@ -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

704
src/drivers/gimbal/gimbal.cpp
View File

@ -1,704 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file gimbal.cpp
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Anton Matosov <anton.matosov@gmail.com>
* @author Andreas Antener <andreas@uaventure.com>
*
* Driver to control a gimbal - relies on input via telemetry or RC
* and output via the standardized control group #2 and a mixer.
*/
#include <px4_config.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdlib.h>
#include <stdbool.h>
#include <semaphore.h>
#include <string.h>
#include <fcntl.h>
#include <poll.h>
#include <errno.h>
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include <termios.h>
#include <nuttx/arch.h>
#include <nuttx/wqueue.h>
#include <nuttx/clock.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_range_finder.h>
#include <drivers/device/device.h>
#include <drivers/device/ringbuffer.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/parameter_update.h>
#include <board_config.h>
#include <mathlib/math/test/test.hpp>
#include <mathlib/math/Quaternion.hpp>
/* Configuration Constants */
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
#ifndef CONFIG_SCHED_WORKQUEUE
# error This requires CONFIG_SCHED_WORKQUEUE.
#endif
#define GIMBAL_DEVICE_PATH "/dev/gimbal"
#define GIMBAL_UPDATE_INTERVAL (5 * 1000)
#define GIMBALIOCATTCOMPENSATE 1
class Gimbal : public device::CDev
{
public:
Gimbal();
virtual ~Gimbal();
virtual int init();
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
/**
* Diagnostics - print some basic information about the driver.
*/
void print_info();
protected:
virtual int probe();
private:
float _min_distance;
float _max_distance;
work_s _work;
int _vehicle_command_sub;
int _att_sub;
int _vehicle_control_mode_sub;
int _manual_control_sub;
int _params_sub;
bool _attitude_compensation_roll;
bool _attitude_compensation_pitch;
bool _attitude_compensation_yaw;
bool _initialized;
bool _control_cmd_set;
bool _config_cmd_set;
int _mount_mode; // see MAV_MOUNT_MODE
orb_advert_t _actuator_controls_2_topic;
struct vehicle_command_s _control_cmd;
struct vehicle_command_s _config_cmd;
struct manual_control_setpoint_s _manual_control;
struct vehicle_control_mode_s _control_mode;
struct {
int aux_mnt_chn;
int use_mnt;
} _parameters;
struct {
param_t aux_mnt_chn;
param_t use_mnt;
} _params_handles;
/**
* Initialise the automatic measurement state machine and start it.
*
* @note This function is called at open and error time. It might make sense
* to make it more aggressive about resetting the bus in case of errors.
*/
void start();
/**
* Stop the automatic measurement state machine.
*/
void stop();
/**
* Update parameters
*/
void update_params();
/**
* Perform a poll cycle; collect from the previous measurement
* and start a new one.
*/
void cycle();
/**
* Static trampoline from the workq context; because we don't have a
* generic workq wrapper yet.
*
* @param arg Instance pointer for the driver that is polling.
*/
static void cycle_trampoline(void *arg);
};
/*
* Driver 'main' command.
*/
extern "C" __EXPORT int gimbal_main(int argc, char *argv[]);
Gimbal::Gimbal() :
CDev("Gimbal", GIMBAL_DEVICE_PATH),
_vehicle_command_sub(-1),
_att_sub(-1),
_vehicle_control_mode_sub(-1),
_manual_control_sub(-1),
_params_sub(-1),
_attitude_compensation_roll(true),
_attitude_compensation_pitch(true),
_attitude_compensation_yaw(true),
_initialized(false),
_actuator_controls_2_topic(nullptr)
{
// disable debug() calls
_debug_enabled = false;
_params_handles.aux_mnt_chn = param_find("GMB_AUX_MNT_CHN");
_params_handles.use_mnt = param_find("GMB_USE_MNT");
update_params();
// work_cancel in the dtor will explode if we don't do this...
memset(&_work, 0, sizeof(_work));
}
Gimbal::~Gimbal()
{
/* make sure we are truly inactive */
stop();
::close(_vehicle_command_sub);
}
int
Gimbal::init()
{
int ret = ERROR;
/* do regular cdev init */
if (CDev::init() != OK) {
goto out;
}
start();
ret = OK;
out:
return ret;
}
int
Gimbal::probe()
{
return OK;
}
int
Gimbal::ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case GIMBALIOCATTCOMPENSATE:
_attitude_compensation_roll = (arg != 0);
_attitude_compensation_pitch = (arg != 0);
_attitude_compensation_yaw = (arg != 0);
return OK;
default:
/* give it to the superclass */
return CDev::ioctl(filp, cmd, arg);
}
}
ssize_t
Gimbal::read(struct file *filp, char *buffer, size_t buflen)
{
return 0;
}
void
Gimbal::start()
{
/* schedule a cycle to start things */
work_queue(LPWORK, &_work, (worker_t)&Gimbal::cycle_trampoline, this, 1);
}
void
Gimbal::stop()
{
work_cancel(LPWORK, &_work);
}
void
Gimbal::cycle_trampoline(void *arg)
{
Gimbal *dev = static_cast<Gimbal *>(arg);
dev->cycle();
}
void
Gimbal::update_params()
{
param_get(_params_handles.aux_mnt_chn, &_parameters.aux_mnt_chn);
param_get(_params_handles.use_mnt, &_parameters.use_mnt);
}
void
Gimbal::cycle()
{
if (!_initialized) {
/* get subscription handles */
_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command));
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_vehicle_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
/* get a publication handle on actuator output topic */
struct actuator_controls_s zero_report;
memset(&zero_report, 0, sizeof(zero_report));
zero_report.timestamp = hrt_absolute_time();
_actuator_controls_2_topic = orb_advertise(ORB_ID(actuator_controls_2), &zero_report);
if (_actuator_controls_2_topic == nullptr) {
warnx("advert err");
}
_mount_mode = vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT;
_initialized = true;
}
bool updated = false;
float roll = 0.0f;
float pitch = 0.0f;
float yaw = 0.0f;
float out_mount_mode = 0.0f;
/* Parameter update */
bool params_updated;
orb_check(_params_sub, &params_updated);
if (params_updated) {
struct parameter_update_s param_update;
orb_copy(ORB_ID(parameter_update), _params_sub, &param_update); // XXX: is this actually necessary?
update_params();
}
/* Control mode update */
bool vehicle_control_mode_updated;
orb_check(_vehicle_control_mode_sub, &vehicle_control_mode_updated);
if (vehicle_control_mode_updated) {
orb_copy(ORB_ID(vehicle_control_mode), _vehicle_control_mode_sub, &_control_mode);
}
/* Check attitude */
struct vehicle_attitude_s att;
orb_copy(ORB_ID(vehicle_attitude), _att_sub, &att);
if (_attitude_compensation_roll) {
roll = 1.0f / M_PI_F * -att.roll;
updated = true;
}
if (_attitude_compensation_pitch) {
pitch = 1.0f / M_PI_F * -att.pitch;
updated = true;
}
if (_attitude_compensation_yaw) {
yaw = 1.0f / M_PI_F * att.yaw;
updated = true;
}
/* Check manual input */
bool manual_updated;
orb_check(_manual_control_sub, &manual_updated);
if (manual_updated) {
orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sub, &_manual_control);
/* only check manual input for mount mode when not in offboard and aux chan is configured */
if (!_control_mode.flag_control_offboard_enabled && _parameters.aux_mnt_chn > 0) {
float aux = 0.0f;
switch (_parameters.aux_mnt_chn) {
case 1:
aux = _manual_control.aux1;
break;
case 2:
aux = _manual_control.aux2;
break;
case 3:
aux = _manual_control.aux3;
break;
}
if (aux < 0.0f && _mount_mode != vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT) {
_mount_mode = vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT;
updated = true;
}
if (aux > 0.0f && _mount_mode != vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL) {
_mount_mode = vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL;
updated = true;
}
}
}
/* Check command input */
struct vehicle_command_s cmd;
bool cmd_updated;
orb_check(_vehicle_command_sub, &cmd_updated);
if (cmd_updated) {
orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd);
if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL
|| cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT) {
_control_cmd = cmd;
_control_cmd_set = true;
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE) {
_config_cmd = cmd;
_config_cmd_set = true;
}
}
if (_config_cmd_set) {
_config_cmd_set = false;
_attitude_compensation_roll = (int)_config_cmd.param2 == 1;
_attitude_compensation_pitch = (int)_config_cmd.param3 == 1;
_attitude_compensation_yaw = (int)_config_cmd.param4 == 1;
/* only check commanded mount mode when in offboard */
if (_control_mode.flag_control_offboard_enabled &&
fabsf(_config_cmd.param1 - _mount_mode) > FLT_EPSILON) {
_mount_mode = int(_config_cmd.param1 + 0.5f);
updated = true;
}
}
if (_control_cmd_set) {
unsigned mountMode = _control_cmd.param7;
DEVICE_DEBUG("control_cmd: %d, mountMode %d | param1: %8.4f param2: %8.4f", _control_cmd.command, mountMode,
(double)_control_cmd.param1, (double)_control_cmd.param2);
if (_control_cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL &&
mountMode == vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) {
/* Convert to range -1 ... 1, which corresponds to -180deg ... 180deg */
roll += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param1;
pitch += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param2;
yaw += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param3;
updated = true;
}
if (_control_cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT &&
mountMode == vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) {
float gimbalDirectionQuat[] = {_control_cmd.param1, _control_cmd.param2, _control_cmd.param3, _control_cmd.param4};
math::Vector<3> gimablDirectionEuler = math::Quaternion(gimbalDirectionQuat).to_dcm().to_euler();
roll += gimablDirectionEuler(0);
pitch += gimablDirectionEuler(1);
yaw += gimablDirectionEuler(2);
updated = true;
}
}
/* consider mount mode if parameter is set */
if (_parameters.use_mnt > 0) {
switch (_mount_mode) {
case vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT:
out_mount_mode = -1.0f;
roll = 0.0f;
pitch = 0.0f;
yaw = 0.0f;
break;
case vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL:
case vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING:
case vehicle_command_s::VEHICLE_MOUNT_MODE_RC_TARGETING:
case vehicle_command_s::VEHICLE_MOUNT_MODE_GPS_POINT:
out_mount_mode = 1.0f;
break;
default:
out_mount_mode = -1.0f;
}
}
if (updated) {
struct actuator_controls_s controls;
// DEVICE_DEBUG("publishing | roll: %8.4f pitch: %8.4f yaw: %8.4f", (double)roll, (double)pitch, (double)yaw);
/* fill in the final control values */
controls.timestamp = hrt_absolute_time();
controls.control[0] = roll;
controls.control[1] = pitch;
controls.control[2] = yaw;
//controls.control[3] = ; // camera shutter
controls.control[4] = out_mount_mode;
/* publish it */
orb_publish(ORB_ID(actuator_controls_2), _actuator_controls_2_topic, &controls);
}
/* notify anyone waiting for data */
poll_notify(POLLIN);
/* schedule a fresh cycle call when the measurement is done */
work_queue(LPWORK,
&_work,
(worker_t)&Gimbal::cycle_trampoline,
this,
USEC2TICK(GIMBAL_UPDATE_INTERVAL));
}
void
Gimbal::print_info()
{
}
/**
* Local functions in support of the shell command.
*/
namespace gimbal
{
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
const int ERROR = -1;
Gimbal *g_dev;
void start();
void stop();
void test();
void reset();
void info();
/**
* Start the driver.
*/
void
start()
{
if (g_dev != nullptr) {
errx(1, "already started");
}
/* create the driver */
g_dev = new Gimbal();
if (g_dev == nullptr) {
goto fail;
}
if (OK != g_dev->init()) {
goto fail;
}
exit(0);
fail:
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
}
errx(1, "driver start failed");
}
/**
* Stop the driver
*/
void stop()
{
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
} else {
errx(1, "driver not running");
}
exit(0);
}
/**
* Perform some basic functional tests on the driver;
* make sure we can collect data from the sensor in polled
* and automatic modes.
*/
void
test()
{
int fd = open(GIMBAL_DEVICE_PATH, O_RDONLY);
if (ioctl(fd, GIMBALIOCATTCOMPENSATE, 1) < 0) {
err(1, "failed enabling compensation");
}
errx(0, "PASS");
}
/**
* Reset the driver.
*/
void
reset()
{
int fd = open(GIMBAL_DEVICE_PATH, O_RDONLY);
if (fd < 0) {
err(1, "failed ");
}
// if (ioctl(fd, GIMBALIOCATTCOMPENSATE, 1) < 0) {
// err(1, "failed enabling compensation");
// }
exit(0);
}
/**
* Print a little info about the driver.
*/
void
info()
{
if (g_dev == nullptr) {
errx(1, "driver not running");
}
printf("state @ %p\n", g_dev);
g_dev->print_info();
exit(0);
}
} // namespace
int
gimbal_main(int argc, char *argv[])
{
/*
* Start/load the driver.
*/
if (!strcmp(argv[1], "start")) {
gimbal::start();
}
/*
* Stop the driver
*/
if (!strcmp(argv[1], "stop")) {
gimbal::stop();
}
/*
* Test the driver/device.
*/
if (!strcmp(argv[1], "test")) {
gimbal::test();
}
/*
* Reset the driver.
*/
if (!strcmp(argv[1], "reset")) {
gimbal::reset();
}
/*
* Print driver information.
*/
if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) {
gimbal::info();
}
errx(1, "unrecognized command, try 'start', 'stop', 'reset', 'test' or 'info'");
}

74
src/drivers/gimbal/gimbal_params.c
View File

@ -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);

15
src/drivers/gimbal/CMakeLists.txt → src/drivers/vmount/CMakeLists.txt
View File

@ -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 :

692
src/drivers/vmount/vmount.cpp Normal file
View File

@ -0,0 +1,692 @@
/****************************************************************************
*
* Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file vmount.cpp
* @author Leon Müller (thedevleon)
* MAV_MOUNT driver for controlling mavlink gimbals, rc gimbals/servors and
* future kinds of mounts.
*
*/
#include <stdlib.h>
#include <stdio.h>
#include <stdbool.h>
#include <string.h>
#include <sys/types.h>
#include <fcntl.h>
#include <unistd.h>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
#include "vmount_mavlink.h"
#include "vmount_rc.h"
#include "vmount_onboard.h"
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_roi.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <px4_config.h>
#include <px4_posix.h>
#include <poll.h>
/* thread state */
static volatile bool thread_should_exit = false;
static volatile bool thread_running = false;
static volatile bool thread_should_restart = false;
static int vmount_task;
typedef enum { IDLE = -1, MAVLINK = 0, RC = 1, ONBOARD = 2 } vmount_state_t;
static vmount_state_t vmount_state = IDLE;
/* polling */
px4_pollfd_struct_t polls[7];
/* functions */
static void usage(void);
static void vmount_update_topics(void);
static void update_params(void);
static bool get_params(void);
static float get_aux_value(int);
static void ack_mount_command(uint16_t command);
static int vmount_thread_main(int argc, char *argv[]);
extern "C" __EXPORT int vmount_main(int argc, char *argv[]);
/* uORB subscriptions */
static int vehicle_roi_sub = -1;
static int vehicle_global_position_sub = -1;
static int vehicle_command_sub = -1;
static int vehicle_attitude_sub = -1;
static int position_setpoint_triplet_sub = -1;
static int manual_control_setpoint_sub = -1;
static int parameter_update_sub = -1;
/* uORB publication */
static orb_advert_t vehicle_command_ack_pub;
static struct vehicle_roi_s vehicle_roi;
static bool vehicle_roi_updated;
static struct vehicle_global_position_s vehicle_global_position;
static bool vehicle_global_position_updated;
static struct vehicle_command_s vehicle_command;
static bool vehicle_command_updated;
static struct vehicle_attitude_s vehicle_attitude;
static bool vehicle_attitude_updated;
static struct position_setpoint_triplet_s position_setpoint_triplet;
static bool position_setpoint_triplet_updated;
static struct manual_control_setpoint_s manual_control_setpoint;
static bool manual_control_setpoint_updated;
static struct parameter_update_s parameter_update;
static bool parameter_update_updated;
static struct vehicle_command_ack_s vehicle_command_ack;
static struct {
int mnt_mode;
int mnt_mav_sysid;
int mnt_mav_compid;
int mnt_ob_lock_mode;
int mnt_ob_norm_mode;
int mnt_man_control;
int mnt_man_pitch;
int mnt_man_roll;
int mnt_man_yaw;
int mnt_mode_ovr;
} params;
static struct {
param_t mnt_mode;
param_t mnt_mav_sysid;
param_t mnt_mav_compid;
param_t mnt_ob_lock_mode;
param_t mnt_ob_norm_mode;
param_t mnt_man_control;
param_t mnt_man_pitch;
param_t mnt_man_roll;
param_t mnt_man_yaw;
param_t mnt_mode_ovr;
} params_handels;
static bool manual_control_desired;
/**
* Print command usage information
*/
static void usage()
{
fprintf(stderr,
"usage: vmount start\n"
" vmount stop\n"
" vmount status\n");
exit(1);
}
/**
* The daemon thread.
*/
static int vmount_thread_main(int argc, char *argv[])
{
if (!get_params()) {
warnx("could not get mount parameters!");
}
if (params.mnt_mode == 0) { vmount_state = IDLE;}
else if (params.mnt_mode == 1) { vmount_state = MAVLINK;}
else if (params.mnt_mode == 2) { vmount_state = RC;}
else if (params.mnt_mode == 3) { vmount_state = ONBOARD;}
//TODO is this needed?
memset(&vehicle_roi, 0, sizeof(vehicle_roi));
memset(&vehicle_global_position, 0, sizeof(vehicle_global_position));
memset(&vehicle_command, 0, sizeof(vehicle_command));
memset(&vehicle_attitude, 0, sizeof(vehicle_attitude));
memset(&position_setpoint_triplet, 0, sizeof(position_setpoint_triplet));
memset(&manual_control_setpoint, 0, sizeof(manual_control_setpoint));
memset(&parameter_update, 0, sizeof(parameter_update));
memset(&vehicle_command_ack, 0, sizeof(vehicle_command_ack));
vehicle_roi_sub = orb_subscribe(ORB_ID(vehicle_roi));
vehicle_global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command));
vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
position_setpoint_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
vehicle_command_ack_pub = orb_advertise(ORB_ID(vehicle_command_ack), &vehicle_command_ack);
if (!vehicle_roi_sub || !position_setpoint_triplet_sub ||
!manual_control_setpoint_sub || !vehicle_global_position_sub ||
!vehicle_command_sub || !parameter_update_sub || !vehicle_command_ack_pub ||
!vehicle_attitude_sub) {
err(1, "could not subscribe to uORB topics");
}
polls[0].fd = vehicle_roi_sub;
polls[0].events = POLLIN;
polls[1].fd = vehicle_global_position_sub;
polls[1].events = POLLIN;
polls[2].fd = vehicle_attitude_sub;
polls[2].events = POLLIN;
polls[3].fd = vehicle_command_sub;
polls[3].events = POLLIN;
polls[4].fd = position_setpoint_triplet_sub;
polls[4].events = POLLIN;
polls[5].fd = manual_control_setpoint_sub;
polls[5].events = POLLIN;
polls[6].fd = parameter_update_sub;
polls[6].events = POLLIN;
thread_running = true;
run: {
if (vmount_state == MAVLINK) {
if (!vmount_mavlink_init()) {
err(1, "could not initiate vmount_mavlink");
}
warnx("running mount driver in mavlink mode");
if (params.mnt_man_control) manual_control_desired = true;
while (!thread_should_exit && !thread_should_restart) {
vmount_update_topics();
if (vehicle_roi_updated) {
vehicle_roi_updated = false;
vmount_mavlink_configure(vehicle_roi.mode, (params.mnt_man_control == 1), params.mnt_mav_sysid, params.mnt_mav_compid);
if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_NONE) {
if (params.mnt_man_control) manual_control_desired = true;
} else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_WPNEXT) {
manual_control_desired = false;
vmount_mavlink_set_location(
position_setpoint_triplet.next.lat,
position_setpoint_triplet.next.lon,
position_setpoint_triplet.next.alt
);
} else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_WPINDEX) {
manual_control_desired = false;
//TODO how to do this?
} else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_LOCATION) {
manual_control_desired = false;
vmount_mavlink_set_location(
vehicle_roi.lat,
vehicle_roi.lon,
vehicle_roi.alt
);
} else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_TARGET) {
manual_control_desired = false;
//TODO is this even suported?
}
}
else if (manual_control_desired && manual_control_setpoint_updated)
{
manual_control_setpoint_updated = false;
vmount_mavlink_point_manual(get_aux_value(params.mnt_man_pitch), get_aux_value(params.mnt_man_roll), get_aux_value(params.mnt_man_yaw));
}
else if (!manual_control_desired && vehicle_global_position_updated)
{
vehicle_global_position_updated = false;
vmount_mavlink_point(vehicle_global_position.lat, vehicle_global_position.lon, vehicle_global_position.alt);
}
vmount_mavlink_deinit();
}
} else if (vmount_state == RC) {
if (!vmount_rc_init()) {
err(1, "could not initiate vmount_rc");
}
warnx("running mount driver in rc mode");
if (params.mnt_man_control) {
manual_control_desired = true;
}
while (!thread_should_exit && !thread_should_restart) {
vmount_update_topics();
if (vehicle_roi_updated) {
vehicle_roi_updated = false;
vmount_rc_configure(vehicle_roi.mode, (params.mnt_man_control == 1), params.mnt_ob_norm_mode, params.mnt_ob_lock_mode);
if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_NONE) {
if (params.mnt_man_control) manual_control_desired = true;
} else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_WPNEXT) {
manual_control_desired = false;
vmount_rc_set_location(
position_setpoint_triplet.next.lat,
position_setpoint_triplet.next.lon,
position_setpoint_triplet.next.alt
);
} else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_WPINDEX) {
manual_control_desired = false;
//TODO how to do this?
} else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_LOCATION) {
manual_control_desired = false;
vmount_rc_set_location(
vehicle_roi.lat,
vehicle_roi.lon,
vehicle_roi.alt
);
} else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_TARGET) {
manual_control_desired = false;
//TODO is this even suported?
}
}
else if (manual_control_desired && manual_control_setpoint_updated) {
manual_control_setpoint_updated = false;
vmount_rc_point_manual(get_aux_value(params.mnt_man_pitch), get_aux_value(params.mnt_man_roll), get_aux_value(params.mnt_man_yaw));
}
else if (!manual_control_desired && vehicle_global_position_updated)
{
vehicle_global_position_updated = false;
vmount_rc_point(vehicle_global_position.lat, vehicle_global_position.lon, vehicle_global_position.alt);
}
}
vmount_rc_deinit();
} else if (vmount_state == ONBOARD) {
if (!vmount_onboard_init()) {
err(1, "could not initiate vmount_onboard");
}
warnx("running mount driver in onboard mode");
if (params.mnt_man_control) manual_control_desired = true;
while (!thread_should_exit && !thread_should_restart) {
vmount_update_topics();
if(vehicle_attitude_updated)
{
vmount_onboard_update_attitude(vehicle_attitude);
vehicle_attitude_updated = false;
}
if (params.mnt_mode_ovr && manual_control_setpoint_updated) {
manual_control_setpoint_updated = false;
float ovr_value = get_aux_value(params.mnt_mode_ovr);
if(ovr_value < 0.0f)
{
vmount_onboard_set_mode(vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT);
}
else if (ovr_value > 0.0f)
{
vmount_onboard_set_mode(vehicle_command_s::VEHICLE_MOUNT_MODE_RC_TARGETING);
}
}
else if (vehicle_command_updated) {
vehicle_command_updated = false;
if(vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL)
{
switch ((int)vehicle_command.param7) {
case vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT:
case vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL:
manual_control_desired = false;
break;
case vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING:
manual_control_desired = false;
vmount_onboard_set_mode(vehicle_command.param7);
vmount_onboard_set_manual(vehicle_command.param1,
vehicle_command.param2, vehicle_command.param3);
case vehicle_command_s::VEHICLE_MOUNT_MODE_RC_TARGETING:
if (params.mnt_man_control) {
manual_control_desired = true;
vmount_onboard_set_mode(vehicle_command.param7);
}
case vehicle_command_s::VEHICLE_MOUNT_MODE_GPS_POINT:
manual_control_desired = false;
vmount_onboard_set_mode(vehicle_command.param7);
vmount_onboard_set_location(
vehicle_command.param1,
vehicle_command.param2,
vehicle_command.param3);
default:
manual_control_desired = false;
break;
}
ack_mount_command(vehicle_command.command);
}
else if(vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE)
{
vmount_onboard_configure(vehicle_command.param1,
((uint8_t) vehicle_command.param2 == 1), ((uint8_t) vehicle_command.param3 == 1), ((uint8_t) vehicle_command.param4 == 1));
ack_mount_command(vehicle_command.command);
}
}
else if (manual_control_desired && manual_control_setpoint_updated)
{
manual_control_setpoint_updated = false;
vmount_onboard_point_manual(get_aux_value(params.mnt_man_pitch), get_aux_value(params.mnt_man_roll), get_aux_value(params.mnt_man_yaw));
}
else if (!manual_control_desired && vehicle_global_position_updated)
{
vehicle_global_position_updated = false;
vmount_onboard_point(vehicle_global_position.lat, vehicle_global_position.lon, vehicle_global_position.alt);
}
}
} else if (vmount_state == IDLE)
{
warnx("running mount driver in idle mode");
while (!thread_should_exit && !thread_should_restart) {
vmount_update_topics();
}
}
}
if (thread_should_restart)
{
thread_should_restart = false;
warnx("parameter update, restarting mount driver!");
goto run;
}
thread_running = false;
return 0;
}
/**
* The main command function.
* Processes command line arguments and starts the daemon.
*/
int vmount_main(int argc, char *argv[])
{
if (argc < 1) {
warnx("missing command");
usage();
}
if (!strcmp(argv[1], "start")) {
/* this is not an error */
if (thread_running) {
errx(0, "mount driver already running");
}
thread_should_exit = false;
vmount_task = px4_task_spawn_cmd("vmount",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT + 40, //TODO we might want a higher priority?
1100,
vmount_thread_main,
(char *const *)argv);
while (!thread_running) {
usleep(200);
}
exit(0);
}
if (!strcmp(argv[1], "stop")) {
/* this is not an error */
if (!thread_running) {
errx(0, "mount driver already stopped");
}
thread_should_exit = true;
while (thread_running) {
usleep(1000000);
warnx(".");
}
warnx("terminated.");
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
switch (vmount_state) {
case IDLE:
errx(0, "running: IDLE");
break;
case MAVLINK:
errx(0, "running: MAVLINK - Manual Control? %d", manual_control_desired);
break;
case RC:
errx(0, "running: RC - Manual Control? %d", manual_control_desired);
break;
case ONBOARD:
errx(0, "running: ONBOARD");
break;
}
} else {
errx(1, "not running");
}
}
warnx("unrecognized command");
usage();
/* not getting here */
return 0;
}
/* Update oURB topics */
void vmount_update_topics()
{
/*
polls = {
{ .fd = vehicle_roi_sub, .events = POLLIN },
{ .fd = vehicle_global_position_sub, .events = POLLIN },
{ .fd = vehicle_attitude_sub, .events = POLLIN },
{ .fd = vehicle_command_sub, .events = POLLIN },
{ .fd = position_setpoint_triplet_sub, .events = POLLIN },
{ .fd = manual_control_setpoint_sub, .events = POLLIN },
{ .fd = parameter_update_sub, .events = POLLIN },
};
*/
/* wait for sensor update of 7 file descriptors for 100 ms */
int poll_ret = px4_poll(polls, 7, 100);
//Nothing updated.
if(poll_ret == 0) return;
if (polls[0].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_roi), vehicle_roi_sub, &vehicle_roi);
vehicle_roi_updated = true;
}
if (polls[1].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &vehicle_global_position);
vehicle_global_position_updated = true;
}
if (polls[2].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &vehicle_attitude);
vehicle_attitude_updated = true;
}
if (polls[3].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_command), vehicle_command_sub, &vehicle_command);
vehicle_command_updated = true;
}
if (polls[4].revents & POLLIN) {
orb_copy(ORB_ID(position_setpoint_triplet), position_setpoint_triplet_sub, &position_setpoint_triplet);
position_setpoint_triplet_updated = true;
}
if (polls[5].revents & POLLIN) {
orb_copy(ORB_ID(manual_control_setpoint), manual_control_setpoint_sub, &manual_control_setpoint);
manual_control_setpoint_updated = true;
}
if (polls[6].revents & POLLIN) {
orb_copy(ORB_ID(parameter_update), parameter_update_sub, &parameter_update);
parameter_update_updated = true;
update_params();
}
}
void update_params()
{
param_get(params_handels.mnt_mode, &params.mnt_mode);
param_get(params_handels.mnt_mav_sysid, &params.mnt_mav_sysid);
param_get(params_handels.mnt_mav_compid, &params.mnt_mav_compid);
param_get(params_handels.mnt_ob_lock_mode, &params.mnt_ob_lock_mode);
param_get(params_handels.mnt_ob_norm_mode, &params.mnt_ob_norm_mode);
param_get(params_handels.mnt_man_control, &params.mnt_man_control);
param_get(params_handels.mnt_man_pitch, &params.mnt_man_pitch);
param_get(params_handels.mnt_man_roll, &params.mnt_man_roll);
param_get(params_handels.mnt_man_yaw, &params.mnt_man_yaw);
param_get(params_handels.mnt_mode_ovr, &params.mnt_mode_ovr);
if (vmount_state != params.mnt_mode)
{
thread_should_restart = true;
}
else if (vmount_state == MAVLINK)
{
vmount_mavlink_configure(vehicle_roi.mode, (params.mnt_man_control == 1), params.mnt_mav_sysid, params.mnt_mav_compid);
}
else if (vmount_state == RC)
{
vmount_rc_configure(vehicle_roi.mode, (params.mnt_man_control == 1), params.mnt_ob_norm_mode, params.mnt_ob_lock_mode);
}
else if(vmount_state == ONBOARD)
{
//None of the parameter changes require a reconfiguration of the onboard mount.
}
}
bool get_params()
{
params_handels.mnt_mode = param_find("MNT_MODE");
params_handels.mnt_mav_sysid = param_find("MNT_MAV_SYSID");
params_handels.mnt_mav_compid = param_find("MNT_MAV_COMPID");
params_handels.mnt_ob_lock_mode = param_find("MNT_OB_LOCK_MODE");
params_handels.mnt_ob_norm_mode = param_find("MNT_OB_NORM_MODE");
params_handels.mnt_man_control = param_find("MNT_MAN_CONTROL");
params_handels.mnt_man_pitch = param_find("MNT_MAN_PITCH");
params_handels.mnt_man_roll = param_find("MNT_MAN_ROLL");
params_handels.mnt_man_yaw = param_find("MNT_MAN_YAW");
params_handels.mnt_mode_ovr = param_find("MNT_MODE_OVR");
if (param_get(params_handels.mnt_mode, &params.mnt_mode) ||
param_get(params_handels.mnt_mav_sysid, &params.mnt_mav_sysid) ||
param_get(params_handels.mnt_mav_compid, &params.mnt_mav_compid) ||
param_get(params_handels.mnt_ob_lock_mode, &params.mnt_ob_lock_mode) ||
param_get(params_handels.mnt_ob_norm_mode, &params.mnt_ob_norm_mode) ||
param_get(params_handels.mnt_man_control, &params.mnt_man_control) ||
param_get(params_handels.mnt_man_pitch, &params.mnt_man_pitch) ||
param_get(params_handels.mnt_man_roll, &params.mnt_man_roll) ||
param_get(params_handels.mnt_man_yaw, &params.mnt_man_yaw) ||
param_get(params_handels.mnt_mode_ovr, &params.mnt_mode_ovr)) {
return false;
}
return true;
}
void ack_mount_command(uint16_t command)
{
vehicle_command_ack.command = command;
vehicle_command_ack.result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
orb_publish(ORB_ID(vehicle_command_ack), vehicle_command_ack_pub, &vehicle_command_ack);
}
float get_aux_value(int aux)
{
switch (aux)
{
case 0:
return 0.0f;
case 1:
return manual_control_setpoint.aux1;
case 2:
return manual_control_setpoint.aux2;
case 3:
return manual_control_setpoint.aux3;
case 4:
return manual_control_setpoint.aux4;
case 5:
return manual_control_setpoint.aux5;
default:
return 0.0f;
}
}

178
src/drivers/vmount/vmount_mavlink.cpp Normal file
View File

@ -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;
}

56
src/drivers/vmount/vmount_mavlink.h Normal file
View File

@ -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 */

193
src/drivers/vmount/vmount_onboard.cpp Normal file
View File

@ -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;
}

59
src/drivers/vmount/vmount_onboard.h Normal file
View File

@ -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 */

164
src/drivers/vmount/vmount_params.c Normal file
View File

@ -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);

155
src/drivers/vmount/vmount_rc.cpp Normal file
View File

@ -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;
}

55
src/drivers/vmount/vmount_rc.h Normal file
View File

@ -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 */

51
src/modules/commander/commander.cpp
View File

@ -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;
}
}