diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2013-06-18 21:56:57 +0400 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2013-06-18 21:56:57 +0400 |
commit | dc0bf64434b7622ed39a9b7761d85ab4912e2bd7 (patch) | |
tree | c5b2a525bce1c02bc9b97e84ccfbf94910f01979 | |
parent | effce6edfa3b4258a855342d8f7a265f2f18f521 (diff) | |
parent | 85b5da8078873a13a5fc0fd4ee3fe0a02917e87c (diff) | |
download | px4-firmware-dc0bf64434b7622ed39a9b7761d85ab4912e2bd7.tar.gz px4-firmware-dc0bf64434b7622ed39a9b7761d85ab4912e2bd7.tar.bz2 px4-firmware-dc0bf64434b7622ed39a9b7761d85ab4912e2bd7.zip |
Merge branch 'master' into seatbelt_multirotor
-rw-r--r-- | ROMFS/px4fmu_common/init.d/rc.PX4IOAR_PX4FLOW_example | 94 | ||||
-rw-r--r-- | src/modules/sdlog2/sdlog2.c | 50 |
2 files changed, 127 insertions, 17 deletions
diff --git a/ROMFS/px4fmu_common/init.d/rc.PX4IOAR_PX4FLOW_example b/ROMFS/px4fmu_common/init.d/rc.PX4IOAR_PX4FLOW_example new file mode 100644 index 000000000..e7173f6e6 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.PX4IOAR_PX4FLOW_example @@ -0,0 +1,94 @@ +#!nsh +# +# Flight startup script for PX4FMU on PX4IOAR carrier board. +# + +# Disable the USB interface +set USB no + +# Disable autostarting other apps +set MODE ardrone + +echo "[init] doing PX4IOAR startup..." + +# +# Start the ORB +# +uorb start + +# +# Load microSD params +# +echo "[init] loading microSD params" +param select /fs/microsd/params +if [ -f /fs/microsd/params ] +then + param load /fs/microsd/params +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor +# see https://pixhawk.ethz.ch/mavlink/ +# +param set MAV_TYPE 2 + +# +# Configure PX4FMU for operation with PX4IOAR +# +fmu mode_gpio_serial + +# +# Start the sensors. +# +sh /etc/init.d/rc.sensors + +# +# Start MAVLink and MAVLink Onboard (Flow Sensor) +# +mavlink start -d /dev/ttyS0 -b 57600 +mavlink_onboard start -d /dev/ttyS3 -b 115200 +usleep 5000 + +# +# Start the commander. +# +commander start + +# +# Start the attitude estimator +# +attitude_estimator_ekf start + +# +# Start the position estimator +# +flow_position_estimator start + +# +# Fire up the multi rotor attitude controller +# +multirotor_att_control start + +# +# Fire up the flow position controller +# +flow_position_control start + +# +# Fire up the flow speed controller +# +flow_speed_control start + +# +# Fire up the AR.Drone interface. +# +ardrone_interface start -d /dev/ttyS1 + +# +# startup is done; we don't want the shell because we +# use the same UART for telemetry +# +echo "[init] startup done" + +exit diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index c318f18c3..c802d5efa 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -103,7 +103,7 @@ //#define SDLOG2_DEBUG -static bool thread_should_exit = false; /**< Deamon exit flag */ +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 */ @@ -236,7 +236,7 @@ int sdlog2_main(int argc, char *argv[]) exit(0); } - thread_should_exit = false; + main_thread_should_exit = false; deamon_task = task_spawn("sdlog2", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT - 30, @@ -251,7 +251,7 @@ int sdlog2_main(int argc, char *argv[]) printf("\tsdlog2 is not started\n"); } - thread_should_exit = true; + main_thread_should_exit = true; exit(0); } @@ -330,10 +330,10 @@ int open_logfile() fd = open(path_buf, O_CREAT | O_WRONLY | O_DSYNC); if (fd == 0) { - errx(1, "opening %s failed.", path_buf); + warn("opening %s failed", path_buf); } - warnx("logging to: %s", path_buf); + warnx("logging to: %s.", path_buf); mavlink_log_info(mavlink_fd, "[sdlog2] log: %s", path_buf); return fd; @@ -341,7 +341,7 @@ int open_logfile() 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 */ - warn("all %d possible files exist already", MAX_NO_LOGFILE); + warnx("all %d possible files exist already.", MAX_NO_LOGFILE); return -1; } @@ -367,8 +367,7 @@ static void *logwriter_thread(void *arg) bool should_wait = false; bool is_part = false; - while (!thread_should_exit && !logwriter_should_exit) { - + while (true) { /* make sure threads are synchronized */ pthread_mutex_lock(&logbuffer_mutex); @@ -378,7 +377,7 @@ static void *logwriter_thread(void *arg) } /* only wait if no data is available to process */ - if (should_wait) { + if (should_wait && !logwriter_should_exit) { /* blocking wait for new data at this line */ pthread_cond_wait(&logbuffer_cond, &logbuffer_mutex); } @@ -411,7 +410,7 @@ static void *logwriter_thread(void *arg) #endif if (n < 0) { - thread_should_exit = true; + main_thread_should_exit = true; err(1, "error writing log file"); } @@ -421,6 +420,16 @@ static void *logwriter_thread(void *arg) } else { n = 0; +#ifdef SDLOG2_DEBUG + printf("no data available, main_thread_should_exit=%i, logwriter_should_exit=%i\n", (int)main_thread_should_exit, (int)logwriter_should_exit); +#endif + /* exit only with empty buffer */ + if (main_thread_should_exit || logwriter_should_exit) { +#ifdef SDLOG2_DEBUG + printf("break logwriter thread\n"); +#endif + break; + } should_wait = true; } @@ -433,6 +442,10 @@ static void *logwriter_thread(void *arg) fsync(log_file); close(log_file); +#ifdef SDLOG2_DEBUG + printf("logwriter thread exit\n"); +#endif + return OK; } @@ -459,10 +472,9 @@ void sdlog2_start_log() pthread_attr_setstacksize(&receiveloop_attr, 2048); logwriter_should_exit = false; - pthread_t thread; /* start log buffer emptying thread */ - if (0 != pthread_create(&thread, &receiveloop_attr, logwriter_thread, &lb)) { + if (0 != pthread_create(&logwriter_pthread, &receiveloop_attr, logwriter_thread, &lb)) { errx(1, "error creating logwriter thread"); } @@ -476,16 +488,20 @@ void sdlog2_stop_log() mavlink_log_info(mavlink_fd, "[sdlog2] stop logging"); logging_enabled = false; - logwriter_should_exit = true; /* wake up write thread one last time */ pthread_mutex_lock(&logbuffer_mutex); + logwriter_should_exit = true; pthread_cond_signal(&logbuffer_cond); /* unlock, now the writer thread may return */ pthread_mutex_unlock(&logbuffer_mutex); /* wait for write thread to return */ - (void)pthread_join(logwriter_pthread, NULL); + int ret; + if ((ret = pthread_join(logwriter_pthread, NULL)) != 0) { + warnx("error joining logwriter thread: %i", ret); + } + logwriter_pthread = 0; sdlog2_status(); } @@ -506,7 +522,7 @@ void write_formats(int fd) for (i = 0; i < log_formats_num; i++) { log_format_packet.body = log_formats[i]; - write(fd, &log_format_packet, sizeof(log_format_packet)); + log_bytes_written += write(fd, &log_format_packet, sizeof(log_format_packet)); } fsync(fd); @@ -810,7 +826,7 @@ int sdlog2_thread_main(int argc, char *argv[]) if (log_on_start) sdlog2_start_log(); - while (!thread_should_exit) { + while (!main_thread_should_exit) { /* decide use usleep() or blocking poll() */ bool use_sleep = sleep_delay > 0 && logging_enabled; @@ -820,7 +836,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* handle the poll result */ if (poll_ret < 0) { warnx("ERROR: poll error, stop logging."); - thread_should_exit = true; + main_thread_should_exit = true; } else if (poll_ret > 0) { |