diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 38dbaf3327..7d836c547c 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -74,6 +74,7 @@ #include #include #include +#include #include @@ -128,6 +129,7 @@ private: int _gps_sub = -1; int _airspeed_sub = -1; int _params_sub = -1; + int _control_mode_sub = -1; orb_advert_t _att_pub; orb_advert_t _lpos_pub; @@ -146,8 +148,6 @@ private: control::BlockParamFloat *_baro_delay_ms; control::BlockParamFloat *_gps_delay_ms; control::BlockParamFloat *_airspeed_delay_ms; - control::BlockParamFloat *_requiredEph; - control::BlockParamFloat *_requiredEpv; control::BlockParamFloat *_gyro_noise; control::BlockParamFloat *_accel_noise; @@ -167,6 +167,15 @@ private: control::BlockParamFloat *_mag_declination_deg; // magnetic declination in degrees control::BlockParamFloat *_heading_innov_gate; // innovation gate for heading innovation test + control::BlockParamInt *_gps_check_mask; // bitmasked integer used to activate the different GPS quality checks + control::BlockParamFloat *_requiredEph; // maximum acceptable horiz position error (m) + control::BlockParamFloat *_requiredEpv; // maximum acceptable vert position error (m) + control::BlockParamFloat *_requiredSacc; // maximum acceptable speed error (m/s) + control::BlockParamInt *_requiredNsats; // minimum acceptable satellite count + control::BlockParamFloat *_requiredGDoP; // maximum acceptable geometric dilution of precision + control::BlockParamFloat *_requiredHdrift; // maximum acceptable horizontal drift speed (m/s) + control::BlockParamFloat *_requiredVdrift; // maximum acceptable vertical drift speed (m/s) + EstimatorBase *_ekf; int update_subscriptions(); @@ -191,8 +200,6 @@ Ekf2::Ekf2(): _baro_delay_ms = new control::BlockParamFloat(this, "EKF2_BARO_DELAY", false, ¶ms->baro_delay_ms); _gps_delay_ms = new control::BlockParamFloat(this, "EKF2_GPS_DELAY", false, ¶ms->gps_delay_ms); _airspeed_delay_ms = new control::BlockParamFloat(this, "EKF2_ASP_DELAY", false, ¶ms->airspeed_delay_ms); - _requiredEph = new control::BlockParamFloat(this, "EKF2_REQ_EPH", false, ¶ms->requiredEph); - _requiredEpv = new control::BlockParamFloat(this, "EKF2_REQ_EPV", false, ¶ms->requiredEpv); _gyro_noise = new control::BlockParamFloat(this, "EKF2_G_NOISE", false, ¶ms->gyro_noise); _accel_noise = new control::BlockParamFloat(this, "EKF2_ACC_NOISE", false, ¶ms->accel_noise); @@ -210,6 +217,16 @@ Ekf2::Ekf2(): _mag_heading_noise = new control::BlockParamFloat(this, "EKF2_HEAD_NOISE", false, ¶ms->mag_heading_noise); _mag_declination_deg = new control::BlockParamFloat(this, "EKF2_MAG_DECL", false, ¶ms->mag_declination_deg); _heading_innov_gate = new control::BlockParamFloat(this, "EKF2_H_INOV_GATE", false, ¶ms->heading_innov_gate); + + _gps_check_mask = new control::BlockParamInt(this, "EKF2_GPS_CHECK", false, ¶ms->gps_check_mask); + _requiredEph = new control::BlockParamFloat(this, "EKF2_REQ_EPH", false, ¶ms->req_hacc); + _requiredEpv = new control::BlockParamFloat(this, "EKF2_REQ_EPV", false, ¶ms->req_vacc); + _requiredSacc = new control::BlockParamFloat(this, "EKF2_REQ_SACC", false, ¶ms->req_sacc); + _requiredNsats = new control::BlockParamInt(this, "EKF2_REQ_NSATS", false, ¶ms->req_nsats); + _requiredGDoP = new control::BlockParamFloat(this, "EKF2_REQ_GDOP", false, ¶ms->req_gdop); + _requiredHdrift = new control::BlockParamFloat(this, "EKF2_REQ_HDRIFT", false, ¶ms->req_hdrift); + _requiredVdrift = new control::BlockParamFloat(this, "EKF2_REQ_VDRIFT", false, ¶ms->req_vdrift); + } Ekf2::~Ekf2() @@ -237,6 +254,7 @@ void Ekf2::task_main() _gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); + _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); px4_pollfd_struct_t fds[2] = {}; fds[0].fd = _sensors_sub; @@ -277,9 +295,11 @@ void Ekf2::task_main() bool gps_updated = false; bool airspeed_updated = false; + bool control_mode_updated = false; sensor_combined_s sensors = {}; airspeed_s airspeed = {}; + vehicle_control_mode_s vehicle_control_mode = {}; orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors); @@ -296,7 +316,15 @@ void Ekf2::task_main() orb_copy(ORB_ID(airspeed), _airspeed_sub, &airspeed); } - hrt_abstime now = hrt_absolute_time(); + // Use the control model data to determine if the motors are armed as a surrogate for an on-ground vs in-air status + // TODO implement a global vehicle on-ground/in-air check + orb_check(_control_mode_sub, &control_mode_updated); + if (control_mode_updated) { + orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &vehicle_control_mode); + _ekf->_vehicle_armed = vehicle_control_mode.flag_armed; + } + + hrt_abstime now = hrt_absolute_time(); // push imu data into estimator _ekf->setIMUData(now, sensors.gyro_integral_dt[0], sensors.accelerometer_integral_dt[0], &sensors.gyro_integral_rad[0], &sensors.accelerometer_integral_m_s[0]); @@ -317,12 +345,16 @@ void Ekf2::task_main() gps_msg.fix_type = gps.fix_type; gps_msg.eph = gps.eph; gps_msg.epv = gps.epv; + gps_msg.sacc = gps.s_variance_m_s; gps_msg.time_usec_vel = gps.timestamp_velocity; gps_msg.vel_m_s = gps.vel_m_s; gps_msg.vel_ned[0] = gps.vel_n_m_s; gps_msg.vel_ned[1] = gps.vel_e_m_s; gps_msg.vel_ned[2] = gps.vel_d_m_s; gps_msg.vel_ned_valid = gps.vel_ned_valid; + gps_msg.nsats = gps.satellites_used; + //TODO add gdop to gps topic + gps_msg.gdop = 0.0f; _ekf->setGpsData(gps.timestamp_position, &gps_msg); } @@ -376,8 +408,8 @@ void Ekf2::task_main() lpos.xy_global = _ekf->position_is_valid();// true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) lpos.z_global = true;// true if z is valid and has valid global reference (ref_alt) - lpos.ref_lat = _ekf->_posRef.lat_rad * 180.0 / M_PI; // Reference point latitude in degrees - lpos.ref_lon = _ekf->_posRef.lon_rad * 180.0 / M_PI; // Reference point longitude in degrees + lpos.ref_lat = _ekf->_pos_ref.lat_rad * 180.0 / M_PI; // Reference point latitude in degrees + lpos.ref_lon = _ekf->_pos_ref.lon_rad * 180.0 / M_PI; // Reference point longitude in degrees lpos.ref_alt = _ekf->_gps_alt_ref; // Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level @@ -451,7 +483,7 @@ void Ekf2::task_main() global_pos.time_utc_usec = gps.time_utc_usec; // GPS UTC timestamp in microseconds double est_lat, est_lon; - map_projection_reproject(&_ekf->_posRef, lpos.x, lpos.y, &est_lat, &est_lon); + map_projection_reproject(&_ekf->_pos_ref, lpos.x, lpos.y, &est_lat, &est_lon); global_pos.lat = est_lat; // Latitude in degrees global_pos.lon = est_lon; // Longitude in degrees @@ -487,6 +519,7 @@ void Ekf2::task_main() status.timestamp = hrt_absolute_time(); _ekf->get_state_delayed(status.states); _ekf->get_covariances(status.covariances); + status.gps_check_fail_flags = _ekf->_gps_check_fail_status.value; if (_estimator_status_pub == nullptr) { _estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &status); @@ -614,4 +647,4 @@ int ekf2_main(int argc, char *argv[]) PX4_WARN("unrecognized command"); return 1; -} \ No newline at end of file +} diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index ea6643d329..4af3fe00fe 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -81,6 +81,25 @@ PARAM_DEFINE_FLOAT(EKF2_GPS_DELAY, 200); */ PARAM_DEFINE_FLOAT(EKF2_ASP_DELAY, 200); +/** + * Integer bitmask controlling GPS checks. Set bits to 1 to enable checks. Checks enabled by the following bit positions + * 0 : Minimum required sat count set by EKF2_REQ_NSATS + * 1 : Minimum required GDoP set by EKF2_REQ_GDOP + * 2 : Maximum allowed horizontal position error set by EKF2_REQ_EPH + * 3 : Maximum allowed vertical position error set by EKF2_REQ_EPV + * 4 : Maximum allowed speed error set by EKF2_REQ_SACC + * 5 : Maximum allowed horizontal position rate set by EKF2_REQ_HDRIFT. This check can only be used if the vehciel is stationary during alignment. + * 6 : Maximum allowed vertical position rate set by EKF2_REQ_VDRIFT. This check can only be used if the vehciel is stationary during alignment. + * 7 : Maximum allowed horizontal speed set by EKF2_REQ_HDRIFT. This check can only be used if the vehciel is stationary during alignment. + * 8 : Maximum allowed vertical velocity discrepancy set by EKF2_REQ_VDRIFT + * + * @group EKF2 + * @min 0 + * @max 511 + * @unit + */ +PARAM_DEFINE_INT32(EKF2_GPS_CHECK, 21); + /** * Required EPH to use GPS. * @@ -89,7 +108,7 @@ PARAM_DEFINE_FLOAT(EKF2_ASP_DELAY, 200); * @max 100 * @unit m */ -PARAM_DEFINE_FLOAT(EKF2_REQ_EPH, 10); +PARAM_DEFINE_FLOAT(EKF2_REQ_EPH, 5.0f); /** * Required EPV to use GPS. @@ -99,7 +118,57 @@ PARAM_DEFINE_FLOAT(EKF2_REQ_EPH, 10); * @max 100 * @unit m */ -PARAM_DEFINE_FLOAT(EKF2_REQ_EPV, 20); +PARAM_DEFINE_FLOAT(EKF2_REQ_EPV, 8.0f); + +/** + * Required speed accuracy to use GPS. + * + * @group EKF2 + * @min 0.5 + * @max 5.0 + * @unit m/s + */ +PARAM_DEFINE_FLOAT(EKF2_REQ_SACC, 1.0f); + +/** + * Required satellite count to use GPS. + * + * @group EKF2 + * @min 4 + * @max 12 + * @unit + */ +PARAM_DEFINE_INT32(EKF2_REQ_NSATS, 6); + +/** + * Required GDoP to use GPS. + * + * @group EKF2 + * @min 1.5 + * @max 5.0 + * @unit + */ +PARAM_DEFINE_FLOAT(EKF2_REQ_GDOP, 2.5f); + +/** + * Maximum horizontal drift speed to use GPS. + * + * @group EKF2 + * @min 0.1 + * @max 1.0 + * @unit m/s + */ +PARAM_DEFINE_FLOAT(EKF2_REQ_HDRIFT, 0.3f); + +/** + * Maximum vertical drift speed to use GPS. + * + * @group EKF2 + * @min 0.1 + * @max 1.5 + * @unit m/s + */ +PARAM_DEFINE_FLOAT(EKF2_REQ_VDRIFT, 0.5f); /** * Gyro noise.