add odometry reset counter (ekf2, mavlink, etc)

This commit is contained in:
Daniel Agar
2022-01-31 09:19:36 -05:00
parent 463ac8e8a1
commit 90358f078f
12 changed files with 69 additions and 11 deletions
+4
View File
@@ -1377,6 +1377,8 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
visual_odom.yawspeed = NAN;
visual_odom.velocity_covariance[0] = NAN;
visual_odom.reset_counter = ev.reset_counter;
_visual_odometry_pub.publish(visual_odom);
}
@@ -1444,6 +1446,8 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg)
PX4_ERR("Body frame %" PRIu8 " not supported. Unable to publish velocity", odom.child_frame_id);
}
odometry.reset_counter = odom.reset_counter;
/**
* Supported local frame of reference is MAV_FRAME_LOCAL_NED or MAV_FRAME_LOCAL_FRD
* The supported sources of the data/tesimator type are MAV_ESTIMATOR_TYPE_VISION,
+2
View File
@@ -159,6 +159,8 @@ private:
msg.velocity_covariance[i] = odom.velocity_covariance[i];
}
msg.reset_counter = odom.reset_counter;
mavlink_msg_odometry_send_struct(_mavlink->get_channel(), &msg);
return true;