diff --git a/launch/mavros.launch b/launch/mavros.launch index e61a913714..ddb78f955a 100644 --- a/launch/mavros.launch +++ b/launch/mavros.launch @@ -6,11 +6,13 @@ + + - - + + @@ -19,3 +21,5 @@ + + diff --git a/launch/mavros_posix_sitl.launch b/launch/mavros_posix_sitl.launch index fea6b03940..e387a5714b 100644 --- a/launch/mavros_posix_sitl.launch +++ b/launch/mavros_posix_sitl.launch @@ -25,6 +25,9 @@ + + + @@ -48,6 +51,8 @@ + + diff --git a/launch/posix_sitl.launch b/launch/posix_sitl.launch index 4228924b65..44838932dd 100644 --- a/launch/posix_sitl.launch +++ b/launch/posix_sitl.launch @@ -6,8 +6,7 @@ - - + diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index df47fbffc3..5055b7313d 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -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,17 +298,23 @@ 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) { - _validXY = true; + if (vxy_stddev_ok) { + if (_flowInitialized || _gpsInitialized || _visionInitialized || _mocapInitialized) { + _validXY = true; + } } } @@ -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 - _pub_lpos.get().z = xLP(X_z); // down + + 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 diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp index 163235bd64..3d90d0fb0f 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp @@ -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; diff --git a/src/modules/local_position_estimator/params.c b/src/modules/local_position_estimator/params.c index 70ffd19313..500c7a2894 100644 --- a/src/modules/local_position_estimator/params.c +++ b/src/modules/local_position_estimator/params.c @@ -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,18 +22,40 @@ 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 + * @min 0.01 + * @max 1 + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(LPE_FLW_VXY, 0.04f); + +/** + * Optical flow xy velocity standard deviation linear factor on distance + * + * @group Local Position Estimator + * @unit m / m * @min 0.01 * @max 1 * @decimal 3 */ -PARAM_DEFINE_FLOAT(LPE_FLW_XY, 0.01f); +PARAM_DEFINE_FLOAT(LPE_FLW_VXY_D, 0.04f); /** - * Optical flow xy standard deviation linear factor on distance + * Optical flow xy velocity standard deviation linear factor on rotation rate * * @group Local Position Estimator * @unit m / m @@ -34,7 +63,7 @@ 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_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 diff --git a/src/modules/local_position_estimator/sensors/flow.cpp b/src/modules/local_position_estimator/sensors/flow.cpp index d5e31c1f98..7617afba33 100644 --- a/src/modules/local_position_estimator/sensors/flow.cpp +++ b/src/modules/local_position_estimator/sensors/flow.cpp @@ -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 &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 &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( - _sub_flow.get().gyro_x_rate_integral); - float gyro_y_rad = _flow_gyro_y_high_pass.update( - _sub_flow.get().gyro_y_rate_integral); + 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); + 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 &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); + // imporant to timestamp flow even if distance is bad + _time_last_flow = _timeStamp; // measurement - y(Y_flow_x) = _flowX; - y(Y_flow_y) = _flowY; + 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)); - // imporant to timestamp flow even if distance is bad - _time_last_flow = _timeStamp; - return OK; } @@ -133,15 +122,20 @@ void BlockLocalPositionEstimator::flowCorrect() // flow measurement matrix and noise matrix Matrix 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 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 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); } } diff --git a/src/modules/local_position_estimator/sensors/lidar.cpp b/src/modules/local_position_estimator/sensors/lidar.cpp index 407a49ace6..dea989b4d1 100644 --- a/src/modules/local_position_estimator/sensors/lidar.cpp +++ b/src/modules/local_position_estimator/sensors/lidar.cpp @@ -49,10 +49,12 @@ int BlockLocalPositionEstimator::lidarMeasure(Vector &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 C; C.setZero(); diff --git a/src/modules/local_position_estimator/sensors/mocap.cpp b/src/modules/local_position_estimator/sensors/mocap.cpp index d87f94593d..525866b563 100644 --- a/src/modules/local_position_estimator/sensors/mocap.cpp +++ b/src/modules/local_position_estimator/sensors/mocap.cpp @@ -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 C; C.setZero(); diff --git a/src/modules/local_position_estimator/sensors/vision.cpp b/src/modules/local_position_estimator/sensors/vision.cpp index 714d7df359..f2350fd606 100644 --- a/src/modules/local_position_estimator/sensors/vision.cpp +++ b/src/modules/local_position_estimator/sensors/vision.cpp @@ -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 C; C.setZero();