dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

delete sdlog2 EKF2 replay (#7982)

This commit is contained in:
Daniel Agar 2017-09-19 10:20:41 -04:00 committed by GitHub
parent b00c93ac21
commit 3498fe0c6f
17 changed files with 11 additions and 1686 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
ROMFS/px4fmu_common/init.d/rcS
View File

@ -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

16
Tools/sitl_run.sh
View File

@ -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

2
Tools/uorb_rtps_message_ids.py
View File

@ -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,

1
cmake/configs/posix_sitl_default.cmake
View File

@ -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

56
cmake/configs/posix_sitl_replay.cmake
View File

@ -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")

1
msg/CMakeLists.txt
View File

@ -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

49
msg/ekf2_replay.msg
View File

@ -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)

4
posix-configs/SITL/init/replay/iris
View File

@ -1,4 +0,0 @@
uorb start
ekf2 start -r
sleep 0.2
ekf2_replay start replay.px4log

1
posix-configs/eagle/200qx/mainapp.config
View File

@ -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

1
posix-configs/eagle/210qc/mainapp.config
View File

@ -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

1
posix-configs/eagle/flight/mainapp.config
View File

@ -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

96
src/modules/ekf2/ekf2_main.cpp
View File

@ -64,7 +64,6 @@
#include <uORB/topics/wind_estimate.h>
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/ekf2_innovations.h>
#include <uORB/topics/ekf2_replay.h>
#include <uORB/topics/ekf2_timestamps.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/distance_sensor.h>
@ -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_s> _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);

11
src/modules/ekf2/ekf2_params.c
View File

@ -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.
*

46
src/modules/ekf2_replay/CMakeLists.txt
View File

@ -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 :

1143
src/modules/ekf2_replay/ekf2_replay_main.cpp
View File

File diff suppressed because it is too large Load Diff

161
src/modules/sdlog2/sdlog2.c
View File

@ -97,7 +97,6 @@
#include <uORB/topics/mc_att_ctrl_status.h>
#include <uORB/topics/ekf2_innovations.h>
#include <uORB/topics/camera_trigger.h>
#include <uORB/topics/ekf2_replay.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/commander_state.h>
#include <uORB/topics/cpuload.h>
@ -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) {

87
src/modules/sdlog2/sdlog2_messages.h
View File

@ -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"),