From 652fb5e99e44a43d4500d4146c5dd93833662f35 Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 17 Feb 2016 14:56:31 +0100 Subject: [PATCH] ekf2 app: - support use of replay --- src/modules/ekf2/ekf2_main.cpp | 83 ++++++++++++++++++++++++++++------ src/modules/ekf2/ekf2_params.c | 10 ++++ 2 files changed, 80 insertions(+), 13 deletions(-) diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 6f3af7882a..b07c083593 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -76,6 +76,7 @@ #include #include #include +#include #include @@ -111,6 +112,8 @@ public: */ int start(); + void set_replay_mode(bool replay) {_replay_mode = true;}; + static void task_main_trampoline(int argc, char *argv[]); void task_main(); @@ -121,8 +124,10 @@ public: private: static constexpr float _dt_max = 0.02; - bool _task_should_exit = false; /**< if true, task should exit */ - int _control_task = -1; /**< task handle for task */ + bool _task_should_exit = false; + int _control_task = -1; // task handle for task + bool _replay_mode; // should we use replay data from a log + int _publish_replay_mode; // defines if we should publish replay messages int _sensors_sub = -1; int _gps_sub = -1; @@ -139,6 +144,7 @@ private: orb_advert_t _vehicle_global_position_pub; orb_advert_t _estimator_status_pub; orb_advert_t _estimator_innovations_pub; + orb_advert_t _replay_pub; /* Low pass filter for attitude rates */ math::LowPassFilter2p _lp_roll_rate; @@ -189,6 +195,7 @@ private: 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 *_param_record_replay_msg; // indicates if we want to record ekf2 replay messages int update_subscriptions(); @@ -196,12 +203,15 @@ private: Ekf2::Ekf2(): SuperBlock(NULL, "EKF"), + _replay_mode(false), + _publish_replay_mode(0), _att_pub(nullptr), _lpos_pub(nullptr), _control_state_pub(nullptr), _vehicle_global_position_pub(nullptr), _estimator_status_pub(nullptr), _estimator_innovations_pub(nullptr), + _replay_pub(nullptr), _lp_roll_rate(250.0f, 30.0f), _lp_pitch_rate(250.0f, 30.0f), _lp_yaw_rate(250.0f, 20.0f), @@ -239,7 +249,8 @@ Ekf2::Ekf2(): _requiredNsats(new control::BlockParamInt(this, "EKF2_REQ_NSATS", false, &_params->req_nsats)), _requiredGDoP(new control::BlockParamFloat(this, "EKF2_REQ_GDOP", false, &_params->req_gdop)), _requiredHdrift(new control::BlockParamFloat(this, "EKF2_REQ_HDRIFT", false, &_params->req_hdrift)), - _requiredVdrift(new control::BlockParamFloat(this, "EKF2_REQ_VDRIFT", false, &_params->req_vdrift)) + _requiredVdrift(new control::BlockParamFloat(this, "EKF2_REQ_VDRIFT", false, &_params->req_vdrift)), + _param_record_replay_msg(new control::BlockParamInt(this, "EKF2_REC_RPL", false, &_publish_replay_mode)) { } @@ -273,7 +284,13 @@ void Ekf2::task_main() // initialise parameter cache updateParams(); + // initialize data structures outside of loop + // because they will else not always be + // properly populated + sensor_combined_s sensors = {}; vehicle_gps_position_s gps = {}; + airspeed_s airspeed = {}; + vehicle_control_mode_s vehicle_control_mode = {}; while (!_task_should_exit) { int ret = px4_poll(fds, sizeof(fds) / sizeof(fds[0]), 1000); @@ -307,12 +324,7 @@ void Ekf2::task_main() bool control_mode_updated = false; bool vehicle_status_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); - // update all other topics if they have new data orb_check(_gps_sub, &gps_updated); @@ -328,6 +340,7 @@ void Ekf2::task_main() // 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 + // XXX: Look at the land_detection module! orb_check(_control_mode_sub, &control_mode_updated); if (control_mode_updated) { @@ -335,7 +348,14 @@ void Ekf2::task_main() _ekf->set_arm_status(vehicle_control_mode.flag_armed); } - hrt_abstime now = hrt_absolute_time(); + // in replay mode we are getting the actual timestamp from the sensor topic + hrt_abstime now = 0; + if (_replay_mode) { + now = sensors.timestamp; + } else { + 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]); @@ -425,9 +445,8 @@ void Ekf2::task_main() // 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) + _ekf->get_ekf_origin(&lpos.ref_timestamp, &ekf_origin, &lpos.ref_alt); // true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) + lpos.xy_global = _ekf->position_is_valid(); 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 @@ -572,8 +591,40 @@ void Ekf2::task_main() _mag_declination_deg->set(decl_deg); } - _prev_motors_armed = vehicle_control_mode.flag_armed; + // publish replay message if in replay mode + bool publish_replay_message = (bool)_param_record_replay_msg->get(); + if (publish_replay_message) { + struct ekf2_replay_s replay = {}; + replay.time_ref = now; + replay.gyro_integral_dt = sensors.gyro_integral_dt[0]; + replay.accelerometer_integral_dt = sensors.accelerometer_integral_dt[0]; + replay.magnetometer_timestamp = sensors.magnetometer_timestamp[0]; + replay.baro_timestamp = sensors.baro_timestamp[0]; + memcpy(&replay.gyro_integral_rad[0], &sensors.gyro_integral_rad[0], sizeof(replay.gyro_integral_rad)); + memcpy(&replay.accelerometer_integral_m_s[0], &sensors.accelerometer_integral_m_s[0], sizeof(replay.accelerometer_integral_m_s)); + memcpy(&replay.magnetometer_ga[0], &sensors.magnetometer_ga[0], sizeof(replay.magnetometer_ga)); + replay.baro_alt_meter = sensors.baro_alt_meter[0]; + replay.time_usec = gps.timestamp_position; + replay.time_usec_vel = gps.timestamp_velocity; + replay.lat = gps.lat; + replay.lon = gps.lon; + replay.alt = gps.alt; + replay.fix_type = gps.fix_type; + replay.eph = gps.eph; + replay.epv = gps.epv; + replay.vel_m_s = gps.vel_m_s; + replay.vel_n_m_s = gps.vel_n_m_s; + replay.vel_e_m_s = gps.vel_e_m_s; + replay.vel_d_m_s = gps.vel_d_m_s; + replay.vel_ned_valid = gps.vel_ned_valid; + if (_replay_pub == nullptr) { + _replay_pub = orb_advertise(ORB_ID(ekf2_replay), &replay); + + } else { + orb_publish(ORB_ID(ekf2_replay), _replay_pub, &replay); + } + } } delete ekf2::instance; @@ -621,6 +672,12 @@ int ekf2_main(int argc, char *argv[]) ekf2::instance = new Ekf2(); + if (argc >= 3) { + if (!strcmp(argv[2], "--replay")) { + ekf2::instance->set_replay_mode(true); + } + } + if (ekf2::instance == nullptr) { PX4_WARN("alloc failed"); return 1; diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index c1dc9b86f1..53bb1aa9a3 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -378,3 +378,13 @@ PARAM_DEFINE_FLOAT(EKF2_GPS_P_GATE, 3.0f); * @unit SD */ PARAM_DEFINE_FLOAT(EKF2_GPS_V_GATE, 3.0f); + +/** + * A value of 1 indicates that the ekf2 module will publish + * replay messages for logging. + * + * @group EKF2 + * @min 0 + * @max 1 + */ +PARAM_DEFINE_INT32(EKF2_REC_RPL, 0);