dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

sdlog2: Main & Nav state logging fixed

This commit is contained in:
Anton Babushkin 2014-01-06 09:17:08 +01:00
parent 40196275d0
commit 187c2a4bca
1 changed files with 16 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
src/modules/sdlog2/sdlog2.c
View File

@ -62,6 +62,7 @@
#include <uORB/uORB.h> #include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h> #include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/sensor_combined.h> #include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/vehicle_attitude_setpoint.h>
@ -684,6 +685,7 @@ int sdlog2_thread_main(int argc, char *argv[])
/* warning! using union here to save memory, elements should be used separately! */ /* warning! using union here to save memory, elements should be used separately! */
union { union {
struct vehicle_command_s cmd; struct vehicle_command_s cmd;
struct vehicle_control_mode_s control_mode;
struct sensor_combined_s sensor; struct sensor_combined_s sensor;
struct vehicle_attitude_s att; struct vehicle_attitude_s att;
struct vehicle_attitude_setpoint_s att_sp; struct vehicle_attitude_setpoint_s att_sp;
@ -709,6 +711,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct { struct {
int cmd_sub; int cmd_sub;
int status_sub; int status_sub;
int control_mode_sub;
int sensor_sub; int sensor_sub;
int att_sub; int att_sub;
int att_sp_sub; int att_sp_sub;
@ -761,7 +764,7 @@ int sdlog2_thread_main(int argc, char *argv[])
/* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
/* number of subscriptions */ /* number of subscriptions */
const ssize_t fdsc = 19; const ssize_t fdsc = 20;
/* sanity check variable and index */ /* sanity check variable and index */
ssize_t fdsc_count = 0; ssize_t fdsc_count = 0;
/* file descriptors to wait for */ /* file descriptors to wait for */
@ -779,6 +782,12 @@ int sdlog2_thread_main(int argc, char *argv[])
fds[fdsc_count].events = POLLIN; fds[fdsc_count].events = POLLIN;
fdsc_count++; fdsc_count++;
/* --- VEHICLE CONTROL MODE --- */
subs.control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
fds[fdsc_count].fd = subs.control_mode_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- GPS POSITION --- */ /* --- GPS POSITION --- */
subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
fds[fdsc_count].fd = subs.gps_pos_sub; fds[fdsc_count].fd = subs.gps_pos_sub;
@ -966,11 +975,12 @@ int sdlog2_thread_main(int argc, char *argv[])
/* --- VEHICLE STATUS --- */ /* --- VEHICLE STATUS --- */
if (fds[ifds++].revents & POLLIN) { if (fds[ifds++].revents & POLLIN) {
// Don't orb_copy, it's already done few lines above /* don't orb_copy, it's already done few lines above */
/* copy control mode here to construct STAT message */
orb_copy(ORB_ID(vehicle_control_mode), subs.control_mode_sub, &buf.control_mode);
log_msg.msg_type = LOG_STAT_MSG; log_msg.msg_type = LOG_STAT_MSG;
log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state; log_msg.body.log_STAT.main_state = (uint8_t) buf.control_mode.main_state;
// TODO use control_mode topic log_msg.body.log_STAT.navigation_state = (uint8_t) buf.control_mode.nav_state;
//log_msg.body.log_STAT.navigation_state = (uint8_t) buf_status.navigation_state;
log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state; log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state;
log_msg.body.log_STAT.battery_voltage = buf_status.battery_voltage; log_msg.body.log_STAT.battery_voltage = buf_status.battery_voltage;
log_msg.body.log_STAT.battery_current = buf_status.battery_current; log_msg.body.log_STAT.battery_current = buf_status.battery_current;
@ -979,6 +989,7 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_STAT.landed = (uint8_t) buf_status.condition_landed; log_msg.body.log_STAT.landed = (uint8_t) buf_status.condition_landed;
LOGBUFFER_WRITE_AND_COUNT(STAT); LOGBUFFER_WRITE_AND_COUNT(STAT);
} }
ifds++; // skip CONTROL MODE, already copied
/* --- GPS POSITION --- */ /* --- GPS POSITION --- */
if (fds[ifds++].revents & POLLIN) { if (fds[ifds++].revents & POLLIN) {