diff --git a/msg/vehicle_attitude.msg b/msg/vehicle_attitude.msg index 6761765640..0f3827c027 100644 --- a/msg/vehicle_attitude.msg +++ b/msg/vehicle_attitude.msg @@ -4,4 +4,4 @@ float32 pitchspeed # Angular velocity about body east axis (y) in rad/s float32 yawspeed # Angular velocity about body down axis (z) in rad/s float32[4] q # Quaternion (NED) -# TOPICS vehicle_attitude vehicle_attitude_groundtruth +# TOPICS vehicle_attitude vehicle_attitude_groundtruth vehicle_vision_attitude diff --git a/msg/vehicle_local_position.msg b/msg/vehicle_local_position.msg index 503353a709..492dcb447a 100644 --- a/msg/vehicle_local_position.msg +++ b/msg/vehicle_local_position.msg @@ -1,5 +1,13 @@ # Fused local position in NED. +uint8 ESTIMATOR_TYPE_NAIVE = 1 # Estimator without real covariance feedback +uint8 ESTIMATOR_TYPE_VISION = 2 # Computer vision based estimate. Might be up to scale. +uint8 ESTIMATOR_TYPE_VIO = 3 # Visual-Inertial estimator. +uint8 ESTIMATOR_TYPE_GPS = 4 # Plain GPS estimate. +uint8 ESTIMATOR_TYPE_GPS_INS = 5 # Estimator integrating GPS and inertial sensing. + +uint8 estimator_type # type of estimator according to above types + bool xy_valid # true if x and y are valid bool z_valid # true if z is valid bool v_xy_valid # true if vy and vy are valid @@ -28,6 +36,11 @@ uint8 z_reset_counter uint8 vxy_reset_counter uint8 vz_reset_counter +# Acceleration in NED frame +float32 ax # North acceleration in NED earth-fixed frame, (metres/sec^2) +float32 ay # East acceleration in NED earth-fixed frame, (metres/sec^2) +float32 az # Down acceleration in NED earth-fixed frame, (metres/sec^2) + # Heading float32 yaw # Euler yaw angle transforming the tangent plane relative to NED earth-fixed frame, -PI..+PI, (radians) @@ -47,4 +60,4 @@ bool dist_bottom_valid # true if distance to bottom surface is valid float32 eph # Standard deviation of horizontal position error, (metres) float32 epv # Standard deviation of vertical position error, (metres) -# TOPICS vehicle_local_position vehicle_local_position_groundtruth +# TOPICS vehicle_local_position vehicle_local_position_groundtruth vehicle_vision_position diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 8e0861818f..ef8de3c849 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -121,6 +121,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _pos_sp_triplet_pub(nullptr), _att_pos_mocap_pub(nullptr), _vision_position_pub(nullptr), + _vision_attitude_pub(nullptr), _telemetry_status_pub(nullptr), _rc_pub(nullptr), _manual_pub(nullptr), @@ -133,6 +134,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _gps_inject_data_pub(nullptr), _command_ack_pub(nullptr), _control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))), + _global_ref_timestamp(0), _hil_frames(0), _old_timestamp(0), _hil_last_frame(0), @@ -216,6 +218,18 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_vision_position_estimate(msg); break; + case MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV: + handle_message_attitude_quaternion_cov(msg); + break; + + case MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV: + handle_message_local_position_ned_cov(msg); + break; + + case MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN: + handle_message_gps_global_origin(msg); + break; + case MAVLINK_MSG_ID_RADIO_STATUS: handle_message_radio_status(msg); break; @@ -1045,6 +1059,93 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m } +void +MavlinkReceiver::handle_message_gps_global_origin(mavlink_message_t *msg) +{ + mavlink_gps_global_origin_t origin; + mavlink_msg_gps_global_origin_decode(msg, &origin); + + if (!globallocalconverter_initialized()) { + /* Set reference point conversion of local coordiantes <--> global coordinates */ + globallocalconverter_init((double)origin.latitude * 1.0e-7, (double)origin.longitude * 1.0e-7, + (float)origin.altitude * 1.0e-3f, hrt_absolute_time()); + _global_ref_timestamp = hrt_absolute_time(); + + } +} + +void +MavlinkReceiver::handle_message_attitude_quaternion_cov(mavlink_message_t *msg) +{ + mavlink_attitude_quaternion_cov_t att; + mavlink_msg_attitude_quaternion_cov_decode(msg, &att); + + struct vehicle_attitude_s vision_attitude = {}; + + vision_attitude.timestamp = sync_stamp(att.time_usec); + + vision_attitude.q[0] = att.q[0]; + vision_attitude.q[1] = att.q[1]; + vision_attitude.q[2] = att.q[2]; + vision_attitude.q[3] = att.q[3]; + + vision_attitude.rollspeed = att.rollspeed; + vision_attitude.pitchspeed = att.pitchspeed; + vision_attitude.yawspeed = att.yawspeed; + + // TODO : full covariance matrix + + int instance_id = 0; + orb_publish_auto(ORB_ID(vehicle_vision_attitude), &_vision_attitude_pub, &vision_attitude, &instance_id, ORB_PRIO_HIGH); + +} + +void +MavlinkReceiver::handle_message_local_position_ned_cov(mavlink_message_t *msg) +{ + mavlink_local_position_ned_cov_t pos; + mavlink_msg_local_position_ned_cov_decode(msg, &pos); + + struct vehicle_local_position_s vision_position = {}; + + vision_position.timestamp = sync_stamp(pos.time_usec); + + // Use the estimator type to identify the external estimate + vision_position.estimator_type = pos.estimator_type; + + vision_position.xy_valid = true; + vision_position.z_valid = true; + vision_position.v_xy_valid = true; + vision_position.v_z_valid = true; + + vision_position.x = pos.x; + vision_position.y = pos.y; + vision_position.z = pos.z; + + vision_position.vx = pos.vx; + vision_position.vy = pos.vy; + vision_position.vz = pos.vz; + + vision_position.ax = pos.ax; + vision_position.ay = pos.ay; + vision_position.az = pos.az; + + // Low risk change for now. TODO : full covariance matrix + vision_position.eph = sqrtf(fmaxf(pos.covariance[0],pos.covariance[9])); + vision_position.epv = sqrtf(pos.covariance[17]); + + vision_position.xy_global = globallocalconverter_initialized(); + vision_position.z_global = globallocalconverter_initialized(); + vision_position.ref_timestamp = _global_ref_timestamp; + globallocalconverter_getref(&vision_position.ref_lat, &vision_position.ref_lon, &vision_position.ref_alt); + + vision_position.dist_bottom_valid = false; + + int instance_id = 0; + orb_publish_auto(ORB_ID(vehicle_vision_position), &_vision_position_pub, &vision_position, &instance_id, ORB_PRIO_HIGH); + +} + void MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index e92f166171..7e37d14cbb 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -59,7 +59,6 @@ #include #include #include -#include #include #include #include @@ -126,6 +125,9 @@ private: void handle_message_set_mode(mavlink_message_t *msg); void handle_message_att_pos_mocap(mavlink_message_t *msg); void handle_message_vision_position_estimate(mavlink_message_t *msg); + void handle_message_gps_global_origin(mavlink_message_t *msg); + void handle_message_attitude_quaternion_cov(mavlink_message_t *msg); + void handle_message_local_position_ned_cov(mavlink_message_t *msg); void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg); void handle_message_set_position_target_local_ned(mavlink_message_t *msg); void handle_message_set_actuator_control_target(mavlink_message_t *msg); @@ -218,6 +220,7 @@ private: orb_advert_t _pos_sp_triplet_pub; orb_advert_t _att_pos_mocap_pub; orb_advert_t _vision_position_pub; + orb_advert_t _vision_attitude_pub; orb_advert_t _telemetry_status_pub; orb_advert_t _rc_pub; orb_advert_t _manual_pub; @@ -231,6 +234,7 @@ private: orb_advert_t _gps_inject_data_pub; orb_advert_t _command_ack_pub; int _control_mode_sub; + uint64_t _global_ref_timestamp; int _hil_frames; uint64_t _old_timestamp; uint64_t _hil_last_frame; diff --git a/src/modules/systemlib/system_params.c b/src/modules/systemlib/system_params.c index 774e78823b..40649ee747 100644 --- a/src/modules/systemlib/system_params.c +++ b/src/modules/systemlib/system_params.c @@ -93,7 +93,7 @@ PARAM_DEFINE_INT32(SYS_RESTART_TYPE, 2); /** * Set multicopter estimator group * - * Set the group of estimators used for multicopters and vtols + * Set the group of estimators used for multicopters and VTOLs * * @value 1 local_position_estimator, attitude_estimator_q * @value 2 ekf2