diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index f41e8da7df..1a7e099f7f 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -774,17 +774,7 @@ then # if param compare SYS_LOGGER 0 then - # check if we should increase logging rate for ekf2 replay message logging - if param greater EKF2_REC_RPL 0 - then - if sdlog2 start -r 500 -e -b 18 -t - then - fi - else - if sdlog2 start -r 100 -a -b 9 -t - then - fi - fi + sdlog2 start -r 100 -a -b 9 -t else set LOGGER_ARGS "" # @@ -795,21 +785,24 @@ then set LOGGER_BUF 64 param set SDLOG_MODE 2 fi + if param compare SDLOG_MODE 1 then set LOGGER_ARGS "-e" fi + if param compare SDLOG_MODE 2 then set LOGGER_ARGS "-f" fi + if ver hwcmp AEROFC_V1 then set LOGGER_ARGS "-m mavlink" fi - if logger start -b ${LOGGER_BUF} -t ${LOGGER_ARGS} - then - fi + + logger start -b ${LOGGER_BUF} -t ${LOGGER_ARGS} + unset LOGGER_BUF unset LOGGER_ARGS fi diff --git a/Tools/sitl_run.sh b/Tools/sitl_run.sh index 28e8c1bc51..d089b1a628 100755 --- a/Tools/sitl_run.sh +++ b/Tools/sitl_run.sh @@ -114,26 +114,10 @@ then echo "You need to have gazebo simulator installed!" exit 1 fi -elif [ "$program" == "replay" ] && [ ! -n "$no_sim" ] -then - echo "Replaying logfile: $logfile" - # This is not a simulator, but a log file to replay - - # Check if we need to creat a param file to allow user to change parameters - if ! [ -f "$rootfs/replay_params.txt" ] - then - mkdir -p $rootfs - touch $rootfs/replay_params.txt - fi fi cd $working_dir -if [ "$logfile" != "" ] -then - cp $logfile $rootfs/replay.px4log -fi - # Do not exit on failure now from here on because we want the complete cleanup set +e diff --git a/Tools/uorb_rtps_message_ids.py b/Tools/uorb_rtps_message_ids.py index 75bed8accd..b337331bbc 100644 --- a/Tools/uorb_rtps_message_ids.py +++ b/Tools/uorb_rtps_message_ids.py @@ -21,7 +21,7 @@ msg_id_map = { 'differential_pressure': 16, 'distance_sensor': 17, 'ekf2_innovations': 18, - 'ekf2_replay': 19, + 'ekf2_timestamps': 20, 'esc_report': 21, 'esc_status': 22, diff --git a/cmake/configs/posix_sitl_default.cmake b/cmake/configs/posix_sitl_default.cmake index 2d7d6d2904..1075fd6797 100644 --- a/cmake/configs/posix_sitl_default.cmake +++ b/cmake/configs/posix_sitl_default.cmake @@ -79,7 +79,6 @@ set(config_module_list # modules/attitude_estimator_q modules/ekf2 - modules/ekf2_replay modules/local_position_estimator modules/position_estimator_inav diff --git a/cmake/configs/posix_sitl_replay.cmake b/cmake/configs/posix_sitl_replay.cmake deleted file mode 100644 index 1ee9b97f01..0000000000 --- a/cmake/configs/posix_sitl_replay.cmake +++ /dev/null @@ -1,56 +0,0 @@ -include(posix/px4_impl_posix) - -set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/toolchains/Toolchain-native.cmake) - -set(config_module_list - drivers/device - drivers/boards/sitl - platforms/common - platforms/posix/px4_layer - platforms/posix/work_queue - systemcmds/param - systemcmds/ver - systemcmds/perf - modules/uORB - modules/systemlib/param - modules/systemlib - modules/ekf2 - modules/ekf2_replay - modules/sdlog2 - modules/logger - lib/controllib - lib/mathlib - lib/mathlib/math/filter - lib/conversion - lib/ecl - lib/external_lgpl - lib/geo - lib/geo_lookup - lib/version - lib/DriverFramework/framework - lib/micro-CDR - ) - -set(config_extra_builtin_cmds - serdis - sercon - ) - -set(config_sitl_rcS_dir - posix-configs/SITL/init/replay - CACHE INTERNAL "init script dir for sitl" - ) - -set(config_sitl_viewer - replay - CACHE STRING "viewer for sitl" - ) -set_property(CACHE config_sitl_viewer - PROPERTY STRINGS "replay;none") - -set(config_sitl_debugger - disable - CACHE STRING "debugger for sitl" - ) -set_property(CACHE config_sitl_debugger - PROPERTY STRINGS "disable;gdb;lldb") diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 27887aee3f..189cd5aff4 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -52,7 +52,6 @@ set(msg_file_names differential_pressure.msg distance_sensor.msg ekf2_innovations.msg - ekf2_replay.msg ekf2_timestamps.msg esc_report.msg esc_status.msg diff --git a/msg/ekf2_replay.msg b/msg/ekf2_replay.msg deleted file mode 100644 index 44fa0b0eb5..0000000000 --- a/msg/ekf2_replay.msg +++ /dev/null @@ -1,49 +0,0 @@ - -uint32 gyro_integral_dt # gyro integration period in us -uint32 accelerometer_integral_dt # accelerometer integration period in us -uint64 magnetometer_timestamp # timestamp of magnetometer measurement in us -uint64 baro_timestamp # timestamp of barometer measurement in us -uint64 rng_timestamp # timestamp of range finder measurement in us -uint64 flow_timestamp # timestamp of optical flow measurement in us -uint64 asp_timestamp # timestamp of airspeed measurement in us -uint64 ev_timestamp # timestamp of external vision measurement in us - -float32[3] gyro_rad # gyro vector in rad -float32[3] accelerometer_m_s2 # accelerometer vector in m/s^2 - -float32[3] magnetometer_ga # magnetometer measurement vector (body fixed NED) in ga -float32 baro_alt_meter # barometer altitude measurement in m - -uint64 time_usec # timestamp of gps position measurement in us -int32 lat # Latitude in 1E-7 degrees -int32 lon # Longitude in 1E-7 degrees -int32 alt # Altitude in 1E-3 meters above MSL, (millimetres) -uint8 fix_type -uint8 nsats # number of GPS satellites used by the receiver -float32 eph # GPS horizontal position accuracy (metres) -float32 epv # GPS vertical position accuracy (metres) -float32 sacc # GPS speed accuracy (metres/sec) -float32 vel_m_s # GPS ground speed, (metres/sec) -float32 vel_n_m_s # GPS North velocity, (metres/sec) -float32 vel_e_m_s # GPS East velocity, (metres/sec) -float32 vel_d_m_s # GPS Down velocity, (metres/sec) -bool vel_ned_valid # True if NED velocity is valid - -# range finder measurements -float32 range_to_ground # range finder measured range to ground (m) - -# optical flow sensor measurements -float32[2] flow_pixel_integral # integrated optical flow rate around x and y axes (rad) -float32[2] flow_gyro_integral # integrated gyro rate around x and y axes (rad) -uint32 flow_time_integral # integration timespan (usec) -uint8 flow_quality # Quality of accumulated optical flow data (0 - 255) - -# airspeed -float32 indicated_airspeed_m_s # indicated airspeed in meters per second, -1 if unknown -float32 true_airspeed_m_s # true filtered airspeed in meters per second, -1 if unknown - -# external vision measurements -float32[3] pos_ev # position in earth (NED) frame (metres) -float32[4] quat_ev # quaternion rotation from earth (NED) to body (XYZ) frame -float32 pos_err # position error 1-std for each axis (metres) -float32 ang_err # angular error 1-std for each axis (rad) diff --git a/posix-configs/SITL/init/replay/iris b/posix-configs/SITL/init/replay/iris deleted file mode 100644 index 224a4f0832..0000000000 --- a/posix-configs/SITL/init/replay/iris +++ /dev/null @@ -1,4 +0,0 @@ -uorb start -ekf2 start -r -sleep 0.2 -ekf2_replay start replay.px4log diff --git a/posix-configs/eagle/200qx/mainapp.config b/posix-configs/eagle/200qx/mainapp.config index dc04cbc32a..0f49363c63 100644 --- a/posix-configs/eagle/200qx/mainapp.config +++ b/posix-configs/eagle/200qx/mainapp.config @@ -2,7 +2,6 @@ uorb start muorb start logger start -t -b 200 param set MAV_BROADCAST 1 -param set SDLOG_PRIO_BOOST 3 dataman start navigator start mavlink start -u 14556 -r 1000000 diff --git a/posix-configs/eagle/210qc/mainapp.config b/posix-configs/eagle/210qc/mainapp.config index dc04cbc32a..0f49363c63 100644 --- a/posix-configs/eagle/210qc/mainapp.config +++ b/posix-configs/eagle/210qc/mainapp.config @@ -2,7 +2,6 @@ uorb start muorb start logger start -t -b 200 param set MAV_BROADCAST 1 -param set SDLOG_PRIO_BOOST 3 dataman start navigator start mavlink start -u 14556 -r 1000000 diff --git a/posix-configs/eagle/flight/mainapp.config b/posix-configs/eagle/flight/mainapp.config index dc04cbc32a..0f49363c63 100644 --- a/posix-configs/eagle/flight/mainapp.config +++ b/posix-configs/eagle/flight/mainapp.config @@ -2,7 +2,6 @@ uorb start muorb start logger start -t -b 200 param set MAV_BROADCAST 1 -param set SDLOG_PRIO_BOOST 3 dataman start navigator start mavlink start -u 14556 -r 1000000 diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index c7dbde7b3f..d0a2636649 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -64,7 +64,6 @@ #include #include #include -#include #include #include #include @@ -162,7 +161,6 @@ private: orb_advert_t _wind_pub; orb_advert_t _estimator_status_pub; orb_advert_t _estimator_innovations_pub; - orb_advert_t _replay_pub; orb_advert_t _ekf2_timestamps_pub; uORB::Publication _vehicle_local_position_pub; @@ -235,8 +233,6 @@ private: control::BlockParamExtFloat _requiredHdrift; ///< maximum acceptable horizontal drift speed (m/s) control::BlockParamExtFloat _requiredVdrift; ///< maximum acceptable vertical drift speed (m/s) - control::BlockParamInt _param_record_replay_msg; ///< turns on recording of ekf2 replay messages - // measurement source control control::BlockParamExtInt _fusion_mode; ///< bitmasked integer that selects which of the GPS and optical flow aiding sources will be used @@ -339,7 +335,6 @@ Ekf2::Ekf2(): _wind_pub(nullptr), _estimator_status_pub(nullptr), _estimator_innovations_pub(nullptr), - _replay_pub(nullptr), _ekf2_timestamps_pub(nullptr), _vehicle_local_position_pub(ORB_ID(vehicle_local_position), -1, &getPublications()), _vehicle_global_position_pub(ORB_ID(vehicle_global_position), -1, &getPublications()), @@ -391,7 +386,6 @@ Ekf2::Ekf2(): _requiredGDoP(this, "EKF2_REQ_GDOP", false, _params->req_gdop), _requiredHdrift(this, "EKF2_REQ_HDRIFT", false, _params->req_hdrift), _requiredVdrift(this, "EKF2_REQ_VDRIFT", false, _params->req_vdrift), - _param_record_replay_msg(this, "EKF2_REC_RPL", false), _fusion_mode(this, "EKF2_AID_MASK", false, _params->fusion_mode), _vdist_sensor_type(this, "EKF2_HGT_MODE", false, _params->vdist_sensor_type), _range_noise(this, "EKF2_RNG_NOISE", false, _params->range_noise), @@ -1355,96 +1349,6 @@ void Ekf2::run() orb_publish(ORB_ID(ekf2_timestamps), _ekf2_timestamps_pub, &ekf2_timestamps); } } - - // publish replay message if in replay mode - if (_param_record_replay_msg.get() == 1) { - struct ekf2_replay_s replay = {}; - replay.timestamp = now; - replay.gyro_integral_dt = sensors.gyro_integral_dt; - replay.accelerometer_integral_dt = sensors.accelerometer_integral_dt; - replay.magnetometer_timestamp = _timestamp_mag_us; - replay.baro_timestamp = _timestamp_balt_us; - memcpy(replay.gyro_rad, sensors.gyro_rad, sizeof(replay.gyro_rad)); - memcpy(replay.accelerometer_m_s2, sensors.accelerometer_m_s2, sizeof(replay.accelerometer_m_s2)); - memcpy(replay.magnetometer_ga, sensors.magnetometer_ga, sizeof(replay.magnetometer_ga)); - replay.baro_alt_meter = sensors.baro_alt_meter; - - // only write gps data if we had a gps update. - if (gps_updated) { - replay.time_usec = gps.timestamp; - replay.lat = gps.lat; - replay.lon = gps.lon; - replay.alt = gps.alt; - replay.fix_type = gps.fix_type; - replay.nsats = gps.satellites_used; - replay.eph = gps.eph; - replay.epv = gps.epv; - replay.sacc = gps.s_variance_m_s; - 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; - - } else { - // this will tell the logging app not to bother logging any gps replay data - replay.time_usec = 0; - } - - if (optical_flow_updated) { - replay.flow_timestamp = optical_flow.timestamp; - replay.flow_pixel_integral[0] = optical_flow.pixel_flow_x_integral; - replay.flow_pixel_integral[1] = optical_flow.pixel_flow_y_integral; - replay.flow_gyro_integral[0] = optical_flow.gyro_x_rate_integral; - replay.flow_gyro_integral[1] = optical_flow.gyro_y_rate_integral; - replay.flow_time_integral = optical_flow.integration_timespan; - replay.flow_quality = optical_flow.quality; - - } else { - replay.flow_timestamp = 0; - } - - if (range_finder_updated) { - replay.rng_timestamp = range_finder.timestamp; - replay.range_to_ground = range_finder.current_distance; - - } else { - replay.rng_timestamp = 0; - } - - if (airspeed_updated) { - replay.asp_timestamp = airspeed.timestamp; - replay.indicated_airspeed_m_s = airspeed.indicated_airspeed_m_s; - replay.true_airspeed_m_s = airspeed.true_airspeed_m_s; - - } else { - replay.asp_timestamp = 0; - } - - if (vision_position_updated || vision_attitude_updated) { - replay.ev_timestamp = vision_position_updated ? ev_pos.timestamp : ev_att.timestamp; - replay.pos_ev[0] = ev_pos.x; - replay.pos_ev[1] = ev_pos.y; - replay.pos_ev[2] = ev_pos.z; - replay.quat_ev[0] = ev_att.q[0]; - replay.quat_ev[1] = ev_att.q[1]; - replay.quat_ev[2] = ev_att.q[2]; - replay.quat_ev[3] = ev_att.q[3]; - // TODO: switch to covariances from topic later - replay.pos_err = _ev_pos_noise.get(); - replay.ang_err = _ev_ang_noise.get(); - - } else { - replay.ev_timestamp = 0; - } - - if (_replay_pub == nullptr) { - _replay_pub = orb_advertise(ORB_ID(ekf2_replay), &replay); - - } else { - orb_publish(ORB_ID(ekf2_replay), _replay_pub, &replay); - } - } } orb_unsubscribe(sensors_sub); diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index 535dac95aa..db52fffd8a 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -521,17 +521,6 @@ PARAM_DEFINE_FLOAT(EKF2_GPS_V_GATE, 5.0f); */ PARAM_DEFINE_FLOAT(EKF2_TAS_GATE, 3.0f); -/** - * Replay mode - * - * A value of 1 indicates that the ekf2 module will publish - * replay messages for logging. - * - * @group EKF2 - * @boolean - */ -PARAM_DEFINE_INT32(EKF2_REC_RPL, 0); - /** * Integer bitmask controlling data fusion and aiding methods. * diff --git a/src/modules/ekf2_replay/CMakeLists.txt b/src/modules/ekf2_replay/CMakeLists.txt deleted file mode 100644 index 529669c7ba..0000000000 --- a/src/modules/ekf2_replay/CMakeLists.txt +++ /dev/null @@ -1,46 +0,0 @@ -############################################################################ -# -# Copyright (c) 2015 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################# -px4_add_module( - MODULE modules__ekf2_replay - MAIN ekf2_replay - COMPILE_FLAGS - -Wno-sign-compare # TODO: fix all sign-compare - STACK_MAIN 1000 - STACK_MAX 4000 - SRCS - ekf2_replay_main.cpp - DEPENDS - platforms__common - git_ecl - ) -# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/modules/ekf2_replay/ekf2_replay_main.cpp b/src/modules/ekf2_replay/ekf2_replay_main.cpp deleted file mode 100644 index 30147af542..0000000000 --- a/src/modules/ekf2_replay/ekf2_replay_main.cpp +++ /dev/null @@ -1,1143 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2015 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file ekf2_replay_main.cpp - * Replay module for ekf2. This module reads ekf2 replay messages from a px4 logfile. - * It uses this data to create sensor data for the ekf2 module. It also subscribes to the - * output data of the estimator and writes it to a replay log file. - * - * @author Roman Bapst -*/ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - - -extern "C" __EXPORT int ekf2_replay_main(int argc, char *argv[]); - -#define PRINT_READ_ERROR PX4_WARN("error reading from log file"); - -// union for log messages to write to log file -#pragma pack(push, 1) -struct { - uint8_t head1, head2, type; - union { - struct log_ATT_s att; - struct log_LPOS_s lpos; - struct log_CTS_s control_state; - struct log_EST0_s est0; - struct log_EST1_s est1; - struct log_EST2_s est2; - struct log_EST3_s est3; - struct log_EST4_s innov; - struct log_EST5_s innov2; - struct log_EST6_s innov3; - } body; -} log_message; - -#pragma pack(pop) - -class Ekf2Replay; - - -namespace ekf2_replay -{ -Ekf2Replay *instance = nullptr; -} - -class Ekf2Replay -{ -public: - // Constructor - Ekf2Replay(char *logfile); - - // Destructor, also kills task - ~Ekf2Replay(); - - // Start task. - // @return OK on success. - int start(); - - void exit() { _task_should_exit = true; } - - static void task_main_trampoline(int argc, char *argv[]); - - void task_main(); - -private: - int _control_task = -1; //task handle for task - bool _task_should_exit = false; - - orb_advert_t _sensors_pub; - orb_advert_t _gps_pub; - orb_advert_t _landed_pub; - orb_advert_t _flow_pub; - orb_advert_t _range_pub; - orb_advert_t _airspeed_pub; - orb_advert_t _vehicle_vision_position_pub; - orb_advert_t _vehicle_vision_attitude_pub; - orb_advert_t _vehicle_status_pub; - - int _att_sub; - int _estimator_status_sub; - int _innov_sub; - int _lpos_sub; - int _control_state_sub; - - char *_file_name; - - struct log_format_s _formats[100]; - struct sensor_combined_s _sensors; - struct vehicle_gps_position_s _gps; - struct vehicle_land_detected_s _land_detected; - struct optical_flow_s _flow; - struct distance_sensor_s _range; - struct airspeed_s _airspeed; - struct vehicle_local_position_s _vehicle_vision_position; - struct vehicle_attitude_s _vehicle_vision_attitude; - struct vehicle_status_s _vehicle_status; - - uint32_t _numInnovSamples; // number of samples used to calculate the RMS innovation values - float _velInnovSumSq; // GPS velocity innovation sum of squares - float _posInnovSumSq; // GPS position innovation sum of squares - float _hgtInnovSumSq; // Vertical position innovation sum of squares - float _magInnovSumSq; // magnetometer innovation sum of squares - float _tasInnovSumSq; // airspeed innovation sum of squares - - unsigned _message_counter; // counter which will increase with every message read from the log - unsigned _part1_counter_ref; // this is the value of _message_counter when the part1 of the replay message is read (imu data) - bool _read_part2; // indicates if part 2 of replay message has been read - bool _read_part3; - bool _read_part4; - bool _read_part6; - bool _read_part5; - - int _write_fd = -1; - px4_pollfd_struct_t _fds[1] {}; - - // parse replay message from buffer - // @source pointer to log message data (excluding header) - // @destination pointer to message struct of type @type - // @type message type - void parseMessage(uint8_t *source, uint8_t *destination, uint8_t type); - - // copy the replay data from the logs into the topic structs which - // will be puplished after - // @data pointer to the message struct of type @type - // @type message type - void setEstimatorInput(uint8_t *data, uint8_t type); - - // publish input data for estimator - void publishEstimatorInput(); - - // write a message to log file - // @fd file descriptor - // @data pointer to log message - // @data size of data to be written - void writeMessage(int &fd, void *data, size_t size); - - // determins if we need so write a specific message to the replay log - // messages which are not regenerated by the estimator copied from the original log file - // @type message type - bool needToSaveMessage(uint8_t type); - - // get estimator output messages and write them to replay log - void logIfUpdated(); - - // this will call the method to publish the input data for the estimator - // it will then wait for the output data from the estimator and call the propoper - // functions to handle it - void publishAndWaitForEstimator(); - - void setUserParams(const char *filename); -}; - -Ekf2Replay::Ekf2Replay(char *logfile) : - _sensors_pub(nullptr), - _gps_pub(nullptr), - _landed_pub(nullptr), - _flow_pub(nullptr), - _range_pub(nullptr), - _airspeed_pub(nullptr), - _vehicle_vision_position_pub(nullptr), - _vehicle_vision_attitude_pub(nullptr), - _vehicle_status_pub(nullptr), - _att_sub(-1), - _estimator_status_sub(-1), - _innov_sub(-1), - _lpos_sub(-1), - _control_state_sub(-1), - _formats{}, - _sensors{}, - _gps{}, - _land_detected{}, - _flow{}, - _range{}, - _airspeed{}, - _vehicle_vision_position{}, - _vehicle_vision_attitude{}, - _vehicle_status{}, - _numInnovSamples(0), - _velInnovSumSq(0.0f), - _posInnovSumSq(0.0f), - _hgtInnovSumSq(0.0f), - _magInnovSumSq(0.0f), - _tasInnovSumSq(0.0f), - _message_counter(0), - _part1_counter_ref(0), - _read_part2(false), - _read_part3(false), - _read_part4(false), - _read_part6(false), - _read_part5(false), - _write_fd(-1) -{ - // build the path to the log - char tmp[] = "./rootfs/"; - char *path_to_log = (char *) malloc(1 + strlen(tmp) + strlen(logfile)); - strcpy(path_to_log, tmp); - strcat(path_to_log, logfile); - _file_name = path_to_log; - - // we always start landed - _land_detected.landed = true; -} - -Ekf2Replay::~Ekf2Replay() -{ - -} - -void Ekf2Replay::publishEstimatorInput() -{ - if (_gps_pub == nullptr && _read_part2) { - _gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_gps); - - } else if (_gps_pub != nullptr && _read_part2) { - orb_publish(ORB_ID(vehicle_gps_position), _gps_pub, &_gps); - } - - _read_part2 = false; - - if (_flow_pub == nullptr && _read_part3) { - _flow_pub = orb_advertise(ORB_ID(optical_flow), &_flow); - - } else if (_flow_pub != nullptr && _read_part3) { - orb_publish(ORB_ID(optical_flow), _flow_pub, &_flow); - } - - _read_part3 = false; - - if (_range_pub == nullptr && _read_part4) { - _range_pub = orb_advertise(ORB_ID(distance_sensor), &_range); - - } else if (_range_pub != nullptr && _read_part4) { - orb_publish(ORB_ID(distance_sensor), _range_pub, &_range); - } - - _read_part4 = false; - - if (_vehicle_vision_attitude_pub == nullptr && _read_part5) { - _vehicle_vision_attitude_pub = orb_advertise(ORB_ID(vehicle_vision_attitude), &_vehicle_vision_attitude); - - } else if (_vehicle_vision_attitude_pub != nullptr && _read_part5) { - orb_publish(ORB_ID(vehicle_vision_attitude), _vehicle_vision_attitude_pub, &_vehicle_vision_attitude); - } - - if (_vehicle_vision_position_pub == nullptr && _read_part5) { - _vehicle_vision_position_pub = orb_advertise(ORB_ID(vehicle_vision_position), &_vehicle_vision_position); - - } else if (_vehicle_vision_position_pub != nullptr && _read_part5) { - orb_publish(ORB_ID(vehicle_vision_position), _vehicle_vision_position_pub, &_vehicle_vision_position); - } - - _read_part5 = false; - - if (_sensors_pub == nullptr) { - _sensors_pub = orb_advertise(ORB_ID(sensor_combined), &_sensors); - - } else if (_sensors_pub != nullptr) { - orb_publish(ORB_ID(sensor_combined), _sensors_pub, &_sensors); - } - - if (_airspeed_pub == nullptr && _read_part6) { - _airspeed_pub = orb_advertise(ORB_ID(airspeed), &_airspeed); - - } else if (_airspeed_pub != nullptr) { - orb_publish(ORB_ID(airspeed), _airspeed_pub, &_airspeed); - } - - _read_part6 = false; -} - -void Ekf2Replay::parseMessage(uint8_t *source, uint8_t *destination, uint8_t type) -{ - int i = 0; - int write_index = 0; - - while (_formats[type].format[i] != '\0') { - char data_type = _formats[type].format[i]; - - switch (data_type) { - case 'f': - memcpy(&destination[write_index], &source[write_index], sizeof(float)); - write_index += sizeof(float); - break; - - case 'Q': - memcpy(&destination[write_index], &source[write_index], sizeof(uint64_t)); - write_index += sizeof(uint64_t); - break; - - case 'L': - memcpy(&destination[write_index], &source[write_index], sizeof(int32_t)); - write_index += sizeof(int32_t); - break; - - case 'M': - memcpy(&destination[write_index], &source[write_index], sizeof(uint8_t)); - write_index += sizeof(uint8_t); - break; - - case 'B': - memcpy(&destination[write_index], &source[write_index], sizeof(uint8_t)); - write_index += sizeof(uint8_t); - break; - - case 'I': - memcpy(&destination[write_index], &source[write_index], sizeof(uint32_t)); - write_index += sizeof(uint32_t); - break; - - case 'i': - memcpy(&destination[write_index], &source[write_index], sizeof(int32_t)); - write_index += sizeof(int32_t); - break; - - default: - PX4_WARN("found unsupported data type in replay message, exiting!"); - _task_should_exit = true; - break; - } - - i++; - } -} - -void Ekf2Replay::setEstimatorInput(uint8_t *data, uint8_t type) -{ - struct log_RPL1_s replay_part1 = {}; - struct log_RPL2_s replay_part2 = {}; - struct log_RPL3_s replay_part3 = {}; - struct log_RPL4_s replay_part4 = {}; - struct log_RPL6_s replay_part6 = {}; - struct log_RPL5_s replay_part5 = {}; - struct log_LAND_s vehicle_landed = {}; - struct log_STAT_s vehicle_status = {}; - - if (type == LOG_RPL1_MSG) { - - uint8_t *dest_ptr = (uint8_t *)&replay_part1.time_ref; - parseMessage(data, dest_ptr, type); - _sensors.timestamp = replay_part1.time_ref; - _sensors.gyro_integral_dt = replay_part1.gyro_integral_dt; - _sensors.accelerometer_integral_dt = replay_part1.accelerometer_integral_dt; - - // If the magnetometer timestamp is zero, then there is no valid data - if (replay_part1.magnetometer_timestamp == 0) { - _sensors.magnetometer_timestamp_relative = (int32_t)sensor_combined_s::RELATIVE_TIMESTAMP_INVALID; - - } else { - _sensors.magnetometer_timestamp_relative = (int32_t)(replay_part1.magnetometer_timestamp - _sensors.timestamp); - } - - // If the barometer timestamp is zero then there is no valid data - if (replay_part1.baro_timestamp == 0) { - _sensors.baro_timestamp_relative = (int32_t)sensor_combined_s::RELATIVE_TIMESTAMP_INVALID; - - } else { - _sensors.baro_timestamp_relative = (int32_t)(replay_part1.baro_timestamp - _sensors.timestamp); - } - - _sensors.gyro_rad[0] = replay_part1.gyro_x_rad; - _sensors.gyro_rad[1] = replay_part1.gyro_y_rad; - _sensors.gyro_rad[2] = replay_part1.gyro_z_rad; - _sensors.accelerometer_m_s2[0] = replay_part1.accelerometer_x_m_s2; - _sensors.accelerometer_m_s2[1] = replay_part1.accelerometer_y_m_s2; - _sensors.accelerometer_m_s2[2] = replay_part1.accelerometer_z_m_s2; - _sensors.magnetometer_ga[0] = replay_part1.magnetometer_x_ga; - _sensors.magnetometer_ga[1] = replay_part1.magnetometer_y_ga; - _sensors.magnetometer_ga[2] = replay_part1.magnetometer_z_ga; - _sensors.baro_alt_meter = replay_part1.baro_alt_meter; - _part1_counter_ref = _message_counter; - - } else if (type == LOG_RPL2_MSG) { - uint8_t *dest_ptr = (uint8_t *)&replay_part2.time_pos_usec; - parseMessage(data, dest_ptr, type); - _gps.timestamp = replay_part2.time_pos_usec; - _gps.lat = replay_part2.lat; - _gps.lon = replay_part2.lon; - _gps.fix_type = replay_part2.fix_type; - _gps.satellites_used = replay_part2.nsats; - _gps.eph = replay_part2.eph; - _gps.epv = replay_part2.epv; - _gps.s_variance_m_s = replay_part2.sacc; - _gps.vel_m_s = replay_part2.vel_m_s; - _gps.vel_n_m_s = replay_part2.vel_n_m_s; - _gps.vel_e_m_s = replay_part2.vel_e_m_s; - _gps.vel_d_m_s = replay_part2.vel_d_m_s; - _gps.vel_ned_valid = replay_part2.vel_ned_valid; - _gps.alt = replay_part2.alt; - _read_part2 = true; - - } else if (type == LOG_RPL3_MSG) { - uint8_t *dest_ptr = (uint8_t *)&replay_part3.time_flow_usec; - parseMessage(data, dest_ptr, type); - _flow.timestamp = replay_part3.time_flow_usec; - _flow.pixel_flow_x_integral = replay_part3.flow_integral_x; - _flow.pixel_flow_y_integral = replay_part3.flow_integral_y; - _flow.gyro_x_rate_integral = replay_part3.gyro_integral_x; - _flow.gyro_y_rate_integral = replay_part3.gyro_integral_y; - _flow.integration_timespan = replay_part3.flow_time_integral; - _flow.quality = replay_part3.flow_quality; - _read_part3 = true; - - } else if (type == LOG_RPL4_MSG) { - uint8_t *dest_ptr = (uint8_t *)&replay_part4.time_rng_usec; - parseMessage(data, dest_ptr, type); - _range.timestamp = replay_part4.time_rng_usec; - _range.current_distance = replay_part4.range_to_ground; - _range.covariance = 0.0f; - // magic values - _range.min_distance = 0.05f; - _range.max_distance = 30.0f; - _read_part4 = true; - - } else if (type == LOG_RPL6_MSG) { - uint8_t *dest_ptr = (uint8_t *)&replay_part6.time_airs_usec; - parseMessage(data, dest_ptr, type); - _airspeed.timestamp = replay_part6.time_airs_usec; - _airspeed.indicated_airspeed_m_s = replay_part6.indicated_airspeed_m_s; - _airspeed.true_airspeed_m_s = replay_part6.true_airspeed_m_s; - _read_part6 = true; - - } else if (type == LOG_RPL5_MSG) { - uint8_t *dest_ptr = (uint8_t *)&replay_part5.time_ev_usec; - parseMessage(data, dest_ptr, type); - _vehicle_vision_attitude.timestamp = replay_part5.time_ev_usec; - _vehicle_vision_position.timestamp = replay_part5.time_ev_usec; - _vehicle_vision_position.x = replay_part5.x; - _vehicle_vision_position.y = replay_part5.y; - _vehicle_vision_position.z = replay_part5.z; - _vehicle_vision_attitude.q[0] = replay_part5.q0; - _vehicle_vision_attitude.q[1] = replay_part5.q1; - _vehicle_vision_attitude.q[2] = replay_part5.q2; - _vehicle_vision_attitude.q[3] = replay_part5.q3; - _read_part5 = true; - - } else if (type == LOG_LAND_MSG) { - uint8_t *dest_ptr = (uint8_t *)&vehicle_landed.landed; - parseMessage(data, dest_ptr, type); - _land_detected.landed = vehicle_landed.landed; - - if (_landed_pub == nullptr) { - _landed_pub = orb_advertise(ORB_ID(vehicle_land_detected), &_land_detected); - - } else if (_landed_pub != nullptr) { - orb_publish(ORB_ID(vehicle_land_detected), _landed_pub, &_land_detected); - } - } - - else if (type == LOG_STAT_MSG) { - uint8_t *dest_ptr = (uint8_t *)&vehicle_status.main_state; - parseMessage(data, dest_ptr, type); - _vehicle_status.is_rotary_wing = vehicle_status.is_rot_wing; - - if (_vehicle_status_pub == nullptr) { - _vehicle_status_pub = orb_advertise(ORB_ID(vehicle_status), &_vehicle_status); - - } else if (_vehicle_status_pub != nullptr) { - orb_publish(ORB_ID(vehicle_status), _vehicle_status_pub, &_vehicle_status); - } - } -} - -void Ekf2Replay::writeMessage(int &fd, void *data, size_t size) -{ - if (size != ::write(fd, data, size)) { - PX4_WARN("error writing to file"); - } -} - -bool Ekf2Replay::needToSaveMessage(uint8_t type) -{ - return !(type == LOG_ATT_MSG || - type == LOG_LPOS_MSG || - type == LOG_EST0_MSG || - type == LOG_EST1_MSG || - type == LOG_EST2_MSG || - type == LOG_EST3_MSG || - type == LOG_EST4_MSG || - type == LOG_EST5_MSG || - type == LOG_EST6_MSG || - type == LOG_CTS_MSG); -} - -// update all estimator topics and write them to log file -void Ekf2Replay::logIfUpdated() -{ - bool updated = false; - - // update attitude - struct vehicle_attitude_s att = {}; - orb_copy(ORB_ID(vehicle_attitude), _att_sub, &att); - - // if the timestamp of the attitude is zero, then this means that the ekf did not - // do an update so we can ignore this message and just continue - if (att.timestamp == 0) { - return; - } - - memset(&log_message.body.att.q_w, 0, sizeof(log_ATT_s)); - - log_message.type = LOG_ATT_MSG; - log_message.head1 = HEAD_BYTE1; - log_message.head2 = HEAD_BYTE2; - log_message.body.att.q_w = att.q[0]; - log_message.body.att.q_x = att.q[1]; - log_message.body.att.q_y = att.q[2]; - log_message.body.att.q_z = att.q[3]; - log_message.body.att.roll = atan2f(2 * (att.q[0] * att.q[1] + att.q[2] * att.q[3]), - 1 - 2 * (att.q[1] * att.q[1] + att.q[2] * att.q[2])); - log_message.body.att.pitch = asinf(2 * (att.q[0] * att.q[2] - att.q[3] * att.q[1])); - log_message.body.att.yaw = atan2f(2 * (att.q[0] * att.q[3] + att.q[1] * att.q[2]), - 1 - 2 * (att.q[2] * att.q[2] + att.q[3] * att.q[3])); - log_message.body.att.roll_rate = att.rollspeed; - log_message.body.att.pitch_rate = att.pitchspeed; - log_message.body.att.yaw_rate = att.yawspeed; - - writeMessage(_write_fd, (void *)&log_message.head1, _formats[LOG_ATT_MSG].length); - - // update local position - orb_check(_lpos_sub, &updated); - - if (updated) { - struct vehicle_local_position_s lpos = {}; - orb_copy(ORB_ID(vehicle_local_position), _lpos_sub, &lpos); - - log_message.type = LOG_LPOS_MSG; - log_message.head1 = HEAD_BYTE1; - log_message.head2 = HEAD_BYTE2; - log_message.body.lpos.x = lpos.x; - log_message.body.lpos.y = lpos.y; - log_message.body.lpos.z = lpos.z; - log_message.body.lpos.ground_dist = lpos.dist_bottom; - log_message.body.lpos.ground_dist_rate = lpos.dist_bottom_rate; - log_message.body.lpos.vx = lpos.vx; - log_message.body.lpos.vy = lpos.vy; - log_message.body.lpos.vz = lpos.vz; - log_message.body.lpos.ref_lat = lpos.ref_lat * 1e7; - log_message.body.lpos.ref_lon = lpos.ref_lon * 1e7; - log_message.body.lpos.ref_alt = lpos.ref_alt; - log_message.body.lpos.pos_flags = (lpos.xy_valid ? 1 : 0) | - (lpos.z_valid ? 2 : 0) | - (lpos.v_xy_valid ? 4 : 0) | - (lpos.v_z_valid ? 8 : 0) | - (lpos.xy_global ? 16 : 0) | - (lpos.z_global ? 32 : 0); - log_message.body.lpos.ground_dist_flags = (lpos.dist_bottom_valid ? 1 : 0); - log_message.body.lpos.eph = lpos.eph; - log_message.body.lpos.epv = lpos.epv; - - writeMessage(_write_fd, (void *)&log_message.head1, _formats[LOG_LPOS_MSG].length); - } - - // update estimator status - orb_check(_estimator_status_sub, &updated); - - if (updated) { - struct estimator_status_s est_status = {}; - orb_copy(ORB_ID(estimator_status), _estimator_status_sub, &est_status); - unsigned maxcopy0 = (sizeof(est_status.states) < sizeof(log_message.body.est0.s)) ? sizeof(est_status.states) : sizeof( - log_message.body.est0.s); - log_message.type = LOG_EST0_MSG; - log_message.head1 = HEAD_BYTE1; - log_message.head2 = HEAD_BYTE2; - memset(&(log_message.body.est0.s), 0, sizeof(log_message.body.est0)); - memcpy(&(log_message.body.est0.s), est_status.states, maxcopy0); - log_message.body.est0.n_states = est_status.n_states; - log_message.body.est0.nan_flags = est_status.nan_flags; - log_message.body.est0.fault_flags = est_status.filter_fault_flags; - log_message.body.est0.timeout_flags = est_status.timeout_flags; - writeMessage(_write_fd, (void *)&log_message.head1, _formats[LOG_EST0_MSG].length); - - log_message.type = LOG_EST1_MSG; - log_message.head1 = HEAD_BYTE1; - log_message.head2 = HEAD_BYTE2; - unsigned maxcopy1 = ((sizeof(est_status.states) - maxcopy0) < sizeof(log_message.body.est1.s)) ? (sizeof( - est_status.states) - maxcopy0) : sizeof(log_message.body.est1.s); - memset(&(log_message.body.est1.s), 0, sizeof(log_message.body.est1.s)); - memcpy(&(log_message.body.est1.s), ((char *)est_status.states) + maxcopy0, maxcopy1); - writeMessage(_write_fd, (void *)&log_message.head1, _formats[LOG_EST1_MSG].length); - - log_message.type = LOG_EST2_MSG; - log_message.head1 = HEAD_BYTE1; - log_message.head2 = HEAD_BYTE2; - unsigned maxcopy2 = (sizeof(est_status.covariances) < sizeof(log_message.body.est2.cov)) ? sizeof( - est_status.covariances) : sizeof(log_message.body.est2.cov); - memset(&(log_message.body.est2.cov), 0, sizeof(log_message.body.est2.cov)); - memcpy(&(log_message.body.est2.cov), est_status.covariances, maxcopy2); - log_message.body.est2.gps_check_fail_flags = est_status.gps_check_fail_flags; - log_message.body.est2.control_mode_flags = est_status.control_mode_flags; - log_message.body.est2.health_flags = est_status.health_flags; - log_message.body.est2.innov_test_flags = est_status.innovation_check_flags; - writeMessage(_write_fd, (void *)&log_message.head1, _formats[LOG_EST2_MSG].length); - - log_message.type = LOG_EST3_MSG; - log_message.head1 = HEAD_BYTE1; - log_message.head2 = HEAD_BYTE2; - unsigned maxcopy3 = ((sizeof(est_status.covariances) - maxcopy2) < sizeof(log_message.body.est3.cov)) ? (sizeof( - est_status.covariances) - maxcopy2) : sizeof(log_message.body.est3.cov); - memset(&(log_message.body.est3.cov), 0, sizeof(log_message.body.est3.cov)); - memcpy(&(log_message.body.est3.cov), ((char *)est_status.covariances) + maxcopy2, maxcopy3); - writeMessage(_write_fd, (void *)&log_message.head1, _formats[LOG_EST3_MSG].length); - - } - - // update ekf2 innovations - orb_check(_innov_sub, &updated); - - if (updated) { - struct ekf2_innovations_s innov = {}; - orb_copy(ORB_ID(ekf2_innovations), _innov_sub, &innov); - memset(&log_message.body.innov.s, 0, sizeof(log_message.body.innov.s)); - - log_message.type = LOG_EST4_MSG; - log_message.head1 = HEAD_BYTE1; - log_message.head2 = HEAD_BYTE2; - - for (unsigned i = 0; i < 6; i++) { - log_message.body.innov.s[i] = innov.vel_pos_innov[i]; - log_message.body.innov.s[i + 6] = innov.vel_pos_innov_var[i]; - } - - for (unsigned i = 0; i < 3; i++) { - log_message.body.innov.s[i + 12] = innov.output_tracking_error[i]; - } - - writeMessage(_write_fd, (void *)&log_message.head1, _formats[LOG_EST4_MSG].length); - - log_message.type = LOG_EST5_MSG; - log_message.head1 = HEAD_BYTE1; - log_message.head2 = HEAD_BYTE2; - memset(&(log_message.body.innov2.s), 0, sizeof(log_message.body.innov2.s)); - - for (unsigned i = 0; i < 3; i++) { - log_message.body.innov2.s[i] = innov.mag_innov[i]; - log_message.body.innov2.s[i + 3] = innov.mag_innov_var[i]; - } - - log_message.body.innov2.s[6] = innov.heading_innov; - log_message.body.innov2.s[7] = innov.heading_innov_var; - log_message.body.innov2.s[8] = innov.airspeed_innov; - log_message.body.innov2.s[9] = innov.airspeed_innov_var; - log_message.body.innov2.s[10] = innov.beta_innov; - log_message.body.innov2.s[11] = innov.beta_innov_var; - - writeMessage(_write_fd, (void *)&log_message.head1, _formats[LOG_EST5_MSG].length); - - // optical flow innovations and innovation variances - log_message.type = LOG_EST6_MSG; - log_message.head1 = HEAD_BYTE1; - log_message.head2 = HEAD_BYTE2; - memset(&(log_message.body.innov3.s), 0, sizeof(log_message.body.innov3.s)); - - for (unsigned i = 0; i < 2; i++) { - log_message.body.innov3.s[i] = innov.flow_innov[i]; - log_message.body.innov3.s[i + 2] = innov.flow_innov_var[i]; - } - - log_message.body.innov3.s[4] = innov.hagl_innov; - log_message.body.innov3.s[5] = innov.hagl_innov_var; - writeMessage(_write_fd, (void *)&log_message.head1, _formats[LOG_EST6_MSG].length); - - // Update tuning metrics - _numInnovSamples++; - _velInnovSumSq += innov.vel_pos_innov[0] * innov.vel_pos_innov[0] + innov.vel_pos_innov[1] * innov.vel_pos_innov[1]; - _posInnovSumSq += innov.vel_pos_innov[3] * innov.vel_pos_innov[3] + innov.vel_pos_innov[4] * innov.vel_pos_innov[4]; - _hgtInnovSumSq += innov.vel_pos_innov[5] * innov.vel_pos_innov[5]; - _magInnovSumSq += innov.mag_innov[0] * innov.mag_innov[0] + innov.mag_innov[1] * innov.mag_innov[1] + innov.mag_innov[2] - * innov.mag_innov[2]; - _tasInnovSumSq += innov.airspeed_innov * innov.airspeed_innov; - - } - - // update control state - orb_check(_control_state_sub, &updated); - - if (updated) { - struct control_state_s control_state = {}; - orb_copy(ORB_ID(control_state), _control_state_sub, &control_state); - log_message.type = LOG_CTS_MSG; - log_message.head1 = HEAD_BYTE1; - log_message.head2 = HEAD_BYTE2; - log_message.body.control_state.vx_body = control_state.x_vel; - log_message.body.control_state.vy_body = control_state.y_vel; - log_message.body.control_state.vz_body = control_state.z_vel; - log_message.body.control_state.airspeed = control_state.airspeed; - log_message.body.control_state.roll_rate = control_state.roll_rate; - log_message.body.control_state.pitch_rate = control_state.pitch_rate; - log_message.body.control_state.yaw_rate = control_state.yaw_rate; - writeMessage(_write_fd, (void *)&log_message.head1, _formats[LOG_CTS_MSG].length); - } -} - -void Ekf2Replay::publishAndWaitForEstimator() -{ - // reset the counter reference for the imu replay topic - _part1_counter_ref = 0; - - publishEstimatorInput(); - - // wait for estimator output to arrive - int pret = px4_poll(&_fds[0], (sizeof(_fds) / sizeof(_fds[0])), 1000); - - if (pret == 0) { - PX4_WARN("timeout"); - } - - if (pret < 0) { - PX4_WARN("poll error"); - } - - if (_fds[0].revents & POLLIN) { - // write all estimator messages to replay log file - logIfUpdated(); - } -} - -void Ekf2Replay::setUserParams(const char *filename) -{ - std::string line; - std::ifstream myfile(filename); - std::string param_name; - std::string value_string; - - if (myfile.is_open()) { - while (! myfile.eof()) { - getline(myfile, line); - - if (line.empty()) { - continue; - } - - std::istringstream mystrstream(line); - mystrstream >> param_name; - mystrstream >> value_string; - - double param_value_double = std::stod(value_string); - - param_t handle = param_find(param_name.c_str()); - param_type_t param_format = param_type(handle); - - if (param_format == PARAM_TYPE_INT32) { - int32_t value = 0; - value = (int32_t)param_value_double; - param_set(handle, (const void *)&value); - - } else if (param_format == PARAM_TYPE_FLOAT) { - float value = 0; - value = (float)param_value_double; - param_set(handle, (const void *)&value); - } - } - - myfile.close(); - } -} - -void Ekf2Replay::task_main() -{ - // formats - const int _k_max_data_size = 1024; // 16x16 bytes - uint8_t data[_k_max_data_size] = {}; - const char param_file[] = "./rootfs/replay_params.txt"; - - // Open log file from which we read data - // TODO Check if file exists - int fd = ::open(_file_name, O_RDONLY); - - // create path to write a replay file - char *replay_log_name; - replay_log_name = strtok(_file_name, "."); - char tmp[] = "_replayed.px4log"; - char *path_to_replay_log = (char *) malloc(1 + strlen(tmp) + strlen(replay_log_name)); - strcpy(path_to_replay_log, "."); - strcat(path_to_replay_log, replay_log_name); - strcat(path_to_replay_log, tmp); - - // create path which tells user location of replay file - char tmp2[] = "./build_posix_sitl_replay/src/firmware/posix"; - char *replay_file_location = (char *) calloc(1 + strlen(tmp) + strlen(tmp2) + strlen(replay_log_name), 1); - strcat(replay_file_location, tmp2); - strcat(replay_file_location, replay_log_name); - strcat(replay_file_location, tmp); - - // open logfile to write - _write_fd = ::open(path_to_replay_log, O_WRONLY | O_CREAT, S_IRWXU); - - std::ifstream tmp_file; - tmp_file.open(param_file); - - std::string line; - bool set_default_params_in_file = false; - - if (tmp_file.is_open() && ! tmp_file.eof()) { - getline(tmp_file, line); - - if (line.empty()) { - // the parameter file is empty so write the default values to it - set_default_params_in_file = true; - } - } - - tmp_file.close(); - - std::ofstream myfile(param_file, std::ios::app); - - // subscribe to estimator topics - _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - _estimator_status_sub = orb_subscribe(ORB_ID(estimator_status)); - _innov_sub = orb_subscribe(ORB_ID(ekf2_innovations)); - _lpos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); - _control_state_sub = orb_subscribe(ORB_ID(control_state)); - - // we use attitude updates from the estimator for synchronisation - _fds[0].fd = _att_sub; - _fds[0].events = POLLIN; - - bool read_first_header = false; - bool set_user_params = false; - - PX4_INFO("Replay in progress... \n"); - PX4_INFO("Log data will be written to %s\n", replay_file_location); - - while (!_task_should_exit) { - _message_counter++; - uint8_t header[3] = {}; - - if (::read(fd, header, 3) != 3) { - if (!read_first_header) { - PX4_WARN("error reading log file, is the path printed above correct?"); - - } else { - PX4_INFO("Done!"); - } - - _task_should_exit = true; - continue; - } - - read_first_header = true; - - if ((header[0] != HEAD_BYTE1 || header[1] != HEAD_BYTE2)) { - // we assume that the log file is finished here - PX4_WARN("Done!"); - _task_should_exit = true; - continue; - } - - // write header but only for messages which are not generated by the estimator - if (needToSaveMessage(header[2])) { - writeMessage(_write_fd, &header[0], 3); - } - - if (header[2] == LOG_FORMAT_MSG) { - // format message - struct log_format_s f = {}; - - if (::read(fd, &f.type, sizeof(f)) != sizeof(f)) { - PRINT_READ_ERROR; - _task_should_exit = true; - continue; - } - - writeMessage(_write_fd, &f.type, sizeof(log_format_s)); - - memcpy(&_formats[f.type], &f, sizeof(f)); - - } else if (header[2] == LOG_PARM_MSG) { - // parameter message - if (::read(fd, &data[0], sizeof(log_PARM_s)) != sizeof(log_PARM_s)) { - PRINT_READ_ERROR; - _task_should_exit = true; - continue; - } - - writeMessage(_write_fd, &data[0], sizeof(log_PARM_s)); - - // apply the parameters - char param_name[16]; - - for (unsigned i = 0; i < 16; i++) { - param_name[i] = data[i]; - - if (data[i] == '\0') { - break; - } - } - - float param_data = 0; - memcpy(¶m_data, &data[16], sizeof(float)); - param_t handle = param_find(param_name); - param_type_t param_format = param_type(handle); - - if (param_format == PARAM_TYPE_INT32) { - int32_t value = 0; - value = (int32_t)param_data; - param_set(handle, (const void *)&value); - - } else if (param_format == PARAM_TYPE_FLOAT) { - param_set(handle, (const void *)¶m_data); - } - - // if the user param file was empty then we fill it with the ekf2 parameter values from - // the log file - if (set_default_params_in_file) { - if (strncmp(param_name, "EKF2", 4) == 0) { - std::ostringstream os; - double value = (double)param_data; - os << std::string(param_name) << " "; - os << value << "\n"; - myfile << os.str(); - } - } - - } else if (header[2] == LOG_VER_MSG) { - // version message - if (::read(fd, &data[0], sizeof(log_VER_s)) != sizeof(log_VER_s)) { - PRINT_READ_ERROR; - _task_should_exit = true; - continue; - } - - writeMessage(_write_fd, &data[0], sizeof(log_VER_s)); - - } else if (header[2] == LOG_TIME_MSG) { - // time message - if (::read(fd, &data[0], sizeof(log_TIME_s)) != sizeof(log_TIME_s)) { - // assume that this is because we have reached the end of the file - PX4_INFO("Done!"); - _task_should_exit = true; - continue; - } - - writeMessage(_write_fd, &data[0], sizeof(log_TIME_s)); - - } else { - // the first time we arrive here we should apply the parameters specified in the user file - // this makes sure they are applied after the parameter values of the log file - if (!set_user_params) { - myfile.close(); - setUserParams(param_file); - set_user_params = true; - } - - // data message - if (::read(fd, &data[0], _formats[header[2]].length - 3) != _formats[header[2]].length - 3) { - PX4_INFO("Done!"); - _task_should_exit = true; - continue; - } - - // all messages which we are not getting from the estimator are written - // back into the replay log file - if (needToSaveMessage(header[2])) { - writeMessage(_write_fd, &data[0], _formats[header[2]].length - 3); - } - - if (header[2] == LOG_RPL1_MSG && _part1_counter_ref > 0) { - // we have found another imu replay message while we still have one waiting to be published. - // so publish that now - publishAndWaitForEstimator(); - } - - // set estimator input data - setEstimatorInput(&data[0], header[2]); - - // we have read the imu replay message (part 1) and have waited 3 more cycles for other replay message parts - // e.g. flow, gps or range. we know that in case they were written to the log file they should come right after - // the first replay message, therefore, we can kick the estimator now - if (_part1_counter_ref > 0 && _part1_counter_ref < _message_counter - 3) { - publishAndWaitForEstimator(); - } - } - } - - ::close(_write_fd); - ::close(fd); - delete ekf2_replay::instance; - ekf2_replay::instance = nullptr; - - // Report sensor innovation RMS values to assist with time delay tuning - if (_numInnovSamples > 0) { - PX4_INFO("GPS vel innov RMS = %6.3f", (double)sqrtf(_velInnovSumSq / _numInnovSamples)); - PX4_INFO("GPS pos innov RMS = %6.3f", (double)sqrtf(_posInnovSumSq / _numInnovSamples)); - PX4_INFO("Hgt innov RMS = %6.3f", (double)sqrtf(_hgtInnovSumSq / _numInnovSamples)); - PX4_INFO("Mag innov RMS = %6.4f", (double)sqrtf(_magInnovSumSq / _numInnovSamples)); - PX4_INFO("TAS innov RMS = %6.3f", (double)sqrtf(_tasInnovSumSq / _numInnovSamples)); - } - - free(path_to_replay_log); - -} - -void Ekf2Replay::task_main_trampoline(int argc, char *argv[]) -{ - ekf2_replay::instance->task_main(); -} - -int Ekf2Replay::start() -{ - ASSERT(_control_task == -1); - - /* start the task */ - _control_task = px4_task_spawn_cmd("ekf2_replay", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 3000, - (px4_main_t)&Ekf2Replay::task_main_trampoline, - nullptr); - - if (_control_task < 0) { - PX4_WARN("task start failed"); - return -errno; - } - - return OK; -} - -int ekf2_replay_main(int argc, char *argv[]) -{ - if (argc < 1) { - PX4_WARN("usage: ekf2_replay {start|stop|status}"); - return 1; - } - - if (!strcmp(argv[1], "start")) { - - if (ekf2_replay::instance != nullptr) { - PX4_WARN("already running"); - return 1; - } - - ekf2_replay::instance = new Ekf2Replay(argv[2]); - - if (ekf2_replay::instance == nullptr) { - PX4_WARN("alloc failed"); - return 1; - } - - if (OK != ekf2_replay::instance->start()) { - delete ekf2_replay::instance; - ekf2_replay::instance = nullptr; - PX4_WARN("start failed"); - return 1; - } - - return 0; - } - - if (!strcmp(argv[1], "stop")) { - if (ekf2_replay::instance == nullptr) { - PX4_WARN("not running"); - return 1; - } - - ekf2_replay::instance->exit(); - - // wait for the destruction of the instance - while (ekf2_replay::instance != nullptr) { - usleep(50000); - } - - return 0; - } - - if (!strcmp(argv[1], "status")) { - if (ekf2_replay::instance) { - PX4_WARN("running"); - return 0; - - } else { - PX4_WARN("not running"); - return 1; - } - } - - PX4_WARN("unrecognized command"); - return 1; -} diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index ea8ef18fed..b6f9ecdc26 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -97,7 +97,6 @@ #include #include #include -#include #include #include #include @@ -1149,25 +1148,10 @@ int sdlog2_thread_main(int argc, char *argv[]) /* There are different log types possible on different platforms. */ enum { LOG_TYPE_NORMAL, - LOG_TYPE_REPLAY_ONLY, LOG_TYPE_ALL } log_type; - /* Check if we are gathering data for a replay log for ekf2. */ - param_t replay_handle = param_find("EKF2_REC_RPL"); - int32_t tmp = 0; - param_get(replay_handle, &tmp); - bool record_replay_log = (bool)tmp; - - if (record_replay_log) { -#if defined(__PX4_QURT) || defined(__PX4_POSIX) - log_type = LOG_TYPE_ALL; -#else - log_type = LOG_TYPE_REPLAY_ONLY; -#endif - } else { - log_type = LOG_TYPE_NORMAL; - } + log_type = LOG_TYPE_NORMAL; /* warning! using union here to save memory, elements should be used separately! */ union { @@ -1206,7 +1190,6 @@ int sdlog2_thread_main(int argc, char *argv[]) struct control_state_s ctrl_state; struct ekf2_innovations_s innovations; struct camera_trigger_s camera_trigger; - struct ekf2_replay_s replay; struct vehicle_land_detected_s land_detected; struct cpuload_s cpuload; struct vehicle_gps_position_s dual_gps_pos; @@ -1263,14 +1246,8 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_EST4_s log_INO1; struct log_EST5_s log_INO2; struct log_CAMT_s log_CAMT; - struct log_RPL1_s log_RPL1; - struct log_RPL2_s log_RPL2; struct log_EST6_s log_INO3; - struct log_RPL3_s log_RPL3; - struct log_RPL4_s log_RPL4; - struct log_RPL5_s log_RPL5; struct log_LAND_s log_LAND; - struct log_RPL6_s log_RPL6; struct log_LOAD_s log_LOAD; struct log_DPRS_s log_DPRS; struct log_STCK_s log_STCK; @@ -1319,7 +1296,6 @@ int sdlog2_thread_main(int argc, char *argv[]) int ctrl_state_sub; int innov_sub; int cam_trig_sub; - int replay_sub; int land_detected_sub; int commander_state_sub; int cpuload_sub; @@ -1363,7 +1339,6 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.ctrl_state_sub = -1; subs.innov_sub = -1; subs.cam_trig_sub = -1; - subs.replay_sub = -1; subs.land_detected_sub = -1; subs.commander_state_sub = -1; subs.cpuload_sub = -1; @@ -1372,7 +1347,6 @@ int sdlog2_thread_main(int argc, char *argv[]) /* add new topics HERE */ - for (unsigned i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { subs.telemetry_subs[i] = -1; } @@ -1408,7 +1382,7 @@ int sdlog2_thread_main(int argc, char *argv[]) thread_running = true; // wakeup source - px4_pollfd_struct_t fds[2]; + px4_pollfd_struct_t fds[1]; unsigned px4_pollfd_len = 0; int poll_counter = 0; @@ -1421,11 +1395,7 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[0].fd = subs.sensor_sub; fds[0].events = POLLIN; - subs.replay_sub = orb_subscribe(ORB_ID(ekf2_replay)); - fds[1].fd = subs.replay_sub; - fds[1].events = POLLIN; - - px4_pollfd_len = 2; + px4_pollfd_len = 1; poll_to_logging_factor = 1; @@ -1442,18 +1412,6 @@ int sdlog2_thread_main(int argc, char *argv[]) // TODO Remove hardcoded rate! poll_to_logging_factor = 250 / (log_rate < 1 ? 1 : log_rate); - break; - - case LOG_TYPE_REPLAY_ONLY: - - subs.replay_sub = orb_subscribe(ORB_ID(ekf2_replay)); - fds[0].fd = subs.replay_sub; - fds[0].events = POLLIN; - - px4_pollfd_len = 1; - - poll_to_logging_factor = 1; - break; } @@ -1521,10 +1479,6 @@ int sdlog2_thread_main(int argc, char *argv[]) if (fds[0].revents & POLLIN) { orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor); } - - if (fds[1].revents & POLLIN) { - orb_copy(ORB_ID(ekf2_replay), subs.replay_sub, &buf.replay); - } break; case LOG_TYPE_NORMAL: @@ -1532,12 +1486,6 @@ int sdlog2_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor); } break; - - case LOG_TYPE_REPLAY_ONLY: - if (fds[0].revents & POLLIN) { - orb_copy(ORB_ID(ekf2_replay), subs.replay_sub, &buf.replay); - } - break; } poll_counter++; continue; @@ -1563,109 +1511,6 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(STAT); } - /* --- EKF2 REPLAY --- */ - if (log_type == LOG_TYPE_ALL || log_type == LOG_TYPE_REPLAY_ONLY) { - - bool replay_updated = false; - - if (log_type == LOG_TYPE_ALL) { - - if (fds[1].revents & POLLIN) { - orb_copy(ORB_ID(ekf2_replay), subs.replay_sub, &buf.replay); - replay_updated = true; - } - - } else if (log_type == LOG_TYPE_REPLAY_ONLY) { - if (fds[0].revents & POLLIN) { - orb_copy(ORB_ID(ekf2_replay), subs.replay_sub, &buf.replay); - replay_updated = true; - } - } - - if (replay_updated) { - log_msg.msg_type = LOG_RPL1_MSG; - log_msg.body.log_RPL1.time_ref = buf.replay.timestamp; - log_msg.body.log_RPL1.gyro_integral_dt = buf.replay.gyro_integral_dt; - log_msg.body.log_RPL1.accelerometer_integral_dt = buf.replay.accelerometer_integral_dt; - log_msg.body.log_RPL1.magnetometer_timestamp = buf.replay.magnetometer_timestamp; - log_msg.body.log_RPL1.baro_timestamp = buf.replay.baro_timestamp; - log_msg.body.log_RPL1.gyro_x_rad = buf.replay.gyro_rad[0]; - log_msg.body.log_RPL1.gyro_y_rad = buf.replay.gyro_rad[1]; - log_msg.body.log_RPL1.gyro_z_rad = buf.replay.gyro_rad[2]; - log_msg.body.log_RPL1.accelerometer_x_m_s2 = buf.replay.accelerometer_m_s2[0]; - log_msg.body.log_RPL1.accelerometer_y_m_s2 = buf.replay.accelerometer_m_s2[1]; - log_msg.body.log_RPL1.accelerometer_z_m_s2 = buf.replay.accelerometer_m_s2[2]; - log_msg.body.log_RPL1.magnetometer_x_ga = buf.replay.magnetometer_ga[0]; - log_msg.body.log_RPL1.magnetometer_y_ga = buf.replay.magnetometer_ga[1]; - log_msg.body.log_RPL1.magnetometer_z_ga = buf.replay.magnetometer_ga[2]; - log_msg.body.log_RPL1.baro_alt_meter = buf.replay.baro_alt_meter; - LOGBUFFER_WRITE_AND_COUNT(RPL1); - - // only log the gps replay data if it actually updated - if (buf.replay.time_usec > 0) { - log_msg.msg_type = LOG_RPL2_MSG; - log_msg.body.log_RPL2.time_pos_usec = buf.replay.time_usec; - log_msg.body.log_RPL2.time_vel_usec = buf.replay.time_usec; - log_msg.body.log_RPL2.lat = buf.replay.lat; - log_msg.body.log_RPL2.lon = buf.replay.lon; - log_msg.body.log_RPL2.alt = buf.replay.alt; - log_msg.body.log_RPL2.fix_type = buf.replay.fix_type; - log_msg.body.log_RPL2.nsats = buf.replay.nsats; - log_msg.body.log_RPL2.eph = buf.replay.eph; - log_msg.body.log_RPL2.epv = buf.replay.epv; - log_msg.body.log_RPL2.sacc = buf.replay.sacc; - log_msg.body.log_RPL2.vel_m_s = buf.replay.vel_m_s; - log_msg.body.log_RPL2.vel_n_m_s = buf.replay.vel_n_m_s; - log_msg.body.log_RPL2.vel_e_m_s = buf.replay.vel_e_m_s; - log_msg.body.log_RPL2.vel_d_m_s = buf.replay.vel_d_m_s; - log_msg.body.log_RPL2.vel_ned_valid = buf.replay.vel_ned_valid; - LOGBUFFER_WRITE_AND_COUNT(RPL2); - } - - if (buf.replay.flow_timestamp > 0) { - log_msg.msg_type = LOG_RPL3_MSG; - log_msg.body.log_RPL3.time_flow_usec = buf.replay.flow_timestamp; - log_msg.body.log_RPL3.flow_integral_x = buf.replay.flow_pixel_integral[0]; - log_msg.body.log_RPL3.flow_integral_y = buf.replay.flow_pixel_integral[1]; - log_msg.body.log_RPL3.gyro_integral_x = buf.replay.flow_gyro_integral[0]; - log_msg.body.log_RPL3.gyro_integral_y = buf.replay.flow_gyro_integral[1]; - log_msg.body.log_RPL3.flow_time_integral = buf.replay.flow_time_integral; - log_msg.body.log_RPL3.flow_quality = buf.replay.flow_quality; - LOGBUFFER_WRITE_AND_COUNT(RPL3); - } - - if (buf.replay.rng_timestamp > 0) { - log_msg.msg_type = LOG_RPL4_MSG; - log_msg.body.log_RPL4.time_rng_usec = buf.replay.rng_timestamp; - log_msg.body.log_RPL4.range_to_ground = buf.replay.range_to_ground; - LOGBUFFER_WRITE_AND_COUNT(RPL4); - } - - if (buf.replay.asp_timestamp > 0) { - log_msg.msg_type = LOG_RPL6_MSG; - log_msg.body.log_RPL6.time_airs_usec = buf.replay.asp_timestamp; - log_msg.body.log_RPL6.indicated_airspeed_m_s = buf.replay.indicated_airspeed_m_s; - log_msg.body.log_RPL6.true_airspeed_m_s = buf.replay.true_airspeed_m_s; - LOGBUFFER_WRITE_AND_COUNT(RPL6); - } - - if (buf.replay.ev_timestamp > 0) { - log_msg.msg_type = LOG_RPL5_MSG; - log_msg.body.log_RPL5.time_ev_usec = buf.replay.ev_timestamp; - log_msg.body.log_RPL5.x = buf.replay.pos_ev[0]; - log_msg.body.log_RPL5.y = buf.replay.pos_ev[1]; - log_msg.body.log_RPL5.z = buf.replay.pos_ev[2]; - log_msg.body.log_RPL5.q0 = buf.replay.quat_ev[0]; - log_msg.body.log_RPL5.q1 = buf.replay.quat_ev[1]; - log_msg.body.log_RPL5.q2 = buf.replay.quat_ev[2]; - log_msg.body.log_RPL5.q3 = buf.replay.quat_ev[3]; - log_msg.body.log_RPL5.pos_err = buf.replay.pos_err; - log_msg.body.log_RPL5.ang_err = buf.replay.ang_err; - LOGBUFFER_WRITE_AND_COUNT(RPL5); - } - } - } - if (log_type == LOG_TYPE_ALL || log_type == LOG_TYPE_NORMAL) { if (fds[0].revents & POLLIN) { diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index defba464e9..371ba61a8f 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -514,63 +514,12 @@ struct log_EST5_s { #define LOG_OUT1_MSG 50 -/* --- EKF2 REPLAY Part 1 --- */ -#define LOG_RPL1_MSG 51 -struct log_RPL1_s { - uint64_t time_ref; - uint32_t gyro_integral_dt; - uint32_t accelerometer_integral_dt; - uint64_t magnetometer_timestamp; - uint64_t baro_timestamp; - float gyro_x_rad; - float gyro_y_rad; - float gyro_z_rad; - float accelerometer_x_m_s2; - float accelerometer_y_m_s2; - float accelerometer_z_m_s2; - float magnetometer_x_ga; - float magnetometer_y_ga; - float magnetometer_z_ga; - float baro_alt_meter; -}; -/* --- EKF2 REPLAY Part 2 --- */ -#define LOG_RPL2_MSG 52 -struct log_RPL2_s { - uint64_t time_pos_usec; - uint64_t time_vel_usec; - int32_t lat; - int32_t lon; - int32_t alt; - uint8_t fix_type; - uint8_t nsats; - float eph; - float epv; - float sacc; - float vel_m_s; - float vel_n_m_s; - float vel_e_m_s; - float vel_d_m_s; - bool vel_ned_valid; -}; - /* --- EST6 - ESTIMATOR INNOVATIONS --- */ #define LOG_EST6_MSG 53 struct log_EST6_s { float s[6]; }; -/* --- EKF2 REPLAY Part 3 --- */ -#define LOG_RPL3_MSG 54 -struct log_RPL3_s { - uint64_t time_flow_usec; - float flow_integral_x; - float flow_integral_y; - float gyro_integral_x; - float gyro_integral_y; - uint32_t flow_time_integral; - uint8_t flow_quality; -}; - /* --- CAMERA TRIGGER --- */ #define LOG_CAMT_MSG 55 struct log_CAMT_s { @@ -578,13 +527,6 @@ struct log_CAMT_s { uint32_t seq; }; -/* --- EKF2 REPLAY Part 4 --- */ -#define LOG_RPL4_MSG 56 -struct log_RPL4_s { - uint64_t time_rng_usec; - float range_to_ground; -}; - /* --- LAND DETECTOR --- */ #define LOG_LAND_MSG 57 struct log_LAND_s { @@ -594,29 +536,6 @@ struct log_LAND_s { /* 58 used for DGPS message shares struct with GPS MSG 8*/ -/* --- EKF2 REPLAY Part 6 --- */ -#define LOG_RPL6_MSG 59 -struct log_RPL6_s { - uint64_t time_airs_usec; - float indicated_airspeed_m_s; - float true_airspeed_m_s; -}; - -/* --- EKF2 REPLAY Part 5 --- */ -#define LOG_RPL5_MSG 60 -struct log_RPL5_s { - uint64_t time_ev_usec; - float x; - float y; - float z; - float q0; - float q1; - float q2; - float q3; - float pos_err; - float ang_err; -}; - /* --- SYSTEM LOAD --- */ #define LOG_LOAD_MSG 61 struct log_LOAD_s { @@ -720,12 +639,6 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(TSYN, "Q", "TimeOffset"), LOG_FORMAT(MACS, "fff", "RRint,PRint,YRint"), LOG_FORMAT(CAMT, "QI", "timestamp,seq"), - LOG_FORMAT(RPL1, "QffQQffffffffff", "t,gIdt,aIdt,Tm,Tb,gx,gy,gz,ax,ay,az,magX,magY,magZ,b_alt"), - LOG_FORMAT(RPL2, "QQLLiMMfffffffM", "Tpos,Tvel,lat,lon,alt,fix,nsats,eph,epv,sacc,v,vN,vE,vD,v_val"), - LOG_FORMAT(RPL3, "QffffIB", "Tflow,fx,fy,gx,gy,delT,qual"), - LOG_FORMAT(RPL4, "Qf", "Trng,rng"), - LOG_FORMAT(RPL5, "Qfffffffff", "Tev,x,y,z,q0,q1,q2,q3,posErr,angErr"), - LOG_FORMAT(RPL6, "Qff", "Tasp,inAsp,trAsp"), LOG_FORMAT(LAND, "B", "Landed"), LOG_FORMAT(LOAD, "f", "CPU"), LOG_FORMAT(DPRS, "Qffff", "errors,DPRESraw,DPRES,DPRESmax,Temp"),