Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-21 21:55:34 +08:00
Code Issues Actions 5 Packages Projects Releases Wiki Activity

vmount: add mount and ROI implementation

Browse Source 
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
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/init.d/10021_H4_680mm
+26
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
ROMFS/px4fmu_common/init.d/4002_quad_x_mount
+21
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
ROMFS/px4fmu_common/mixers/mount.aux.mix
+27
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
cmake/configs/nuttx_mindpx-v2_default.cmake
+1 -1
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
cmake/configs/nuttx_px4fmu-v1_default.cmake
+2 -1
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
#
cmake/configs/nuttx_px4fmu-v2_default.cmake
+3 -3
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
#
cmake/configs/nuttx_px4fmu-v2_test.cmake
+2 -2
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
#
cmake/configs/nuttx_px4fmu-v4_default.cmake
+1 -1
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
cmake/configs/nuttx_tap-v1_default.cmake
+1
View File
@@ -22,6 +22,7 @@ set(config_module_list
drivers/airspeed
drivers/meas_airspeed
modules/sensors
drivers/vmount
drivers/camera_trigger
#
msg/CMakeLists.txt
+1
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
msg/vehicle_command.msg
+52 -44
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)
msg/vehicle_roi.msg
+15
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
src/drivers/gimbal/gimbal.cpp
-704
View File
File diff suppressed because it is too large Load Diff
src/drivers/gimbal/gimbal_params.c
-74
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);
src/drivers/gimbal/CMakeLists.txt → src/drivers/vmount/CMakeLists.txt
+9 -6
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 :
src/drivers/vmount/vmount.cpp
+692
View File
File diff suppressed because it is too large Load Diff
src/drivers/vmount/vmount_mavlink.cpp
+178
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;
}
src/drivers/vmount/vmount_mavlink.h
+56
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 */
src/drivers/vmount/vmount_onboard.cpp
+193
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;
}
src/drivers/vmount/vmount_onboard.h
+59
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 */
src/drivers/vmount/vmount_params.c
+164
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);
src/drivers/vmount/vmount_rc.cpp
+155
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;
}
src/drivers/vmount/vmount_rc.h
+55
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 */
src/modules/commander/commander.cpp
+44 -7
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;
}
}