yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00:00
Code Issues Packages Projects Releases Wiki Activity

general fixes on VIO data access

Browse Source
This commit is contained in:
TSC21
2018-07-26 15:53:46 +01:00
committed by Lorenz Meier
parent 7d7ee8e4bf
commit 8325724079
12 changed files with 56 additions and 141 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
ROMFS/px4fmu_common/init.d-posix/1013_iris_vision
View File

@@ -11,4 +11,7 @@ if [ $AUTOCNF = yes ]
then
param set EKF2_AID_MASK 8
param set EKF2_EV_DELAY 5
# LPE: Vision + baro
param set LPE_FUSION 132
fi

2
msg/vehicle_odometry.msg
View File

@@ -5,7 +5,7 @@ float32 x # North position
float32 y # East position
float32 z # Down position
# Orientation quaternion. NaN if invalid/unknown
# Orientation quaternion. First value NaN if invalid/unknown
float32[4] q # Quaternion rotation from NED earth-fixed frame to XYZ body frame
# Row-major representation of 6x6 pose cross-covariance matrix URT.

94
posix-configs/SITL/init/lpe/iris_vision
View File

@@ -1,94 +0,0 @@
uorb start
param load
param set MAV_TYPE 2
param set SYS_AUTOSTART 4010
param set SYS_RESTART_TYPE 2
param set SYS_MC_EST_GROUP 1
dataman start
param set BAT_N_CELLS 3
param set CAL_GYRO0_ID 2293768
param set CAL_ACC0_ID 1376264
param set CAL_ACC1_ID 1310728
param set CAL_MAG0_ID 196616
param set CAL_GYRO0_XOFF 0.01
param set CAL_ACC0_XOFF 0.01
param set CAL_ACC0_YOFF -0.01
param set CAL_ACC0_ZOFF 0.01
param set CAL_ACC0_XSCALE 1.01
param set CAL_ACC0_YSCALE 1.01
param set CAL_ACC0_ZSCALE 1.01
param set CAL_ACC1_XOFF 0.01
param set CAL_MAG0_XOFF 0.01
param set SENS_BOARD_ROT 0
param set SENS_BOARD_X_OFF 0.000001
param set COM_RC_IN_MODE 1
param set NAV_DLL_ACT 2
param set COM_DISARM_LAND 3
param set NAV_ACC_RAD 2.0
param set RTL_RETURN_ALT 30.0
param set RTL_DESCEND_ALT 10.0
param set RTL_LAND_DELAY 0
param set MIS_TAKEOFF_ALT 2.5
param set MC_ROLLRATE_P 0.3
param set MC_PITCHRATE_P 0.3
param set MC_PITCH_P 5.5
param set MC_ROLL_P 5.5
param set MC_ROLLRATE_I 0.1
param set MC_PITCHRATE_I 0.1
param set MPC_HOLD_MAX_Z 2.0
param set MPC_Z_VEL_P 0.8
param set MPC_Z_VEL_I 0.15
param set MPC_XY_VEL_P 0.15
param set MPC_XY_VEL_I 0.2
param set SDLOG_DIRS_MAX 7
param set MC_PITCH_P 5.0
param set MC_ROLL_P 5.0
param set MC_ROLLRATE_P 0.2
param set MC_PITCHRATE_P 0.2
param set MC_ROLLRATE_I 0.05
param set MC_PITCHRATE_I 0.05
param set MPC_ALT_MODE 1
param set LPE_T_MAX_GRADE 0
param set MPC_XY_VEL_MAX 2
param set MPC_Z_VEL_MAX_DN 2
param set MPC_TILTMAX_AIR 10
param set MPC_TILTMAX_LND 10
param set MPC_XY_P 0.5
param set MIS_TAKEOFF_ALT 2
param set NAV_ACC_RAD 1.0
param set CBRK_GPSFAIL 240024
# Vision + baro
param set LPE_FUSION 132
replay tryapplyparams
simulator start -s
tone_alarm start
gyrosim start
accelsim start
barosim start
gpssim start
pwm_out_sim start
sensors start
commander start
land_detector start multicopter
navigator start
attitude_estimator_q start
local_position_estimator start
mc_pos_control start
mc_att_control start
mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_w.main.mix
mavlink start -x -u 14556 -r 2000000
mavlink start -x -u 14557 -r 2000000 -m onboard -o 14540
mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556
mavlink stream -r 80 -s LOCAL_POSITION_NED -u 14556
mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556
mavlink stream -r 80 -s ATTITUDE -u 14556
mavlink stream -r 80 -s ATTITUDE_QUATERNION -u 14556
mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556
mavlink stream -r 20 -s RC_CHANNELS -u 14556
mavlink stream -r 250 -s HIGHRES_IMU -u 14556
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556
mavlink stream -r 80 -s VISION_POSITION_ESTIMATE -u 14556
logger start -e -t
mavlink boot_complete
replay trystart

