forked from Archive/PX4-Autopilot
LPE vision estimation fixes. (#5505)
This commit is contained in:
James Goppert
GitHub
parent
f6bed6f2d2
commit
fe40e9cfae
|
|
@ -6,11 +6,13 @@
|
|||
<arg name="gcs_url" default="udp://:14550@:14555" />
|
||||
<arg name="tgt_system" default="1" />
|
||||
<arg name="tgt_component" default="1" />
|
||||
<arg name="pluginlists_yaml" default="$(find mavros)/launch/px4_pluginlists.yaml" />
|
||||
<arg name="config_yaml" default="$(find mavros)/launch/px4_config.yaml" />
|
||||
|
||||
<group ns="$(arg ns)">
|
||||
<include file="$(find mavros)/launch/node.launch">
|
||||
<arg name="pluginlists_yaml" value="$(find mavros)/launch/px4_pluginlists.yaml" />
|
||||
<arg name="config_yaml" value="$(find mavros)/launch/px4_config.yaml" />
|
||||
<arg name="pluginlists_yaml" value="$(arg pluginlists_yaml)" />
|
||||
<arg name="config_yaml" value="$(arg config_yaml)" />
|
||||
|
||||
<arg name="fcu_url" value="$(arg fcu_url)" />
|
||||
<arg name="gcs_url" value="$(arg gcs_url)" />
|
||||
|
|
@ -19,3 +21,5 @@
|
|||
</include>
|
||||
</group>
|
||||
</launch>
|
||||
|
||||
<!-- vim: set ft=xml et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : -->
|
||||
|
|
|
|||
|
|
@ -25,6 +25,9 @@
|
|||
<arg name="verbose" default="false"/>
|
||||
<arg name="paused" default="false"/>
|
||||
|
||||
<arg name="pluginlists_yaml" default="$(find mavros)/launch/px4_pluginlists.yaml" />
|
||||
<arg name="config_yaml" default="$(find mavros)/launch/px4_config.yaml" />
|
||||
|
||||
<include file="$(find px4)/launch/posix_sitl.launch">
|
||||
<arg name="x" value="$(arg x)"/>
|
||||
<arg name="y" value="$(arg y)"/>
|
||||
|
|
@ -48,6 +51,8 @@
|
|||
<arg name="ns" value="$(arg ns)"/>
|
||||
<arg name="gcs_url" value=""/> <!-- GCS link is provided by SITL -->
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="pluginlists_yaml" value="$(arg pluginlists_yaml)" />
|
||||
<arg name="config_yaml" value="$(arg config_yaml)" />
|
||||
</include>
|
||||
</launch>
|
||||
|
||||
|
|
|
|||
|
|
@ -7,7 +7,6 @@
|
|||
<arg name="R" default="0"/>
|
||||
<arg name="P" default="0"/>
|
||||
<arg name="Y" default="0"/>
|
||||
|
||||
<arg name="est" default="lpe"/>
|
||||
<arg name="vehicle" default="iris"/>
|
||||
<arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/empty.world"/>
|
||||
|
|
|
|||
|
|
@ -57,7 +57,8 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
|
|||
_map_ref(),
|
||||
|
||||
// block parameters
|
||||
_xy_pub_thresh(this, "XY_PUB"),
|
||||
_pub_agl_z(this, "PUB_AGL_Z"),
|
||||
_vxy_pub_thresh(this, "VXY_PUB"),
|
||||
_z_pub_thresh(this, "Z_PUB"),
|
||||
_sonar_z_stddev(this, "SNR_Z"),
|
||||
_sonar_z_offset(this, "SNR_OFF_Z"),
|
||||
|
|
@ -78,9 +79,11 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
|
|||
_vision_z_stddev(this, "VIS_Z"),
|
||||
_vision_on(this, "VIS_ON"),
|
||||
_mocap_p_stddev(this, "VIC_P"),
|
||||
_flow_gyro_comp(this, "FLW_GYRO_CMP"),
|
||||
_flow_z_offset(this, "FLW_OFF_Z"),
|
||||
_flow_xy_stddev(this, "FLW_XY"),
|
||||
_flow_xy_d_stddev(this, "FLW_XY_D"),
|
||||
_flow_vxy_stddev(this, "FLW_VXY"),
|
||||
_flow_vxy_d_stddev(this, "FLW_VXY_D"),
|
||||
_flow_vxy_r_stddev(this, "FLW_VXY_R"),
|
||||
//_flow_board_x_offs(NULL, "SENS_FLW_XOFF"),
|
||||
//_flow_board_y_offs(NULL, "SENS_FLW_YOFF"),
|
||||
_flow_min_q(this, "FLW_QMIN"),
|
||||
|
|
@ -147,12 +150,6 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
|
|||
_altOriginInitialized(false),
|
||||
_baroAltOrigin(0),
|
||||
_gpsAltOrigin(0),
|
||||
_visionOrigin(),
|
||||
_mocapOrigin(),
|
||||
|
||||
// flow integration
|
||||
_flowX(0),
|
||||
_flowY(0),
|
||||
|
||||
// status
|
||||
_validXY(false),
|
||||
|
|
@ -266,10 +263,6 @@ void BlockLocalPositionEstimator::update()
|
|||
// _x(X_y) = 0;
|
||||
// // reset Z or not? _x(X_z) = 0;
|
||||
|
||||
// // reset flow integral
|
||||
// _flowX = 0;
|
||||
// _flowY = 0;
|
||||
|
||||
// // we aren't moving, all velocities are zero
|
||||
// _x(X_vx) = 0;
|
||||
// _x(X_vy) = 0;
|
||||
|
|
@ -305,19 +298,25 @@ void BlockLocalPositionEstimator::update()
|
|||
}
|
||||
|
||||
// is xy valid?
|
||||
bool xy_stddev_ok = sqrtf(math::max(_P(X_x, X_x), _P(X_y, X_y))) < _xy_pub_thresh.get();
|
||||
bool vxy_stddev_ok = false;
|
||||
|
||||
if (math::max(_P(X_vx, X_vx), _P(X_vy, X_vy)) < _vxy_pub_thresh.get()*_vxy_pub_thresh.get()) {
|
||||
vxy_stddev_ok = true;
|
||||
}
|
||||
|
||||
if (_validXY) {
|
||||
// if valid and gps has timed out, set to not valid
|
||||
if (!xy_stddev_ok && !_gpsInitialized) {
|
||||
if (!vxy_stddev_ok && !_gpsInitialized) {
|
||||
_validXY = false;
|
||||
}
|
||||
|
||||
} else {
|
||||
if (xy_stddev_ok) {
|
||||
if (vxy_stddev_ok) {
|
||||
if (_flowInitialized || _gpsInitialized || _visionInitialized || _mocapInitialized) {
|
||||
_validXY = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// is z valid?
|
||||
bool z_stddev_ok = sqrtf(_P(X_z, X_z)) < _z_pub_thresh.get();
|
||||
|
|
@ -380,6 +379,7 @@ void BlockLocalPositionEstimator::update()
|
|||
// don't want it to take too long
|
||||
if (!PX4_ISFINITE(_x(i))) {
|
||||
reinit_x = true;
|
||||
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] reinit x, x(%d) not finite", i);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
|
@ -684,7 +684,14 @@ void BlockLocalPositionEstimator::publishLocalPos()
|
|||
_pub_lpos.get().v_z_valid = _validZ;
|
||||
_pub_lpos.get().x = xLP(X_x); // north
|
||||
_pub_lpos.get().y = xLP(X_y); // east
|
||||
|
||||
if (_pub_agl_z.get()) {
|
||||
_pub_lpos.get().z = -_aglLowPass.getState(); // agl
|
||||
|
||||
} else {
|
||||
_pub_lpos.get().z = xLP(X_z); // down
|
||||
}
|
||||
|
||||
_pub_lpos.get().vx = xLP(X_vx); // north
|
||||
_pub_lpos.get().vy = xLP(X_vy); // east
|
||||
_pub_lpos.get().vz = xLP(X_vz); // down
|
||||
|
|
|
|||
|
|
@ -126,7 +126,7 @@ public:
|
|||
enum {U_ax = 0, U_ay, U_az, n_u};
|
||||
enum {Y_baro_z = 0, n_y_baro};
|
||||
enum {Y_lidar_z = 0, n_y_lidar};
|
||||
enum {Y_flow_x = 0, Y_flow_y, n_y_flow};
|
||||
enum {Y_flow_vx = 0, Y_flow_vy, n_y_flow};
|
||||
enum {Y_sonar_z = 0, n_y_sonar};
|
||||
enum {Y_gps_x = 0, Y_gps_y, Y_gps_z, Y_gps_vx, Y_gps_vy, Y_gps_vz, n_y_gps};
|
||||
enum {Y_vision_x = 0, Y_vision_y, Y_vision_z, n_y_vision};
|
||||
|
|
@ -244,7 +244,8 @@ private:
|
|||
struct map_projection_reference_s _map_ref;
|
||||
|
||||
// general parameters
|
||||
BlockParamFloat _xy_pub_thresh;
|
||||
BlockParamInt _pub_agl_z;
|
||||
BlockParamFloat _vxy_pub_thresh;
|
||||
BlockParamFloat _z_pub_thresh;
|
||||
|
||||
// sonar parameters
|
||||
|
|
@ -281,9 +282,11 @@ private:
|
|||
BlockParamFloat _mocap_p_stddev;
|
||||
|
||||
// flow parameters
|
||||
BlockParamInt _flow_gyro_comp;
|
||||
BlockParamFloat _flow_z_offset;
|
||||
BlockParamFloat _flow_xy_stddev;
|
||||
BlockParamFloat _flow_xy_d_stddev;
|
||||
BlockParamFloat _flow_vxy_stddev;
|
||||
BlockParamFloat _flow_vxy_d_stddev;
|
||||
BlockParamFloat _flow_vxy_r_stddev;
|
||||
//BlockParamFloat _flow_board_x_offs;
|
||||
//BlockParamFloat _flow_board_y_offs;
|
||||
BlockParamInt _flow_min_q;
|
||||
|
|
@ -351,12 +354,6 @@ private:
|
|||
bool _altOriginInitialized;
|
||||
float _baroAltOrigin;
|
||||
float _gpsAltOrigin;
|
||||
Vector3f _visionOrigin;
|
||||
Vector3f _mocapOrigin;
|
||||
|
||||
// flow integration
|
||||
float _flowX;
|
||||
float _flowY;
|
||||
|
||||
// status
|
||||
bool _validXY;
|
||||
|
|
|
|||
|
|
@ -2,6 +2,13 @@
|
|||
|
||||
// 16 is max name length
|
||||
|
||||
/**
|
||||
* Publish AGL as Z
|
||||
*
|
||||
* @group Local Position Estimator
|
||||
* @boolean
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LPE_PUB_AGL_Z, 0);
|
||||
|
||||
/**
|
||||
* Optical flow z offset from center
|
||||
|
|
@ -15,7 +22,18 @@
|
|||
PARAM_DEFINE_FLOAT(LPE_FLW_OFF_Z, 0.0f);
|
||||
|
||||
/**
|
||||
* Optical flow xy standard deviation.
|
||||
* Optical flow gyro compensation
|
||||
*
|
||||
* @group Local Position Estimator
|
||||
* @unit m
|
||||
* @min -1
|
||||
* @max 1
|
||||
* @decimal 3
|
||||
*/
|
||||
PARAM_DEFINE_INT32(LPE_FLW_GYRO_CMP, 1);
|
||||
|
||||
/**
|
||||
* Optical flow xy velocity standard deviation.
|
||||
*
|
||||
* @group Local Position Estimator
|
||||
* @unit m
|
||||
|
|
@ -23,10 +41,10 @@ PARAM_DEFINE_FLOAT(LPE_FLW_OFF_Z, 0.0f);
|
|||
* @max 1
|
||||
* @decimal 3
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LPE_FLW_XY, 0.01f);
|
||||
PARAM_DEFINE_FLOAT(LPE_FLW_VXY, 0.04f);
|
||||
|
||||
/**
|
||||
* Optical flow xy standard deviation linear factor on distance
|
||||
* Optical flow xy velocity standard deviation linear factor on distance
|
||||
*
|
||||
* @group Local Position Estimator
|
||||
* @unit m / m
|
||||
|
|
@ -34,7 +52,18 @@ PARAM_DEFINE_FLOAT(LPE_FLW_XY, 0.01f);
|
|||
* @max 1
|
||||
* @decimal 3
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LPE_FLW_XY_D, 0.01f);
|
||||
PARAM_DEFINE_FLOAT(LPE_FLW_VXY_D, 0.04f);
|
||||
|
||||
/**
|
||||
* Optical flow xy velocity standard deviation linear factor on rotation rate
|
||||
*
|
||||
* @group Local Position Estimator
|
||||
* @unit m / m
|
||||
* @min 0.01
|
||||
* @max 1
|
||||
* @decimal 3
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LPE_FLW_VXY_R, 1.0f);
|
||||
|
||||
/**
|
||||
* Optical flow minimum quality threshold
|
||||
|
|
@ -44,7 +73,7 @@ PARAM_DEFINE_FLOAT(LPE_FLW_XY_D, 0.01f);
|
|||
* @max 255
|
||||
* @decimal 0
|
||||
*/
|
||||
PARAM_DEFINE_INT32(LPE_FLW_QMIN, 75);
|
||||
PARAM_DEFINE_INT32(LPE_FLW_QMIN, 150);
|
||||
|
||||
/**
|
||||
* Sonar z standard deviation.
|
||||
|
|
@ -225,7 +254,7 @@ PARAM_DEFINE_FLOAT(LPE_EPV_MAX, 5.0f);
|
|||
* @max 1
|
||||
* @decimal 3
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LPE_VIS_XY, 0.5f);
|
||||
PARAM_DEFINE_FLOAT(LPE_VIS_XY, 0.1f);
|
||||
|
||||
/**
|
||||
* Vision z standard deviation.
|
||||
|
|
@ -328,7 +357,7 @@ PARAM_DEFINE_FLOAT(LPE_T_MAX_GRADE, 1.0f);
|
|||
* @max 2
|
||||
* @decimal 3
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LPE_FGYRO_HP, 0.1f);
|
||||
PARAM_DEFINE_FLOAT(LPE_FGYRO_HP, 0.001f);
|
||||
|
||||
/**
|
||||
* Local origin latitude for nav w/o GPS
|
||||
|
|
@ -364,15 +393,15 @@ PARAM_DEFINE_FLOAT(LPE_LON, -86.929);
|
|||
PARAM_DEFINE_FLOAT(LPE_X_LP, 5.0f);
|
||||
|
||||
/**
|
||||
* Required xy standard deviation to publish position
|
||||
* Required velocity xy standard deviation to publish position
|
||||
*
|
||||
* @group Local Position Estimator
|
||||
* @unit m
|
||||
* @min 0.3
|
||||
* @max 5.0
|
||||
* @decimal 1
|
||||
* @unit m/s
|
||||
* @min 0.01
|
||||
* @max 1.0
|
||||
* @decimal 3
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LPE_XY_PUB, 1.0f);
|
||||
PARAM_DEFINE_FLOAT(LPE_VXY_PUB, 0.1f);
|
||||
|
||||
/**
|
||||
* Required z standard deviation to publish altitude/ terrain
|
||||
|
|
|
|||
|
|
@ -29,10 +29,6 @@ void BlockLocalPositionEstimator::flowInit()
|
|||
"quality %d std %d",
|
||||
int(_flowQStats.getMean()(0)),
|
||||
int(_flowQStats.getStdDev()(0)));
|
||||
// set flow x, y as estimate x, y at beginning of optical
|
||||
// flow tracking
|
||||
_flowX = _x(X_x);
|
||||
_flowY = _x(X_y);
|
||||
_flowInitialized = true;
|
||||
_flowFault = FAULT_NONE;
|
||||
}
|
||||
|
|
@ -46,6 +42,11 @@ void BlockLocalPositionEstimator::flowDeinit()
|
|||
|
||||
int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
|
||||
{
|
||||
// check for sane pitch/roll
|
||||
if (_sub_att.get().roll > 0.5f || _sub_att.get().pitch > 0.5f) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
// check for agl
|
||||
if (agl() < flow_min_agl) {
|
||||
return -1;
|
||||
|
|
@ -65,33 +66,25 @@ int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
|
|||
|
||||
float d = agl() * cosf(_sub_att.get().roll) * cosf(_sub_att.get().pitch);
|
||||
|
||||
// check for global accuracy
|
||||
if (_gpsInitialized) {
|
||||
double lat = _sub_gps.get().lat * 1.0e-7;
|
||||
double lon = _sub_gps.get().lon * 1.0e-7;
|
||||
float px = 0;
|
||||
float py = 0;
|
||||
map_projection_project(&_map_ref, lat, lon, &px, &py);
|
||||
Vector2f delta(px - _flowX, py - _flowY);
|
||||
|
||||
if (delta.norm() > 3) {
|
||||
mavlink_and_console_log_info(&mavlink_log_pub,
|
||||
"[lpe] flow too far from GPS, resetting position");
|
||||
_flowX = px;
|
||||
_flowY = py;
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
// optical flow in x, y axis
|
||||
float flow_x_rad = _sub_flow.get().pixel_flow_x_integral;
|
||||
float flow_y_rad = _sub_flow.get().pixel_flow_y_integral;
|
||||
float dt_flow = _sub_flow.get().integration_timespan / 1.0e6f;
|
||||
|
||||
if (dt_flow > 0.5f || dt_flow < 1.0e-6f) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
// angular rotation in x, y axis
|
||||
float gyro_x_rad = _flow_gyro_x_high_pass.update(
|
||||
float gyro_x_rad = 0;
|
||||
float gyro_y_rad = 0;
|
||||
|
||||
if (_flow_gyro_comp.get()) {
|
||||
gyro_x_rad = _flow_gyro_x_high_pass.update(
|
||||
_sub_flow.get().gyro_x_rate_integral);
|
||||
float gyro_y_rad = _flow_gyro_y_high_pass.update(
|
||||
gyro_y_rad = _flow_gyro_y_high_pass.update(
|
||||
_sub_flow.get().gyro_y_rate_integral);
|
||||
}
|
||||
|
||||
//warnx("flow x: %10.4f y: %10.4f gyro_x: %10.4f gyro_y: %10.4f d: %10.4f",
|
||||
//double(flow_x_rad), double(flow_y_rad), double(gyro_x_rad), double(gyro_y_rad), double(d));
|
||||
|
|
@ -107,19 +100,15 @@ int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
|
|||
Matrix3f R_nb(_sub_att.get().R);
|
||||
Vector3f delta_n = R_nb * delta_b;
|
||||
|
||||
// flow integration
|
||||
_flowX += delta_n(0);
|
||||
_flowY += delta_n(1);
|
||||
|
||||
// measurement
|
||||
y(Y_flow_x) = _flowX;
|
||||
y(Y_flow_y) = _flowY;
|
||||
|
||||
_flowQStats.update(Scalarf(_sub_flow.get().quality));
|
||||
|
||||
// imporant to timestamp flow even if distance is bad
|
||||
_time_last_flow = _timeStamp;
|
||||
|
||||
// measurement
|
||||
y(Y_flow_vx) = delta_n(0) / dt_flow;
|
||||
y(Y_flow_vy) = delta_n(1) / dt_flow;
|
||||
|
||||
_flowQStats.update(Scalarf(_sub_flow.get().quality));
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
|
|
@ -133,15 +122,20 @@ void BlockLocalPositionEstimator::flowCorrect()
|
|||
// flow measurement matrix and noise matrix
|
||||
Matrix<float, n_y_flow, n_x> C;
|
||||
C.setZero();
|
||||
C(Y_flow_x, X_x) = 1;
|
||||
C(Y_flow_y, X_y) = 1;
|
||||
C(Y_flow_vx, X_vx) = 1;
|
||||
C(Y_flow_vy, X_vy) = 1;
|
||||
|
||||
SquareMatrix<float, n_y_flow> R;
|
||||
R.setZero();
|
||||
float d = agl() * cosf(_sub_att.get().roll) * cosf(_sub_att.get().pitch);
|
||||
float flow_xy_stddev = _flow_xy_stddev.get() + _flow_xy_d_stddev.get() * d ;
|
||||
R(Y_flow_x, Y_flow_x) = flow_xy_stddev * flow_xy_stddev;
|
||||
R(Y_flow_y, Y_flow_y) = R(Y_flow_x, Y_flow_x);
|
||||
float rot_rate_norm = sqrtf(_sub_att.get().rollspeed * _sub_att.get().rollspeed
|
||||
+ _sub_att.get().pitchspeed * _sub_att.get().pitchspeed
|
||||
+ _sub_att.get().yawspeed * _sub_att.get().yawspeed);
|
||||
float flow_vxy_stddev = _flow_vxy_stddev.get()
|
||||
+ _flow_vxy_d_stddev.get() * d
|
||||
+ _flow_vxy_r_stddev.get() * rot_rate_norm;
|
||||
R(Y_flow_vx, Y_flow_vx) = flow_vxy_stddev * flow_vxy_stddev;
|
||||
R(Y_flow_vy, Y_flow_vy) = R(Y_flow_vx, Y_flow_vx);
|
||||
|
||||
// residual
|
||||
Vector<float, 2> r = y - C * _x;
|
||||
|
|
@ -176,11 +170,6 @@ void BlockLocalPositionEstimator::flowCorrect()
|
|||
_x += dx;
|
||||
_P -= K * C * _P;
|
||||
|
||||
} else {
|
||||
// reset flow integral to current estimate of position
|
||||
// if a fault occurred
|
||||
_flowX = _x(X_x);
|
||||
_flowY = _x(X_y);
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -49,10 +49,12 @@ int BlockLocalPositionEstimator::lidarMeasure(Vector<float, n_y_lidar> &y)
|
|||
}
|
||||
|
||||
// update stats
|
||||
_lidarStats.update(Scalarf(d + _lidar_z_offset.get()));
|
||||
_lidarStats.update(Scalarf(d));
|
||||
_time_last_lidar = _timeStamp;
|
||||
y.setZero();
|
||||
y(0) = d;
|
||||
y(0) = (d + _lidar_z_offset.get()) *
|
||||
cosf(_sub_att.get().roll) *
|
||||
cosf(_sub_att.get().pitch);
|
||||
return OK;
|
||||
}
|
||||
|
||||
|
|
@ -63,11 +65,6 @@ void BlockLocalPositionEstimator::lidarCorrect()
|
|||
|
||||
if (lidarMeasure(y) != OK) { return; }
|
||||
|
||||
// account for leaning
|
||||
y(0) = y(0) *
|
||||
cosf(_sub_att.get().roll) *
|
||||
cosf(_sub_att.get().pitch);
|
||||
|
||||
// measurement matrix
|
||||
Matrix<float, n_y_lidar, n_x> C;
|
||||
C.setZero();
|
||||
|
|
|
|||
|
|
@ -21,7 +21,6 @@ void BlockLocalPositionEstimator::mocapInit()
|
|||
|
||||
// if finished
|
||||
if (_mocapStats.getCount() > REQ_MOCAP_INIT_COUNT) {
|
||||
_mocapOrigin = _mocapStats.getMean();
|
||||
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] mocap position init: "
|
||||
"%5.2f, %5.2f, %5.2f m std %5.2f, %5.2f, %5.2f m",
|
||||
double(_mocapStats.getMean()(0)),
|
||||
|
|
@ -35,7 +34,7 @@ void BlockLocalPositionEstimator::mocapInit()
|
|||
|
||||
if (!_altOriginInitialized) {
|
||||
_altOriginInitialized = true;
|
||||
_altOrigin = _mocapOrigin(2);
|
||||
_altOrigin = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -58,9 +57,6 @@ void BlockLocalPositionEstimator::mocapCorrect()
|
|||
|
||||
if (mocapMeasure(y) != OK) { return; }
|
||||
|
||||
// make measurement relative to origin
|
||||
y -= _mocapOrigin;
|
||||
|
||||
// mocap measurement matrix, measures position
|
||||
Matrix<float, n_y_mocap, n_x> C;
|
||||
C.setZero();
|
||||
|
|
|
|||
|
|
@ -6,8 +6,13 @@ extern orb_advert_t mavlink_log_pub;
|
|||
|
||||
// required number of samples for sensor
|
||||
// to initialize
|
||||
static const uint32_t REQ_VISION_INIT_COUNT = 20;
|
||||
static const uint32_t VISION_TIMEOUT = 500000; // 0.5 s
|
||||
|
||||
// this is a vision based position measurement so we assume as soon as we get one
|
||||
// measurement it is initialized, we also don't want to deinitialize it because
|
||||
// this will throw away a correction before it starts using the data so we
|
||||
// set the timeout to 10 seconds
|
||||
static const uint32_t REQ_VISION_INIT_COUNT = 1;
|
||||
static const uint32_t VISION_TIMEOUT = 10000000; // 10 s
|
||||
|
||||
void BlockLocalPositionEstimator::visionInit()
|
||||
{
|
||||
|
|
@ -21,7 +26,6 @@ void BlockLocalPositionEstimator::visionInit()
|
|||
|
||||
// increament sums for mean
|
||||
if (_visionStats.getCount() > REQ_VISION_INIT_COUNT) {
|
||||
_visionOrigin = _visionStats.getMean();
|
||||
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] vision position init: "
|
||||
"%5.2f %5.2f %5.2f m std %5.2f %5.2f %5.2f m",
|
||||
double(_visionStats.getMean()(0)),
|
||||
|
|
@ -35,7 +39,7 @@ void BlockLocalPositionEstimator::visionInit()
|
|||
|
||||
if (!_altOriginInitialized) {
|
||||
_altOriginInitialized = true;
|
||||
_altOrigin = _visionOrigin(2);
|
||||
_altOrigin = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -58,9 +62,6 @@ void BlockLocalPositionEstimator::visionCorrect()
|
|||
|
||||
if (visionMeasure(y) != OK) { return; }
|
||||
|
||||
// make measurement relative to origin
|
||||
y -= _visionOrigin;
|
||||
|
||||
// vision measurement matrix, measures position
|
||||
Matrix<float, n_y_vision, n_x> C;
|
||||
C.setZero();
|
||||
|
|
|
|||
Loading…
Reference in New Issue