diff options
Diffstat (limited to 'src/modules/sdlog2')
-rw-r--r-- | src/modules/sdlog2/module.mk | 2 | ||||
-rw-r--r-- | src/modules/sdlog2/sdlog2.c | 447 | ||||
-rw-r--r-- | src/modules/sdlog2/sdlog2_format.h | 8 | ||||
-rw-r--r-- | src/modules/sdlog2/sdlog2_messages.h | 143 |
4 files changed, 519 insertions, 81 deletions
diff --git a/src/modules/sdlog2/module.mk b/src/modules/sdlog2/module.mk index f53129688..a28d43e72 100644 --- a/src/modules/sdlog2/module.mk +++ b/src/modules/sdlog2/module.mk @@ -41,3 +41,5 @@ MODULE_PRIORITY = "SCHED_PRIORITY_MAX-30" SRCS = sdlog2.c \ logbuffer.c + +MODULE_STACKSIZE = 1200 diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index b74d4183b..31861c3fc 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -74,6 +74,7 @@ #include <uORB/topics/vehicle_global_position.h> #include <uORB/topics/position_setpoint_triplet.h> #include <uORB/topics/vehicle_gps_position.h> +#include <uORB/topics/satellite_info.h> #include <uORB/topics/vehicle_vicon_position.h> #include <uORB/topics/vehicle_global_velocity_setpoint.h> #include <uORB/topics/optical_flow.h> @@ -84,11 +85,14 @@ #include <uORB/topics/esc_status.h> #include <uORB/topics/telemetry_status.h> #include <uORB/topics/estimator_status.h> +#include <uORB/topics/tecs_status.h> #include <uORB/topics/system_power.h> #include <uORB/topics/servorail_status.h> +#include <uORB/topics/wind_estimate.h> #include <systemlib/systemlib.h> #include <systemlib/param/param.h> +#include <systemlib/perf_counter.h> #include <version/version.h> #include <mavlink/mavlink_log.h> @@ -97,6 +101,36 @@ #include "sdlog2_format.h" #include "sdlog2_messages.h" +/** + * Logging rate. + * + * A value of -1 indicates the commandline argument + * should be obeyed. A value of 0 sets the minimum rate, + * any other value is interpreted as rate in Hertz. This + * parameter is only read out before logging starts (which + * commonly is before arming). + * + * @min -1 + * @max 1 + * @group SD Logging + */ +PARAM_DEFINE_INT32(SDLOG_RATE, -1); + +/** + * Enable extended logging mode. + * + * A value of -1 indicates the commandline argument + * should be obeyed. A value of 0 disables extended + * logging mode, a value of 1 enables it. This + * parameter is only read out before logging starts + * (which commonly is before arming). + * + * @min -1 + * @max 1 + * @group SD Logging + */ +PARAM_DEFINE_INT32(SDLOG_EXT, -1); + #define LOGBUFFER_WRITE_AND_COUNT(_msg) if (logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(_msg))) { \ log_msgs_written++; \ } else { \ @@ -108,16 +142,20 @@ fds[fdsc_count].events = POLLIN; \ fdsc_count++; +#define MIN(X,Y) ((X) < (Y) ? (X) : (Y)) + static bool main_thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int deamon_task; /**< Handle of deamon task / thread */ static bool logwriter_should_exit = false; /**< Logwriter thread exit flag */ -static const int MAX_NO_LOGFOLDER = 999; /**< Maximum number of log dirs */ -static const int MAX_NO_LOGFILE = 999; /**< Maximum number of log files */ +static const unsigned MAX_NO_LOGFOLDER = 999; /**< Maximum number of log dirs */ +static const unsigned MAX_NO_LOGFILE = 999; /**< Maximum number of log files */ static const int LOG_BUFFER_SIZE_DEFAULT = 8192; static const int MAX_WRITE_CHUNK = 512; static const int MIN_BYTES_TO_WRITE = 512; +static bool _extended_logging = false; + static const char *log_root = "/fs/microsd/log"; static int mavlink_fd = -1; struct logbuffer_s lb; @@ -218,6 +256,8 @@ static int create_log_dir(void); */ static int open_log_file(void); +static int open_perf_file(const char* str); + static void sdlog2_usage(const char *reason) { @@ -225,12 +265,13 @@ sdlog2_usage(const char *reason) fprintf(stderr, "%s\n", reason); } - errx(1, "usage: sdlog2 {start|stop|status} [-r <log rate>] [-b <buffer size>] -e -a -t\n" + errx(1, "usage: sdlog2 {start|stop|status} [-r <log rate>] [-b <buffer size>] -e -a -t -x\n" "\t-r\tLog rate in Hz, 0 means unlimited rate\n" "\t-b\tLog buffer size in KiB, default is 8\n" "\t-e\tEnable logging by default (if not, can be started by command)\n" "\t-a\tLog only when armed (can be still overriden by command)\n" - "\t-t\tUse date/time for naming log directories and files\n"); + "\t-t\tUse date/time for naming log directories and files\n" + "\t-x\tExtended logging"); } /** @@ -349,8 +390,8 @@ int create_log_dir() int open_log_file() { /* string to hold the path to the log */ - char log_file_name[16] = ""; - char log_file_path[48] = ""; + char log_file_name[32] = ""; + char log_file_path[64] = ""; if (log_name_timestamp && gps_time != 0) { /* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ @@ -378,7 +419,7 @@ int open_log_file() if (file_number > MAX_NO_LOGFILE) { /* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */ - warnx("all %d possible files exist already", MAX_NO_LOGFILE); + mavlink_log_critical(mavlink_fd, "[sdlog2] ERR: max files %d", MAX_NO_LOGFILE); return -1; } } @@ -387,7 +428,58 @@ int open_log_file() if (fd < 0) { warn("failed opening log: %s", log_file_name); - mavlink_log_info(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name); + mavlink_log_critical(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name); + + } else { + warnx("log file: %s", log_file_name); + mavlink_log_info(mavlink_fd, "[sdlog2] log file: %s", log_file_name); + } + + return fd; +} + +int open_perf_file(const char* str) +{ + /* string to hold the path to the log */ + char log_file_name[32] = ""; + char log_file_path[64] = ""; + + if (log_name_timestamp && gps_time != 0) { + /* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ + time_t gps_time_sec = gps_time / 1000000; + struct tm t; + gmtime_r(&gps_time_sec, &t); + strftime(log_file_name, sizeof(log_file_name), "perf%H_%M_%S.txt", &t); + snprintf(log_file_path, sizeof(log_file_path), "%s/%s_%s", log_dir, str, log_file_name); + + } else { + unsigned file_number = 1; // start with file log001 + + /* look for the next file that does not exist */ + while (file_number <= MAX_NO_LOGFILE) { + /* format log file path: e.g. /fs/microsd/sess001/log001.bin */ + snprintf(log_file_name, sizeof(log_file_name), "perf%03u.txt", file_number); + snprintf(log_file_path, sizeof(log_file_path), "%s/%s_%s", log_dir, str, log_file_name); + + if (!file_exist(log_file_path)) { + break; + } + + file_number++; + } + + if (file_number > MAX_NO_LOGFILE) { + /* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */ + mavlink_log_critical(mavlink_fd, "[sdlog2] ERR: max files %d", MAX_NO_LOGFILE); + return -1; + } + } + + int fd = open(log_file_path, O_CREAT | O_WRONLY | O_DSYNC); + + if (fd < 0) { + warn("failed opening log: %s", log_file_name); + mavlink_log_critical(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name); } else { warnx("log file: %s", log_file_name); @@ -529,6 +621,12 @@ void sdlog2_start_log() errx(1, "error creating logwriter thread"); } + /* write all performance counters */ + int perf_fd = open_perf_file("preflight"); + dprintf(perf_fd, "PERFORMANCE COUNTERS PRE-FLIGHT\n\n"); + perf_print_all(perf_fd); + close(perf_fd); + logging_enabled = true; } @@ -556,6 +654,12 @@ void sdlog2_stop_log() logwriter_pthread = 0; pthread_attr_destroy(&logwriter_attr); + /* write all performance counters */ + int perf_fd = open_perf_file("postflight"); + dprintf(perf_fd, "PERFORMANCE COUNTERS POST-FLIGHT\n\n"); + perf_print_all(perf_fd); + close(perf_fd); + sdlog2_status(); } @@ -572,7 +676,7 @@ int write_formats(int fd) int written = 0; /* fill message format packet for each format and write it */ - for (int i = 0; i < log_formats_num; i++) { + for (unsigned i = 0; i < log_formats_num; i++) { log_msg_format.body = log_formats[i]; written += write(fd, &log_msg_format, sizeof(log_msg_format)); } @@ -679,12 +783,12 @@ int sdlog2_thread_main(int argc, char *argv[]) * set error flag instead */ bool err_flag = false; - while ((ch = getopt(argc, argv, "r:b:eat")) != EOF) { + while ((ch = getopt(argc, argv, "r:b:eatx")) != EOF) { switch (ch) { case 'r': { unsigned long r = strtoul(optarg, NULL, 10); - if (r <= 0) { + if (r == 0) { r = 1; } @@ -715,6 +819,10 @@ int sdlog2_thread_main(int argc, char *argv[]) log_name_timestamp = true; break; + case 'x': + _extended_logging = true; + break; + case '?': if (optopt == 'c') { warnx("option -%c requires an argument", optopt); @@ -741,6 +849,44 @@ int sdlog2_thread_main(int argc, char *argv[]) gps_time = 0; + /* interpret logging params */ + + param_t log_rate_ph = param_find("SDLOG_RATE"); + + if (log_rate_ph != PARAM_INVALID) { + int32_t param_log_rate; + param_get(log_rate_ph, ¶m_log_rate); + + if (param_log_rate > 0) { + + /* we can't do more than ~ 500 Hz, even with a massive buffer */ + if (param_log_rate > 500) { + param_log_rate = 500; + } + + sleep_delay = 1000000 / param_log_rate; + } else if (param_log_rate == 0) { + /* we need at minimum 10 Hz to be able to see anything */ + sleep_delay = 1000000 / 10; + } + } + + param_t log_ext_ph = param_find("SDLOG_EXT"); + + if (log_ext_ph != PARAM_INVALID) { + + int32_t param_log_extended; + param_get(log_ext_ph, ¶m_log_extended); + + if (param_log_extended > 0) { + _extended_logging = true; + } else if (param_log_extended == 0) { + _extended_logging = false; + } + /* any other value means to ignore the parameter, so no else case */ + + } + /* create log root dir */ int mkdir_ret = mkdir(log_root, S_IRWXU | S_IRWXG | S_IRWXO); @@ -798,8 +944,11 @@ int sdlog2_thread_main(int argc, char *argv[]) struct telemetry_status_s telemetry; struct range_finder_report range_finder; struct estimator_status_report estimator_status; + struct tecs_status_s tecs_status; struct system_power_s system_power; struct servorail_status_s servorail_status; + struct satellite_info_s sat_info; + struct wind_estimate_s wind_estimate; } buf; memset(&buf, 0, sizeof(buf)); @@ -830,10 +979,17 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_GVSP_s log_GVSP; struct log_BATT_s log_BATT; struct log_DIST_s log_DIST; - struct log_TELE_s log_TELE; - struct log_ESTM_s log_ESTM; + struct log_TEL_s log_TEL; + struct log_EST0_s log_EST0; + struct log_EST1_s log_EST1; struct log_PWR_s log_PWR; struct log_VICN_s log_VICN; + struct log_GS0A_s log_GS0A; + struct log_GS0B_s log_GS0B; + struct log_GS1A_s log_GS1A; + struct log_GS1B_s log_GS1B; + struct log_TECS_s log_TECS; + struct log_WIND_s log_WIND; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -855,6 +1011,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int global_pos_sub; int triplet_sub; int gps_pos_sub; + int sat_info_sub; int vicon_pos_sub; int flow_sub; int rc_sub; @@ -862,16 +1019,19 @@ int sdlog2_thread_main(int argc, char *argv[]) int esc_sub; int global_vel_sp_sub; int battery_sub; - int telemetry_sub; + int telemetry_subs[TELEMETRY_STATUS_ORB_ID_NUM]; int range_finder_sub; int estimator_status_sub; + int tecs_status_sub; int system_power_sub; int servorail_status_sub; + int wind_sub; } subs; subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); subs.status_sub = orb_subscribe(ORB_ID(vehicle_status)); subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + subs.sat_info_sub = orb_subscribe(ORB_ID(satellite_info)); subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); subs.att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); @@ -889,11 +1049,17 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.esc_sub = orb_subscribe(ORB_ID(esc_status)); subs.global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint)); subs.battery_sub = orb_subscribe(ORB_ID(battery_status)); - subs.telemetry_sub = orb_subscribe(ORB_ID(telemetry_status)); + for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { + subs.telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]); + } subs.range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder)); subs.estimator_status_sub = orb_subscribe(ORB_ID(estimator_status)); + subs.tecs_status_sub = orb_subscribe(ORB_ID(tecs_status)); subs.system_power_sub = orb_subscribe(ORB_ID(system_power)); subs.servorail_status_sub = orb_subscribe(ORB_ID(servorail_status)); + subs.wind_sub = orb_subscribe(ORB_ID(wind_estimate)); + /* we need to rate-limit wind, as we do not need the full update rate */ + orb_set_interval(subs.wind_sub, 90); thread_running = true; @@ -907,12 +1073,21 @@ int sdlog2_thread_main(int argc, char *argv[]) hrt_abstime magnetometer_timestamp = 0; hrt_abstime barometer_timestamp = 0; hrt_abstime differential_pressure_timestamp = 0; + hrt_abstime gyro1_timestamp = 0; + hrt_abstime accelerometer1_timestamp = 0; + hrt_abstime magnetometer1_timestamp = 0; + hrt_abstime gyro2_timestamp = 0; + hrt_abstime accelerometer2_timestamp = 0; + hrt_abstime magnetometer2_timestamp = 0; + + /* initialize calculated mean SNR */ + float snr_mean = 0.0f; /* enable logging on start if needed */ if (log_on_start) { /* check GPS topic to get GPS time */ if (log_name_timestamp) { - if (copy_if_updated(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos)) { + if (!orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos)) { gps_time = buf_gps_pos.time_gps_usec; } } @@ -960,20 +1135,21 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.msg_type = LOG_STAT_MSG; log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state; log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state; - log_msg.body.log_STAT.failsafe_state = (uint8_t) buf_status.failsafe_state; + log_msg.body.log_STAT.failsafe_state = (uint8_t) buf_status.failsafe; log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining; log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning; log_msg.body.log_STAT.landed = (uint8_t) buf_status.condition_landed; LOGBUFFER_WRITE_AND_COUNT(STAT); } - /* --- GPS POSITION --- */ + /* --- GPS POSITION - UNIT #1 --- */ if (gps_pos_updated) { + log_msg.msg_type = LOG_GPS_MSG; log_msg.body.log_GPS.gps_time = buf_gps_pos.time_gps_usec; log_msg.body.log_GPS.fix_type = buf_gps_pos.fix_type; - log_msg.body.log_GPS.eph = buf_gps_pos.eph_m; - log_msg.body.log_GPS.epv = buf_gps_pos.epv_m; + log_msg.body.log_GPS.eph = buf_gps_pos.eph; + log_msg.body.log_GPS.epv = buf_gps_pos.epv; log_msg.body.log_GPS.lat = buf_gps_pos.lat; log_msg.body.log_GPS.lon = buf_gps_pos.lon; log_msg.body.log_GPS.alt = buf_gps_pos.alt * 0.001f; @@ -981,12 +1157,66 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_GPS.vel_e = buf_gps_pos.vel_e_m_s; log_msg.body.log_GPS.vel_d = buf_gps_pos.vel_d_m_s; log_msg.body.log_GPS.cog = buf_gps_pos.cog_rad; + log_msg.body.log_GPS.sats = buf_gps_pos.satellites_used; + log_msg.body.log_GPS.snr_mean = snr_mean; + log_msg.body.log_GPS.noise_per_ms = buf_gps_pos.noise_per_ms; + log_msg.body.log_GPS.jamming_indicator = buf_gps_pos.jamming_indicator; LOGBUFFER_WRITE_AND_COUNT(GPS); } + /* --- SATELLITE INFO - UNIT #1 --- */ + if (_extended_logging) { + + if (copy_if_updated(ORB_ID(satellite_info), subs.sat_info_sub, &buf.sat_info)) { + + /* log the SNR of each satellite for a detailed view of signal quality */ + unsigned sat_info_count = MIN(buf.sat_info.count, sizeof(buf.sat_info.snr) / sizeof(buf.sat_info.snr[0])); + unsigned log_max_snr = sizeof(log_msg.body.log_GS0A.satellite_snr) / sizeof(log_msg.body.log_GS0A.satellite_snr[0]); + + log_msg.msg_type = LOG_GS0A_MSG; + memset(&log_msg.body.log_GS0A, 0, sizeof(log_msg.body.log_GS0A)); + snr_mean = 0.0f; + + /* fill set A and calculate mean SNR */ + for (unsigned i = 0; i < sat_info_count; i++) { + + snr_mean += buf.sat_info.snr[i]; + + int satindex = buf.sat_info.svid[i] - 1; + + /* handles index exceeding and wraps to to arithmetic errors */ + if ((satindex >= 0) && (satindex < (int)log_max_snr)) { + /* map satellites by their ID so that logs from two receivers can be compared */ + log_msg.body.log_GS0A.satellite_snr[satindex] = buf.sat_info.snr[i]; + } + } + LOGBUFFER_WRITE_AND_COUNT(GS0A); + snr_mean /= sat_info_count; + + log_msg.msg_type = LOG_GS0B_MSG; + memset(&log_msg.body.log_GS0B, 0, sizeof(log_msg.body.log_GS0B)); + + /* fill set B */ + for (unsigned i = 0; i < sat_info_count; i++) { + + /* get second bank of satellites, thus deduct bank size from index */ + int satindex = buf.sat_info.svid[i] - 1 - log_max_snr; + + /* handles index exceeding and wraps to to arithmetic errors */ + if ((satindex >= 0) && (satindex < (int)log_max_snr)) { + /* map satellites by their ID so that logs from two receivers can be compared */ + log_msg.body.log_GS0B.satellite_snr[satindex] = buf.sat_info.snr[i]; + } + } + LOGBUFFER_WRITE_AND_COUNT(GS0B); + } + } + /* --- SENSOR COMBINED --- */ if (copy_if_updated(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor)) { bool write_IMU = false; + bool write_IMU1 = false; + bool write_IMU2 = false; bool write_SENS = false; if (buf.sensor.timestamp != gyro_timestamp) { @@ -1038,6 +1268,64 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(SENS); } + if (buf.sensor.accelerometer1_timestamp != accelerometer1_timestamp) { + accelerometer1_timestamp = buf.sensor.accelerometer1_timestamp; + write_IMU1 = true; + } + + if (buf.sensor.gyro1_timestamp != gyro1_timestamp) { + gyro1_timestamp = buf.sensor.gyro1_timestamp; + write_IMU1 = true; + } + + if (buf.sensor.magnetometer1_timestamp != magnetometer1_timestamp) { + magnetometer1_timestamp = buf.sensor.magnetometer1_timestamp; + write_IMU1 = true; + } + + if (write_IMU1) { + log_msg.msg_type = LOG_IMU1_MSG; + log_msg.body.log_IMU.gyro_x = buf.sensor.gyro1_rad_s[0]; + log_msg.body.log_IMU.gyro_y = buf.sensor.gyro1_rad_s[1]; + log_msg.body.log_IMU.gyro_z = buf.sensor.gyro1_rad_s[2]; + log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer1_m_s2[0]; + log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer1_m_s2[1]; + log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer1_m_s2[2]; + log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer1_ga[0]; + log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer1_ga[1]; + log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer1_ga[2]; + LOGBUFFER_WRITE_AND_COUNT(IMU); + } + + if (buf.sensor.accelerometer2_timestamp != accelerometer2_timestamp) { + accelerometer2_timestamp = buf.sensor.accelerometer2_timestamp; + write_IMU2 = true; + } + + if (buf.sensor.gyro2_timestamp != gyro2_timestamp) { + gyro2_timestamp = buf.sensor.gyro2_timestamp; + write_IMU2 = true; + } + + if (buf.sensor.magnetometer2_timestamp != magnetometer2_timestamp) { + magnetometer2_timestamp = buf.sensor.magnetometer2_timestamp; + write_IMU2 = true; + } + + if (write_IMU2) { + log_msg.msg_type = LOG_IMU2_MSG; + log_msg.body.log_IMU.gyro_x = buf.sensor.gyro2_rad_s[0]; + log_msg.body.log_IMU.gyro_y = buf.sensor.gyro2_rad_s[1]; + log_msg.body.log_IMU.gyro_z = buf.sensor.gyro2_rad_s[2]; + log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer2_m_s2[0]; + log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer2_m_s2[1]; + log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer2_m_s2[2]; + log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer2_ga[0]; + log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer2_ga[1]; + log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer2_ga[2]; + LOGBUFFER_WRITE_AND_COUNT(IMU); + } + } /* --- ATTITUDE --- */ @@ -1105,10 +1393,16 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_LPOS.ref_lat = buf.local_pos.ref_lat * 1e7; log_msg.body.log_LPOS.ref_lon = buf.local_pos.ref_lon * 1e7; log_msg.body.log_LPOS.ref_alt = buf.local_pos.ref_alt; - log_msg.body.log_LPOS.xy_flags = (buf.local_pos.xy_valid ? 1 : 0) | (buf.local_pos.v_xy_valid ? 2 : 0) | (buf.local_pos.xy_global ? 8 : 0); - log_msg.body.log_LPOS.z_flags = (buf.local_pos.z_valid ? 1 : 0) | (buf.local_pos.v_z_valid ? 2 : 0) | (buf.local_pos.z_global ? 8 : 0); + log_msg.body.log_LPOS.pos_flags = (buf.local_pos.xy_valid ? 1 : 0) | + (buf.local_pos.z_valid ? 2 : 0) | + (buf.local_pos.v_xy_valid ? 4 : 0) | + (buf.local_pos.v_z_valid ? 8 : 0) | + (buf.local_pos.xy_global ? 16 : 0) | + (buf.local_pos.z_global ? 32 : 0); log_msg.body.log_LPOS.landed = buf.local_pos.landed; log_msg.body.log_LPOS.ground_dist_flags = (buf.local_pos.dist_bottom_valid ? 1 : 0); + log_msg.body.log_LPOS.eph = buf.local_pos.eph; + log_msg.body.log_LPOS.epv = buf.local_pos.epv; LOGBUFFER_WRITE_AND_COUNT(LPOS); } @@ -1138,17 +1432,20 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- GLOBAL POSITION SETPOINT --- */ if (copy_if_updated(ORB_ID(position_setpoint_triplet), subs.triplet_sub, &buf.triplet)) { - log_msg.msg_type = LOG_GPSP_MSG; - log_msg.body.log_GPSP.nav_state = buf.triplet.nav_state; - log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7d); - log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7d); - log_msg.body.log_GPSP.alt = buf.triplet.current.alt; - log_msg.body.log_GPSP.yaw = buf.triplet.current.yaw; - log_msg.body.log_GPSP.type = buf.triplet.current.type; - log_msg.body.log_GPSP.loiter_radius = buf.triplet.current.loiter_radius; - log_msg.body.log_GPSP.loiter_direction = buf.triplet.current.loiter_direction; - log_msg.body.log_GPSP.pitch_min = buf.triplet.current.pitch_min; - LOGBUFFER_WRITE_AND_COUNT(GPSP); + + if (buf.triplet.current.valid) { + log_msg.msg_type = LOG_GPSP_MSG; + log_msg.body.log_GPSP.nav_state = buf.triplet.nav_state; + log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7d); + log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7d); + log_msg.body.log_GPSP.alt = buf.triplet.current.alt; + log_msg.body.log_GPSP.yaw = buf.triplet.current.yaw; + log_msg.body.log_GPSP.type = buf.triplet.current.type; + log_msg.body.log_GPSP.loiter_radius = buf.triplet.current.loiter_radius; + log_msg.body.log_GPSP.loiter_direction = buf.triplet.current.loiter_direction; + log_msg.body.log_GPSP.pitch_min = buf.triplet.current.pitch_min; + LOGBUFFER_WRITE_AND_COUNT(GPSP); + } } /* --- VICON POSITION --- */ @@ -1180,8 +1477,8 @@ int sdlog2_thread_main(int argc, char *argv[]) if (copy_if_updated(ORB_ID(rc_channels), subs.rc_sub, &buf.rc)) { log_msg.msg_type = LOG_RC_MSG; /* Copy only the first 8 channels of 14 */ - memcpy(log_msg.body.log_RC.channel, buf.rc.chan, sizeof(log_msg.body.log_RC.channel)); - log_msg.body.log_RC.channel_count = buf.rc.chan_count; + memcpy(log_msg.body.log_RC.channel, buf.rc.channels, sizeof(log_msg.body.log_RC.channel)); + log_msg.body.log_RC.channel_count = buf.rc.channel_count; log_msg.body.log_RC.signal_lost = buf.rc.signal_lost; LOGBUFFER_WRITE_AND_COUNT(RC); } @@ -1253,16 +1550,19 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- TELEMETRY --- */ - if (copy_if_updated(ORB_ID(telemetry_status), subs.telemetry_sub, &buf.telemetry)) { - log_msg.msg_type = LOG_TELE_MSG; - log_msg.body.log_TELE.rssi = buf.telemetry.rssi; - log_msg.body.log_TELE.remote_rssi = buf.telemetry.remote_rssi; - log_msg.body.log_TELE.noise = buf.telemetry.noise; - log_msg.body.log_TELE.remote_noise = buf.telemetry.remote_noise; - log_msg.body.log_TELE.rxerrors = buf.telemetry.rxerrors; - log_msg.body.log_TELE.fixed = buf.telemetry.fixed; - log_msg.body.log_TELE.txbuf = buf.telemetry.txbuf; - LOGBUFFER_WRITE_AND_COUNT(TELE); + for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { + if (copy_if_updated(telemetry_status_orb_id[i], subs.telemetry_subs[i], &buf.telemetry)) { + log_msg.msg_type = LOG_TEL0_MSG + i; + log_msg.body.log_TEL.rssi = buf.telemetry.rssi; + log_msg.body.log_TEL.remote_rssi = buf.telemetry.remote_rssi; + log_msg.body.log_TEL.noise = buf.telemetry.noise; + log_msg.body.log_TEL.remote_noise = buf.telemetry.remote_noise; + log_msg.body.log_TEL.rxerrors = buf.telemetry.rxerrors; + log_msg.body.log_TEL.fixed = buf.telemetry.fixed; + log_msg.body.log_TEL.txbuf = buf.telemetry.txbuf; + log_msg.body.log_TEL.heartbeat_time = buf.telemetry.heartbeat_time; + LOGBUFFER_WRITE_AND_COUNT(TEL); + } } /* --- BOTTOM DISTANCE --- */ @@ -1276,15 +1576,53 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- ESTIMATOR STATUS --- */ if (copy_if_updated(ORB_ID(estimator_status), subs.estimator_status_sub, &buf.estimator_status)) { - log_msg.msg_type = LOG_ESTM_MSG; - unsigned maxcopy = (sizeof(buf.estimator_status.states) < sizeof(log_msg.body.log_ESTM.s)) ? sizeof(buf.estimator_status.states) : sizeof(log_msg.body.log_ESTM.s); - memset(&(log_msg.body.log_ESTM.s), 0, sizeof(log_msg.body.log_ESTM.s)); - memcpy(&(log_msg.body.log_ESTM.s), buf.estimator_status.states, maxcopy); - log_msg.body.log_ESTM.n_states = buf.estimator_status.n_states; - log_msg.body.log_ESTM.states_nan = buf.estimator_status.states_nan; - log_msg.body.log_ESTM.covariance_nan = buf.estimator_status.covariance_nan; - log_msg.body.log_ESTM.kalman_gain_nan = buf.estimator_status.kalman_gain_nan; - LOGBUFFER_WRITE_AND_COUNT(ESTM); + log_msg.msg_type = LOG_EST0_MSG; + unsigned maxcopy0 = (sizeof(buf.estimator_status.states) < sizeof(log_msg.body.log_EST0.s)) ? sizeof(buf.estimator_status.states) : sizeof(log_msg.body.log_EST0.s); + memset(&(log_msg.body.log_EST0.s), 0, sizeof(log_msg.body.log_EST0.s)); + memcpy(&(log_msg.body.log_EST0.s), buf.estimator_status.states, maxcopy0); + log_msg.body.log_EST0.n_states = buf.estimator_status.n_states; + log_msg.body.log_EST0.nan_flags = buf.estimator_status.nan_flags; + log_msg.body.log_EST0.health_flags = buf.estimator_status.health_flags; + log_msg.body.log_EST0.timeout_flags = buf.estimator_status.timeout_flags; + LOGBUFFER_WRITE_AND_COUNT(EST0); + + log_msg.msg_type = LOG_EST1_MSG; + unsigned maxcopy1 = ((sizeof(buf.estimator_status.states) - maxcopy0) < sizeof(log_msg.body.log_EST1.s)) ? (sizeof(buf.estimator_status.states) - maxcopy0) : sizeof(log_msg.body.log_EST1.s); + memset(&(log_msg.body.log_EST1.s), 0, sizeof(log_msg.body.log_EST1.s)); + memcpy(&(log_msg.body.log_EST1.s), buf.estimator_status.states + maxcopy0, maxcopy1); + LOGBUFFER_WRITE_AND_COUNT(EST1); + } + + /* --- TECS STATUS --- */ + if (copy_if_updated(ORB_ID(tecs_status), subs.tecs_status_sub, &buf.tecs_status)) { + log_msg.msg_type = LOG_TECS_MSG; + log_msg.body.log_TECS.altitudeSp = buf.tecs_status.altitudeSp; + log_msg.body.log_TECS.altitude = buf.tecs_status.altitude; + log_msg.body.log_TECS.altitudeFiltered = buf.tecs_status.altitudeFiltered; + log_msg.body.log_TECS.flightPathAngleSp = buf.tecs_status.flightPathAngleSp; + log_msg.body.log_TECS.flightPathAngle = buf.tecs_status.flightPathAngle; + log_msg.body.log_TECS.flightPathAngleFiltered = buf.tecs_status.flightPathAngleFiltered; + log_msg.body.log_TECS.airspeedSp = buf.tecs_status.airspeedSp; + log_msg.body.log_TECS.airspeed = buf.tecs_status.airspeed; + log_msg.body.log_TECS.airspeedFiltered = buf.tecs_status.airspeedFiltered; + log_msg.body.log_TECS.airspeedDerivativeSp = buf.tecs_status.airspeedDerivativeSp; + log_msg.body.log_TECS.airspeedDerivative = buf.tecs_status.airspeedDerivative; + log_msg.body.log_TECS.totalEnergyRateSp = buf.tecs_status.totalEnergyRateSp; + log_msg.body.log_TECS.totalEnergyRate = buf.tecs_status.totalEnergyRate; + log_msg.body.log_TECS.energyDistributionRateSp = buf.tecs_status.energyDistributionRateSp; + log_msg.body.log_TECS.energyDistributionRate = buf.tecs_status.energyDistributionRate; + log_msg.body.log_TECS.mode = (uint8_t)buf.tecs_status.mode; + LOGBUFFER_WRITE_AND_COUNT(TECS); + } + + /* --- WIND ESTIMATE --- */ + if (copy_if_updated(ORB_ID(wind_estimate), subs.wind_sub, &buf.wind_estimate)) { + log_msg.msg_type = LOG_WIND_MSG; + log_msg.body.log_WIND.x = buf.wind_estimate.windspeed_north; + log_msg.body.log_WIND.y = buf.wind_estimate.windspeed_east; + log_msg.body.log_WIND.cov_x = buf.wind_estimate.covariance_north; + log_msg.body.log_WIND.cov_y = buf.wind_estimate.covariance_east; + LOGBUFFER_WRITE_AND_COUNT(WIND); } /* signal the other thread new data, but not yet unlock */ @@ -1320,6 +1658,7 @@ void sdlog2_status() float seconds = ((float)(hrt_absolute_time() - start_time)) / 1000000.0f; warnx("wrote %lu msgs, %4.2f MiB (average %5.3f KiB/s), skipped %lu msgs", log_msgs_written, (double)mebibytes, (double)(kibibytes / seconds), log_msgs_skipped); + warnx("extended logging: %s", (_extended_logging) ? "ON" : "OFF"); mavlink_log_info(mavlink_fd, "[sdlog2] wrote %lu msgs, skipped %lu msgs", log_msgs_written, log_msgs_skipped); } diff --git a/src/modules/sdlog2/sdlog2_format.h b/src/modules/sdlog2/sdlog2_format.h index dc5e6c8bd..aff0e3f48 100644 --- a/src/modules/sdlog2/sdlog2_format.h +++ b/src/modules/sdlog2/sdlog2_format.h @@ -91,6 +91,14 @@ struct log_format_s { .labels = _labels \ } +#define LOG_FORMAT_S(_name, _struct_name, _format, _labels) { \ + .type = LOG_##_name##_MSG, \ + .length = sizeof(struct log_##_struct_name##_s) + LOG_PACKET_HEADER_LEN, \ + .name = #_name, \ + .format = _format, \ + .labels = _labels \ + } + #define LOG_FORMAT_MSG 0x80 #define LOG_PACKET_SIZE(_name) LOG_PACKET_HEADER_LEN + sizeof(struct log_##_name##_s) diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 595a787d6..853a3811f 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -73,6 +73,8 @@ struct log_ATSP_s { /* --- IMU - IMU SENSORS --- */ #define LOG_IMU_MSG 4 +#define LOG_IMU1_MSG 22 +#define LOG_IMU2_MSG 23 struct log_IMU_s { float acc_x; float acc_y; @@ -109,10 +111,11 @@ struct log_LPOS_s { int32_t ref_lat; int32_t ref_lon; float ref_alt; - uint8_t xy_flags; - uint8_t z_flags; + uint8_t pos_flags; uint8_t landed; uint8_t ground_dist_flags; + float eph; + float epv; }; /* --- LPSP - LOCAL POSITION SETPOINT --- */ @@ -138,6 +141,10 @@ struct log_GPS_s { float vel_e; float vel_d; float cog; + uint8_t sats; + uint16_t snr_mean; + uint16_t noise_per_ms; + uint16_t jamming_indicator; }; /* --- ATTC - ATTITUDE CONTROLS (ACTUATOR_0 CONTROLS)--- */ @@ -271,27 +278,8 @@ struct log_DIST_s { uint8_t flags; }; -/* --- TELE - TELEMETRY STATUS --- */ -#define LOG_TELE_MSG 22 -struct log_TELE_s { - uint8_t rssi; - uint8_t remote_rssi; - uint8_t noise; - uint8_t remote_noise; - uint16_t rxerrors; - uint16_t fixed; - uint8_t txbuf; -}; +/* LOG IMU1 and IMU2 MSGs consume IDs 22 and 23 */ -/* --- ESTM - ESTIMATOR STATUS --- */ -#define LOG_ESTM_MSG 23 -struct log_ESTM_s { - float s[10]; - uint8_t n_states; - uint8_t states_nan; - uint8_t covariance_nan; - uint8_t kalman_gain_nan; -}; /* --- PWR - ONBOARD POWER SYSTEM --- */ #define LOG_PWR_MSG 24 @@ -317,6 +305,95 @@ struct log_VICN_s { float yaw; }; +/* --- GS0A - GPS SNR #0, SAT GROUP A --- */ +#define LOG_GS0A_MSG 26 +struct log_GS0A_s { + uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */ +}; + +/* --- GS0B - GPS SNR #0, SAT GROUP B --- */ +#define LOG_GS0B_MSG 27 +struct log_GS0B_s { + uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */ +}; + +/* --- GS1A - GPS SNR #1, SAT GROUP A --- */ +#define LOG_GS1A_MSG 28 +struct log_GS1A_s { + uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */ +}; + +/* --- GS1B - GPS SNR #1, SAT GROUP B --- */ +#define LOG_GS1B_MSG 29 +struct log_GS1B_s { + uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */ +}; + +/* --- TECS - TECS STATUS --- */ +#define LOG_TECS_MSG 30 +struct log_TECS_s { + float altitudeSp; + float altitude; + float altitudeFiltered; + float flightPathAngleSp; + float flightPathAngle; + float flightPathAngleFiltered; + float airspeedSp; + float airspeed; + float airspeedFiltered; + float airspeedDerivativeSp; + float airspeedDerivative; + + float totalEnergyRateSp; + float totalEnergyRate; + float energyDistributionRateSp; + float energyDistributionRate; + + uint8_t mode; +}; + +/* --- WIND - WIND ESTIMATE --- */ +#define LOG_WIND_MSG 31 +struct log_WIND_s { + float x; + float y; + float cov_x; + float cov_y; +}; + +/* --- EST0 - ESTIMATOR STATUS --- */ +#define LOG_EST0_MSG 32 +struct log_EST0_s { + float s[12]; + uint8_t n_states; + uint8_t nan_flags; + uint8_t health_flags; + uint8_t timeout_flags; +}; + +/* --- EST1 - ESTIMATOR STATUS --- */ +#define LOG_EST1_MSG 33 +struct log_EST1_s { + float s[16]; +}; + +/* --- TEL0..3 - TELEMETRY STATUS --- */ +#define LOG_TEL0_MSG 34 +#define LOG_TEL1_MSG 35 +#define LOG_TEL2_MSG 36 +#define LOG_TEL3_MSG 37 +struct log_TEL_s { + uint8_t rssi; + uint8_t remote_rssi; + uint8_t noise; + uint8_t remote_noise; + uint16_t rxerrors; + uint16_t fixed; + uint8_t txbuf; + uint64_t heartbeat_time; +}; + + /********** SYSTEM MESSAGES, ID > 0x80 **********/ /* --- TIME - TIME STAMP --- */ @@ -346,11 +423,13 @@ static const struct log_format_s log_formats[] = { /* business-level messages, ID < 0x80 */ LOG_FORMAT(ATT, "fffffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"), LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), - LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), + LOG_FORMAT_S(IMU, IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), + LOG_FORMAT_S(IMU1, IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), + LOG_FORMAT_S(IMU2, IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), LOG_FORMAT(SENS, "fffff", "BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"), - LOG_FORMAT(LPOS, "ffffffffLLfBBBB", "X,Y,Z,dist,distR,VX,VY,VZ,RLat,RLon,RAlt,XYFlg,ZFlg,LFlg,GFlg"), + LOG_FORMAT(LPOS, "ffffffffLLfBBBff", "X,Y,Z,Dist,DistR,VX,VY,VZ,RLat,RLon,RAlt,PFlg,LFlg,GFlg,EPH,EPV"), LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), - LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), + LOG_FORMAT(GPS, "QBffLLfffffBHHH", "GPSTime,Fix,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog,nSat,SNR,N,J"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), LOG_FORMAT(STAT, "BBBfBB", "MainState,ArmState,FailsafeState,BatRem,BatWarn,Landed"), LOG_FORMAT(RC, "ffffffffBB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count,SignalLost"), @@ -364,10 +443,20 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"), LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"), - LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"), - LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,nStat,statNaN,covNaN,kGainNaN"), + LOG_FORMAT_S(TEL0, TEL, "BBBBHHBQ", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"), + LOG_FORMAT_S(TEL1, TEL, "BBBBHHBQ", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"), + LOG_FORMAT_S(TEL2, TEL, "BBBBHHBQ", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"), + LOG_FORMAT_S(TEL3, TEL, "BBBBHHBQ", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"), + LOG_FORMAT(EST0, "ffffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,nStat,fNaN,fHealth,fTOut"), + LOG_FORMAT(EST1, "ffffffffffffffff", "s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27"), LOG_FORMAT(PWR, "fffBBBBB", "Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"), LOG_FORMAT(VICN, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"), + LOG_FORMAT(GS0A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), + LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), + LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), + LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), + LOG_FORMAT(TECS, "fffffffffffffffB", "ASP,A,AF,FSP,F,FF,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"), + LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"), /* system-level messages, ID >= 0x80 */ /* FMT: don't write format of format message, it's useless */ |