diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt
index cc702a6757..0110445f4c 100644
--- a/msg/CMakeLists.txt
+++ b/msg/CMakeLists.txt
@@ -74,7 +74,6 @@ set(msg_files
 	mission_result.msg
 	mount_orientation.msg
 	multirotor_motor_limits.msg
-	obstacle_avoidance.msg
 	obstacle_distance.msg
 	offboard_control_mode.msg
 	optical_flow.msg
@@ -107,6 +106,8 @@ set(msg_files
 	telemetry_status.msg
 	test_motor.msg
 	timesync_status.msg
+	trajectory_bezier.msg
+	trajectory_waypoint.msg
 	transponder_report.msg
 	tune_control.msg
 	uavcan_parameter_request.msg
diff --git a/msg/obstacle_avoidance.msg b/msg/obstacle_avoidance.msg
deleted file mode 100644
index 9bb025fb16..0000000000
--- a/msg/obstacle_avoidance.msg
+++ /dev/null
@@ -1,16 +0,0 @@
-# Output of a external avoidance. Obstacle avoidance message with trajectory points, PCA and FOV. See also Mavlink OBSTACLE_AVOIDANCE msg
-
-uint8 MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS = 0
-uint8 MAV_TRAJECTORY_REPRESENTATION_BEZIER = 1
-
-uint8 type # Type from MAV_TRAJECTORY_REPRESENTATION enum.
-
-float32[11] point_1    # Depending on the type (see MAV_TRAJECTORY_REPRESENTATION)
-float32[11] point_2    # Depending on the type (see MAV_TRAJECTORY_REPRESENTATION)
-float32[11] point_3    # Depending on the type (see MAV_TRAJECTORY_REPRESENTATION)
-float32[11] point_4    # Depending on the type (see MAV_TRAJECTORY_REPRESENTATION)
-float32[11] point_5    # Depending on the type (see MAV_TRAJECTORY_REPRESENTATION)
-
-bool[5] point_valid   # States if respective point is valid
-
-int16[4] field_of_view    # Field of View of the sensor/s being used. The first two parameters are used to indicate the direction (Azimuth: 0 - 360 degrees and elevation: -90 - 90 degrees) and the last two indicate the range (Azimuth and elevation). The range is applied symmetrically in the direction. Set first element to -1 to mark as unknown.
diff --git a/msg/trajectory_bezier.msg b/msg/trajectory_bezier.msg
new file mode 100644
index 0000000000..934b45ff69
--- /dev/null
+++ b/msg/trajectory_bezier.msg
@@ -0,0 +1,28 @@
+# Bezier Trajectory description. See also Mavlink TRAJECTORY msg
+
+uint8 POINT_0 = 0
+uint8 POINT_1 = 1
+uint8 POINT_2 = 2
+uint8 POINT_3 = 3
+uint8 POINT_4 = 4
+
+uint8 X = 0
+uint8 Y = 1
+uint8 Z = 2
+uint8 TIME_HORIZON = 3
+uint8 YAW = 4
+uint8 YAW_SPEED = 5
+
+uint8 MAV_TRAJECTORY_REPRESENTATION_BEZIER = 1
+
+uint8 type # Type from MAV_TRAJECTORY_REPRESENTATION enum.
+
+float32[6] point_0    # MAV_TRAJECTORY_REPRESENTATION_BEZIER, index 0-2 position sp, index 3 time horizon, index 9 yaw, index 10 yaw speed
+float32[6] point_1    # MAV_TRAJECTORY_REPRESENTATION_BEZIER, index 0-2 position sp, index 3 time horizon, index 9 yaw, index 10 yaw speed
+float32[6] point_2    # MAV_TRAJECTORY_REPRESENTATION_BEZIER, index 0-2 position sp, index 3 time horizon, index 9 yaw, index 10 yaw speed
+float32[6] point_3    # MAV_TRAJECTORY_REPRESENTATION_BEZIER, index 0-2 position sp, index 3 time horizon, index 9 yaw, index 10 yaw speed
+float32[6] point_4    # MAV_TRAJECTORY_REPRESENTATION_BEZIER, index 0-2 position sp, index 3 time horizon, index 9 yaw, index 10 yaw speed
+
+bool[5] point_valid   # States if respective point is valid
+
+# TOPICS trajectory_bezier trajectory_bezier_desired
diff --git a/msg/trajectory_waypoint.msg b/msg/trajectory_waypoint.msg
new file mode 100644
index 0000000000..4c380b5afb
--- /dev/null
+++ b/msg/trajectory_waypoint.msg
@@ -0,0 +1,33 @@
+# Waypoints Trajectory description. See also Mavlink TRAJECTORY msg
+
+uint8 POINT_0 = 0
+uint8 POINT_1 = 1
+uint8 POINT_2 = 2
+uint8 POINT_3 = 3
+uint8 POINT_4 = 4
+
+uint8 X = 0
+uint8 Y = 1
+uint8 Z = 2
+uint8 VX = 3
+uint8 VY = 4
+uint8 VZ = 5
+uint8 AX = 6
+uint8 AY = 7
+uint8 AZ = 8
+uint8 YAW = 9
+uint8 YAW_SPEED = 10
+
+uint8 MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS = 0
+
+uint8 type # Type from MAV_TRAJECTORY_REPRESENTATION enum.
+
+float32[11] point_0    # MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS, index 0-2 position sp, index 3-5 velocity sp, index 6-8 acceleration sp, index 9 yaw, index 10 yaw speed
+float32[11] point_1    # MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS, index 0-2 position sp, index 3-5 velocity sp, index 6-8 acceleration sp, index 9 yaw, index 10 yaw speed
+float32[11] point_2    # MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS, index 0-2 position sp, index 3-5 velocity sp, index 6-8 acceleration sp, index 9 yaw, index 10 yaw speed
+float32[11] point_3    # MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS, index 0-2 position sp, index 3-5 velocity sp, index 6-8 acceleration sp, index 9 yaw, index 10 yaw speed
+float32[11] point_4    # MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS, index 0-2 position sp, index 3-5 velocity sp, index 6-8 acceleration sp, index 9 yaw, index 10 yaw speed
+
+bool[5] point_valid   # States if respective point is valid
+
+# TOPICS trajectory_waypoint trajectory_waypoint_desired