diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 4c53b1fef7..aaf8d93e5e 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -129,7 +129,7 @@ private: int _gps_sub = -1; int _airspeed_sub = -1; int _params_sub = -1; - int _control_mode_sub = -1; + int _control_mode_sub = -1; orb_advert_t _att_pub; orb_advert_t _lpos_pub; @@ -162,23 +162,23 @@ private: control::BlockParamFloat *_gps_vel_noise; control::BlockParamFloat *_gps_pos_noise; control::BlockParamFloat *_baro_noise; - control::BlockParamFloat *_baro_innov_gate; // innovation gate for barometric height innovation test (std dev) - control::BlockParamFloat *_posNE_innov_gate; // innovation gate for GPS horizontal position innovation test (std dev) - control::BlockParamFloat *_vel_innov_gate; // innovation gate for GPS velocity innovation test (std dev) + control::BlockParamFloat *_baro_innov_gate; // innovation gate for barometric height innovation test (std dev) + control::BlockParamFloat *_posNE_innov_gate; // innovation gate for GPS horizontal position innovation test (std dev) + control::BlockParamFloat *_vel_innov_gate; // innovation gate for GPS velocity innovation test (std dev) control::BlockParamFloat *_mag_heading_noise; // measurement noise used for simple heading fusion control::BlockParamFloat *_mag_declination_deg; // magnetic declination in degrees control::BlockParamFloat *_heading_innov_gate; // innovation gate for heading innovation test - control::BlockParamFloat *_mag_innov_gate; // innovation gate for magnetometer innovation test + control::BlockParamFloat *_mag_innov_gate; // innovation gate for magnetometer 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) + 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) EstimatorInterface *_ekf; @@ -217,23 +217,23 @@ Ekf2::Ekf2(): _gps_vel_noise = new control::BlockParamFloat(this, "EKF2_GPS_V_NOISE", false, ¶ms->gps_vel_noise); _gps_pos_noise = new control::BlockParamFloat(this, "EKF2_GPS_P_NOISE", false, ¶ms->gps_pos_noise); _baro_noise = new control::BlockParamFloat(this, "EKF2_BARO_NOISE", false, ¶ms->baro_noise); - _baro_innov_gate = new control::BlockParamFloat(this, "EKF2_BARO_GATE", false, ¶ms->baro_innov_gate); - _posNE_innov_gate = new control::BlockParamFloat(this, "EKF2_GPS_P_GATE", false, ¶ms->posNE_innov_gate); - _vel_innov_gate = new control::BlockParamFloat(this, "EKF2_GPS_V_GATE", false, ¶ms->vel_innov_gate); + _baro_innov_gate = new control::BlockParamFloat(this, "EKF2_BARO_GATE", false, ¶ms->baro_innov_gate); + _posNE_innov_gate = new control::BlockParamFloat(this, "EKF2_GPS_P_GATE", false, ¶ms->posNE_innov_gate); + _vel_innov_gate = new control::BlockParamFloat(this, "EKF2_GPS_V_GATE", false, ¶ms->vel_innov_gate); _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_HDG_GATE", false, ¶ms->heading_innov_gate); - _mag_innov_gate = new control::BlockParamFloat(this, "EKF2_MAG_GATE", false, ¶ms->mag_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); + _heading_innov_gate = new control::BlockParamFloat(this, "EKF2_HDG_GATE", false, ¶ms->heading_innov_gate); + _mag_innov_gate = new control::BlockParamFloat(this, "EKF2_MAG_GATE", false, ¶ms->mag_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); } @@ -262,7 +262,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)); + _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); px4_pollfd_struct_t fds[2] = {}; fds[0].fd = _sensors_sub; @@ -296,6 +296,7 @@ void Ekf2::task_main() // fetch sensor data in next loop continue; + } else if (!(fds[0].revents & POLLIN)) { // no new data continue; @@ -303,11 +304,11 @@ void Ekf2::task_main() bool gps_updated = false; bool airspeed_updated = false; - bool control_mode_updated = false; + bool control_mode_updated = false; sensor_combined_s sensors = {}; airspeed_s airspeed = {}; - vehicle_control_mode_s vehicle_control_mode = {}; + vehicle_control_mode_s vehicle_control_mode = {}; orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors); @@ -324,15 +325,16 @@ void Ekf2::task_main() orb_copy(ORB_ID(airspeed), _airspeed_sub, &airspeed); } - // 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->set_arm_status(vehicle_control_mode.flag_armed); - } + // 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->set_arm_status(vehicle_control_mode.flag_armed); + } - hrt_abstime now = hrt_absolute_time(); + 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]); @@ -353,16 +355,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.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; + gps_msg.nsats = gps.satellites_used; + //TODO add gdop to gps topic + gps_msg.gdop = 0.0f; _ekf->setGpsData(gps.timestamp_position, &gps_msg); } @@ -412,12 +414,13 @@ void Ekf2::task_main() lpos.v_z_valid = true; // Position of local NED origin in GPS / WGS84 frame - struct map_projection_reference_s ekf_origin = {}; - _ekf->get_ekf_origin(&lpos.ref_timestamp,&ekf_origin,&lpos.ref_alt); - 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_origin.lat_rad * 180.0 / M_PI; // Reference point latitude in degrees - lpos.ref_lon = ekf_origin.lon_rad * 180.0 / M_PI; // Reference point longitude in degrees + struct map_projection_reference_s ekf_origin = {}; + _ekf->get_ekf_origin(&lpos.ref_timestamp, &ekf_origin, &lpos.ref_alt); + 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_origin.lat_rad * 180.0 / M_PI; // Reference point latitude in degrees + lpos.ref_lon = ekf_origin.lon_rad * 180.0 / M_PI; // Reference point longitude in degrees // The rotation of the tangent plane vs. geographical north lpos.yaw = 0.0f; @@ -525,7 +528,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; + //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);