mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
Feature: VIO: add ODOMETRY stream (#11084)
* 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
James Goppert
parent
b3018646d5
commit
cfd1be584e
@ -1,3 +1,3 @@
|
||||
|
||||
# shellcheck disable=SC2154
|
||||
mavlink stream -r 10 -s VISION_POSITION_ESTIMATE -u $udp_gcs_port_local
|
||||
mavlink stream -r 50 -s DISTANCE_SENSOR -u $udp_gcs_port_local
|
||||
|
||||
@ -1,3 +1,3 @@
|
||||
|
||||
# shellcheck disable=SC2154
|
||||
mavlink stream -r 10 -s VISION_POSITION_ESTIMATE -u $udp_gcs_port_local
|
||||
mavlink stream -r 30 -s ODOMETRY -u $udp_gcs_port_local
|
||||
|
||||
@ -283,6 +283,7 @@ private:
|
||||
|
||||
uORB::Publication<vehicle_local_position_s> _vehicle_local_position_pub;
|
||||
uORB::Publication<vehicle_global_position_s> _vehicle_global_position_pub;
|
||||
uORB::Publication<vehicle_odometry_s> _vehicle_odometry_pub;
|
||||
|
||||
Ekf _ekf;
|
||||
|
||||
@ -509,6 +510,7 @@ Ekf2::Ekf2():
|
||||
_perf_ekf_update(perf_alloc_once(PC_ELAPSED, "EKF2 update")),
|
||||
_vehicle_local_position_pub(ORB_ID(vehicle_local_position)),
|
||||
_vehicle_global_position_pub(ORB_ID(vehicle_global_position)),
|
||||
_vehicle_odometry_pub(ORB_ID(vehicle_odometry)),
|
||||
_params(_ekf.getParamHandle()),
|
||||
_obs_dt_min_ms(_params->sensor_interval_min_ms),
|
||||
_mag_delay_ms(_params->mag_delay_ms),
|
||||
@ -1297,7 +1299,13 @@ void Ekf2::run()
|
||||
// generate vehicle local position data
|
||||
vehicle_local_position_s &lpos = _vehicle_local_position_pub.get();
|
||||
|
||||
// generate vehicle odometry data
|
||||
vehicle_odometry_s &odom = _vehicle_odometry_pub.get();
|
||||
|
||||
lpos.timestamp = now;
|
||||
odom.timestamp = lpos.timestamp;
|
||||
|
||||
odom.local_frame = odom.LOCAL_FRAME_NED;
|
||||
|
||||
// Position of body origin in local NED frame
|
||||
float position[3];
|
||||
@ -1308,6 +1316,11 @@ void Ekf2::run()
|
||||
lpos.y = (_ekf.local_position_is_valid()) ? position[1] : 0.0f;
|
||||
lpos.z = position[2];
|
||||
|
||||
// Vehicle odometry position
|
||||
odom.x = lpos.x;
|
||||
odom.y = lpos.y;
|
||||
odom.z = lpos.z;
|
||||
|
||||
// Velocity of body origin in local NED frame (m/s)
|
||||
float velocity[3];
|
||||
_ekf.get_velocity(velocity);
|
||||
@ -1315,6 +1328,11 @@ void Ekf2::run()
|
||||
lpos.vy = velocity[1];
|
||||
lpos.vz = velocity[2];
|
||||
|
||||
// Vehicle odometry linear velocity
|
||||
odom.vx = lpos.vx;
|
||||
odom.vy = lpos.vy;
|
||||
odom.vz = lpos.vz;
|
||||
|
||||
// vertical position time derivative (m/s)
|
||||
_ekf.get_pos_d_deriv(&lpos.z_deriv);
|
||||
|
||||
@ -1352,6 +1370,16 @@ void Ekf2::run()
|
||||
|
||||
lpos.yaw = matrix::Eulerf(q).psi();
|
||||
|
||||
// Vehicle odometry quaternion
|
||||
q.copyTo(odom.q);
|
||||
|
||||
// Vehicle odometry angular rates
|
||||
float gyro_bias[3];
|
||||
_ekf.get_gyro_bias(gyro_bias);
|
||||
odom.rollspeed = sensors.gyro_rad[0] - gyro_bias[0];
|
||||
odom.pitchspeed = sensors.gyro_rad[1] - gyro_bias[1];
|
||||
odom.yawspeed = sensors.gyro_rad[2] - gyro_bias[2];
|
||||
|
||||
lpos.dist_bottom_valid = _ekf.get_terrain_valid();
|
||||
|
||||
float terrain_vpos;
|
||||
@ -1394,9 +1422,43 @@ void Ekf2::run()
|
||||
lpos.hagl_max = INFINITY;
|
||||
}
|
||||
|
||||
// Get covariances to vehicle odometry
|
||||
float covariances[24];
|
||||
_ekf.get_covariances(covariances);
|
||||
|
||||
// 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]);
|
||||
|
||||
// initially set pose covariances to 0
|
||||
for (size_t i = 0; i < POS_URT_SIZE; i++) {
|
||||
odom.pose_covariance[i] = 0.0;
|
||||
}
|
||||
|
||||
// set the position variances
|
||||
odom.pose_covariance[odom.COVARIANCE_MATRIX_X_VARIANCE] = covariances[7];
|
||||
odom.pose_covariance[odom.COVARIANCE_MATRIX_Y_VARIANCE] = covariances[8];
|
||||
odom.pose_covariance[odom.COVARIANCE_MATRIX_Z_VARIANCE] = covariances[9];
|
||||
|
||||
// TODO: implement propagation from quaternion covariance to Euler angle covariance
|
||||
// by employing the covariance law
|
||||
|
||||
// initially set velocity covariances to 0
|
||||
for (size_t i = 0; i < VEL_URT_SIZE; i++) {
|
||||
odom.velocity_covariance[i] = 0.0;
|
||||
}
|
||||
|
||||
// set the linear velocity variances
|
||||
odom.velocity_covariance[odom.COVARIANCE_MATRIX_VX_VARIANCE] = covariances[4];
|
||||
odom.velocity_covariance[odom.COVARIANCE_MATRIX_VY_VARIANCE] = covariances[5];
|
||||
odom.velocity_covariance[odom.COVARIANCE_MATRIX_VZ_VARIANCE] = covariances[6];
|
||||
|
||||
// publish vehicle local position data
|
||||
_vehicle_local_position_pub.update();
|
||||
|
||||
// publish vehicle odometry data
|
||||
_vehicle_odometry_pub.update();
|
||||
|
||||
if (_ekf.global_position_is_valid() && !_preflt_fail) {
|
||||
// generate and publish global position data
|
||||
vehicle_global_position_s &global_pos = _vehicle_global_position_pub.get();
|
||||
|
||||
@ -51,6 +51,7 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
|
||||
// publications
|
||||
_pub_lpos(ORB_ID(vehicle_local_position), -1, &getPublications()),
|
||||
_pub_gpos(ORB_ID(vehicle_global_position), -1, &getPublications()),
|
||||
_pub_odom(ORB_ID(vehicle_odometry), -1, &getPublications()),
|
||||
_pub_est_status(ORB_ID(estimator_status), -1, &getPublications()),
|
||||
_pub_innov(ORB_ID(ekf2_innovations), -1, &getPublications()),
|
||||
|
||||
@ -509,6 +510,7 @@ void BlockLocalPositionEstimator::update()
|
||||
if (_altOriginInitialized) {
|
||||
// update all publications if possible
|
||||
publishLocalPos();
|
||||
publishOdom();
|
||||
publishEstimatorStatus();
|
||||
_pub_innov.get().timestamp = _timeStamp;
|
||||
_pub_innov.update();
|
||||
@ -637,6 +639,82 @@ void BlockLocalPositionEstimator::publishLocalPos()
|
||||
}
|
||||
}
|
||||
|
||||
void BlockLocalPositionEstimator::publishOdom()
|
||||
{
|
||||
const Vector<float, n_x> &xLP = _xLowPass.getState();
|
||||
|
||||
// publish vehicle odometry
|
||||
if (PX4_ISFINITE(_x(X_x)) && PX4_ISFINITE(_x(X_y)) && PX4_ISFINITE(_x(X_z)) &&
|
||||
PX4_ISFINITE(_x(X_vx)) && PX4_ISFINITE(_x(X_vy))
|
||||
&& PX4_ISFINITE(_x(X_vz))) {
|
||||
_pub_odom.get().timestamp = _timeStamp;
|
||||
_pub_odom.get().local_frame = _pub_odom.get().LOCAL_FRAME_NED;
|
||||
|
||||
// position
|
||||
_pub_odom.get().x = xLP(X_x); // north
|
||||
_pub_odom.get().y = xLP(X_y); // east
|
||||
|
||||
if (_fusion.get() & FUSE_PUB_AGL_Z) {
|
||||
_pub_odom.get().z = -_aglLowPass.getState(); // agl
|
||||
|
||||
} else {
|
||||
_pub_odom.get().z = xLP(X_z); // down
|
||||
}
|
||||
|
||||
// orientation
|
||||
matrix::Quatf q = matrix::Quatf(_sub_att.get().q);
|
||||
q.copyTo(_pub_odom.get().q);
|
||||
|
||||
// linear velocity
|
||||
_pub_odom.get().vx = xLP(X_vx); // vel north
|
||||
_pub_odom.get().vy = xLP(X_vy); // vel east
|
||||
_pub_odom.get().vz = xLP(X_vz); // vel down
|
||||
|
||||
// angular velocity
|
||||
_pub_odom.get().rollspeed = _sub_att.get().rollspeed; // roll rate
|
||||
_pub_odom.get().pitchspeed = _sub_att.get().pitchspeed; // pitch rate
|
||||
_pub_odom.get().yawspeed = _sub_att.get().yawspeed; // yaw rate
|
||||
|
||||
// get the covariance matrix size
|
||||
const size_t POS_URT_SIZE = sizeof(_pub_odom.get().pose_covariance) / sizeof(_pub_odom.get().pose_covariance[0]);
|
||||
const size_t VEL_URT_SIZE = sizeof(_pub_odom.get().velocity_covariance) / sizeof(
|
||||
_pub_odom.get().velocity_covariance[0]);
|
||||
|
||||
// initially set pose covariances to 0
|
||||
for (size_t i = 0; i < POS_URT_SIZE; i++) {
|
||||
_pub_odom.get().pose_covariance[i] = 0.0;
|
||||
}
|
||||
|
||||
// set the position variances
|
||||
_pub_odom.get().pose_covariance[_pub_odom.get().COVARIANCE_MATRIX_X_VARIANCE] = _P(X_vx, X_vx);
|
||||
_pub_odom.get().pose_covariance[_pub_odom.get().COVARIANCE_MATRIX_Y_VARIANCE] = _P(X_vy, X_vy);
|
||||
_pub_odom.get().pose_covariance[_pub_odom.get().COVARIANCE_MATRIX_Z_VARIANCE] = _P(X_vz, X_vz);
|
||||
|
||||
// unknown orientation covariances
|
||||
// TODO: add orientation covariance to vehicle_attitude
|
||||
_pub_odom.get().pose_covariance[_pub_odom.get().COVARIANCE_MATRIX_ROLL_VARIANCE] = NAN;
|
||||
_pub_odom.get().pose_covariance[_pub_odom.get().COVARIANCE_MATRIX_PITCH_VARIANCE] = NAN;
|
||||
_pub_odom.get().pose_covariance[_pub_odom.get().COVARIANCE_MATRIX_YAW_VARIANCE] = NAN;
|
||||
|
||||
// initially set velocity covariances to 0
|
||||
for (size_t i = 0; i < VEL_URT_SIZE; i++) {
|
||||
_pub_odom.get().velocity_covariance[i] = 0.0;
|
||||
}
|
||||
|
||||
// set the linear velocity variances
|
||||
_pub_odom.get().velocity_covariance[_pub_odom.get().COVARIANCE_MATRIX_VX_VARIANCE] = _P(X_vx, X_vx);
|
||||
_pub_odom.get().velocity_covariance[_pub_odom.get().COVARIANCE_MATRIX_VY_VARIANCE] = _P(X_vy, X_vy);
|
||||
_pub_odom.get().velocity_covariance[_pub_odom.get().COVARIANCE_MATRIX_VZ_VARIANCE] = _P(X_vz, X_vz);
|
||||
|
||||
// unknown angular velocity covariances
|
||||
_pub_odom.get().velocity_covariance[_pub_odom.get().COVARIANCE_MATRIX_ROLLRATE_VARIANCE] = NAN;
|
||||
_pub_odom.get().velocity_covariance[_pub_odom.get().COVARIANCE_MATRIX_PITCHRATE_VARIANCE] = NAN;
|
||||
_pub_odom.get().velocity_covariance[_pub_odom.get().COVARIANCE_MATRIX_YAWRATE_VARIANCE] = NAN;
|
||||
|
||||
_pub_odom.update();
|
||||
}
|
||||
}
|
||||
|
||||
void BlockLocalPositionEstimator::publishEstimatorStatus()
|
||||
{
|
||||
_pub_est_status.get().timestamp = _timeStamp;
|
||||
|
||||
@ -239,6 +239,7 @@ private:
|
||||
// publications
|
||||
void publishLocalPos();
|
||||
void publishGlobalPos();
|
||||
void publishOdom();
|
||||
void publishEstimatorStatus();
|
||||
|
||||
// attributes
|
||||
@ -267,6 +268,7 @@ private:
|
||||
// publications
|
||||
uORB::Publication<vehicle_local_position_s> _pub_lpos;
|
||||
uORB::Publication<vehicle_global_position_s> _pub_gpos;
|
||||
uORB::Publication<vehicle_odometry_s> _pub_odom;
|
||||
uORB::Publication<estimator_status_s> _pub_est_status;
|
||||
uORB::Publication<ekf2_innovations_s> _pub_innov;
|
||||
|
||||
|
||||
@ -1775,7 +1775,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
|
||||
configure_stream_local("TRAJECTORY_REPRESENTATION_WAYPOINTS", 5.0f);
|
||||
configure_stream_local("UTM_GLOBAL_POSITION", 1.0f);
|
||||
configure_stream_local("VFR_HUD", 4.0f);
|
||||
configure_stream_local("VISION_POSITION_ESTIMATE", 1.0f);
|
||||
configure_stream_local("ODOMETRY", 3.0f);
|
||||
configure_stream_local("WIND_COV", 1.0f);
|
||||
configure_stream_local("ORBIT_EXECUTION_STATUS", 5.f);
|
||||
break;
|
||||
@ -1818,7 +1818,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
|
||||
configure_stream_local("TRAJECTORY_REPRESENTATION_WAYPOINTS", 5.0f);
|
||||
configure_stream_local("UTM_GLOBAL_POSITION", 1.0f);
|
||||
configure_stream_local("VFR_HUD", 10.0f);
|
||||
configure_stream_local("VISION_POSITION_ESTIMATE", 10.0f);
|
||||
configure_stream_local("ODOMETRY", 30.0f);
|
||||
configure_stream_local("WIND_COV", 10.0f);
|
||||
configure_stream_local("ORBIT_EXECUTION_STATUS", 5.f);
|
||||
break;
|
||||
@ -1884,7 +1884,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
|
||||
configure_stream_local("TIMESYNC", 10.0f);
|
||||
configure_stream_local("UTM_GLOBAL_POSITION", 1.0f);
|
||||
configure_stream_local("VFR_HUD", 20.0f);
|
||||
configure_stream_local("VISION_POSITION_ESTIMATE", 10.0f);
|
||||
configure_stream_local("ODOMETRY", 30.0f);
|
||||
configure_stream_local("WIND_COV", 10.0f);
|
||||
configure_stream_local("ORBIT_EXECUTION_STATUS", 5.f);
|
||||
break;
|
||||
|
||||
@ -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),
|
||||
|
||||
@ -164,3 +164,14 @@ PARAM_DEFINE_INT32(MAV_HASH_CHK_EN, 1);
|
||||
* @group MAVLink
|
||||
*/
|
||||
PARAM_DEFINE_INT32(MAV_HB_FORW_EN, 1);
|
||||
|
||||
/**
|
||||
* Activate ODOMETRY loopback.
|
||||
*
|
||||
* If set, it gets the data from 'vehicle_visual_odometry' instead of 'vehicle_odometry'
|
||||
* serving as a loopback of the received ODOMETRY messages on the Mavlink receiver.
|
||||
*
|
||||
* @boolean
|
||||
* @group MAVLink
|
||||
*/
|
||||
PARAM_DEFINE_INT32(MAV_ODOM_LP, 0);
|
||||
|
||||
@ -45,6 +45,7 @@
|
||||
#include "mavlink_main.h"
|
||||
|
||||
MavlinkStream::MavlinkStream(Mavlink *mavlink) :
|
||||
ModuleParams(nullptr),
|
||||
_mavlink(mavlink)
|
||||
{
|
||||
_last_sent = hrt_absolute_time();
|
||||
@ -57,6 +58,7 @@ int
|
||||
MavlinkStream::update(const hrt_abstime &t)
|
||||
{
|
||||
update_data();
|
||||
ModuleParams::updateParams();
|
||||
|
||||
// If the message has never been sent before we want
|
||||
// to send it immediately and can return right away
|
||||
|
||||
@ -42,10 +42,11 @@
|
||||
#define MAVLINK_STREAM_H_
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <px4_module_params.h>
|
||||
|
||||
class Mavlink;
|
||||
|
||||
class MavlinkStream
|
||||
class MavlinkStream : public ModuleParams
|
||||
{
|
||||
|
||||
public:
|
||||
@ -126,6 +127,10 @@ protected:
|
||||
*/
|
||||
virtual void update_data() { }
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamBool<px4::params::MAV_ODOM_LP>) _send_odom_loopback
|
||||
)
|
||||
|
||||
private:
|
||||
hrt_abstime _last_sent{0};
|
||||
bool _first_message_sent{false};
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user