diff options
Diffstat (limited to 'src')
-rw-r--r-- | src/modules/sdlog2/module.mk | 2 | ||||
-rw-r--r-- | src/modules/sdlog2/sdlog2.c | 68 | ||||
-rw-r--r-- | src/modules/sdlog2/sdlog2_messages.h | 9 |
3 files changed, 61 insertions, 18 deletions
diff --git a/src/modules/sdlog2/module.mk b/src/modules/sdlog2/module.mk index 91a9611d4..8fded0bdb 100644 --- a/src/modules/sdlog2/module.mk +++ b/src/modules/sdlog2/module.mk @@ -46,5 +46,5 @@ MODULE_STACKSIZE = 1200 MAXOPTIMIZATION = -Os -EXTRACFLAGS = -Wframe-larger-than=1300 +EXTRACFLAGS = -Wframe-larger-than=1400 diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 04bf71c17..1c8cdd4be 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -340,15 +340,32 @@ int sdlog2_main(int argc, char *argv[]) exit(0); } + if (!thread_running) { + warnx("not started\n"); + return 1; + } + if (!strcmp(argv[1], "status")) { - if (thread_running) { - sdlog2_status(); + sdlog2_status(); + return 0; + } - } else { - warnx("not started\n"); - } + if (!strcmp(argv[1], "on")) { + struct vehicle_command_s cmd; + cmd.command = VEHICLE_CMD_PREFLIGHT_STORAGE; + cmd.param3 = 1; + int fd = orb_advertise(ORB_ID(vehicle_command), &cmd); + close(fd); + return 0; + } - exit(0); + if (!strcmp(argv[1], "off")) { + struct vehicle_command_s cmd; + cmd.command = VEHICLE_CMD_PREFLIGHT_STORAGE; + cmd.param3 = 0; + int fd = orb_advertise(ORB_ID(vehicle_command), &cmd); + close(fd); + return 0; } sdlog2_usage("unrecognized command"); @@ -627,13 +644,16 @@ static void *logwriter_thread(void *arg) void sdlog2_start_log() { + if (logging_enabled) { + return; + } + /* create log dir if needed */ if (create_log_dir() != 0) { mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] error creating log dir"); exit(1); } - /* initialize statistics counter */ log_bytes_written = 0; start_time = hrt_absolute_time(); @@ -674,6 +694,10 @@ void sdlog2_start_log() void sdlog2_stop_log() { + if (!logging_enabled) { + return; + } + logging_enabled = false; /* wake up write thread one last time */ @@ -1347,6 +1371,9 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer_ga[0]; log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer_ga[1]; log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer_ga[2]; + log_msg.body.log_IMU.temp_gyro = buf.sensor.gyro_temp; + log_msg.body.log_IMU.temp_acc = buf.sensor.accelerometer_temp; + log_msg.body.log_IMU.temp_mag = buf.sensor.magnetometer_temp; LOGBUFFER_WRITE_AND_COUNT(IMU); } @@ -1402,6 +1429,9 @@ int sdlog2_thread_main(int argc, char *argv[]) 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]; + log_msg.body.log_IMU.temp_gyro = buf.sensor.gyro1_temp; + log_msg.body.log_IMU.temp_acc = buf.sensor.accelerometer1_temp; + log_msg.body.log_IMU.temp_mag = buf.sensor.magnetometer1_temp; LOGBUFFER_WRITE_AND_COUNT(IMU); } @@ -1431,6 +1461,9 @@ int sdlog2_thread_main(int argc, char *argv[]) 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]; + log_msg.body.log_IMU.temp_gyro = buf.sensor.gyro2_temp; + log_msg.body.log_IMU.temp_acc = buf.sensor.accelerometer2_temp; + log_msg.body.log_IMU.temp_mag = buf.sensor.magnetometer2_temp; LOGBUFFER_WRITE_AND_COUNT(IMU); } @@ -1810,13 +1843,18 @@ int sdlog2_thread_main(int argc, char *argv[]) void sdlog2_status() { - float kibibytes = log_bytes_written / 1024.0f; - float mebibytes = kibibytes / 1024.0f; - 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); + if (!logging_enabled) { + warnx("standing by"); + } else { + + float kibibytes = log_bytes_written / 1024.0f; + float mebibytes = kibibytes / 1024.0f; + 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); + mavlink_log_info(mavlink_fd, "[sdlog2] wrote %lu msgs, skipped %lu msgs", log_msgs_written, log_msgs_skipped); + } } /** @@ -1904,13 +1942,15 @@ void handle_command(struct vehicle_command_s *cmd) switch (cmd->command) { case VEHICLE_CMD_PREFLIGHT_STORAGE: - param = (int)(cmd->param3); + param = (int)(cmd->param3 + 0.5f); if (param == 1) { sdlog2_start_log(); } else if (param == 0) { sdlog2_stop_log(); + } else { + warnx("unknown storage cmd"); } break; diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 54b0fe73f..17401218a 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -89,6 +89,9 @@ struct log_IMU_s { float mag_x; float mag_y; float mag_z; + float temp_acc; + float temp_gyro; + float temp_mag; }; /* --- SENS - OTHER SENSORS --- */ @@ -471,9 +474,9 @@ static const struct log_format_s log_formats[] = { /* business-level messages, ID < 0x80 */ LOG_FORMAT(ATT, "fffffffffffff", "qw,qx,qy,qz,Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"), LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), - 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_S(IMU, IMU, "ffffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ,tA,tG,tM"), + LOG_FORMAT_S(IMU1, IMU, "ffffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ,tA,tG,tM"), + LOG_FORMAT_S(IMU2, IMU, "ffffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ,tA,tG,tM"), LOG_FORMAT_S(SENS, SENS, "fffff", "BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"), LOG_FORMAT_S(AIR1, SENS, "fffff", "BaroPa,BaroAlt,BaroTmp,DiffPres,DiffPresF"), LOG_FORMAT(LPOS, "ffffffffLLfBBff", "X,Y,Z,Dist,DistR,VX,VY,VZ,RLat,RLon,RAlt,PFlg,GFlg,EPH,EPV"), |