fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-17 02:03:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

delete sdlog2 EKF2 replay (#7982)

Browse Source
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
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sdlog2/sdlog2.c
+3 -158
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) {
src/modules/sdlog2/sdlog2_messages.h
-87
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"),