aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--apps/mavlink/mavlink.c61
-rw-r--r--apps/px4/tests/tests.h1
-rw-r--r--apps/px4/tests/tests_file.c76
-rw-r--r--apps/px4/tests/tests_main.c1
-rw-r--r--apps/sdlog/Makefile2
-rw-r--r--apps/sdlog/sdlog.c79
-rw-r--r--nuttx/arch/arm/src/stm32/stm32_i2c.c9
-rw-r--r--nuttx/arch/arm/src/stm32/stm32_serial.c24
8 files changed, 206 insertions, 47 deletions
diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c
index d6b9cb7b2..ecaf6452c 100644
--- a/apps/mavlink/mavlink.c
+++ b/apps/mavlink/mavlink.c
@@ -748,6 +748,13 @@ static void *uorb_receiveloop(void *arg)
*/
const int timeout = 1000;
+ /*
+ * Last sensor loop time
+ * some outputs are better timestamped
+ * with this "global" reference.
+ */
+ uint64_t last_sensor_timestamp = 0;
+
while (!thread_should_exit) {
int poll_ret = poll(fds, fdsc_count, timeout);
@@ -767,8 +774,10 @@ static void *uorb_receiveloop(void *arg)
/* copy sensors raw data into local buffer */
orb_copy(ORB_ID(sensor_combined), subs->sensor_sub, &buf.raw);
+ last_sensor_timestamp = buf.raw.timestamp;
+
/* send raw imu data */
- mavlink_msg_raw_imu_send(MAVLINK_COMM_0, buf.raw.timestamp, buf.raw.accelerometer_raw[0],
+ mavlink_msg_raw_imu_send(MAVLINK_COMM_0, last_sensor_timestamp, buf.raw.accelerometer_raw[0],
buf.raw.accelerometer_raw[1], buf.raw.accelerometer_raw[2], buf.raw.gyro_raw[0],
buf.raw.gyro_raw[1], buf.raw.gyro_raw[2], buf.raw.magnetometer_raw[0],
buf.raw.magnetometer_raw[1], buf.raw.magnetometer_raw[2]);
@@ -804,7 +813,7 @@ static void *uorb_receiveloop(void *arg)
baro_counter = buf.raw.baro_counter;
}
- mavlink_msg_highres_imu_send(MAVLINK_COMM_0, buf.raw.timestamp,
+ mavlink_msg_highres_imu_send(MAVLINK_COMM_0, last_sensor_timestamp,
buf.raw.accelerometer_m_s2[0], buf.raw.accelerometer_m_s2[1],
buf.raw.accelerometer_m_s2[2], buf.raw.gyro_rad_s[0],
buf.raw.gyro_rad_s[1], buf.raw.gyro_rad_s[2],
@@ -826,7 +835,7 @@ static void *uorb_receiveloop(void *arg)
orb_copy(ORB_ID(vehicle_attitude), subs->att_sub, &buf.att);
/* send sensor values */
- mavlink_msg_attitude_send(MAVLINK_COMM_0, buf.att.timestamp / 1000, buf.att.roll, buf.att.pitch, buf.att.yaw, buf.att.rollspeed, buf.att.pitchspeed, buf.att.yawspeed);
+ mavlink_msg_attitude_send(MAVLINK_COMM_0, last_sensor_timestamp, buf.att.roll, buf.att.pitch, buf.att.yaw, buf.att.rollspeed, buf.att.pitchspeed, buf.att.yawspeed);
attitude_counter++;
}
@@ -933,17 +942,17 @@ static void *uorb_receiveloop(void *arg)
/* HIL message as per MAVLink spec */
mavlink_msg_hil_controls_send(chan,
- hrt_absolute_time(),
- buf.att_sp.roll_body, /* this may be replaced by ctrl0 later */
- buf.att_sp.pitch_body,
- buf.att_sp.yaw_body,
- buf.att_sp.thrust,
- 0,
- 0,
- 0,
- 0,
- mavlink_mode,
- 0);
+ hrt_absolute_time(),
+ buf.att_sp.roll_body, /* this may be replaced by ctrl0 later */
+ buf.att_sp.pitch_body,
+ buf.att_sp.yaw_body,
+ buf.att_sp.thrust,
+ 0,
+ 0,
+ 0,
+ 0,
+ mavlink_mode,
+ 0);
}
}
@@ -951,7 +960,7 @@ static void *uorb_receiveloop(void *arg)
if (fds[ifds++].revents & POLLIN) {
/* copy actuator data into local buffer */
orb_copy(ORB_ID(actuator_outputs_0), subs->act_0_sub, &buf.act_outputs);
- mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(),
+ mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp/1000,
0 /* port 0 */,
buf.act_outputs.output[0],
buf.act_outputs.output[1],
@@ -979,7 +988,7 @@ static void *uorb_receiveloop(void *arg)
if (fds[ifds++].revents & POLLIN) {
/* copy actuator data into local buffer */
orb_copy(ORB_ID(actuator_outputs_1), subs->act_1_sub, &buf.act_outputs);
- mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(),
+ mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp/1000,
(NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) ? 2 : 1 /* port 2 or 1*/,
buf.act_outputs.output[0],
buf.act_outputs.output[1],
@@ -990,7 +999,7 @@ static void *uorb_receiveloop(void *arg)
buf.act_outputs.output[6],
buf.act_outputs.output[7]);
if (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) {
- mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(),
+ mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp/1000,
3 /* port 3 */,
buf.act_outputs.output[ 8],
buf.act_outputs.output[ 9],
@@ -1007,7 +1016,7 @@ static void *uorb_receiveloop(void *arg)
if (fds[ifds++].revents & POLLIN) {
/* copy actuator data into local buffer */
orb_copy(ORB_ID(actuator_outputs_2), subs->act_2_sub, &buf.act_outputs);
- mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(),
+ mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp/1000,
(NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) ? 4 : 2 /* port 4 or 2 */,
buf.act_outputs.output[0],
buf.act_outputs.output[1],
@@ -1018,7 +1027,7 @@ static void *uorb_receiveloop(void *arg)
buf.act_outputs.output[6],
buf.act_outputs.output[7]);
if (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) {
- mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(),
+ mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp/1000,
5 /* port 5 */,
buf.act_outputs.output[ 8],
buf.act_outputs.output[ 9],
@@ -1035,7 +1044,7 @@ static void *uorb_receiveloop(void *arg)
if (fds[ifds++].revents & POLLIN) {
/* copy actuator data into local buffer */
orb_copy(ORB_ID(actuator_outputs_3), subs->act_3_sub, &buf.act_outputs);
- mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(),
+ mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp/1000,
(NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) ? 6 : 3 /* port 6 or 3 */,
buf.act_outputs.output[0],
buf.act_outputs.output[1],
@@ -1046,7 +1055,7 @@ static void *uorb_receiveloop(void *arg)
buf.act_outputs.output[6],
buf.act_outputs.output[7]);
if (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) {
- mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(),
+ mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp/1000,
7 /* port 7 */,
buf.act_outputs.output[ 8],
buf.act_outputs.output[ 9],
@@ -1075,16 +1084,16 @@ static void *uorb_receiveloop(void *arg)
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs->actuators_sub, &buf.actuators);
/* send, add spaces so that string buffer is at least 10 chars long */
- mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, "ctrl0 ", buf.actuators.control[0]);
- mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, "ctrl1 ", buf.actuators.control[1]);
- mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, "ctrl2 ", buf.actuators.control[2]);
- mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, "ctrl3 ", buf.actuators.control[3]);
+ mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "ctrl0 ", buf.actuators.control[0]);
+ mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "ctrl1 ", buf.actuators.control[1]);
+ mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "ctrl2 ", buf.actuators.control[2]);
+ mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "ctrl3 ", buf.actuators.control[3]);
}
/* --- DEBUG KEY/VALUE --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(debug_key_value), subs->debug_key_value, &buf.debug);
- mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, buf.debug.key, buf.debug.value);
+ mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, buf.debug.key, buf.debug.value);
}
}
}
diff --git a/apps/px4/tests/tests.h b/apps/px4/tests/tests.h
index f2e35972f..8dc9d34e5 100644
--- a/apps/px4/tests/tests.h
+++ b/apps/px4/tests/tests.h
@@ -96,5 +96,6 @@ extern int test_time(int argc, char *argv[]);
extern int test_uart_console(int argc, char *argv[]);
extern int test_jig_voltages(int argc, char *argv[]);
extern int test_param(int argc, char *argv[]);
+extern int test_file(int argc, char *argv[]);
#endif /* __APPS_PX4_TESTS_H */
diff --git a/apps/px4/tests/tests_file.c b/apps/px4/tests/tests_file.c
new file mode 100644
index 000000000..2cff622f7
--- /dev/null
+++ b/apps/px4/tests/tests_file.c
@@ -0,0 +1,76 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file tests_file.c
+ *
+ * File write test.
+ */
+
+#include <stdio.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <systemlib/err.h>
+#include <systemlib/perf_counter.h>
+#include <string.h>
+
+#include <arch/board/up_hrt.h>
+
+#include "tests.h"
+
+int
+test_file(int argc, char *argv[])
+{
+ uint8_t buf[512];
+ hrt_abstime start, end;
+ perf_counter_t wperf = perf_alloc(PC_ELAPSED, "SD writes");
+
+ int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT);
+ memset(buf, 0, sizeof(buf));
+
+ start = hrt_absolute_time();
+ for (unsigned i = 0; i < 1024; i++) {
+ perf_begin(wperf);
+ write(fd, buf, sizeof(buf));
+ perf_end(wperf);
+ }
+ end = hrt_absolute_time();
+
+ close(fd);
+
+ warnx("512KiB in %llu microseconds", end - start);
+ perf_print_counter(wperf);
+ perf_free(wperf);
+
+ return 0;
+}
diff --git a/apps/px4/tests/tests_main.c b/apps/px4/tests/tests_main.c
index a4613288f..9604710c5 100644
--- a/apps/px4/tests/tests_main.c
+++ b/apps/px4/tests/tests_main.c
@@ -109,6 +109,7 @@ struct {
{"all", test_all, OPT_NOALLTEST | OPT_NOJIGTEST, 0},
{"jig", test_jig, OPT_NOJIGTEST | OPT_NOALLTEST, 0},
{"param", test_param, 0, 0},
+ {"file", test_file, 0, 0},
{"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST, 0},
{NULL, NULL, 0, 0}
};
diff --git a/apps/sdlog/Makefile b/apps/sdlog/Makefile
index 73490f6e5..61f224e99 100644
--- a/apps/sdlog/Makefile
+++ b/apps/sdlog/Makefile
@@ -36,7 +36,7 @@
#
APPNAME = sdlog
-PRIORITY = SCHED_PRIORITY_DEFAULT - 1
+PRIORITY = SCHED_PRIORITY_DEFAULT - 30
STACKSIZE = 2048
include $(APPDIR)/mk/app.mk
diff --git a/apps/sdlog/sdlog.c b/apps/sdlog/sdlog.c
index ab5f5c2a8..1ef838850 100644
--- a/apps/sdlog/sdlog.c
+++ b/apps/sdlog/sdlog.c
@@ -50,6 +50,8 @@
#include <stdlib.h>
#include <string.h>
#include <systemlib/err.h>
+#include <unistd.h>
+#include <arch/board/up_hrt.h>
#include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h>
@@ -57,6 +59,7 @@
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/vehicle_command.h>
static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
@@ -117,12 +120,15 @@ int sdlog_main(int argc, char *argv[])
}
thread_should_exit = false;
- deamon_task = task_create("sdlog", SCHED_PRIORITY_DEFAULT - 10, 4096, sdlog_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL);
+ deamon_task = task_create("sdlog", SCHED_PRIORITY_DEFAULT - 30, 4096, sdlog_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL);
thread_running = true;
exit(0);
}
if (!strcmp(argv[1], "stop")) {
+ if (!thread_running) {
+ printf("\tsdlog is not started\n");
+ }
thread_should_exit = true;
exit(0);
}
@@ -190,17 +196,31 @@ int sdlog_thread_main(int argc, char *argv[]) {
/* create sensorfile */
int sensorfile;
+ unsigned sensor_combined_bytes = 0;
+ int actuator_outputs_file;
+ unsigned actuator_outputs_bytes = 0;
FILE *gpsfile;
+ unsigned blackbox_file_bytes = 0;
+ FILE *blackbox_file;
// FILE *vehiclefile;
char path_buf[64] = ""; // string to hold the path to the sensorfile
+ printf("[sdlog] logging to directory %s\n", folder_path);
+
/* set up file path: e.g. /mnt/sdcard/session0001/sensors_combined.bin */
sprintf(path_buf, "%s/%s.bin", folder_path, "sensors_combined");
if (0 == (sensorfile = open(path_buf, O_CREAT | O_WRONLY | O_DSYNC))) {
errx(1, "opening %s failed.\n", path_buf);
}
+ /* set up file path: e.g. /mnt/sdcard/session0001/actuator_outputs0.bin */
+ sprintf(path_buf, "%s/%s.bin", folder_path, "actuator_outputs0");
+ if (0 == (actuator_outputs_file = open(path_buf, O_CREAT | O_WRONLY | O_DSYNC))) {
+ errx(1, "opening %s failed.\n", path_buf);
+ }
+ int actuator_outputs_file_no = actuator_outputs_file;
+
/* set up file path: e.g. /mnt/sdcard/session0001/gps.txt */
sprintf(path_buf, "%s/%s.txt", folder_path, "gps");
if (NULL == (gpsfile = fopen(path_buf, "w"))) {
@@ -208,6 +228,13 @@ int sdlog_thread_main(int argc, char *argv[]) {
}
int gpsfile_no = fileno(gpsfile);
+ /* set up file path: e.g. /mnt/sdcard/session0001/blackbox.txt */
+ sprintf(path_buf, "%s/%s.txt", folder_path, "blackbox");
+ if (NULL == (blackbox_file = fopen(path_buf, "w"))) {
+ errx(1, "opening %s failed.\n", path_buf);
+ }
+ int blackbox_file_no = fileno(blackbox_file);
+
/* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
/* number of messages */
const ssize_t fdsc = 25;
@@ -223,9 +250,11 @@ int sdlog_thread_main(int argc, char *argv[]) {
struct vehicle_attitude_setpoint_s att_sp;
struct actuator_outputs_s act_outputs;
struct actuator_controls_s actuators;
+ struct vehicle_command_s cmd;
} buf;
struct {
+ int cmd_sub;
int sensor_sub;
int att_sub;
int spa_sub;
@@ -233,6 +262,13 @@ int sdlog_thread_main(int argc, char *argv[]) {
int actuators_sub;
} subs;
+ /* --- MANAGEMENT - LOGGING COMMAND --- */
+ /* subscribe to ORB for sensors raw */
+ subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
+ fds[fdsc_count].fd = subs.cmd_sub;
+ fds[fdsc_count].events = POLLIN;
+ fdsc_count++;
+
/* --- SENSORS RAW VALUE --- */
/* subscribe to ORB for sensors raw */
subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
@@ -243,7 +279,6 @@ int sdlog_thread_main(int argc, char *argv[]) {
/* --- ATTITUDE VALUE --- */
/* subscribe to ORB for attitude */
subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
- orb_set_interval(subs.att_sub, 100);
fds[fdsc_count].fd = subs.att_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
@@ -252,7 +287,6 @@ int sdlog_thread_main(int argc, char *argv[]) {
/* subscribe to ORB for attitude setpoint */
/* struct already allocated */
subs.spa_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
- orb_set_interval(subs.spa_sub, 2000); /* 0.5 Hz updates */
fds[fdsc_count].fd = subs.spa_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
@@ -287,6 +321,10 @@ int sdlog_thread_main(int argc, char *argv[]) {
thread_running = true;
+ int poll_count = 0;
+
+ uint64_t starttime = hrt_absolute_time();
+
while (!thread_should_exit) {
int poll_ret = poll(fds, fdsc_count, timeout);
@@ -300,13 +338,29 @@ int sdlog_thread_main(int argc, char *argv[]) {
int ifds = 0;
+ if (poll_count % 2000 == 0) {
+ fsync(sensorfile);
+ fsync(actuator_outputs_file_no);
+ fsync(blackbox_file_no);
+ }
+ poll_count++;
+
+ /* --- VEHICLE COMMAND VALUE --- */
+ if (fds[ifds++].revents & POLLIN) {
+ /* copy command into local buffer */
+ orb_copy(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd);
+ blackbox_file_bytes += fprintf(blackbox_file, "[%10.4f\tVCMD] CMD #%d [%f-%f-%f-%f-%f-%f-%f]\n", hrt_absolute_time()/1000000.0d,
+ buf.cmd.command, (double)buf.cmd.param1, (double)buf.cmd.param2, (double)buf.cmd.param3, (double)buf.cmd.param4,
+ (double)buf.cmd.param5, (double)buf.cmd.param6, (double)buf.cmd.param7);
+ }
+
/* --- SENSORS RAW VALUE --- */
if (fds[ifds++].revents & POLLIN) {
/* copy sensors raw data into local buffer */
orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.raw);
/* write out */
- write(sensorfile, (const char*)&buf.raw, sizeof(buf.raw));
+ sensor_combined_bytes += write(sensorfile, (const char*)&buf.raw, sizeof(buf.raw));
}
/* --- ATTITUDE VALUE --- */
@@ -329,7 +383,8 @@ int sdlog_thread_main(int argc, char *argv[]) {
if (fds[ifds++].revents & POLLIN) {
/* copy actuator data into local buffer */
orb_copy(ORB_ID(actuator_outputs_0), subs.act_0_sub, &buf.act_outputs);
-
+ /* write out */
+ actuator_outputs_bytes += write(actuator_outputs_file, (const char*)&buf.raw, sizeof(buf.raw));
}
/* --- ACTUATOR CONTROL --- */
@@ -337,17 +392,21 @@ int sdlog_thread_main(int argc, char *argv[]) {
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.actuators_sub, &buf.actuators);
}
-
- /* enforce write to disk */
- fsync(sensorfile);
- fsync(gpsfile_no);
}
}
- warn("exiting.");
+ unsigned bytes = sensor_combined_bytes + actuator_outputs_bytes + blackbox_file_bytes;
+ float mebibytes = bytes / 1024.0f / 1024.0f;
+ float seconds = ((float)(hrt_absolute_time() - starttime)) / 1000000.0f;
+
+ printf("[sdlog] wrote %4.2f MiB (average %5.3f MiB/s).\n", (double)mebibytes, (double)(mebibytes / seconds));
+
+ printf("[sdlog] exiting.\n");
close(sensorfile);
+ close(gpsfile);
fclose(gpsfile);
+ fclose(blackbox_file);
thread_running = false;
diff --git a/nuttx/arch/arm/src/stm32/stm32_i2c.c b/nuttx/arch/arm/src/stm32/stm32_i2c.c
index cca7a24aa..2d1a7142c 100644
--- a/nuttx/arch/arm/src/stm32/stm32_i2c.c
+++ b/nuttx/arch/arm/src/stm32/stm32_i2c.c
@@ -1198,6 +1198,10 @@ static int stm32_i2c_isr(struct stm32_i2c_priv_s *priv)
{
stm32_i2c_traceevent(priv, I2CEVENT_RCVBYTE, priv->dcnt);
+#ifdef CONFIG_I2C_POLLED
+ //irqstate_t state = irqsave();
+#endif
+
/* Receive a byte */
*priv->ptr++ = stm32_i2c_getreg(priv, STM32_I2C_DR_OFFSET);
@@ -1209,6 +1213,11 @@ static int stm32_i2c_isr(struct stm32_i2c_priv_s *priv)
stm32_i2c_modifyreg(priv, STM32_I2C_CR1_OFFSET, I2C_CR1_ACK, 0);
}
priv->dcnt--;
+
+#ifdef CONFIG_I2C_POLLED
+ //irqrestore(state);
+#endif
+
}
}
diff --git a/nuttx/arch/arm/src/stm32/stm32_serial.c b/nuttx/arch/arm/src/stm32/stm32_serial.c
index 83acc676b..80cf5d61e 100644
--- a/nuttx/arch/arm/src/stm32/stm32_serial.c
+++ b/nuttx/arch/arm/src/stm32/stm32_serial.c
@@ -956,7 +956,8 @@ static int up_setup(struct uart_dev_s *dev)
/* Set up the cached interrupt enables value */
- priv->ie = 0;
+ up_restoreusartint(priv, 0);
+
return OK;
}
@@ -976,12 +977,15 @@ static int up_dma_setup(struct uart_dev_s *dev)
int result;
uint32_t regval;
- /* Do the basic UART setup first */
+ /* Do the basic UART setup first, unless we are the console */
- result = up_setup(dev);
- if (result != OK)
- {
- return result;
+ if (!dev->isconsole)
+ {
+ result = up_setup(dev);
+ if (result != OK)
+ {
+ return result;
+ }
}
/* Acquire the DMA channel. This should always succeed. */
@@ -1941,10 +1945,10 @@ void stm32_serial_dma_poll(void)
int up_putc(int ch)
{
#if CONSOLE_UART > 0
- struct up_dev_s *priv = uart_devs[CONSOLE_UART - 1];
- uint16_t ie;
+// struct up_dev_s *priv = uart_devs[CONSOLE_UART - 1];
+// uint16_t ie;
- up_disableusartint(priv, &ie);
+// up_disableusartint(priv, &ie);
/* Check for LF */
@@ -1956,7 +1960,7 @@ int up_putc(int ch)
}
up_lowputc(ch);
- up_restoreusartint(priv, ie);
+// up_restoreusartint(priv, ie);
#endif
return ch;
}
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-10 09:26:16 +0000