fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-02 02:41:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

added control state to logging

Browse Source
This commit is contained in:
Youssef Demitri
2015-10-08 21:19:51 +02:00
parent 9c26e71ef6
commit 2f4afa2da7
2 changed files with 65 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sdlog2/sdlog2.c
+18
View File
@@ -66,6 +66,7 @@
#include <uORB/topics/vehicle_status.h> #include <uORB/topics/vehicle_status.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/control_state.h>
#include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_rates_setpoint.h> #include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/actuator_outputs.h> #include <uORB/topics/actuator_outputs.h>
@@ -1108,6 +1109,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct vehicle_command_s cmd; struct vehicle_command_s cmd;
struct sensor_combined_s sensor; struct sensor_combined_s sensor;
struct vehicle_attitude_s att; struct vehicle_attitude_s att;
struct control_state_s ctrl_state;
struct vehicle_attitude_setpoint_s att_sp; struct vehicle_attitude_setpoint_s att_sp;
struct vehicle_rates_setpoint_s rates_sp; struct vehicle_rates_setpoint_s rates_sp;
struct actuator_outputs_s act_outputs; struct actuator_outputs_s act_outputs;
@@ -1158,6 +1160,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct log_ATTC_s log_ATTC; struct log_ATTC_s log_ATTC;
struct log_STAT_s log_STAT; struct log_STAT_s log_STAT;
struct log_VTOL_s log_VTOL; struct log_VTOL_s log_VTOL;
struct log_CTS_s log_CTS;
struct log_RC_s log_RC; struct log_RC_s log_RC;
struct log_OUT0_s log_OUT0; struct log_OUT0_s log_OUT0;
struct log_AIRS_s log_AIRS; struct log_AIRS_s log_AIRS;
@@ -1199,6 +1202,7 @@ int sdlog2_thread_main(int argc, char *argv[])
int vtol_status_sub; int vtol_status_sub;
int sensor_sub; int sensor_sub;
int att_sub; int att_sub;
int ctrl_state_sub;
int att_sp_sub; int att_sp_sub;
int rates_sp_sub; int rates_sp_sub;
int act_outputs_sub; int act_outputs_sub;
@@ -1236,6 +1240,7 @@ int sdlog2_thread_main(int argc, char *argv[])
subs.gps_pos_sub = -1; subs.gps_pos_sub = -1;
subs.sensor_sub = -1; subs.sensor_sub = -1;
subs.att_sub = -1; subs.att_sub = -1;
subs.ctrl_state_sub = -1;
subs.att_sp_sub = -1; subs.att_sp_sub = -1;
subs.rates_sp_sub = -1; subs.rates_sp_sub = -1;
subs.act_outputs_sub = -1; subs.act_outputs_sub = -1;
@@ -1714,6 +1719,19 @@ int sdlog2_thread_main(int argc, char *argv[])
LOGBUFFER_WRITE_AND_COUNT(FLOW); LOGBUFFER_WRITE_AND_COUNT(FLOW);
} }
/* --- CONTROL STATE --- */
if (copy_if_updated(ORB_ID(control_state), &subs.ctrl_state_sub, &buf.ctrl_state)) {
log_msg.msg_type = LOG_CTS_MSG;
log_msg.body.log_CTS.vx_body = buf.ctrl_state.x_vel;
log_msg.body.log_CTS.vy_body = buf.ctrl_state.y_vel;
log_msg.body.log_CTS.vz_body = buf.ctrl_state.z_vel;
log_msg.body.log_CTS.airspeed = buf.ctrl_state.airspeed;
log_msg.body.log_CTS.roll_rate = buf.ctrl_state.roll_rate;
log_msg.body.log_CTS.pitch_rate = buf.ctrl_state.pitch_rate;
log_msg.body.log_CTS.yaw_rate = buf.ctrl_state.yaw_rate;
LOGBUFFER_WRITE_AND_COUNT(CTS);
}
/* --- RC CHANNELS --- */ /* --- RC CHANNELS --- */
if (copy_if_updated(ORB_ID(rc_channels), &subs.rc_sub, &buf.rc)) { if (copy_if_updated(ORB_ID(rc_channels), &subs.rc_sub, &buf.rc)) {
log_msg.msg_type = LOG_RC_MSG; log_msg.msg_type = LOG_RC_MSG;
src/modules/sdlog2/sdlog2_messages.h
+47 -34
View File
@@ -185,8 +185,20 @@ struct log_STAT_s {
float load; float load;
}; };
/* --- CONTROL STATE --- */
#define LOG_CTS_MSG 11
struct log_CTS_s {
float vx_body;
float vy_body;
float vz_body;
float airspeed;
float roll_rate;
float pitch_rate;
float yaw_rate;
};
/* --- RC - RC INPUT CHANNELS --- */ /* --- RC - RC INPUT CHANNELS --- */
#define LOG_RC_MSG 11 #define LOG_RC_MSG 12
struct log_RC_s { struct log_RC_s {
float channel[8]; float channel[8];
uint8_t channel_count; uint8_t channel_count;
@@ -194,13 +206,13 @@ struct log_RC_s {
}; };
/* --- OUT0 - ACTUATOR_0 OUTPUT --- */ /* --- OUT0 - ACTUATOR_0 OUTPUT --- */
#define LOG_OUT0_MSG 12 #define LOG_OUT0_MSG 13
struct log_OUT0_s { struct log_OUT0_s {
float output[8]; float output[8];
}; };
/* --- AIRS - AIRSPEED --- */ /* --- AIRS - AIRSPEED --- */
#define LOG_AIRS_MSG 13 #define LOG_AIRS_MSG 14
struct log_AIRS_s { struct log_AIRS_s {
float indicated_airspeed; float indicated_airspeed;
float true_airspeed; float true_airspeed;
@@ -208,7 +220,7 @@ struct log_AIRS_s {
}; };
/* --- ARSP - ATTITUDE RATE SET POINT --- */ /* --- ARSP - ATTITUDE RATE SET POINT --- */
#define LOG_ARSP_MSG 14 #define LOG_ARSP_MSG 15
struct log_ARSP_s { struct log_ARSP_s {
float roll_rate_sp; float roll_rate_sp;
float pitch_rate_sp; float pitch_rate_sp;
@@ -216,7 +228,7 @@ struct log_ARSP_s {
}; };
/* --- FLOW - OPTICAL FLOW --- */ /* --- FLOW - OPTICAL FLOW --- */
#define LOG_FLOW_MSG 15 #define LOG_FLOW_MSG 16
struct log_FLOW_s { struct log_FLOW_s {
uint8_t sensor_id; uint8_t sensor_id;
float pixel_flow_x_integral; float pixel_flow_x_integral;
@@ -233,7 +245,7 @@ struct log_FLOW_s {
}; };
/* --- GPOS - GLOBAL POSITION ESTIMATE --- */ /* --- GPOS - GLOBAL POSITION ESTIMATE --- */
#define LOG_GPOS_MSG 16 #define LOG_GPOS_MSG 17
struct log_GPOS_s { struct log_GPOS_s {
int32_t lat; int32_t lat;
int32_t lon; int32_t lon;
@@ -247,7 +259,7 @@ struct log_GPOS_s {
}; };
/* --- GPSP - GLOBAL POSITION SETPOINT --- */ /* --- GPSP - GLOBAL POSITION SETPOINT --- */
#define LOG_GPSP_MSG 17 #define LOG_GPSP_MSG 18
struct log_GPSP_s { struct log_GPSP_s {
uint8_t nav_state; uint8_t nav_state;
int32_t lat; int32_t lat;
@@ -261,7 +273,7 @@ struct log_GPSP_s {
}; };
/* --- ESC - ESC STATE --- */ /* --- ESC - ESC STATE --- */
#define LOG_ESC_MSG 18 #define LOG_ESC_MSG 19
struct log_ESC_s { struct log_ESC_s {
uint16_t counter; uint16_t counter;
uint8_t esc_count; uint8_t esc_count;
@@ -278,7 +290,7 @@ struct log_ESC_s {
}; };
/* --- GVSP - GLOBAL VELOCITY SETPOINT --- */ /* --- GVSP - GLOBAL VELOCITY SETPOINT --- */
#define LOG_GVSP_MSG 19 #define LOG_GVSP_MSG 20
struct log_GVSP_s { struct log_GVSP_s {
float vx; float vx;
float vy; float vy;
@@ -286,7 +298,7 @@ struct log_GVSP_s {
}; };
/* --- BATT - BATTERY --- */ /* --- BATT - BATTERY --- */
#define LOG_BATT_MSG 20 #define LOG_BATT_MSG 21
struct log_BATT_s { struct log_BATT_s {
float voltage; float voltage;
float voltage_filtered; float voltage_filtered;
@@ -295,7 +307,7 @@ struct log_BATT_s {
}; };
/* --- DIST - RANGE SENSOR DISTANCE --- */ /* --- DIST - RANGE SENSOR DISTANCE --- */
#define LOG_DIST_MSG 21 #define LOG_DIST_MSG 22
struct log_DIST_s { struct log_DIST_s {
uint8_t id; uint8_t id;
uint8_t type; uint8_t type;
@@ -304,11 +316,11 @@ struct log_DIST_s {
float covariance; float covariance;
}; };
/* LOG IMU1 and IMU2 MSGs consume IDs 22 and 23 */ /* LOG IMU1 and IMU2 MSGs consume IDs 23 and 24 */
/* --- PWR - ONBOARD POWER SYSTEM --- */ /* --- PWR - ONBOARD POWER SYSTEM --- */
#define LOG_PWR_MSG 24 #define LOG_PWR_MSG 25
struct log_PWR_s { struct log_PWR_s {
float peripherals_5v; float peripherals_5v;
float servo_rail_5v; float servo_rail_5v;
@@ -321,7 +333,7 @@ struct log_PWR_s {
}; };
/* --- MOCP - MOCAP ATTITUDE AND POSITION --- */ /* --- MOCP - MOCAP ATTITUDE AND POSITION --- */
#define LOG_MOCP_MSG 25 #define LOG_MOCP_MSG 26
struct log_MOCP_s { struct log_MOCP_s {
float qw; float qw;
float qx; float qx;
@@ -333,31 +345,31 @@ struct log_MOCP_s {
}; };
/* --- GS0A - GPS SNR #0, SAT GROUP A --- */ /* --- GS0A - GPS SNR #0, SAT GROUP A --- */
#define LOG_GS0A_MSG 26 #define LOG_GS0A_MSG 27
struct log_GS0A_s { struct log_GS0A_s {
uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */ uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */
}; };
/* --- GS0B - GPS SNR #0, SAT GROUP B --- */ /* --- GS0B - GPS SNR #0, SAT GROUP B --- */
#define LOG_GS0B_MSG 27 #define LOG_GS0B_MSG 28
struct log_GS0B_s { struct log_GS0B_s {
uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */ uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */
}; };
/* --- GS1A - GPS SNR #1, SAT GROUP A --- */ /* --- GS1A - GPS SNR #1, SAT GROUP A --- */
#define LOG_GS1A_MSG 28 #define LOG_GS1A_MSG 29
struct log_GS1A_s { struct log_GS1A_s {
uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */ uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */
}; };
/* --- GS1B - GPS SNR #1, SAT GROUP B --- */ /* --- GS1B - GPS SNR #1, SAT GROUP B --- */
#define LOG_GS1B_MSG 29 #define LOG_GS1B_MSG 30
struct log_GS1B_s { struct log_GS1B_s {
uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */ uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */
}; };
/* --- TECS - TECS STATUS --- */ /* --- TECS - TECS STATUS --- */
#define LOG_TECS_MSG 30 #define LOG_TECS_MSG 31
struct log_TECS_s { struct log_TECS_s {
float altitudeSp; float altitudeSp;
float altitudeFiltered; float altitudeFiltered;
@@ -378,7 +390,7 @@ struct log_TECS_s {
}; };
/* --- WIND - WIND ESTIMATE --- */ /* --- WIND - WIND ESTIMATE --- */
#define LOG_WIND_MSG 31 #define LOG_WIND_MSG 32
struct log_WIND_s { struct log_WIND_s {
float x; float x;
float y; float y;
@@ -387,7 +399,7 @@ struct log_WIND_s {
}; };
/* --- EST0 - ESTIMATOR STATUS --- */ /* --- EST0 - ESTIMATOR STATUS --- */
#define LOG_EST0_MSG 32 #define LOG_EST0_MSG 33
struct log_EST0_s { struct log_EST0_s {
float s[12]; float s[12];
uint8_t n_states; uint8_t n_states;
@@ -397,28 +409,28 @@ struct log_EST0_s {
}; };
/* --- EST1 - ESTIMATOR STATUS --- */ /* --- EST1 - ESTIMATOR STATUS --- */
#define LOG_EST1_MSG 33 #define LOG_EST1_MSG 34
struct log_EST1_s { struct log_EST1_s {
float s[16]; float s[16];
}; };
/* --- EST2 - ESTIMATOR STATUS --- */ /* --- EST2 - ESTIMATOR STATUS --- */
#define LOG_EST2_MSG 34 #define LOG_EST2_MSG 35
struct log_EST2_s { struct log_EST2_s {
float cov[12]; float cov[12];
}; };
/* --- EST3 - ESTIMATOR STATUS --- */ /* --- EST3 - ESTIMATOR STATUS --- */
#define LOG_EST3_MSG 35 #define LOG_EST3_MSG 36
struct log_EST3_s { struct log_EST3_s {
float cov[16]; float cov[16];
}; };
/* --- TEL0..3 - TELEMETRY STATUS --- */ /* --- TEL0..3 - TELEMETRY STATUS --- */
#define LOG_TEL0_MSG 36 #define LOG_TEL0_MSG 37
#define LOG_TEL1_MSG 37 #define LOG_TEL1_MSG 38
#define LOG_TEL2_MSG 38 #define LOG_TEL2_MSG 39
#define LOG_TEL3_MSG 39 #define LOG_TEL3_MSG 40
struct log_TEL_s { struct log_TEL_s {
uint8_t rssi; uint8_t rssi;
uint8_t remote_rssi; uint8_t remote_rssi;
@@ -431,7 +443,7 @@ struct log_TEL_s {
}; };
/* --- VISN - VISION POSITION --- */ /* --- VISN - VISION POSITION --- */
#define LOG_VISN_MSG 40 #define LOG_VISN_MSG 41
struct log_VISN_s { struct log_VISN_s {
float x; float x;
float y; float y;
@@ -446,7 +458,7 @@ struct log_VISN_s {
}; };
/* --- ENCODERS - ENCODER DATA --- */ /* --- ENCODERS - ENCODER DATA --- */
#define LOG_ENCD_MSG 41 #define LOG_ENCD_MSG 42
struct log_ENCD_s { struct log_ENCD_s {
int64_t cnt0; int64_t cnt0;
float vel0; float vel0;
@@ -455,22 +467,22 @@ struct log_ENCD_s {
}; };
/* --- AIR SPEED SENSORS - DIFF. PRESSURE --- */ /* --- AIR SPEED SENSORS - DIFF. PRESSURE --- */
#define LOG_AIR1_MSG 42 #define LOG_AIR1_MSG 43
/* --- VTOL - VTOL VEHICLE STATUS */ /* --- VTOL - VTOL VEHICLE STATUS */
#define LOG_VTOL_MSG 43 #define LOG_VTOL_MSG 44
struct log_VTOL_s { struct log_VTOL_s {
float airspeed_tot; float airspeed_tot;
}; };
/* --- TIMESYNC - TIME SYNCHRONISATION OFFSET */ /* --- TIMESYNC - TIME SYNCHRONISATION OFFSET */
#define LOG_TSYN_MSG 44 #define LOG_TSYN_MSG 45
struct log_TSYN_s { struct log_TSYN_s {
uint64_t time_offset; uint64_t time_offset;
}; };
/* --- MACS - MULTIROTOR ATTITUDE CONTROLLER STATUS */ /* --- MACS - MULTIROTOR ATTITUDE CONTROLLER STATUS */
#define LOG_MACS_MSG 45 #define LOG_MACS_MSG 47
struct log_MACS_s { struct log_MACS_s {
float roll_rate_integ; float roll_rate_integ;
float pitch_rate_integ; float pitch_rate_integ;
@@ -519,6 +531,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT_S(ATC1, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), LOG_FORMAT_S(ATC1, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
LOG_FORMAT(STAT, "BBBfBBf", "MainState,ArmS,Failsafe,BatRem,BatWarn,Landed,Load"), LOG_FORMAT(STAT, "BBBfBBf", "MainState,ArmS,Failsafe,BatRem,BatWarn,Landed,Load"),
LOG_FORMAT(VTOL, "f", "Arsp"), LOG_FORMAT(VTOL, "f", "Arsp"),
LOG_FORMAT(CTS, "fffffff", "Vx_b,Vy_b,Vz_b,Vinf,P,Q,R"),
LOG_FORMAT(RC, "ffffffffBB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count,SignalLost"), LOG_FORMAT(RC, "ffffffffBB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count,SignalLost"),
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, "fff", "IndSpeed,TrueSpeed,AirTemp"), LOG_FORMAT(AIRS, "fff", "IndSpeed,TrueSpeed,AirTemp"),