From affa3af4e6415d1f68dd6242d0ded6e5ed8a1b1d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 3 Oct 2012 13:39:26 +0200 Subject: Clean 250 Hz updates in filter, partial updates enabled --- apps/sdlog/sdlog.c | 37 ++++++++++++++++++++++++++++++++++++- 1 file changed, 36 insertions(+), 1 deletion(-) (limited to 'apps/sdlog/sdlog.c') diff --git a/apps/sdlog/sdlog.c b/apps/sdlog/sdlog.c index f4af17597..0426dfb9c 100644 --- a/apps/sdlog/sdlog.c +++ b/apps/sdlog/sdlog.c @@ -60,6 +60,9 @@ #include #include #include +#include +#include +#include #include @@ -273,6 +276,9 @@ int sdlog_thread_main(int argc, char *argv[]) { struct actuator_outputs_s act_outputs; struct actuator_controls_s act_controls; 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; } buf; memset(&buf, 0, sizeof(buf)); @@ -283,6 +289,9 @@ int sdlog_thread_main(int argc, char *argv[]) { int spa_sub; int act_0_sub; int controls0_sub; + int local_pos_sub; + int global_pos_sub; + int gps_pos_sub; } subs; /* --- MANAGEMENT - LOGGING COMMAND --- */ @@ -329,6 +338,27 @@ int sdlog_thread_main(int argc, char *argv[]) { fds[fdsc_count].events = POLLIN; fdsc_count++; + /* --- LOCAL POSITION --- */ + /* subscribe to ORB for local position */ + subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); + fds[fdsc_count].fd = subs.local_pos_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + + /* --- GLOBAL POSITION --- */ + /* subscribe to ORB for global position */ + subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + fds[fdsc_count].fd = subs.global_pos_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + + /* --- GPS POSITION --- */ + /* subscribe to ORB for global position */ + subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + fds[fdsc_count].fd = subs.gps_pos_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. @@ -435,6 +465,9 @@ int sdlog_thread_main(int argc, char *argv[]) { /* 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); #pragma pack(push, 1) struct { @@ -450,6 +483,7 @@ int sdlog_thread_main(int argc, char *argv[]) { float actuators[8]; float vbat; float adc[3]; + float local_pos[3]; } sysvector = { .timestamp = buf.raw.timestamp, .gyro = {buf.raw.gyro_rad_s[0], buf.raw.gyro_rad_s[1], buf.raw.gyro_rad_s[2]}, @@ -462,7 +496,8 @@ int sdlog_thread_main(int argc, char *argv[]) { .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.raw.battery_voltage_v, - .adc = {buf.raw.adc_voltage_v[0], buf.raw.adc_voltage_v[1], buf.raw.adc_voltage_v[2]} + .adc = {buf.raw.adc_voltage_v[0], buf.raw.adc_voltage_v[1], buf.raw.adc_voltage_v[2]}, + .local_pos = {buf.local_pos.x, buf.local_pos.y, buf.local_pos.z} }; #pragma pack(pop) -- cgit v1.2.3