From 25e304c8691f2162b34cde46414f5b7f805cff67 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 21 Nov 2012 15:32:59 +0100 Subject: Added vicon, actuator controls effective and optical flow to logging --- ROMFS/logging/logconv.m | 11 ++++++- apps/mavlink/mavlink_receiver.c | 4 +++ apps/sdlog/sdlog.c | 51 +++++++++++++++++++++++++++---- apps/uORB/topics/vehicle_vicon_position.h | 6 ++-- 4 files changed, 62 insertions(+), 10 deletions(-) diff --git a/ROMFS/logging/logconv.m b/ROMFS/logging/logconv.m index 88bcfec96..48399f1eb 100644 --- a/ROMFS/logging/logconv.m +++ b/ROMFS/logging/logconv.m @@ -32,7 +32,7 @@ if exist(filePath, 'file') fileSize = fileInfo.bytes; fid = fopen(filePath, 'r'); - elements = int64(fileSize./(16*4+8))/4 + elements = int64(fileSize./(8+(3+3+3+1+1+1+4+8+1+3+3+3+3+9+6+4+6)*4)) for i=1:elements % timestamp @@ -79,6 +79,15 @@ if exist(filePath, 'file') % RotMatrix (9 channels) sensors(i,39:47) = fread(fid, 9, 'float', 0, 'ieee-le'); + + % vicon position (6 channels) + sensors(i,48:53) = fread(fid, 6, 'float', 0, 'ieee-le'); + + % actuator control effective (4 channels) + sensors(i,54:57) = fread(fid, 4, 'float', 0, 'ieee-le'); + + % optical flow (6 values) + sensors(i,58:63) = fread(fid, 6, 'float', 0, 'ieee-le'); end time_us = sensors(elements,1) - sensors(1,1); time_s = time_us*1e-6 diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c index 4790a673d..6b4375445 100644 --- a/apps/mavlink/mavlink_receiver.c +++ b/apps/mavlink/mavlink_receiver.c @@ -206,6 +206,10 @@ handle_message(mavlink_message_t *msg) vicon_position.y = pos.y; vicon_position.z = pos.z; + vicon_position.roll = pos.roll; + vicon_position.pitch = pos.pitch; + vicon_position.yaw = pos.yaw; + if (vicon_position_pub <= 0) { vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position); } else { diff --git a/apps/sdlog/sdlog.c b/apps/sdlog/sdlog.c index eed72125a..d38bf9122 100644 --- a/apps/sdlog/sdlog.c +++ b/apps/sdlog/sdlog.c @@ -59,10 +59,13 @@ #include #include #include +#include #include #include #include #include +#include +#include #include @@ -295,10 +298,13 @@ 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 act_controls; + struct actuator_controls_effective_s act_controls_effective; struct vehicle_command_s cmd; struct vehicle_local_position_s local_pos; struct vehicle_global_position_s global_pos; struct vehicle_gps_position_s gps_pos; + struct vehicle_vicon_position_s vicon_pos; + struct optical_flow_s flow; } buf; memset(&buf, 0, sizeof(buf)); @@ -308,10 +314,13 @@ int sdlog_thread_main(int argc, char *argv[]) { int att_sub; int spa_sub; int act_0_sub; - int controls0_sub; + int controls_0_sub; + int controls_effective_0_sub; int local_pos_sub; int global_pos_sub; int gps_pos_sub; + int vicon_pos_sub; + int flow_sub; } subs; /* --- MANAGEMENT - LOGGING COMMAND --- */ @@ -353,8 +362,15 @@ int sdlog_thread_main(int argc, char *argv[]) { /* --- ACTUATOR CONTROL VALUE --- */ /* subscribe to ORB for actuator control */ - subs.controls0_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); - fds[fdsc_count].fd = subs.controls0_sub; + subs.controls_0_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); + fds[fdsc_count].fd = subs.controls_0_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + + /* --- ACTUATOR CONTROL EFFECTIVE VALUE --- */ + /* subscribe to ORB for actuator control */ + subs.controls_effective_0_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE); + fds[fdsc_count].fd = subs.controls_effective_0_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; @@ -379,6 +395,20 @@ int sdlog_thread_main(int argc, char *argv[]) { fds[fdsc_count].events = POLLIN; fdsc_count++; + /* --- VICON POSITION --- */ + /* subscribe to ORB for vicon position */ + subs.vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position)); + fds[fdsc_count].fd = subs.vicon_pos_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + + /* --- FLOW measurements --- */ + /* subscribe to ORB for flow measurements */ + subs.flow_sub = orb_subscribe(ORB_ID(optical_flow)); + fds[fdsc_count].fd = subs.flow_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. @@ -481,7 +511,8 @@ int sdlog_thread_main(int argc, char *argv[]) { /* 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.controls0_sub, &buf.act_controls); + 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); @@ -489,6 +520,8 @@ int sdlog_thread_main(int argc, char *argv[]) { 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 { @@ -507,6 +540,9 @@ int sdlog_thread_main(int argc, char *argv[]) { 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]}, @@ -523,13 +559,16 @@ int sdlog_thread_main(int argc, char *argv[]) { .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]} + .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(10000); //10000 corresponds in reality to ca. 76 Hz + usleep(3500); // roughly 150 Hz } fsync(sysvector_file); diff --git a/apps/uORB/topics/vehicle_vicon_position.h b/apps/uORB/topics/vehicle_vicon_position.h index 91d933f44..0822fa89a 100644 --- a/apps/uORB/topics/vehicle_vicon_position.h +++ b/apps/uORB/topics/vehicle_vicon_position.h @@ -60,9 +60,9 @@ struct vehicle_vicon_position_s float x; /**< X positin in meters in NED earth-fixed frame */ float y; /**< X positin in meters in NED earth-fixed frame */ float z; /**< Z positin in meters in NED earth-fixed frame (negative altitude) */ - float vx; - float vy; - float vz; + float roll; + float pitch; + float yaw; // TODO Add covariances here -- cgit v1.2.3