14
src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
View File

@@ -349,9 +349,10 @@ void AttitudeEstimatorQ::task_main()
if (orb_copy(ORB_ID(vehicle_visual_odometry), _vision_odom_sub, &vision) == PX4_OK) {
// validation check for vision attitude data
bool vision_att_valid = !PX4_ISFINITE(vision.pose_covariance[0]) ? fabsf(sqrtf(fmaxf(vision.pose_covariance[15],
fmaxf(vision.pose_covariance[18],
vision.pose_covariance[20]))) - _eo_max_std_dev) < FLT_EPSILON : true;
bool vision_att_valid = PX4_ISFINITE(vision.q[0])
&& (PX4_ISFINITE(vision.pose_covariance[0]) ? fabsf(sqrtf(fmaxf(vision.pose_covariance[15],
fmaxf(vision.pose_covariance[18],
vision.pose_covariance[20]))) - _eo_max_std_dev) < FLT_EPSILON : true);
if (vision_att_valid) {
Dcmf Rvis = Quatf(vision.q);
@@ -379,9 +380,10 @@ void AttitudeEstimatorQ::task_main()
if (orb_copy(ORB_ID(vehicle_mocap_odometry), _mocap_odom_sub, &mocap) == PX4_OK) {
// validation check for mocap attitude data
bool mocap_att_valid = !PX4_ISFINITE(mocap.pose_covariance[0]) ? fabsf(sqrtf(fmaxf(mocap.pose_covariance[15],
fmaxf(mocap.pose_covariance[18],
mocap.pose_covariance[20]))) - _eo_max_std_dev) < FLT_EPSILON : true;
bool mocap_att_valid = PX4_ISFINITE(mocap.q[0])
&& (PX4_ISFINITE(mocap.pose_covariance[0]) ? fabsf(sqrtf(fmaxf(mocap.pose_covariance[15],
fmaxf(mocap.pose_covariance[18],
mocap.pose_covariance[20]))) - _eo_max_std_dev) < FLT_EPSILON : true);
if (mocap_att_valid) {
Dcmf Rmoc = Quatf(mocap.q);

50
src/modules/ekf2/ekf2_main.cpp
View File

@@ -221,7 +221,7 @@ private:
// set pose/velocity as invalid if standard deviation is bigger than max_std_dev
// TODO: the user should be allowed to set these values by a parameter
static constexpr float ep_max_std_dev = 100.0f; ///< Maximum permissible standard deviation for estimated position
//static constexpr float eo_max_std_dev = 100.0f; ///< Maximum permissible standard deviation for estimated orientation
static constexpr float eo_max_std_dev = 100.0f; ///< Maximum permissible standard deviation for estimated orientation
//static constexpr float ev_max_std_dev = 100.0f; ///< Maximum permissible standard deviation for estimated velocity
// GPS blending and switching
@@ -1160,35 +1160,43 @@ void Ekf2::run()
if (visual_odometry_updated) {
// copy both attitude & position, we need both to fill a single ext_vision_message
vehicle_odometry_s ev_odom = {};
vehicle_odometry_s ev_odom;
orb_copy(ORB_ID(vehicle_visual_odometry), _ev_odom_sub, &ev_odom);
ext_vision_message ev_data;
ev_data.posNED(0) = ev_odom.x;
ev_data.posNED(1) = ev_odom.y;
ev_data.posNED(2) = ev_odom.z;
ev_data.quat = matrix::Quatf(ev_odom.q);
// position measurement error from parameters
if (!PX4_ISFINITE(ev_odom.pose_covariance[0])) {
ev_data.posErr = fmaxf(_ev_pos_noise.get(), sqrtf(fmaxf(ev_odom.pose_covariance[0], ev_odom.pose_covariance[6])));
ev_data.hgtErr = fmaxf(_ev_pos_noise.get(), sqrtf(fmaxf(ev_odom.pose_covariance[11]));
} else {
ev_data.posErr = _ev_pos_noise.get();
ev_data.hgtErr = _ev_pos_noise.get();
// check for valid position data
if (PX4_ISFINITE(ev_odom.x) && PX4_ISFINITE(ev_odom.y) && PX4_ISFINITE(ev_odom.z)) {
ev_data.posNED(0) = ev_odom.x;
ev_data.posNED(1) = ev_odom.y;
ev_data.posNED(2) = ev_odom.z;
// position measurement error from parameters
if (PX4_ISFINITE(ev_odom.pose_covariance[0])) {
ev_data.posErr = fmaxf(_ev_pos_noise.get(), sqrtf(fmaxf(ev_odom.pose_covariance[0], ev_odom.pose_covariance[6])));
ev_data.hgtErr = fmaxf(_ev_pos_noise.get(), sqrtf(fmaxf(ev_odom.pose_covariance[11]));
} else {
ev_data.posErr = _ev_pos_noise.get();
ev_data.hgtErr = _ev_pos_noise.get();
}
}
// orientation measurement error from parameters
if (!PX4_ISFINITE(ev_odom.pose_covariance[0])) {
ev_data.angErr = fmaxf(_ev_ang_noise.get(), sqrtf(fmaxf(ev_odom.pose_covariance[15], fmaxf(ev_odom.pose_covariance[18],
ev_odom.pose_covariance[20]))));
// check for valid orientation data
if (PX4_ISFINITE(ev_odom.q[0])) {
ev_data.quat = matrix::Quatf(ev_odom.q);
} else {
ev_data.angErr = _ev_ang_noise.get();
// orientation measurement error from parameters
if (PX4_ISFINITE(ev_odom.pose_covariance[15])) {
ev_data.angErr = fmaxf(_ev_ang_noise.get(), sqrtf(fmaxf(ev_odom.pose_covariance[15], fmaxf(ev_odom.pose_covariance[18],
ev_odom.pose_covariance[20]))));
} else {
ev_data.angErr = _ev_ang_noise.get();
}
}
// only set data if all positions are valid
if (sqrtf(ev_odom.pose_covariance[0]) < ep_max_std_dev && sqrtf(ev_odom.pose_covariance[6]) < ep_max_std_dev) {
// only set data if all positions and orientation are valid
if (ev_data.posErr < ep_max_std_dev && ev_data.angErr < eo_max_std_dev) {
// use timestamp from external computer, clocks are synchronized when using MAVROS
_ekf.setExtVisionData(ev_odom.timestamp, &ev_data);
}

7
src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
View File

@@ -599,7 +599,7 @@ void BlockLocalPositionEstimator::publishLocalPos()
_pub_lpos.get().z = xLP(X_z); // down
}
_pub_gpos.get().yaw = matrix::Eulerf(_q).psi();
_pub_gpos.get().yaw = matrix::Eulerf(matrix::Quatf(_sub_att.get().q)).psi();
_pub_lpos.get().vx = xLP(X_vx); // north
_pub_lpos.get().vy = xLP(X_vy); // east
@@ -723,7 +723,7 @@ void BlockLocalPositionEstimator::publishGlobalPos()
_pub_gpos.get().vel_n = xLP(X_vx);
_pub_gpos.get().vel_e = xLP(X_vy);
_pub_gpos.get().vel_d = xLP(X_vz);
_pub_gpos.get().yaw = matrix::Eulerf(_q).psi();
_pub_gpos.get().yaw = matrix::Eulerf(matrix::Quatf(_sub_att.get().q)).psi();
_pub_gpos.get().eph = eph;
_pub_gpos.get().epv = epv;
_pub_gpos.get().terrain_alt = _altOrigin - xLP(X_tz);
@@ -827,8 +827,7 @@ void BlockLocalPositionEstimator::updateSSParams()
void BlockLocalPositionEstimator::predict()
{
// get acceleration
_q = matrix::Quatf(&_sub_att.get().q[0]);
_R_att = matrix::Dcm<float>(_q);
_R_att = matrix::Dcm<float>(matrix::Quatf(_sub_att.get().q));
Vector3f a(_sub_sensor.get().accelerometer_m_s2);
// note, bias is removed in dynamics function
_u = _R_att * a;

1
src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
View File

@@ -436,7 +436,6 @@ private:
Vector<float, n_u> _u; // input vector
Matrix<float, n_x, n_x> _P; // state covariance matrix
matrix::Quatf _q;
matrix::Dcm<float> _R_att;
Matrix<float, n_x, n_x> _A; // dynamics matrix

4
src/modules/local_position_estimator/sensors/mocap.cpp
View File

@@ -60,7 +60,7 @@ void BlockLocalPositionEstimator::mocapInit()
int BlockLocalPositionEstimator::mocapMeasure(Vector<float, n_y_mocap> &y)
{
if (!PX4_ISFINITE(_sub_mocap_odom.get().pose_covariance[0])) {
if (PX4_ISFINITE(_sub_mocap_odom.get().pose_covariance[0])) {
// check if the mocap data is valid based on the covariances
_mocap_eph = sqrtf(fmaxf(_sub_mocap_odom.get().pose_covariance[0], _sub_mocap_odom.get().pose_covariance[6]));
_mocap_epv = sqrtf(_sub_mocap_odom.get().pose_covariance[11]);
@@ -75,7 +75,7 @@ int BlockLocalPositionEstimator::mocapMeasure(Vector<float, n_y_mocap> &y)
if (!_mocap_xy_valid || !_mocap_z_valid) {
_time_last_mocap = _sub_mocap_odom.get().timestamp;
return !OK;
return -1;
} else {
y.setZero();

4
src/modules/local_position_estimator/sensors/vision.cpp
View File

@@ -65,7 +65,7 @@ void BlockLocalPositionEstimator::visionInit()
int BlockLocalPositionEstimator::visionMeasure(Vector<float, n_y_vision> &y)
{
if (!PX4_ISFINITE(_sub_visual_odom.get().pose_covariance[0])) {
if (PX4_ISFINITE(_sub_visual_odom.get().pose_covariance[0])) {
// check if the vision data is valid based on the covariances
_vision_eph = sqrtf(fmaxf(_sub_visual_odom.get().pose_covariance[0], _sub_visual_odom.get().pose_covariance[6]));
_vision_epv = sqrtf(_sub_visual_odom.get().pose_covariance[11]);
@@ -80,7 +80,7 @@ int BlockLocalPositionEstimator::visionMeasure(Vector<float, n_y_vision> &y)
if (!_vision_xy_valid || !_vision_z_valid) {
_time_last_vision_p = _sub_visual_odom.get().timestamp;
return !OK;
return -1;
} else {
y.setZero();

2
src/modules/mavlink/mavlink_messages.cpp
View File

@@ -2109,7 +2109,7 @@ protected:
bool send(const hrt_abstime t)
{
vehicle_odometry_s vodom = {};
vehicle_odometry_s vodom;
if (_odom_sub->update(&_odom_time, &vodom)) {
mavlink_vision_position_estimate_t vmsg = {};

3
src/modules/simulator/simulator.h
View File

@@ -344,8 +344,7 @@ private:
// class methods
int publish_sensor_topics(mavlink_hil_sensor_t *imu);
int publish_flow_topic(mavlink_hil_optical_flow_t *flow);
template<typename T>
int publish_odometry_topic(T *odom_mavlink);
int publish_odometry_topic(mavlink_message_t *odom_mavlink);
int publish_distance_topic(mavlink_distance_sensor_t *dist);
#ifndef __PX4_QURT

13
src/modules/simulator/simulator_mavlink.cpp
View File

@@ -1123,18 +1123,17 @@ int Simulator::publish_flow_topic(mavlink_hil_optical_flow_t *flow_mavlink)
return OK;
}
template<typename T>
int Simulator::publish_odometry_topic(T *msg)
int Simulator::publish_odometry_topic(mavlink_message_t *odom_mavlink)
{
uint64_t timestamp = hrt_absolute_time();
struct vehicle_odometry_s odom = {};
struct vehicle_odometry_s odom;
odom.timestamp = timestamp;
if (msg->msgid == MAVLINK_MSG_ID_ODOMETRY) {
if (odom_mavlink->msgid == MAVLINK_MSG_ID_ODOMETRY) {
mavlink_odometry_t odom_msg;
mavlink_msg_odometry_decode(msg, &odom_msg);
mavlink_msg_odometry_decode(odom_mavlink, &odom_msg);
/* Dcm rotation matrix from body frame to local NED frame */
matrix::Dcm<float> Rbl;
@@ -1175,9 +1174,9 @@ int Simulator::publish_odometry_topic(T *msg)
odom.velocity_covariance[i] = odom_msg.twist_covariance[i];
}
} else if (msg->msgid == MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE) {
} else if (odom_mavlink->msgid == MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE) {
mavlink_vision_position_estimate_t ev;
mavlink_msg_vision_position_estimate_decode(msg, &ev);
mavlink_msg_vision_position_estimate_decode(odom_mavlink, &ev);
/* The position in the local NED frame */
odom.x = ev.x;
odom.y = ev.y;