From 8c0542bdb83f9fea22f8e93cf177e993a813b5d9 Mon Sep 17 00:00:00 2001 From: Nicolas de Palezieux Date: Sun, 14 Jan 2018 23:22:00 +0530 Subject: [PATCH] msgs : add support for precision landing --- msg/CMakeLists.txt | 3 +++ msg/commander_state.msg | 3 ++- msg/ekf2_innovations.msg | 1 + msg/irlock_report.msg | 9 +++++++++ msg/landing_target_innovations.msg | 7 +++++++ msg/landing_target_pose.msg | 24 ++++++++++++++++++++++++ msg/vehicle_command.msg | 1 + msg/vehicle_status.msg | 3 ++- 8 files changed, 49 insertions(+), 2 deletions(-) create mode 100644 msg/irlock_report.msg create mode 100644 msg/landing_target_innovations.msg create mode 100644 msg/landing_target_pose.msg diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index b061b1b06b..1d5ab1fc5d 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -62,6 +62,9 @@ set(msg_files gps_inject_data.msg home_position.msg input_rc.msg + irlock_report.msg + landing_target_innovations.msg + landing_target_pose.msg led_control.msg log_message.msg manual_control_setpoint.msg diff --git a/msg/commander_state.msg b/msg/commander_state.msg index a9c92ebb5e..00b5f1acac 100644 --- a/msg/commander_state.msg +++ b/msg/commander_state.msg @@ -12,7 +12,8 @@ uint8 MAIN_STATE_RATTITUDE = 9 uint8 MAIN_STATE_AUTO_TAKEOFF = 10 uint8 MAIN_STATE_AUTO_LAND = 11 uint8 MAIN_STATE_AUTO_FOLLOW_TARGET = 12 -uint8 MAIN_STATE_MAX = 13 +uint8 MAIN_STATE_AUTO_PRECLAND = 13 +uint8 MAIN_STATE_MAX = 14 uint8 main_state # main state machine diff --git a/msg/ekf2_innovations.msg b/msg/ekf2_innovations.msg index e60162dd9b..85fd48b89f 100644 --- a/msg/ekf2_innovations.msg +++ b/msg/ekf2_innovations.msg @@ -15,3 +15,4 @@ float32 hagl_innov_var # innovation variance from the terrain estimator float32[3] output_tracking_error # return a vector containing the output predictor angular, velocity and position tracking error magnitudes (rad), (m/s), (m) float32[2] drag_innov # drag specific force innovation (m/s/s) float32[2] drag_innov_var # drag specific force innovation variance (m/s/s)**2 +float32[2] aux_vel_innov # auxiliary NE velocity innovations from landing target measurement (m/s) diff --git a/msg/irlock_report.msg b/msg/irlock_report.msg new file mode 100644 index 0000000000..ded99a2581 --- /dev/null +++ b/msg/irlock_report.msg @@ -0,0 +1,9 @@ +# IRLOCK_REPORT message data + +uint16 signature + +# When looking along the optical axis of the camera, x points right, y points down, and z points along the optical axis. +float32 pos_x # tan(theta), where theta is the angle between the target and the camera center of projection in camera x-axis +float32 pos_y # tan(theta), where theta is the angle between the target and the camera center of projection in camera y-axis +float32 size_x #/** size of target along camera x-axis in units of tan(theta) **/ +float32 size_y #/** size of target along camera y-axis in units of tan(theta) **/ diff --git a/msg/landing_target_innovations.msg b/msg/landing_target_innovations.msg new file mode 100644 index 0000000000..73286aa595 --- /dev/null +++ b/msg/landing_target_innovations.msg @@ -0,0 +1,7 @@ +# Innovation of landing target position estimator +float32 innov_x +float32 innov_y + +# Innovation covariance of landing target position estimator +float32 innov_cov_x +float32 innov_cov_y diff --git a/msg/landing_target_pose.msg b/msg/landing_target_pose.msg new file mode 100644 index 0000000000..5a31fa953f --- /dev/null +++ b/msg/landing_target_pose.msg @@ -0,0 +1,24 @@ +# Relative position of precision land target in navigation (body fixed, north aligned, NED) and inertial (world fixed, north aligned, NED) frames + +bool is_static # Flag indicating whether the landing target is static or moving with respect to the ground + +bool rel_pos_valid # Flag showing whether relative position is valid +bool rel_vel_valid # Flag showing whether relative velocity is valid + +float32 x_rel # X/north position of target, relative to vehicle (navigation frame) [meters] +float32 y_rel # Y/east position of target, relative to vehicle (navigation frame) [meters] +float32 z_rel # Z/down position of target, relative to vehicle (navigation frame) [meters] + +float32 vx_rel # X/north velocity of target, relative to vehicle (navigation frame) [meters/second] +float32 vy_rel # Y/east velocity of target, relative to vehicle (navigation frame) [meters/second] + +float32 cov_x_rel # X/north position variance [meters^2] +float32 cov_y_rel # Y/east position variance [meters^2] + +float32 cov_vx_rel # X/north velocity variance [(meters/second)^2] +float32 cov_vy_rel # Y/east velocity variance [(meters/second)^2] + +bool abs_pos_valid # Flag showing whether absolute position is valid +float32 x_abs # X/north position of target, relative to origin (navigation frame) [meters] +float32 y_abs # Y/east position of target, relative to origin (navigation frame) [meters] +float32 z_abs # Z/down position of target, relative to origin (navigation frame) [meters] diff --git a/msg/vehicle_command.msg b/msg/vehicle_command.msg index 49f30232c7..b48dc4bafe 100644 --- a/msg/vehicle_command.msg +++ b/msg/vehicle_command.msg @@ -9,6 +9,7 @@ uint16 VEHICLE_CMD_NAV_LOITER_TIME = 19 # Loiter around this MISSION for X sec uint16 VEHICLE_CMD_NAV_RETURN_TO_LAUNCH = 20 # Return to launch location |Empty| Empty| Empty| Empty| Empty| Empty| Empty| uint16 VEHICLE_CMD_NAV_LAND = 21 # Land at location |Empty| Empty| Empty| Desired yaw angle.| Latitude| Longitude| Altitude| uint16 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| +uint16 VEHICLE_CMD_NAV_PRECLAND = 23 # Attempt a precision landing uint16 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| uint16 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| uint16 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| diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index a60f550aec..1d506eb088 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -33,7 +33,8 @@ uint8 NAVIGATION_STATE_RATTITUDE = 16 # Rattitude (aka "flip") mode uint8 NAVIGATION_STATE_AUTO_TAKEOFF = 17 # Takeoff uint8 NAVIGATION_STATE_AUTO_LAND = 18 # Land uint8 NAVIGATION_STATE_AUTO_FOLLOW_TARGET = 19 # Auto Follow -uint8 NAVIGATION_STATE_MAX = 20 +uint8 NAVIGATION_STATE_AUTO_PRECLAND = 20 # Precision land with landing target +uint8 NAVIGATION_STATE_MAX = 21 uint8 RC_IN_MODE_DEFAULT = 0 uint8 RC_IN_MODE_OFF = 1