From 4052652a28232edcdcb8089dcb05a8dc426343e4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 6 Jun 2013 23:19:16 +0400 Subject: [PATCH 01/12] sdlog2: ATTC - vehicle attitude control logging added --- src/modules/sdlog2/sdlog2.c | 8 +++++++- src/modules/sdlog2/sdlog2_messages.h | 10 ++++++++++ 2 files changed, 17 insertions(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index b5821098ff..290577790d 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -652,6 +652,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_LPOS_s log_LPOS; struct log_LPSP_s log_LPSP; struct log_GPS_s log_GPS; + struct log_ATTC_s log_ATTC; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -930,7 +931,12 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- ACTUATOR CONTROL --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.act_controls_sub, &buf.act_controls); - // TODO not implemented yet + log_msg.msg_type = LOG_ATTC_MSG; + log_msg.body.log_ATTC.roll = buf.act_controls.control[0]; + log_msg.body.log_ATTC.pitch = buf.act_controls.control[1]; + log_msg.body.log_ATTC.yaw = buf.act_controls.control[2]; + log_msg.body.log_ATTC.thrust = buf.act_controls.control[3]; + LOGBUFFER_WRITE_AND_COUNT(ATTC); } /* --- ACTUATOR CONTROL EFFECTIVE --- */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 316459b1e9..44c6b9cbab 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -132,6 +132,15 @@ struct log_GPS_s { float vel_d; float cog; }; + +/* --- ATTC - ATTITUDE CONTROLS --- */ +#define LOG_ATTC_MSG 9 +struct log_ATTC_s { + float roll; + float pitch; + float yaw; + float thrust; +}; #pragma pack(pop) /* construct list of all message formats */ @@ -145,6 +154,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(LPOS, "fffffffLLf", "X,Y,Z,VX,VY,VZ,Heading,HomeLat,HomeLon,HomeAlt"), 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(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), }; static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s); From 4302f7640216b2bef88d270a268dbea2f712119e Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 6 Jun 2013 22:49:49 -0700 Subject: [PATCH 02/12] Hotfix: fix building firmware parallel --- makefiles/firmware.mk | 12 ++++++------ makefiles/nuttx.mk | 6 +++++- 2 files changed, 11 insertions(+), 7 deletions(-) diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk index 6b09e6ec32..f1c1b496a0 100644 --- a/makefiles/firmware.mk +++ b/makefiles/firmware.mk @@ -176,6 +176,12 @@ GLOBAL_DEPS += $(MAKEFILE_LIST) # EXTRA_CLEANS = +################################################################################ +# NuttX libraries and paths +################################################################################ + +include $(PX4_MK_DIR)/nuttx.mk + ################################################################################ # Modules ################################################################################ @@ -296,12 +302,6 @@ $(LIBRARY_CLEANS): LIBRARY_MK=$(mkfile) \ clean -################################################################################ -# NuttX libraries and paths -################################################################################ - -include $(PX4_MK_DIR)/nuttx.mk - ################################################################################ # ROMFS generation ################################################################################ diff --git a/makefiles/nuttx.mk b/makefiles/nuttx.mk index 346735a02a..d283096b25 100644 --- a/makefiles/nuttx.mk +++ b/makefiles/nuttx.mk @@ -69,10 +69,14 @@ INCLUDE_DIRS += $(NUTTX_EXPORT_DIR)include \ LIB_DIRS += $(NUTTX_EXPORT_DIR)libs LIBS += -lapps -lnuttx -LINK_DEPS += $(NUTTX_EXPORT_DIR)libs/libapps.a \ +NUTTX_LIBS = $(NUTTX_EXPORT_DIR)libs/libapps.a \ $(NUTTX_EXPORT_DIR)libs/libnuttx.a +LINK_DEPS += $(NUTTX_LIBS) $(NUTTX_CONFIG_HEADER): $(NUTTX_ARCHIVE) @$(ECHO) %% Unpacking $(NUTTX_ARCHIVE) $(Q) $(UNZIP_CMD) -q -o -d $(WORK_DIR) $(NUTTX_ARCHIVE) $(Q) $(TOUCH) $@ + + $(LDSCRIPT): $(NUTTX_CONFIG_HEADER) + $(NUTTX_LIBS): $(NUTTX_CONFIG_HEADER) From 6c7c130de72b3323211c1ac2e08c8ccf1630c865 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 7 Jun 2013 10:34:55 +0200 Subject: [PATCH 03/12] Hotfix: Make IOs mixer loading pedantic to make sure the full mixer loads --- src/modules/px4iofirmware/mixer.cpp | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 0b8ed6dc59..a2193b526b 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -294,8 +294,7 @@ mixer_handle_text(const void *buffer, size_t length) case F2I_MIXER_ACTION_APPEND: isr_debug(2, "append %d", length); - /* check for overflow - this is really fatal */ - /* XXX could add just what will fit & try to parse, then repeat... */ + /* check for overflow - this would be really fatal */ if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) { r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; return; @@ -314,8 +313,13 @@ mixer_handle_text(const void *buffer, size_t length) /* if anything was parsed */ if (resid != mixer_text_length) { - /* ideally, this should test resid == 0 ? */ - r_status_flags |= PX4IO_P_STATUS_FLAGS_MIXER_OK; + /* only set mixer ok if no residual is left over */ + if (resid == 0) { + r_status_flags |= PX4IO_P_STATUS_FLAGS_MIXER_OK; + } else { + /* not yet reached the end of the mixer, set as not ok */ + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; + } isr_debug(2, "used %u", mixer_text_length - resid); @@ -338,11 +342,13 @@ mixer_set_failsafe() { /* * Check if a custom failsafe value has been written, - * else use the opportunity to set decent defaults. + * or if the mixer is not ok and bail out. */ - if (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) + if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) || + !(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) return; + /* set failsafe defaults to the values for all inputs = 0 */ float outputs[IO_SERVO_COUNT]; unsigned mixed; From 11544d27b7629078b6a7a2247f159b535816e019 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 7 Jun 2013 10:35:37 +0200 Subject: [PATCH 04/12] Hotfix: Enlarge the buffer size for mixers, ensure that reasonable setups with 16 outputs can work --- src/systemcmds/mixer/mixer.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/systemcmds/mixer/mixer.c b/src/systemcmds/mixer/mixer.c index 55c4f08362..e642ed0676 100644 --- a/src/systemcmds/mixer/mixer.c +++ b/src/systemcmds/mixer/mixer.c @@ -88,8 +88,8 @@ load(const char *devname, const char *fname) { int dev; FILE *fp; - char line[80]; - char buf[512]; + char line[120]; + char buf[2048]; /* open the device */ if ((dev = open(devname, 0)) < 0) From 4e3f4b57e3e603aaea665758ea0240c48ea9e54f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 7 Jun 2013 10:36:56 +0200 Subject: [PATCH 05/12] Hotfix: Allow the IO mixer loading to load larger mixers, fix up the px4io test command to allow a clean exit --- src/drivers/px4io/px4io.cpp | 21 +++++++++++++++++++-- 1 file changed, 19 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 0934e614b3..19163cebe3 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1302,7 +1302,7 @@ PX4IO::print_status() io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VBATT), io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT), io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE)); - printf("amp_per_volt %.3f amp_offset %.3f mAhDischarged %.3f\n", + printf("amp_per_volt %.3f amp_offset %.3f mAh discharged %.3f\n", (double)_battery_amp_per_volt, (double)_battery_amp_bias, (double)_battery_mamphour_total); @@ -1496,7 +1496,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) case MIXERIOCLOADBUF: { const char *buf = (const char *)arg; - ret = mixer_send(buf, strnlen(buf, 1024)); + ret = mixer_send(buf, strnlen(buf, 2048)); break; } @@ -1637,6 +1637,13 @@ test(void) if (ioctl(fd, PWM_SERVO_ARM, 0)) err(1, "failed to arm servos"); + /* Open console directly to grab CTRL-C signal */ + int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY); + if (!console) + err(1, "failed opening console"); + + warnx("Press CTRL-C or 'c' to abort."); + for (;;) { /* sweep all servos between 1000..2000 */ @@ -1671,6 +1678,16 @@ test(void) if (value != servos[i]) errx(1, "servo %d readback error, got %u expected %u", i, value, servos[i]); } + + /* Check if user wants to quit */ + char c; + if (read(console, &c, 1) == 1) { + if (c == 0x03 || c == 0x63) { + warnx("User abort\n"); + close(console); + exit(0); + } + } } } From 5b5d20bb638c884f943612da43a5ce30c1742eb8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 7 Jun 2013 10:37:31 +0200 Subject: [PATCH 06/12] Hotfix: Add an IO pass mixer with 8 outputs --- ROMFS/px4fmu_common/mixers/IO_pass.mix | 38 ++++++++++++++++++++++++++ 1 file changed, 38 insertions(+) create mode 100644 ROMFS/px4fmu_common/mixers/IO_pass.mix diff --git a/ROMFS/px4fmu_common/mixers/IO_pass.mix b/ROMFS/px4fmu_common/mixers/IO_pass.mix new file mode 100644 index 0000000000..39f875ddb9 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/IO_pass.mix @@ -0,0 +1,38 @@ +Passthrough mixer for PX4IO +============================ + +This file defines passthrough mixers suitable for testing. + +Channel group 0, channels 0-7 are passed directly through to the outputs. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 0 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 1 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 2 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 3 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 7 10000 10000 0 -10000 10000 From 5c74809dac57f58f92ad92433496731481703982 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 7 Jun 2013 10:38:09 +0200 Subject: [PATCH 07/12] Config change: Set USB console as default. --- nuttx/configs/px4fmu/nsh/defconfig | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/nuttx/configs/px4fmu/nsh/defconfig b/nuttx/configs/px4fmu/nsh/defconfig index 02e2243020..0662f7fbe3 100755 --- a/nuttx/configs/px4fmu/nsh/defconfig +++ b/nuttx/configs/px4fmu/nsh/defconfig @@ -248,7 +248,7 @@ CONFIG_SERIAL_TERMIOS=y CONFIG_SERIAL_CONSOLE_REINIT=y CONFIG_STANDARD_SERIAL=y -CONFIG_USART1_SERIAL_CONSOLE=y +CONFIG_USART1_SERIAL_CONSOLE=n CONFIG_USART2_SERIAL_CONSOLE=n CONFIG_USART3_SERIAL_CONSOLE=n CONFIG_UART4_SERIAL_CONSOLE=n @@ -561,7 +561,7 @@ CONFIG_START_MONTH=1 CONFIG_START_DAY=1 CONFIG_GREGORIAN_TIME=n CONFIG_JULIAN_TIME=n -CONFIG_DEV_CONSOLE=y +CONFIG_DEV_CONSOLE=n CONFIG_DEV_LOWCONSOLE=n CONFIG_MUTEX_TYPES=n CONFIG_PRIORITY_INHERITANCE=y @@ -717,7 +717,7 @@ CONFIG_ARCH_BZERO=n # zero for all dynamic allocations. # CONFIG_MAX_TASKS=32 -CONFIG_MAX_TASK_ARGS=8 +CONFIG_MAX_TASK_ARGS=10 CONFIG_NPTHREAD_KEYS=4 CONFIG_NFILE_DESCRIPTORS=32 CONFIG_NFILE_STREAMS=25 @@ -925,7 +925,7 @@ CONFIG_USBDEV_TRACE_NRECORDS=512 # Size of the serial receive/transmit buffers. Default 256. # CONFIG_CDCACM=y -CONFIG_CDCACM_CONSOLE=n +CONFIG_CDCACM_CONSOLE=y #CONFIG_CDCACM_EP0MAXPACKET CONFIG_CDCACM_EPINTIN=1 #CONFIG_CDCACM_EPINTIN_FSSIZE From 5bad18691649b4b50d46c11384c3aa5051b6519e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 7 Jun 2013 13:36:15 +0400 Subject: [PATCH 08/12] sdlog2: STAT (vehicle state) log message added, minor optimizations --- src/modules/sdlog2/sdlog2.c | 34 +++++++++++++++++++++++----- src/modules/sdlog2/sdlog2_messages.h | 15 ++++++++++++ 2 files changed, 43 insertions(+), 6 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 290577790d..32bd422bc7 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -597,10 +597,12 @@ int sdlog2_thread_main(int argc, char *argv[]) /* file descriptors to wait for */ struct pollfd fds[fdsc]; + struct vehicle_status_s buf_status; + memset(&buf_status, 0, sizeof(buf_status)); + /* warning! using union here to save memory, elements should be used separately! */ union { struct vehicle_command_s cmd; - struct vehicle_status_s status; struct sensor_combined_s sensor; struct vehicle_attitude_s att; struct vehicle_attitude_setpoint_s att_sp; @@ -653,6 +655,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_LPSP_s log_LPSP; struct log_GPS_s log_GPS; struct log_ATTC_s log_ATTC; + struct log_STAT_s log_STAT; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -803,22 +806,25 @@ int sdlog2_thread_main(int argc, char *argv[]) * logging_enabled can be changed while checking vehicle_command and vehicle_status */ bool check_data = logging_enabled; int ifds = 0; + int handled_topics = 0; - /* --- VEHICLE COMMAND --- */ + /* --- VEHICLE COMMAND - LOG MANAGEMENT --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd); handle_command(&buf.cmd); + handled_topics++; } - /* --- VEHICLE STATUS --- */ + /* --- VEHICLE STATUS - LOG MANAGEMENT --- */ if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_status), subs.status_sub, &buf.status); + orb_copy(ORB_ID(vehicle_status), subs.status_sub, &buf_status); if (log_when_armed) { - handle_status(&buf.status); + handle_status(&buf_status); } + handled_topics++; } - if (!logging_enabled || !check_data) { + if (!logging_enabled || !check_data || handled_topics >= poll_ret) { continue; } @@ -829,6 +835,22 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_TIME.t = hrt_absolute_time(); LOGBUFFER_WRITE_AND_COUNT(TIME); + /* --- VEHICLE STATUS --- */ + if (fds[ifds++].revents & POLLIN) { + // Don't orb_copy, it's already done few lines above + log_msg.msg_type = LOG_STAT_MSG; + log_msg.body.log_STAT.state = (unsigned char) buf_status.state_machine; + log_msg.body.log_STAT.flight_mode = (unsigned char) buf_status.flight_mode; + log_msg.body.log_STAT.manual_control_mode = (unsigned char) buf_status.manual_control_mode; + log_msg.body.log_STAT.manual_sas_mode = (unsigned char) buf_status.manual_sas_mode; + log_msg.body.log_STAT.armed = (unsigned char) buf_status.flag_system_armed; + log_msg.body.log_STAT.battery_voltage = buf_status.voltage_battery; + log_msg.body.log_STAT.battery_current = buf_status.current_battery; + log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining; + log_msg.body.log_STAT.battery_warning = (unsigned char) buf_status.battery_warning; + LOGBUFFER_WRITE_AND_COUNT(STAT); + } + /* --- GPS POSITION --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 44c6b9cbab..2b6b865215 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -141,6 +141,20 @@ struct log_ATTC_s { float yaw; float thrust; }; + +/* --- STAT - VEHICLE STATE --- */ +#define LOG_STAT_MSG 10 +struct log_STAT_s { + unsigned char state; + unsigned char flight_mode; + unsigned char manual_control_mode; + unsigned char manual_sas_mode; + unsigned char armed; + float battery_voltage; + float battery_current; + float battery_remaining; + unsigned char battery_warning; +}; #pragma pack(pop) /* construct list of all message formats */ @@ -155,6 +169,7 @@ static const struct log_format_s log_formats[] = { 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(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), + LOG_FORMAT(STAT, "BBBBBfffB", "State,FlightMode,CtlMode,SASMode,Armed,BatV,BatC,BatRem,BatWarn"), }; static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s); From aa641b5c348b5236f5d7f882daa6a7b2ff98fd83 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 7 Jun 2013 18:00:37 +0200 Subject: [PATCH 09/12] Hotfix: Renamed max NSH argument variable to correct define --- nuttx/configs/px4fmu/nsh/defconfig | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) mode change 100755 => 100644 nuttx/configs/px4fmu/nsh/defconfig diff --git a/nuttx/configs/px4fmu/nsh/defconfig b/nuttx/configs/px4fmu/nsh/defconfig old mode 100755 new mode 100644 index 0662f7fbe3..248e508c20 --- a/nuttx/configs/px4fmu/nsh/defconfig +++ b/nuttx/configs/px4fmu/nsh/defconfig @@ -955,7 +955,7 @@ CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v1.6" # CONFIG_NSH_FILEIOSIZE - Size of a static I/O buffer # CONFIG_NSH_STRERROR - Use strerror(errno) # CONFIG_NSH_LINELEN - Maximum length of one command line -# CONFIG_NSH_MAX_ARGUMENTS - Maximum number of arguments for command line +# CONFIG_NSH_MAXARGUMENTS - Maximum number of arguments for command line # CONFIG_NSH_NESTDEPTH - Max number of nested if-then[-else]-fi # CONFIG_NSH_DISABLESCRIPT - Disable scripting support # CONFIG_NSH_DISABLEBG - Disable background commands @@ -988,7 +988,7 @@ CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_FILEIOSIZE=512 CONFIG_NSH_STRERROR=y CONFIG_NSH_LINELEN=128 -CONFIG_NSH_MAX_ARGUMENTS=12 +CONFIG_NSH_MAXARGUMENTS=12 CONFIG_NSH_NESTDEPTH=8 CONFIG_NSH_DISABLESCRIPT=n CONFIG_NSH_DISABLEBG=n From d39999425d92997ca9db77305d5c87268c601d49 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 7 Jun 2013 21:32:58 +0400 Subject: [PATCH 10/12] sdlog2 fixes --- src/modules/sdlog2/sdlog2.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 32bd422bc7..6fc430fc93 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -484,7 +484,7 @@ void sdlog2_stop_log() warnx("stop logging."); mavlink_log_info(mavlink_fd, "[sdlog2] stop logging"); - logging_enabled = true; + logging_enabled = false; logwriter_should_exit = true; /* wake up write thread one last time */ @@ -828,6 +828,8 @@ int sdlog2_thread_main(int argc, char *argv[]) continue; } + ifds = 1; // Begin from fds[1] again + pthread_mutex_lock(&logbuffer_mutex); /* write time stamp message */ From 59b26eca48212f13a467724f9445169b78d6c70a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 7 Jun 2013 22:02:40 +0400 Subject: [PATCH 11/12] sdlog2 -b option (log buffer size) added, minor cleanup --- src/modules/sdlog2/logbuffer.c | 3 +- src/modules/sdlog2/logbuffer.h | 4 +-- src/modules/sdlog2/sdlog2.c | 50 ++++++++++++++++++++++++---------- 3 files changed, 40 insertions(+), 17 deletions(-) diff --git a/src/modules/sdlog2/logbuffer.c b/src/modules/sdlog2/logbuffer.c index 4205bcf20d..8aaafaf316 100644 --- a/src/modules/sdlog2/logbuffer.c +++ b/src/modules/sdlog2/logbuffer.c @@ -45,12 +45,13 @@ #include "logbuffer.h" -void logbuffer_init(struct logbuffer_s *lb, int size) +int logbuffer_init(struct logbuffer_s *lb, int size) { lb->size = size; lb->write_ptr = 0; lb->read_ptr = 0; lb->data = malloc(lb->size); + return (lb->data == 0) ? ERROR : OK; } int logbuffer_count(struct logbuffer_s *lb) diff --git a/src/modules/sdlog2/logbuffer.h b/src/modules/sdlog2/logbuffer.h index e9635e5e73..31521f722e 100644 --- a/src/modules/sdlog2/logbuffer.h +++ b/src/modules/sdlog2/logbuffer.h @@ -46,14 +46,14 @@ #include struct logbuffer_s { - // all pointers are in bytes + // pointers and size are in bytes int write_ptr; int read_ptr; int size; char *data; }; -void logbuffer_init(struct logbuffer_s *lb, int size); +int logbuffer_init(struct logbuffer_s *lb, int size); int logbuffer_count(struct logbuffer_s *lb); diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 6fc430fc93..4ad339aede 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -94,9 +94,9 @@ } #define LOG_ORB_SUBSCRIBE(_var, _topic) subs.##_var##_sub = orb_subscribe(ORB_ID(##_topic##)); \ - fds[fdsc_count].fd = subs.##_var##_sub; \ - fds[fdsc_count].events = POLLIN; \ - fdsc_count++; + fds[fdsc_count].fd = subs.##_var##_sub; \ + fds[fdsc_count].events = POLLIN; \ + fdsc_count++; //#define SDLOG2_DEBUG @@ -107,7 +107,7 @@ 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 folders */ static const int MAX_NO_LOGFILE = 999; /**< Maximum number of log files */ -static const int LOG_BUFFER_SIZE = 8192; +static const int LOG_BUFFER_SIZE_DEFAULT = 8192; static const int MAX_WRITE_CHUNK = 512; static const int MIN_BYTES_TO_WRITE = 512; @@ -207,8 +207,9 @@ sdlog2_usage(const char *reason) if (reason) fprintf(stderr, "%s\n", reason); - errx(1, "usage: sdlog2 {start|stop|status} [-r ] -e -a\n" + errx(1, "usage: sdlog2 {start|stop|status} [-r ] [-b ] -e -a\n" "\t-r\tLog rate in Hz, 0 means unlimited rate\n" + "\t-r\tLog buffer size in KBytes, 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"); } @@ -529,18 +530,18 @@ int sdlog2_thread_main(int argc, char *argv[]) warnx("failed to open MAVLink log stream, start mavlink app first."); } - /* log every n'th value (skip three per default) */ - int skip_value = 3; + /* log buffer size */ + int log_buffer_size = LOG_BUFFER_SIZE_DEFAULT; /* work around some stupidity in task_create's argv handling */ argc -= 2; argv += 2; int ch; - while ((ch = getopt(argc, argv, "r:ea")) != EOF) { + while ((ch = getopt(argc, argv, "r:b:ea")) != EOF) { switch (ch) { case 'r': { - unsigned r = strtoul(optarg, NULL, 10); + unsigned long r = strtoul(optarg, NULL, 10); if (r == 0) { sleep_delay = 0; @@ -551,6 +552,20 @@ int sdlog2_thread_main(int argc, char *argv[]) } break; + case 'b': { + unsigned long s = strtoul(optarg, NULL, 10); + + if (s < 1) { + s = 1; + + } else if (s > 640) { + s = 640; + } + + log_buffer_size = 1024 * s; + } + break; + case 'e': log_on_start = true; break; @@ -572,7 +587,7 @@ int sdlog2_thread_main(int argc, char *argv[]) default: sdlog2_usage("unrecognized flag"); - errx(1, "exiting"); + errx(1, "exiting."); } } @@ -580,12 +595,20 @@ int sdlog2_thread_main(int argc, char *argv[]) errx(1, "logging mount point %s not present, exiting.", mountpoint); } - if (create_logfolder()) + if (create_logfolder()) { errx(1, "unable to create logging folder, exiting."); + } /* only print logging path, important to find log file later */ warnx("logging to directory: %s", folder_path); + /* initialize log buffer with specified size */ + warnx("log buffer size: %i bytes.", log_buffer_size); + + if (OK != logbuffer_init(&lb, log_buffer_size)) { + errx(1, "can't allocate log buffer, exiting."); + } + /* file descriptors to wait for */ struct pollfd fds_control[2]; @@ -770,9 +793,6 @@ int sdlog2_thread_main(int argc, char *argv[]) thread_running = true; - /* initialize log buffer with specified size */ - logbuffer_init(&lb, LOG_BUFFER_SIZE); - /* initialize thread synchronization */ pthread_mutex_init(&logbuffer_mutex, NULL); pthread_cond_init(&logbuffer_cond, NULL); @@ -818,9 +838,11 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- VEHICLE STATUS - LOG MANAGEMENT --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_status), subs.status_sub, &buf_status); + if (log_when_armed) { handle_status(&buf_status); } + handled_topics++; } From 7b98f0a56749809ce0150bad6983ac9956250abd Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 7 Jun 2013 22:12:21 +0400 Subject: [PATCH 12/12] sdlog2 minor fix --- src/modules/sdlog2/sdlog2.c | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 4ad339aede..b6ab8c2edb 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -557,9 +557,6 @@ int sdlog2_thread_main(int argc, char *argv[]) if (s < 1) { s = 1; - - } else if (s > 640) { - s = 640; } log_buffer_size = 1024 * s;