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

sdlog2: NavState moved to GPSP message

This commit is contained in:
Anton Babushkin 2014-01-28 00:23:23 +01:00
parent 3fdb082cb8
commit f9b5709e9b
2 changed files with 6 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -62,7 +62,6 @@
#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>
@ -740,7 +739,6 @@ 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;
@ -767,7 +765,6 @@ 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;
@ -847,12 +844,6 @@ 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++;
/* --- SENSORS COMBINED --- */ /* --- SENSORS COMBINED --- */
subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
fds[fdsc_count].fd = subs.sensor_sub; fds[fdsc_count].fd = subs.sensor_sub;
@ -1002,7 +993,7 @@ int sdlog2_thread_main(int argc, char *argv[])
/* decide use usleep() or blocking poll() */ /* decide use usleep() or blocking poll() */
bool use_sleep = sleep_delay > 0 && logging_enabled; bool use_sleep = sleep_delay > 0 && logging_enabled;
/* poll all topics if logging enabled or only management (first 2) if not */ /* poll all topics if logging enabled or only management (first 3) if not */
int poll_ret = poll(fds, logging_enabled ? fdsc_count : 3, use_sleep ? 0 : poll_timeout); int poll_ret = poll(fds, logging_enabled ? fdsc_count : 3, use_sleep ? 0 : poll_timeout);
/* handle the poll result */ /* handle the poll result */
@ -1064,11 +1055,8 @@ 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 VEHICLE CONTROL MODE 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_status.main_state;
//log_msg.body.log_STAT.navigation_state = (uint8_t) buf.control_mode.nav_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_remaining = buf_status.battery_remaining; log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining;
log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning; log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning;
@ -1078,7 +1066,7 @@ int sdlog2_thread_main(int argc, char *argv[])
/* --- GPS POSITION --- */ /* --- GPS POSITION --- */
if (fds[ifds++].revents & POLLIN) { if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); /* don't orb_copy, it's already done few lines above */
log_msg.msg_type = LOG_GPS_MSG; log_msg.msg_type = LOG_GPS_MSG;
log_msg.body.log_GPS.gps_time = buf.gps_pos.time_gps_usec; log_msg.body.log_GPS.gps_time = buf.gps_pos.time_gps_usec;
log_msg.body.log_GPS.fix_type = buf.gps_pos.fix_type; log_msg.body.log_GPS.fix_type = buf.gps_pos.fix_type;
@ -1094,8 +1082,6 @@ int sdlog2_thread_main(int argc, char *argv[])
LOGBUFFER_WRITE_AND_COUNT(GPS); LOGBUFFER_WRITE_AND_COUNT(GPS);
} }
ifds++; // skip CONTROL MODE, already handled
/* --- SENSOR COMBINED --- */ /* --- SENSOR COMBINED --- */
if (fds[ifds++].revents & POLLIN) { if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor); orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor);
@ -1265,6 +1251,7 @@ int sdlog2_thread_main(int argc, char *argv[])
if (fds[ifds++].revents & POLLIN) { if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(position_setpoint_triplet), subs.triplet_sub, &buf.triplet); orb_copy(ORB_ID(position_setpoint_triplet), subs.triplet_sub, &buf.triplet);
log_msg.msg_type = LOG_GPSP_MSG; log_msg.msg_type = LOG_GPSP_MSG;
log_msg.body.log_GPSP.nav_state = buf.triplet.nav_state;
log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7d); log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7d);
log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7d); log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7d);
log_msg.body.log_GPSP.alt = buf.triplet.current.alt; log_msg.body.log_GPSP.alt = buf.triplet.current.alt;

6
src/modules/sdlog2/sdlog2_messages.h
View File

@ -149,7 +149,6 @@ struct log_ATTC_s {
#define LOG_STAT_MSG 10 #define LOG_STAT_MSG 10
struct log_STAT_s { struct log_STAT_s {
uint8_t main_state; uint8_t main_state;
uint8_t navigation_state;
uint8_t arming_state; uint8_t arming_state;
float battery_remaining; float battery_remaining;
uint8_t battery_warning; uint8_t battery_warning;
@ -210,6 +209,7 @@ struct log_GPOS_s {
/* --- GPSP - GLOBAL POSITION SETPOINT --- */ /* --- GPSP - GLOBAL POSITION SETPOINT --- */
#define LOG_GPSP_MSG 17 #define LOG_GPSP_MSG 17
struct log_GPSP_s { struct log_GPSP_s {
uint8_t nav_state;
int32_t lat; int32_t lat;
int32_t lon; int32_t lon;
float alt; float alt;
@ -297,14 +297,14 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"),
LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"),
LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
LOG_FORMAT(STAT, "BBBfBB", "MainState,NavState,ArmState,BatRem,BatWarn,Landed"), LOG_FORMAT(STAT, "BBfBB", "MainState,ArmState,BatRem,BatWarn,Landed"),
LOG_FORMAT(RC, "ffffffffB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count"), LOG_FORMAT(RC, "ffffffffB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count"),
LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"), LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"),
LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"),
LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"), LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"),
LOG_FORMAT(GPOS, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"), LOG_FORMAT(GPOS, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"),
LOG_FORMAT(GPSP, "LLffBfbf", "Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"), LOG_FORMAT(GPSP, "LLffBfbf", "NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"),
LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"),
LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"), LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"),