fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-15 12:51:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Feature: VIO: add ODOMETRY stream (#11084)

Browse Source 
* mavlink_messages: remove LOCAL_POSITION_NED_COV stream

* mavlink_messages.cpp: add ODOMETRY stream

* add MAV_ODOM_LP parameter to activate odometry loopback

* EKF2: add vehicle_odometry publisher

* Replace VISION_POSITION_ESTIMATE loopback with ODOMETRY

* LPE: add vehicle_odometry publisher

* set vehicle_odometry local_frame field

* mavlink_messages.cpp: ODOMETRY frame_id depends on MAV_ODOM_LP
This commit is contained in:
Nuno Marques
2018-12-21 12:54:04 -05:00
committed by James Goppert
parent b3018646d5
commit cfd1be584e
10 changed files with 250 additions and 115 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_messages.cpp
+84 -109
View File
@@ -2258,23 +2258,22 @@ protected:
}
};
//TODO: remove this -> add ODOMETRY loopback only
class MavlinkStreamVisionPositionEstimate : public MavlinkStream
class MavlinkStreamOdometry : public MavlinkStream
{
public:
const char *get_name() const
{
return MavlinkStreamVisionPositionEstimate::get_name_static();
return MavlinkStreamOdometry::get_name_static();
}
static const char *get_name_static()
{
return "VISION_POSITION_ESTIMATE";
return "ODOMETRY";
}
static uint16_t get_id_static()
{
return MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE;
return MAVLINK_MSG_ID_ODOMETRY;
}
uint16_t get_id()
@@ -2284,50 +2283,109 @@ public:
static MavlinkStream *new_instance(Mavlink *mavlink)
{
return new MavlinkStreamVisionPositionEstimate(mavlink);
return new MavlinkStreamOdometry(mavlink);
}
unsigned get_size()
{
return (_odom_time > 0) ? MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
return (_odom_time > 0) ? MAVLINK_MSG_ID_ODOMETRY_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
}
private:
private:
MavlinkOrbSubscription *_odom_sub;
uint64_t _odom_time;
MavlinkOrbSubscription *_vodom_sub;
uint64_t _vodom_time;
/* do not allow top copying this class */
MavlinkStreamVisionPositionEstimate(MavlinkStreamVisionPositionEstimate &) = delete;
MavlinkStreamVisionPositionEstimate &operator = (const MavlinkStreamVisionPositionEstimate &) = delete;
MavlinkStreamOdometry(MavlinkStreamOdometry &) = delete;
MavlinkStreamOdometry &operator = (const MavlinkStreamOdometry &) = delete;
protected:
explicit MavlinkStreamVisionPositionEstimate(Mavlink *mavlink) : MavlinkStream(mavlink),
_odom_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_visual_odometry))),
_odom_time(0)
explicit MavlinkStreamOdometry(Mavlink *mavlink) : MavlinkStream(mavlink),
_odom_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_odometry))),
_odom_time(0),
_vodom_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_visual_odometry))),
_vodom_time(0)
{}
bool send(const hrt_abstime t)
{
vehicle_odometry_s vodom;
vehicle_odometry_s odom;
// check if it is to send visual odometry loopback or not
bool odom_updated = false;
if (_odom_sub->update(&_odom_time, &vodom)) {
mavlink_vision_position_estimate_t vmsg = {};
vmsg.usec = vodom.timestamp;
vmsg.x = vodom.x;
vmsg.y = vodom.y;
vmsg.z = vodom.z;
mavlink_odometry_t msg = {};
matrix::Eulerf euler = matrix::Quatf(vodom.q);
vmsg.roll = euler.phi();
vmsg.pitch = euler.theta();
vmsg.yaw = euler.psi();
if (_send_odom_loopback.get()) {
odom_updated = _vodom_sub->update(&_vodom_time, &odom);
// frame matches the external vision system
msg.frame_id = MAV_FRAME_VISION_NED;
mavlink_msg_vision_position_estimate_send_struct(_mavlink->get_channel(), &vmsg);
} else {
odom_updated = _odom_sub->update(&_odom_time, &odom);
// frame matches the PX4 local NED frame
msg.frame_id = MAV_FRAME_ESTIM_NED;
}
if (odom_updated) {
msg.time_usec = odom.timestamp;
msg.child_frame_id = MAV_FRAME_BODY_NED;
// Current position
msg.x = odom.x;
msg.y = odom.y;
msg.z = odom.z;
// Current orientation
msg.q[0] = odom.q[0];
msg.q[1] = odom.q[1];
msg.q[2] = odom.q[2];
msg.q[3] = odom.q[3];
// Local NED to body-NED Dcm matrix
matrix::Dcmf Rlb(matrix::Quatf(odom.q));
// Rotate linear and angular velocity from local NED to body-NED frame
matrix::Vector3f linvel_body(Rlb * matrix::Vector3f(odom.vx, odom.vy, odom.vz));
// Current linear velocity
msg.vx = linvel_body(0);
msg.vy = linvel_body(1);
msg.vz = linvel_body(2);
// Current body rates
msg.rollspeed = odom.rollspeed;
msg.pitchspeed = odom.pitchspeed;
msg.yawspeed = odom.yawspeed;
// get the covariance matrix size
const size_t POS_URT_SIZE = sizeof(odom.pose_covariance) / sizeof(odom.pose_covariance[0]);
const size_t VEL_URT_SIZE = sizeof(odom.velocity_covariance) / sizeof(odom.velocity_covariance[0]);
static_assert(POS_URT_SIZE == (sizeof(msg.pose_covariance) / sizeof(msg.pose_covariance[0])),
"Odometry Pose Covariance matrix URT array size mismatch");
static_assert(VEL_URT_SIZE == (sizeof(msg.twist_covariance) / sizeof(msg.twist_covariance[0])),
"Odometry Velocity Covariance matrix URT array size mismatch");
// copy pose covariances
for (size_t i = 0; i < POS_URT_SIZE; i++) {
msg.pose_covariance[i] = odom.pose_covariance[i];
}
// copy velocity covariances
//TODO: Apply rotation matrix to transform from body-fixed NED to earth-fixed NED frame
for (size_t i = 0; i < VEL_URT_SIZE; i++) {
msg.twist_covariance[i] = odom.velocity_covariance[i];
}
mavlink_msg_odometry_send_struct(_mavlink->get_channel(), &msg);
return true;
}
return false;
}
};
@@ -2402,88 +2460,6 @@ protected:
}
};
class MavlinkStreamLocalPositionNEDCOV : public MavlinkStream
{
public:
const char *get_name() const
{
return MavlinkStreamLocalPositionNEDCOV::get_name_static();
}
static const char *get_name_static()
{
return "LOCAL_POSITION_NED_COV";
}
static uint16_t get_id_static()
{
return MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV;
}
uint16_t get_id()
{
return get_id_static();
}
static MavlinkStream *new_instance(Mavlink *mavlink)
{
return new MavlinkStreamLocalPositionNEDCOV(mavlink);
}
unsigned get_size()
{
return MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
}
private:
MavlinkOrbSubscription *_est_sub;
uint64_t _est_time;
/* do not allow top copying this class */
MavlinkStreamLocalPositionNEDCOV(MavlinkStreamLocalPositionNEDCOV &) = delete;
MavlinkStreamLocalPositionNEDCOV &operator = (const MavlinkStreamLocalPositionNEDCOV &) = delete;
protected:
explicit MavlinkStreamLocalPositionNEDCOV(Mavlink *mavlink) : MavlinkStream(mavlink),
_est_sub(_mavlink->add_orb_subscription(ORB_ID(estimator_status))),
_est_time(0)
{}
bool send(const hrt_abstime t)
{
struct estimator_status_s est = {};
if (_est_sub->update(&_est_time, &est)) {
mavlink_local_position_ned_cov_t msg = {};
msg.time_usec = est.timestamp;
msg.x = est.states[0];
msg.y = est.states[1];
msg.z = est.states[2];
msg.vx = est.states[3];
msg.vy = est.states[4];
msg.vz = est.states[5];
msg.ax = est.states[6];
msg.ay = est.states[7];
msg.az = est.states[8];
for (int i = 0; i < 9; i++) {
msg.covariance[i] = est.covariances[i];
}
msg.covariance[10] = est.health_flags;
msg.covariance[11] = est.timeout_flags;
mavlink_msg_local_position_ned_cov_send_struct(_mavlink->get_channel(), &msg);
return true;
}
return false;
}
};
class MavlinkStreamEstimatorStatus : public MavlinkStream
{
public:
@@ -4857,8 +4833,7 @@ static const StreamListItem streams_list[] = {
StreamListItem(&MavlinkStreamTimesync::new_instance, &MavlinkStreamTimesync::get_name_static, &MavlinkStreamTimesync::get_id_static),
StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static, &MavlinkStreamGlobalPositionInt::get_id_static),
StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static, &MavlinkStreamLocalPositionNED::get_id_static),
StreamListItem(&MavlinkStreamVisionPositionEstimate::new_instance, &MavlinkStreamVisionPositionEstimate::get_name_static, &MavlinkStreamVisionPositionEstimate::get_id_static),
StreamListItem(&MavlinkStreamLocalPositionNEDCOV::new_instance, &MavlinkStreamLocalPositionNEDCOV::get_name_static, &MavlinkStreamLocalPositionNEDCOV::get_id_static),
StreamListItem(&MavlinkStreamOdometry::new_instance, &MavlinkStreamOdometry::get_name_static, &MavlinkStreamOdometry::get_id_static),
StreamListItem(&MavlinkStreamEstimatorStatus::new_instance, &MavlinkStreamEstimatorStatus::get_name_static, &MavlinkStreamEstimatorStatus::get_id_static),
StreamListItem(&MavlinkStreamAttPosMocap::new_instance, &MavlinkStreamAttPosMocap::get_name_static, &MavlinkStreamAttPosMocap::get_id_static),
StreamListItem(&MavlinkStreamHomePosition::new_instance, &MavlinkStreamHomePosition::get_name_static, &MavlinkStreamHomePosition::get_id_static),