diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2013-07-12 10:09:38 +0400 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2013-07-12 10:09:38 +0400 |
commit | 1d986d6c040ed0123bdb8cccad1e444f9d0113f3 (patch) | |
tree | 30606b18ab9c4f168e0298d53ef3d2a64fb7bd0a /src/modules/sdlog2 | |
parent | f6b45ac2e1309e193ca95afee19cad1c8e0c18b7 (diff) | |
download | px4-firmware-1d986d6c040ed0123bdb8cccad1e444f9d0113f3.tar.gz px4-firmware-1d986d6c040ed0123bdb8cccad1e444f9d0113f3.tar.bz2 px4-firmware-1d986d6c040ed0123bdb8cccad1e444f9d0113f3.zip |
sdlog2: Global Position Set Point message added, vehicle_global_position_setpoint topic fixed
Diffstat (limited to 'src/modules/sdlog2')
-rw-r--r-- | src/modules/sdlog2/sdlog2.c | 31 | ||||
-rw-r--r-- | src/modules/sdlog2/sdlog2_messages.h | 34 |
2 files changed, 56 insertions, 9 deletions
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 3e6b20472..3713e0b30 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -72,6 +72,7 @@ #include <uORB/topics/vehicle_local_position.h> #include <uORB/topics/vehicle_local_position_setpoint.h> #include <uORB/topics/vehicle_global_position.h> +#include <uORB/topics/vehicle_global_position_setpoint.h> #include <uORB/topics/vehicle_gps_position.h> #include <uORB/topics/vehicle_vicon_position.h> #include <uORB/topics/optical_flow.h> @@ -615,7 +616,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ /* number of messages */ - const ssize_t fdsc = 18; + const ssize_t fdsc = 19; /* Sanity check variable and index */ ssize_t fdsc_count = 0; /* file descriptors to wait for */ @@ -637,6 +638,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_local_position_s local_pos; struct vehicle_local_position_setpoint_s local_pos_sp; struct vehicle_global_position_s global_pos; + struct vehicle_global_position_setpoint_s global_pos_sp; struct vehicle_gps_position_s gps_pos; struct vehicle_vicon_position_s vicon_pos; struct optical_flow_s flow; @@ -660,6 +662,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int local_pos_sub; int local_pos_sp_sub; int global_pos_sub; + int global_pos_sp_sub; int gps_pos_sub; int vicon_pos_sub; int flow_sub; @@ -689,6 +692,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_ARSP_s log_ARSP; struct log_FLOW_s log_FLOW; struct log_GPOS_s log_GPOS; + struct log_GPSP_s log_GPSP; struct log_ESC_s log_ESC; } body; } log_msg = { @@ -775,6 +779,12 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; + /* --- GLOBAL POSITION SETPOINT--- */ + subs.global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); + fds[fdsc_count].fd = subs.global_pos_sp_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + /* --- VICON POSITION --- */ subs.vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position)); fds[fdsc_count].fd = subs.vicon_pos_sub; @@ -1077,6 +1087,25 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(GPOS); } + /* --- GLOBAL POSITION SETPOINT --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_global_position_setpoint), subs.global_pos_sp_sub, &buf.global_pos_sp); + log_msg.msg_type = LOG_GPSP_MSG; + log_msg.body.log_GPSP.altitude_is_relative = buf.global_pos_sp.altitude_is_relative; + log_msg.body.log_GPSP.lat = buf.global_pos_sp.lat; + log_msg.body.log_GPSP.lon = buf.global_pos_sp.lon; + log_msg.body.log_GPSP.altitude = buf.global_pos_sp.altitude; + log_msg.body.log_GPSP.yaw = buf.global_pos_sp.yaw; + log_msg.body.log_GPSP.loiter_radius = buf.global_pos_sp.loiter_radius; + log_msg.body.log_GPSP.loiter_direction = buf.global_pos_sp.loiter_direction; + log_msg.body.log_GPSP.nav_cmd = buf.global_pos_sp.nav_cmd; + log_msg.body.log_GPSP.param1 = buf.global_pos_sp.param1; + log_msg.body.log_GPSP.param2 = buf.global_pos_sp.param2; + log_msg.body.log_GPSP.param3 = buf.global_pos_sp.param3; + log_msg.body.log_GPSP.param4 = buf.global_pos_sp.param4; + LOGBUFFER_WRITE_AND_COUNT(GPSP); + } + /* --- VICON POSITION --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos); diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index abc882d23..934e4dec8 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -149,15 +149,15 @@ struct log_ATTC_s { /* --- STAT - VEHICLE STATE --- */ #define LOG_STAT_MSG 10 struct log_STAT_s { - unsigned char state; - unsigned char flight_mode; - unsigned char manual_control_mode; - unsigned char manual_sas_mode; - unsigned char armed; + uint8_t state; + uint8_t flight_mode; + uint8_t manual_control_mode; + uint8_t manual_sas_mode; + uint8_t armed; float battery_voltage; float battery_current; float battery_remaining; - unsigned char battery_warning; + uint8_t battery_warning; }; /* --- RC - RC INPUT CHANNELS --- */ @@ -210,13 +210,29 @@ struct log_GPOS_s { float vel_d; }; +/* --- GPSP - GLOBAL POSITION SETPOINT --- */ +#define LOG_GPSP_MSG 17 +struct log_GPSP_s { + uint8_t altitude_is_relative; + int32_t lat; + int32_t lon; + float altitude; + float yaw; + float loiter_radius; + int8_t loiter_direction; + uint8_t nav_cmd; + float param1; + float param2; + float param3; + float param4; +}; + /* --- ESC - ESC STATE --- */ -#define LOG_ESC_MSG 64 +#define LOG_ESC_MSG 18 struct log_ESC_s { uint16_t counter; uint8_t esc_count; uint8_t esc_connectiontype; - uint8_t esc_num; uint16_t esc_address; uint16_t esc_version; @@ -227,6 +243,7 @@ struct log_ESC_s { float esc_setpoint; uint16_t esc_setpoint_raw; }; + #pragma pack(pop) /* construct list of all message formats */ @@ -248,6 +265,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"), LOG_FORMAT(GPOS, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"), + LOG_FORMAT(GPSP, "BLLfffbBffff", "AltRel,Lat,Lon,Alt,Yaw,LoiterR,LoiterDir,NavCmd,P1,P2,P3,P4"), LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,No,Version,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), }; |