aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorLorenz Meier <lm@inf.ethz.ch>2012-11-21 15:32:59 +0100
committerLorenz Meier <lm@inf.ethz.ch>2012-11-21 15:32:59 +0100
commit25e304c8691f2162b34cde46414f5b7f805cff67 (patch)
tree3cdbd19f0b3280beee7f764173eb10634a454114
parentffda224b414204af7fb0a4f660f7b72ee013bcd2 (diff)
downloadpx4-firmware-25e304c8691f2162b34cde46414f5b7f805cff67.tar.gz
px4-firmware-25e304c8691f2162b34cde46414f5b7f805cff67.tar.bz2
px4-firmware-25e304c8691f2162b34cde46414f5b7f805cff67.zip
Added vicon, actuator controls effective and optical flow to logging
-rw-r--r--ROMFS/logging/logconv.m11
-rw-r--r--apps/mavlink/mavlink_receiver.c4
-rw-r--r--apps/sdlog/sdlog.c51
-rw-r--r--apps/uORB/topics/vehicle_vicon_position.h6
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 <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_effective.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/vehicle_vicon_position.h>
+#include <uORB/topics/optical_flow.h>
#include <systemlib/systemlib.h>
@@ -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