diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-01-10 14:43:59 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-01-10 14:43:59 +0100 |
commit | 1d4feb690512fbb5e0ae3399c1b1595ca8ddb79a (patch) | |
tree | e641a72a91c8c600dce78fefeddcb41fec51836f /apps | |
parent | 1a107bd7bf75a01ad199a5cdb3eb54c46011a3d9 (diff) | |
parent | 46cf2a538f3c48ad7aba4cfad2a47b406733f365 (diff) | |
download | px4-firmware-1d4feb690512fbb5e0ae3399c1b1595ca8ddb79a.tar.gz px4-firmware-1d4feb690512fbb5e0ae3399c1b1595ca8ddb79a.tar.bz2 px4-firmware-1d4feb690512fbb5e0ae3399c1b1595ca8ddb79a.zip |
Merged sdlog_buffering branch
Diffstat (limited to 'apps')
-rw-r--r-- | apps/sdlog/Makefile | 3 | ||||
-rw-r--r-- | apps/sdlog/sdlog.c | 346 | ||||
-rw-r--r-- | apps/sdlog/sdlog_ringbuffer.c | 91 | ||||
-rw-r--r-- | apps/sdlog/sdlog_ringbuffer.h | 91 | ||||
-rw-r--r-- | apps/systemlib/airspeed.c | 79 | ||||
-rw-r--r-- | apps/systemlib/airspeed.h | 86 | ||||
-rw-r--r-- | apps/systemlib/conversions.c | 11 | ||||
-rw-r--r-- | apps/systemlib/conversions.h | 8 | ||||
-rw-r--r-- | apps/uORB/objects_common.cpp | 6 | ||||
-rw-r--r-- | apps/uORB/topics/differential_pressure.h | 70 | ||||
-rw-r--r-- | apps/uORB/topics/omnidirectional_flow.h | 75 | ||||
-rw-r--r-- | apps/uORB/topics/optical_flow.h | 9 |
12 files changed, 772 insertions, 103 deletions
diff --git a/apps/sdlog/Makefile b/apps/sdlog/Makefile index 61f224e99..225b14a32 100644 --- a/apps/sdlog/Makefile +++ b/apps/sdlog/Makefile @@ -36,7 +36,8 @@ # APPNAME = sdlog -PRIORITY = SCHED_PRIORITY_DEFAULT - 30 +# The main thread only buffers to RAM, needs a high priority +PRIORITY = SCHED_PRIORITY_MAX - 30 STACKSIZE = 2048 include $(APPDIR)/mk/app.mk diff --git a/apps/sdlog/sdlog.c b/apps/sdlog/sdlog.c index 9b4cd1622..f8668a2e3 100644 --- a/apps/sdlog/sdlog.c +++ b/apps/sdlog/sdlog.c @@ -36,12 +36,14 @@ * @file sdlog.c * @author Lorenz Meier <lm@inf.ethz.ch> * - * Simple SD logger for flight data + * Simple SD logger for flight data. Buffers new sensor values and + * does the heavy SD I/O in a low-priority worker thread. */ #include <nuttx/config.h> #include <sys/types.h> #include <sys/stat.h> +#include <sys/prctl.h> #include <fcntl.h> #include <errno.h> #include <unistd.h> @@ -66,9 +68,13 @@ #include <uORB/topics/vehicle_gps_position.h> #include <uORB/topics/vehicle_vicon_position.h> #include <uORB/topics/optical_flow.h> +#include <uORB/topics/battery_status.h> +#include <uORB/topics/differential_pressure.h> #include <systemlib/systemlib.h> +#include "sdlog_ringbuffer.h" + static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int deamon_task; /**< Handle of deamon task / thread */ @@ -76,6 +82,22 @@ static const int MAX_NO_LOGFOLDER = 999; /**< Maximum number of log folders */ static const char *mountpoint = "/fs/microsd"; static const char *mfile_in = "/etc/logging/logconv.m"; +int sysvector_file = -1; +struct sdlog_logbuffer lb; + +/* mutex / condition to synchronize threads */ +pthread_mutex_t sysvector_mutex; +pthread_cond_t sysvector_cond; + +/** + * System state vector log buffer writing + */ +static void *sdlog_sysvector_write_thread(void *arg); + +/** + * Create the thread to write the system vector + */ +pthread_t sysvector_write_start(struct sdlog_logbuffer *logbuf); /** * SD log management function. @@ -94,7 +116,7 @@ static void usage(const char *reason); static int file_exist(const char *filename); -static int file_copy(const char* file_old, const char* file_new); +static int file_copy(const char *file_old, const char *file_new); /** * Print the current status. @@ -104,13 +126,14 @@ static void print_sdlog_status(void); /** * Create folder for current logging session. */ -static int create_logfolder(char* folder_path); +static int create_logfolder(char *folder_path); static void usage(const char *reason) { if (reason) fprintf(stderr, "%s\n", reason); + errx(1, "usage: sdlog {start|stop|status} [-p <additional params>]\n\n"); } @@ -126,7 +149,7 @@ uint64_t starttime = 0; * The sd log deamon app only briefly exists to start * the background job. The stack size assigned in the * Makefile does only apply to this management task. - * + * * The actual stack size should be set in the call * to task_spawn(). */ @@ -157,6 +180,7 @@ int sdlog_main(int argc, char *argv[]) if (!thread_running) { printf("\tsdlog is not started\n"); } + thread_should_exit = true; exit(0); } @@ -164,9 +188,11 @@ int sdlog_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { print_sdlog_status(); + } else { printf("\tsdlog not started\n"); } + exit(0); } @@ -174,7 +200,8 @@ int sdlog_main(int argc, char *argv[]) exit(1); } -int create_logfolder(char* folder_path) { +int create_logfolder(char *folder_path) +{ /* make folder on sdcard */ uint16_t foldernumber = 1; // start with folder 0001 int mkdir_ret; @@ -193,11 +220,14 @@ int create_logfolder(char* folder_path) { char mfile_out[100]; sprintf(mfile_out, "%s/session%04u/run_to_plot_data.m", mountpoint, foldernumber); int ret = file_copy(mfile_in, mfile_out); + if (!ret) { warnx("copied m file to %s", mfile_out); + } else { warnx("failed copying m file from %s to\n %s", mfile_in, mfile_out); } + break; } else if (mkdir_ret == -1) { @@ -216,11 +246,78 @@ int create_logfolder(char* folder_path) { warn("all %d possible folders exist already", MAX_NO_LOGFOLDER); return -1; } + return 0; } -int sdlog_thread_main(int argc, char *argv[]) { +static void * +sdlog_sysvector_write_thread(void *arg) +{ + /* set name */ + prctl(PR_SET_NAME, "sdlog microSD I/O", 0); + + struct sdlog_logbuffer *logbuf = (struct sdlog_logbuffer *)arg; + + int poll_count = 0; + struct sdlog_sysvector sysvect; + memset(&sysvect, 0, sizeof(sysvect)); + + while (!thread_should_exit) { + + /* make sure threads are synchronized */ + pthread_mutex_lock(&sysvector_mutex); + + /* only wait if no data is available to process */ + if (sdlog_logbuffer_is_empty(logbuf)) { + /* blocking wait for new data at this line */ + pthread_cond_wait(&sysvector_cond, &sysvector_mutex); + } + + /* only quickly load data, do heavy I/O a few lines down */ + int ret = sdlog_logbuffer_read(logbuf, &sysvect); + /* continue */ + pthread_mutex_unlock(&sysvector_mutex); + + if (ret == OK) { + sysvector_bytes += write(sysvector_file, (const char *)&sysvect, sizeof(sysvect)); + } + + if (poll_count % 100 == 0) { + fsync(sysvector_file); + } + + poll_count++; + } + + fsync(sysvector_file); + + return OK; +} + +pthread_t +sysvector_write_start(struct sdlog_logbuffer *logbuf) +{ + pthread_attr_t receiveloop_attr; + pthread_attr_init(&receiveloop_attr); + + struct sched_param param; + /* low priority, as this is expensive disk I/O */ + param.sched_priority = SCHED_PRIORITY_DEFAULT - 40; + (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); + + pthread_attr_setstacksize(&receiveloop_attr, 2048); + + pthread_t thread; + pthread_create(&thread, &receiveloop_attr, sdlog_sysvector_write_thread, logbuf); + return thread; + + // XXX we have to destroy the attr at some point +} + + +int sdlog_thread_main(int argc, char *argv[]) +{ warnx("starting\n"); @@ -229,6 +326,7 @@ int sdlog_thread_main(int argc, char *argv[]) { } char folder_path[64]; + if (create_logfolder(folder_path)) errx(1, "unable to create logging folder, exiting."); @@ -236,7 +334,6 @@ int sdlog_thread_main(int argc, char *argv[]) { int sensorfile = -1; int actuator_outputs_file = -1; int actuator_controls_file = -1; - int sysvector_file = -1; FILE *gpsfile; FILE *blackbox_file; // FILE *vehiclefile; @@ -247,6 +344,7 @@ int sdlog_thread_main(int argc, char *argv[]) { /* set up file path: e.g. /mnt/sdcard/session0001/sensor_combined.bin */ sprintf(path_buf, "%s/%s.bin", folder_path, "sensor_combined"); + if (0 == (sensorfile = open(path_buf, O_CREAT | O_WRONLY | O_DSYNC))) { errx(1, "opening %s failed.\n", path_buf); } @@ -259,28 +357,34 @@ int sdlog_thread_main(int argc, char *argv[]) { /* set up file path: e.g. /mnt/sdcard/session0001/actuator_controls0.bin */ sprintf(path_buf, "%s/%s.bin", folder_path, "sysvector"); + if (0 == (sysvector_file = 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_controls0.bin */ sprintf(path_buf, "%s/%s.bin", folder_path, "actuator_controls0"); + if (0 == (actuator_controls_file = 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/gps.txt */ sprintf(path_buf, "%s/%s.txt", folder_path, "gps"); + if (NULL == (gpsfile = fopen(path_buf, "w"))) { errx(1, "opening %s failed.\n", path_buf); } + 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 --- */ @@ -305,6 +409,8 @@ int sdlog_thread_main(int argc, char *argv[]) { struct vehicle_gps_position_s gps_pos; struct vehicle_vicon_position_s vicon_pos; struct optical_flow_s flow; + struct battery_status_s batt; + struct differential_pressure_s diff_pressure; } buf; memset(&buf, 0, sizeof(buf)); @@ -321,6 +427,8 @@ int sdlog_thread_main(int argc, char *argv[]) { int gps_pos_sub; int vicon_pos_sub; int flow_sub; + int batt_sub; + int diff_pressure_sub; } subs; /* --- MANAGEMENT - LOGGING COMMAND --- */ @@ -409,6 +517,20 @@ int sdlog_thread_main(int argc, char *argv[]) { fds[fdsc_count].events = POLLIN; fdsc_count++; + /* --- BATTERY STATUS --- */ + /* subscribe to ORB for flow measurements */ + subs.batt_sub = orb_subscribe(ORB_ID(battery_status)); + fds[fdsc_count].fd = subs.batt_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + + /* --- DIFFERENTIAL PRESSURE --- */ + /* subscribe to ORB for flow measurements */ + subs.diff_pressure_sub = orb_subscribe(ORB_ID(differential_pressure)); + fds[fdsc_count].fd = subs.diff_pressure_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + /* WARNING: If you get the error message below, * then the number of registered messages (fdsc) * differs from the number of messages in the above list. @@ -426,20 +548,64 @@ int sdlog_thread_main(int argc, char *argv[]) { thread_running = true; - int poll_count = 0; + /* initialize log buffer with a size of 10 */ + sdlog_logbuffer_init(&lb, 10); + + /* initialize thread synchronization */ + pthread_mutex_init(&sysvector_mutex, NULL); + pthread_cond_init(&sysvector_cond, NULL); + + /* start logbuffer emptying thread */ + pthread_t sysvector_pthread = sysvector_write_start(&lb); starttime = hrt_absolute_time(); + // XXX clock the log for now with the gyro output rate / 2 + struct pollfd gyro_fd; + gyro_fd.fd = subs.sensor_sub; + gyro_fd.events = POLLIN; + + /* log every 2nd value (skip one) */ + int skip_value = 0; + /* track skipping */ + int skip_count = 0; + while (!thread_should_exit) { + // XXX only use gyro for now + int poll_ret = poll(&gyro_fd, 1, 1000); + // int poll_ret = poll(fds, fdsc_count, timeout); - // /* handle the poll result */ - // if (poll_ret == 0) { - // /* XXX this means none of our providers is giving us data - might be an error? */ - // } else if (poll_ret < 0) { - // /* XXX this is seriously bad - should be an emergency */ - // } else { + /* handle the poll result */ + if (poll_ret == 0) { + /* XXX this means none of our providers is giving us data - might be an error? */ + } else if (poll_ret < 0) { + /* XXX this is seriously bad - should be an emergency */ + } else { + + /* always copy sensors raw data into local buffer, since poll flags won't clear else */ + orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.raw); + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.controls_0_sub, &buf.act_controls); + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, subs.controls_effective_0_sub, &buf.act_controls_effective); + /* copy actuator data into local buffer */ + orb_copy(ORB_ID(actuator_outputs_0), subs.act_0_sub, &buf.act_outputs); + orb_copy(ORB_ID(vehicle_attitude_setpoint), subs.spa_sub, &buf.att_sp); + orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); + orb_copy(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos); + orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos); + orb_copy(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att); + orb_copy(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos); + orb_copy(ORB_ID(optical_flow), subs.flow_sub, &buf.flow); + + if (skip_count < skip_value) { + skip_count++; + /* do not log data */ + continue; + } else { + /* log data, reset */ + skip_count = 0; + } // int ifds = 0; @@ -450,7 +616,7 @@ int sdlog_thread_main(int argc, char *argv[]) { // fsync(blackbox_file_no); // } - + // /* --- VEHICLE COMMAND VALUE --- */ // if (fds[ifds++].revents & POLLIN) { @@ -476,14 +642,14 @@ int sdlog_thread_main(int argc, char *argv[]) { // /* copy attitude data into local buffer */ // orb_copy(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att); - + // } // /* --- VEHICLE ATTITUDE SETPOINT --- */ // if (fds[ifds++].revents & POLLIN) { // /* copy local position data into local buffer */ // orb_copy(ORB_ID(vehicle_attitude_setpoint), subs.spa_sub, &buf.att_sp); - + // } // /* --- ACTUATOR OUTPUTS 0 --- */ @@ -500,81 +666,72 @@ int sdlog_thread_main(int argc, char *argv[]) { // /* write out */ // actuator_controls_bytes += write(actuator_controls_file, (const char*)&buf.act_controls, sizeof(buf.act_controls)); // } - // } - - if (poll_count % 100 == 0) { - fsync(sysvector_file); - } - poll_count++; + /* copy sensors raw data into local buffer */ + orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.raw); + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.controls_0_sub, &buf.act_controls); + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, subs.controls_effective_0_sub, &buf.act_controls_effective); + /* copy actuator data into local buffer */ + orb_copy(ORB_ID(actuator_outputs_0), subs.act_0_sub, &buf.act_outputs); + orb_copy(ORB_ID(vehicle_attitude_setpoint), subs.spa_sub, &buf.att_sp); + orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); + orb_copy(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos); + orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos); + orb_copy(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att); + orb_copy(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos); + orb_copy(ORB_ID(optical_flow), subs.flow_sub, &buf.flow); + orb_copy(ORB_ID(differential_pressure), subs.diff_pressure_sub, &buf.diff_pressure); + orb_copy(ORB_ID(battery_status), subs.batt_sub, &buf.batt); + + struct sdlog_sysvector sysvect = { + .timestamp = buf.raw.timestamp, + .gyro = {buf.raw.gyro_rad_s[0], buf.raw.gyro_rad_s[1], buf.raw.gyro_rad_s[2]}, + .accel = {buf.raw.accelerometer_m_s2[0], buf.raw.accelerometer_m_s2[1], buf.raw.accelerometer_m_s2[2]}, + .mag = {buf.raw.magnetometer_ga[0], buf.raw.magnetometer_ga[1], buf.raw.magnetometer_ga[2]}, + .baro = buf.raw.baro_pres_mbar, + .baro_alt = buf.raw.baro_alt_meter, + .baro_temp = buf.raw.baro_temp_celcius, + .control = {buf.act_controls.control[0], buf.act_controls.control[1], buf.act_controls.control[2], buf.act_controls.control[3]}, + .actuators = { + buf.act_outputs.output[0], buf.act_outputs.output[1], buf.act_outputs.output[2], buf.act_outputs.output[3], + buf.act_outputs.output[4], buf.act_outputs.output[5], buf.act_outputs.output[6], buf.act_outputs.output[7] + }, + .vbat = buf.batt.voltage_v, + .bat_current = buf.batt.current_a, + .bat_discharged = buf.batt.discharged_mah, + .adc = {buf.raw.adc_voltage_v[0], buf.raw.adc_voltage_v[1], buf.raw.adc_voltage_v[2]}, + .local_position = {buf.local_pos.x, buf.local_pos.y, buf.local_pos.z}, + .gps_raw_position = {buf.gps_pos.lat, buf.gps_pos.lon, buf.gps_pos.alt}, + .attitude = {buf.att.pitch, buf.att.roll, buf.att.yaw}, + .rotMatrix = {buf.att.R[0][0], buf.att.R[0][1], buf.att.R[0][2], buf.att.R[1][0], buf.att.R[1][1], buf.att.R[1][2], buf.att.R[2][0], buf.att.R[2][1], buf.att.R[2][2]}, + .vicon = {buf.vicon_pos.x, buf.vicon_pos.y, buf.vicon_pos.z, buf.vicon_pos.roll, buf.vicon_pos.pitch, buf.vicon_pos.yaw}, + .control_effective = {buf.act_controls_effective.control_effective[0], buf.act_controls_effective.control_effective[1], buf.act_controls_effective.control_effective[2], buf.act_controls_effective.control_effective[3]}, + .flow = {buf.flow.flow_raw_x, buf.flow.flow_raw_y, buf.flow.flow_comp_x_m, buf.flow.flow_comp_y_m, buf.flow.ground_distance_m, buf.flow.quality}, + .diff_pressure = buf.diff_pressure.differential_pressure_mbar, + .ind_airspeed = buf.diff_pressure.indicated_airspeed_m_s, + .true_airspeed = buf.diff_pressure.true_airspeed_m_s + }; + + /* put into buffer for later IO */ + pthread_mutex_lock(&sysvector_mutex); + sdlog_logbuffer_write(&lb, &sysvect); + /* signal the other thread new data, but not yet unlock */ + pthread_cond_signal(&sysvector_cond); + /* unlock, now the writer thread may run */ + pthread_mutex_unlock(&sysvector_mutex); + } - /* copy sensors raw data into local buffer */ - orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.raw); - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.controls_0_sub, &buf.act_controls); - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, subs.controls_effective_0_sub, &buf.act_controls_effective); - /* copy actuator data into local buffer */ - orb_copy(ORB_ID(actuator_outputs_0), subs.act_0_sub, &buf.act_outputs); - orb_copy(ORB_ID(vehicle_attitude_setpoint), subs.spa_sub, &buf.att_sp); - orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); - orb_copy(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos); - orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos); - orb_copy(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att); - orb_copy(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos); - orb_copy(ORB_ID(optical_flow), subs.flow_sub, &buf.flow); - - #pragma pack(push, 1) - struct { - uint64_t timestamp; //[us] - float gyro[3]; //[rad/s] - float accel[3]; //[m/s^2] - float mag[3]; //[gauss] - float baro; //pressure [millibar] - float baro_alt; //altitude above MSL [meter] - float baro_temp; //[degree celcius] - float control[4]; //roll, pitch, yaw [-1..1], thrust [0..1] - float actuators[8]; //motor 1-8, in motor units (PWM: 1000-2000,AR.Drone: 0-512) - float vbat; //battery voltage in [volt] - float adc[3]; //remaining auxiliary ADC ports [volt] - float local_position[3]; //tangent plane mapping into x,y,z [m] - int32_t gps_raw_position[3]; //latitude [degrees] north, longitude [degrees] east, altitude above MSL [millimeter] - float attitude[3]; //pitch, roll, yaw [rad] - float rotMatrix[9]; //unitvectors - float vicon[6]; - float control_effective[4]; //roll, pitch, yaw [-1..1], thrust [0..1] - float flow[6]; // flow raw x, y, flow metric x, y, flow ground dist, flow quality - } sysvector = { - .timestamp = buf.raw.timestamp, - .gyro = {buf.raw.gyro_rad_s[0], buf.raw.gyro_rad_s[1], buf.raw.gyro_rad_s[2]}, - .accel = {buf.raw.accelerometer_m_s2[0], buf.raw.accelerometer_m_s2[1], buf.raw.accelerometer_m_s2[2]}, - .mag = {buf.raw.magnetometer_ga[0], buf.raw.magnetometer_ga[1], buf.raw.magnetometer_ga[2]}, - .baro = buf.raw.baro_pres_mbar, - .baro_alt = buf.raw.baro_alt_meter, - .baro_temp = buf.raw.baro_temp_celcius, - .control = {buf.act_controls.control[0], buf.act_controls.control[1], buf.act_controls.control[2], buf.act_controls.control[3]}, - .actuators = {buf.act_outputs.output[0], buf.act_outputs.output[1], buf.act_outputs.output[2], buf.act_outputs.output[3], - buf.act_outputs.output[4], buf.act_outputs.output[5], buf.act_outputs.output[6], buf.act_outputs.output[7]}, - .vbat = 0.0f, /* XXX use battery_status uORB topic */ - .adc = {buf.raw.adc_voltage_v[0], buf.raw.adc_voltage_v[1], buf.raw.adc_voltage_v[2]}, - .local_position = {buf.local_pos.x, buf.local_pos.y, buf.local_pos.z}, - .gps_raw_position = {buf.gps_pos.lat, buf.gps_pos.lon, buf.gps_pos.alt}, - .attitude = {buf.att.pitch, buf.att.roll, buf.att.yaw}, - .rotMatrix = {buf.att.R[0][0], buf.att.R[0][1], buf.att.R[0][2], buf.att.R[1][0], buf.att.R[1][1], buf.att.R[1][2], buf.att.R[2][0], buf.att.R[2][1], buf.att.R[2][2]}, - .vicon = {buf.vicon_pos.x, buf.vicon_pos.y, buf.vicon_pos.z, buf.vicon_pos.roll, buf.vicon_pos.pitch, buf.vicon_pos.yaw}, - .control_effective = {buf.act_controls_effective.control_effective[0], buf.act_controls_effective.control_effective[1], buf.act_controls_effective.control_effective[2], buf.act_controls_effective.control_effective[3]}, - .flow = {buf.flow.flow_raw_x, buf.flow.flow_raw_y, buf.flow.flow_comp_x_m, buf.flow.flow_comp_y_m, buf.flow.ground_distance_m, buf.flow.quality} - }; - #pragma pack(pop) - - sysvector_bytes += write(sysvector_file, (const char*)&sysvector, sizeof(sysvector)); - - usleep(3500); // roughly 150 Hz } - fsync(sysvector_file); - print_sdlog_status(); + /* wait for write thread to return */ + (void)pthread_join(sysvector_pthread, NULL); + + pthread_mutex_destroy(&sysvector_mutex); + pthread_cond_destroy(&sysvector_cond); + warnx("exiting.\n"); close(sensorfile); @@ -606,43 +763,44 @@ int file_exist(const char *filename) return stat(filename, &buffer); } -int file_copy(const char* file_old, const char* file_new) +int file_copy(const char *file_old, const char *file_new) { FILE *source, *target; source = fopen(file_old, "r"); int ret = 0; - - if( source == NULL ) - { + + if (source == NULL) { warnx("failed opening input file to copy"); return 1; } target = fopen(file_new, "w"); - - if( target == NULL ) - { + + if (target == NULL) { fclose(source); warnx("failed to open output file to copy"); return 1; } - + char buf[128]; int nread; + while ((nread = fread(buf, 1, sizeof(buf), source)) > 0) { int ret = fwrite(buf, 1, nread, target); + if (ret <= 0) { warnx("error writing file"); ret = 1; break; } } + fsync(fileno(target)); fclose(source); fclose(target); - return ret; + return ret; } diff --git a/apps/sdlog/sdlog_ringbuffer.c b/apps/sdlog/sdlog_ringbuffer.c new file mode 100644 index 000000000..d7c8a4759 --- /dev/null +++ b/apps/sdlog/sdlog_ringbuffer.c @@ -0,0 +1,91 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier <lm@inf.ethz.ch> + * + * 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 sdlog_log.c + * MAVLink text logging. + * + * @author Lorenz Meier <lm@inf.ethz.ch> + */ + +#include <string.h> +#include <stdlib.h> + +#include "sdlog_ringbuffer.h" + +void sdlog_logbuffer_init(struct sdlog_logbuffer *lb, int size) +{ + lb->size = size; + lb->start = 0; + lb->count = 0; + lb->elems = (struct sdlog_sysvector *)calloc(lb->size, sizeof(struct sdlog_sysvector)); +} + +int sdlog_logbuffer_is_full(struct sdlog_logbuffer *lb) +{ + return lb->count == (int)lb->size; +} + +int sdlog_logbuffer_is_empty(struct sdlog_logbuffer *lb) +{ + return lb->count == 0; +} + + +// XXX make these functions thread-safe +void sdlog_logbuffer_write(struct sdlog_logbuffer *lb, const struct sdlog_sysvector *elem) +{ + int end = (lb->start + lb->count) % lb->size; + memcpy(&(lb->elems[end]), elem, sizeof(struct sdlog_sysvector)); + + if (sdlog_logbuffer_is_full(lb)) { + lb->start = (lb->start + 1) % lb->size; /* full, overwrite */ + + } else { + ++lb->count; + } +} + +int sdlog_logbuffer_read(struct sdlog_logbuffer *lb, struct sdlog_sysvector *elem) +{ + if (!sdlog_logbuffer_is_empty(lb)) { + memcpy(elem, &(lb->elems[lb->start]), sizeof(struct sdlog_sysvector)); + lb->start = (lb->start + 1) % lb->size; + --lb->count; + return 0; + + } else { + return 1; + } +} diff --git a/apps/sdlog/sdlog_ringbuffer.h b/apps/sdlog/sdlog_ringbuffer.h new file mode 100644 index 000000000..f7fc0d450 --- /dev/null +++ b/apps/sdlog/sdlog_ringbuffer.h @@ -0,0 +1,91 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier <lm@inf.ethz.ch> + * + * 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 sdlog_ringbuffer.h + * microSD logging + * + * @author Lorenz Meier <lm@inf.ethz.ch> + */ + +#ifndef SDLOG_RINGBUFFER_H_ +#define SDLOG_RINGBUFFER_H_ + +#pragma pack(push, 1) +struct sdlog_sysvector { + uint64_t timestamp; /**< time [us] */ + float gyro[3]; /**< [rad/s] */ + float accel[3]; /**< [m/s^2] */ + float mag[3]; /**< [gauss] */ + float baro; /**< pressure [millibar] */ + float baro_alt; /**< altitude above MSL [meter] */ + float baro_temp; /**< [degree celcius] */ + float control[4]; /**< roll, pitch, yaw [-1..1], thrust [0..1] */ + float actuators[8]; /**< motor 1-8, in motor units (PWM: 1000-2000,AR.Drone: 0-512) */ + float vbat; /**< battery voltage in [volt] */ + float bat_current; /**< battery discharge current */ + float bat_discharged; /**< discharged energy in mAh */ + float adc[3]; /**< remaining auxiliary ADC ports [volt] */ + float local_position[3]; /**< tangent plane mapping into x,y,z [m] */ + int32_t gps_raw_position[3]; /**< latitude [degrees] north, longitude [degrees] east, altitude above MSL [millimeter] */ + float attitude[3]; /**< roll, pitch, yaw [rad] */ + float rotMatrix[9]; /**< unitvectors */ + float vicon[6]; /**< Vicon ground truth x, y, z and roll, pitch, yaw */ + float control_effective[4]; /**< roll, pitch, yaw [-1..1], thrust [0..1] */ + float flow[6]; /**< flow raw x, y, flow metric x, y, flow ground dist, flow quality */ + float diff_pressure; /**< differential pressure */ + float ind_airspeed; /**< indicated airspeed */ + float true_airspeed; /**< true airspeed */ +}; +#pragma pack(pop) + +struct sdlog_logbuffer { + unsigned int start; + // unsigned int end; + unsigned int size; + int count; + struct sdlog_sysvector *elems; +}; + +void sdlog_logbuffer_init(struct sdlog_logbuffer *lb, int size); + +int sdlog_logbuffer_is_full(struct sdlog_logbuffer *lb); + +int sdlog_logbuffer_is_empty(struct sdlog_logbuffer *lb); + +void sdlog_logbuffer_write(struct sdlog_logbuffer *lb, const struct sdlog_sysvector *elem); + +int sdlog_logbuffer_read(struct sdlog_logbuffer *lb, struct sdlog_sysvector *elem); + +#endif
\ No newline at end of file diff --git a/apps/systemlib/airspeed.c b/apps/systemlib/airspeed.c new file mode 100644 index 000000000..e213b66c2 --- /dev/null +++ b/apps/systemlib/airspeed.c @@ -0,0 +1,79 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier <lm@inf.ethz.ch> + * + * 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 airspeed.c + * Airspeed estimation + * + * @author Lorenz Meier <lm@inf.ethz.ch> + * + */ + +#include "math.h" + + +float calc_indicated_airspeed(float pressure_front, float pressure_ambient, float temperature) +{ + return sqrtf((2.0f*(pressure_front - pressure_ambient)) / air_density_sea_level); +} + +/** + * Calculate true airspeed from indicated airspeed. + * + * Note that the true airspeed is NOT the groundspeed, because of the effects of wind + * + * @param speed current indicated airspeed + * @param pressure_ambient pressure at the side of the tube/airplane + * @param temperature air temperature in degrees celcius + * @return true airspeed in m/s + */ +float calc_true_airspeed_from_indicated(float speed, float pressure_ambient, float temperature) +{ + return speed * sqrtf(air_density_sea_level / get_air_density(pressure_ambient, temperature)); +} + +/** + * Directly calculate true airspeed + * + * Note that the true airspeed is NOT the groundspeed, because of the effects of wind + * + * @param pressure_front pressure inside the pitot/prandl tube + * @param pressure_ambient pressure at the side of the tube/airplane + * @param temperature air temperature in degrees celcius + * @return true airspeed in m/s + */ +float calc_true_airspeed(float pressure_front, float pressure_ambient, float temperature) +{ + return sqrtf((2.0f*(pressure_front - pressure_ambient)) / get_air_density(pressure_ambient, temperature)); +}
\ No newline at end of file diff --git a/apps/systemlib/airspeed.h b/apps/systemlib/airspeed.h new file mode 100644 index 000000000..62acfe2b0 --- /dev/null +++ b/apps/systemlib/airspeed.h @@ -0,0 +1,86 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier <lm@inf.ethz.ch> + * + * 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 airspeed.c + * Airspeed estimation + * + * @author Lorenz Meier <lm@inf.ethz.ch> + * + */ + +#include "math.h" +#include "conversions.h" + + __BEGIN_DECLS + +/** + * Calculate indicated airspeed. + * + * Note that the indicated airspeed is not the true airspeed because it + * lacks the air density compensation. Use the calc_true_airspeed functions to get + * the true airspeed. + * + * @param pressure_front pressure inside the pitot/prandl tube + * @param pressure_ambient pressure at the side of the tube/airplane + * @param temperature air temperature in degrees celcius + * @return indicated airspeed in m/s + */ +__EXPORT float calc_indicated_airspeed(float pressure_front, float pressure_ambient, float temperature); + +/** + * Calculate true airspeed from indicated airspeed. + * + * Note that the true airspeed is NOT the groundspeed, because of the effects of wind + * + * @param speed current indicated airspeed + * @param pressure_ambient pressure at the side of the tube/airplane + * @param temperature air temperature in degrees celcius + * @return true airspeed in m/s + */ +__EXPORT float calc_true_airspeed_from_indicated(float speed, float pressure_ambient, float temperature); + +/** + * Directly calculate true airspeed + * + * Note that the true airspeed is NOT the groundspeed, because of the effects of wind + * + * @param pressure_front pressure inside the pitot/prandl tube + * @param pressure_ambient pressure at the side of the tube/airplane + * @param temperature air temperature in degrees celcius + * @return true airspeed in m/s + */ +__EXPORT float calc_true_airspeed(float pressure_front, float pressure_ambient, float temperature); + +__END_DECLS
\ No newline at end of file diff --git a/apps/systemlib/conversions.c b/apps/systemlib/conversions.c index 32df446d9..b08b9fc69 100644 --- a/apps/systemlib/conversions.c +++ b/apps/systemlib/conversions.c @@ -42,6 +42,10 @@ #include "conversions.h" +#define air_gas_constant 8.31432f +#define air_density_sea_level 1.225f +#define absolute_null_kelvin 273.15f + int16_t int16_t_from_bytes(uint8_t bytes[]) { @@ -143,4 +147,9 @@ void quat2rot(const float Q[4], float R[9]) R[2] = 2.0F * (Q[1] * Q[3] - Q[0] * Q[2]); R[5] = 2.0F * (Q[2] * Q[3] + Q[0] * Q[1]); R[8] = ((q0_2 + q3_2) - q1_2) - q2_2; -}
\ No newline at end of file +} + +float get_air_density(float static_pressure, float temperature_celsius) +{ + return static_pressure/(air_gas_constant * (temperature_celsius + absolute_null_kelvin)); +} diff --git a/apps/systemlib/conversions.h b/apps/systemlib/conversions.h index 3a24ca9b7..4db6de772 100644 --- a/apps/systemlib/conversions.h +++ b/apps/systemlib/conversions.h @@ -78,6 +78,14 @@ __EXPORT void rot2quat(const float R[9], float Q[4]); */ __EXPORT void quat2rot(const float Q[4], float R[9]); +/** + * Calculates air density. + * + * @param static_pressure ambient pressure in millibar + * @param temperature_celcius air / ambient temperature in celcius + */ +__EXPORT float get_air_density(float static_pressure, float temperature_celsius); + __END_DECLS #endif /* CONVERSIONS_H_ */ diff --git a/apps/uORB/objects_common.cpp b/apps/uORB/objects_common.cpp index 2d249a47f..82893b3e9 100644 --- a/apps/uORB/objects_common.cpp +++ b/apps/uORB/objects_common.cpp @@ -113,6 +113,12 @@ ORB_DEFINE(offboard_control_setpoint, struct offboard_control_setpoint_s); #include "topics/optical_flow.h" ORB_DEFINE(optical_flow, struct optical_flow_s); +#include "topics/omnidirectional_flow.h" +ORB_DEFINE(omnidirectional_flow, struct omnidirectional_flow_s); + +#include "topics/differential_pressure.h" +ORB_DEFINE(differential_pressure, struct differential_pressure_s); + #include "topics/subsystem_info.h" ORB_DEFINE(subsystem_info, struct subsystem_info_s); diff --git a/apps/uORB/topics/differential_pressure.h b/apps/uORB/topics/differential_pressure.h new file mode 100644 index 000000000..fd7670cbc --- /dev/null +++ b/apps/uORB/topics/differential_pressure.h @@ -0,0 +1,70 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2013 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 differential_pressure.h + * + * Definition of differential pressure topic + */ + +#ifndef TOPIC_DIFFERENTIAL_PRESSURE_H_ +#define TOPIC_DIFFERENTIAL_PRESSURE_H_ + +#include "../uORB.h" +#include <stdint.h> + +/** + * @addtogroup topics + * @{ + */ + +/** + * Battery voltages and status + */ +struct differential_pressure_s { + uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ + float static_pressure_mbar; /**< Static / environment pressure */ + float differential_pressure_mbar; /**< Differential pressure reading */ + float temperature_celcius; /**< ambient temperature in celcius, -1 if unknown */ + float indicated_airspeed_m_s; /**< indicated airspeed in meters per second, -1 if unknown */ + float true_airspeed_m_s; /**< true airspeed in meters per second, -1 if unknown */ +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(differential_pressure); + +#endif
\ No newline at end of file diff --git a/apps/uORB/topics/omnidirectional_flow.h b/apps/uORB/topics/omnidirectional_flow.h new file mode 100644 index 000000000..8f4be3b3f --- /dev/null +++ b/apps/uORB/topics/omnidirectional_flow.h @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier <lm@inf.ethz.ch> + * + * 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 optical_flow.h + * Definition of the optical flow uORB topic. + */ + +#ifndef TOPIC_OMNIDIRECTIONAL_FLOW_H_ +#define TOPIC_OMNIDIRECTIONAL_FLOW_H_ + +#include <stdint.h> +#include <stdbool.h> +#include "../uORB.h" + +/** + * @addtogroup topics + */ + +/** + * Omnidirectional optical flow in NED body frame in SI units. + * + * @see http://en.wikipedia.org/wiki/International_System_of_Units + */ +struct omnidirectional_flow_s { + + uint64_t timestamp; /**< in microseconds since system start */ + + uint16_t left[10]; /**< Left flow, in decipixels */ + uint16_t right[10]; /**< Right flow, in decipixels */ + float front_distance_m; /**< Altitude / distance to object front in meters */ + uint8_t quality; /**< Quality of the measurement, 0: bad quality, 255: maximum quality */ + uint8_t sensor_id; /**< id of the sensor emitting the flow value */ + +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(omnidirectional_flow); + +#endif diff --git a/apps/uORB/topics/optical_flow.h b/apps/uORB/topics/optical_flow.h index 24f842825..c854f0079 100644 --- a/apps/uORB/topics/optical_flow.h +++ b/apps/uORB/topics/optical_flow.h @@ -55,11 +55,6 @@ */ struct optical_flow_s { - /* - * Actual data, this is specific to the type of data which is stored in this struct - * A line containing L0GME will be added by the Python logging code generator to the - * logged dataset. - */ uint64_t timestamp; /**< in microseconds since system start */ uint16_t flow_raw_x; /**< flow in pixels in X direction, not rotation-compensated */ @@ -67,8 +62,8 @@ struct optical_flow_s { float flow_comp_x_m; /**< speed over ground in meters, rotation-compensated */ float flow_comp_y_m; /**< speed over ground in meters, rotation-compensated */ float ground_distance_m; /**< Altitude / distance to ground in meters */ - uint8_t quality; /**< Quality of the measurement, 0: bad quality, 255: maximum quality */ - uint8_t sensor_id; /**< id of the sensor emitting the flow value */ + uint8_t quality; /**< Quality of the measurement, 0: bad quality, 255: maximum quality */ + uint8_t sensor_id; /**< id of the sensor emitting the flow value */ }; |