fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-03 05:30:34 +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/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_rates_setpoint.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 sensor_combined_s sensor;
struct vehicle_attitude_s att;
struct control_state_s ctrl_state;
struct vehicle_attitude_setpoint_s att_sp;
struct vehicle_rates_setpoint_s rates_sp;
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_STAT_s log_STAT;
struct log_VTOL_s log_VTOL;
struct log_CTS_s log_CTS;
struct log_RC_s log_RC;
struct log_OUT0_s log_OUT0;
struct log_AIRS_s log_AIRS;
@@ -1199,6 +1202,7 @@ int sdlog2_thread_main(int argc, char *argv[])
int vtol_status_sub;
int sensor_sub;
int att_sub;
int ctrl_state_sub;
int att_sp_sub;
int rates_sp_sub;
int act_outputs_sub;
@@ -1236,6 +1240,7 @@ int sdlog2_thread_main(int argc, char *argv[])
subs.gps_pos_sub = -1;
subs.sensor_sub = -1;
subs.att_sub = -1;
subs.ctrl_state_sub = -1;
subs.att_sp_sub = -1;
subs.rates_sp_sub = -1;
subs.act_outputs_sub = -1;
@@ -1714,6 +1719,19 @@ int sdlog2_thread_main(int argc, char *argv[])
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 --- */
if (copy_if_updated(ORB_ID(rc_channels), &subs.rc_sub, &buf.rc)) {
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;
};
/* --- 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 --- */
#define LOG_RC_MSG 11
#define LOG_RC_MSG 12
struct log_RC_s {
float channel[8];
uint8_t channel_count;
@@ -194,13 +206,13 @@ struct log_RC_s {
};
/* --- OUT0 - ACTUATOR_0 OUTPUT --- */
#define LOG_OUT0_MSG 12
#define LOG_OUT0_MSG 13
struct log_OUT0_s {
float output[8];
};
/* --- AIRS - AIRSPEED --- */
#define LOG_AIRS_MSG 13
#define LOG_AIRS_MSG 14
struct log_AIRS_s {
float indicated_airspeed;
float true_airspeed;
@@ -208,7 +220,7 @@ struct log_AIRS_s {
};
/* --- ARSP - ATTITUDE RATE SET POINT --- */
#define LOG_ARSP_MSG 14
#define LOG_ARSP_MSG 15
struct log_ARSP_s {
float roll_rate_sp;
float pitch_rate_sp;
@@ -216,7 +228,7 @@ struct log_ARSP_s {
};
/* --- FLOW - OPTICAL FLOW --- */
#define LOG_FLOW_MSG 15
#define LOG_FLOW_MSG 16
struct log_FLOW_s {
uint8_t sensor_id;
float pixel_flow_x_integral;
@@ -233,7 +245,7 @@ struct log_FLOW_s {
};
/* --- GPOS - GLOBAL POSITION ESTIMATE --- */
#define LOG_GPOS_MSG 16
#define LOG_GPOS_MSG 17
struct log_GPOS_s {
int32_t lat;
int32_t lon;
@@ -247,7 +259,7 @@ struct log_GPOS_s {
};
/* --- GPSP - GLOBAL POSITION SETPOINT --- */
#define LOG_GPSP_MSG 17
#define LOG_GPSP_MSG 18
struct log_GPSP_s {
uint8_t nav_state;
int32_t lat;
@@ -261,7 +273,7 @@ struct log_GPSP_s {
};
/* --- ESC - ESC STATE --- */
#define LOG_ESC_MSG 18
#define LOG_ESC_MSG 19
struct log_ESC_s {
uint16_t counter;
uint8_t esc_count;
@@ -278,7 +290,7 @@ struct log_ESC_s {
};
/* --- GVSP - GLOBAL VELOCITY SETPOINT --- */
#define LOG_GVSP_MSG 19
#define LOG_GVSP_MSG 20
struct log_GVSP_s {
float vx;
float vy;
@@ -286,7 +298,7 @@ struct log_GVSP_s {
};
/* --- BATT - BATTERY --- */
#define LOG_BATT_MSG 20
#define LOG_BATT_MSG 21
struct log_BATT_s {
float voltage;
float voltage_filtered;
@@ -295,7 +307,7 @@ struct log_BATT_s {
};
/* --- DIST - RANGE SENSOR DISTANCE --- */
#define LOG_DIST_MSG 21
#define LOG_DIST_MSG 22
struct log_DIST_s {
uint8_t id;
uint8_t type;
@@ -304,11 +316,11 @@ struct log_DIST_s {
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 --- */
#define LOG_PWR_MSG 24
#define LOG_PWR_MSG 25
struct log_PWR_s {
float peripherals_5v;
float servo_rail_5v;
@@ -321,7 +333,7 @@ struct log_PWR_s {
};
/* --- MOCP - MOCAP ATTITUDE AND POSITION --- */
#define LOG_MOCP_MSG 25
#define LOG_MOCP_MSG 26
struct log_MOCP_s {
float qw;
float qx;
@@ -333,31 +345,31 @@ struct log_MOCP_s {
};
/* --- GS0A - GPS SNR #0, SAT GROUP A --- */
#define LOG_GS0A_MSG 26
#define LOG_GS0A_MSG 27
struct log_GS0A_s {
uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */
};
/* --- GS0B - GPS SNR #0, SAT GROUP B --- */
#define LOG_GS0B_MSG 27
#define LOG_GS0B_MSG 28
struct log_GS0B_s {
uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */
};
/* --- GS1A - GPS SNR #1, SAT GROUP A --- */
#define LOG_GS1A_MSG 28
#define LOG_GS1A_MSG 29
struct log_GS1A_s {
uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */
};
/* --- GS1B - GPS SNR #1, SAT GROUP B --- */
#define LOG_GS1B_MSG 29
#define LOG_GS1B_MSG 30
struct log_GS1B_s {
uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */
};
/* --- TECS - TECS STATUS --- */
#define LOG_TECS_MSG 30
#define LOG_TECS_MSG 31
struct log_TECS_s {
float altitudeSp;
float altitudeFiltered;
@@ -378,7 +390,7 @@ struct log_TECS_s {
};
/* --- WIND - WIND ESTIMATE --- */
#define LOG_WIND_MSG 31
#define LOG_WIND_MSG 32
struct log_WIND_s {
float x;
float y;
@@ -387,7 +399,7 @@ struct log_WIND_s {
};
/* --- EST0 - ESTIMATOR STATUS --- */
#define LOG_EST0_MSG 32
#define LOG_EST0_MSG 33
struct log_EST0_s {
float s[12];
uint8_t n_states;
@@ -397,28 +409,28 @@ struct log_EST0_s {
};
/* --- EST1 - ESTIMATOR STATUS --- */
#define LOG_EST1_MSG 33
#define LOG_EST1_MSG 34
struct log_EST1_s {
float s[16];
};
/* --- EST2 - ESTIMATOR STATUS --- */
#define LOG_EST2_MSG 34
#define LOG_EST2_MSG 35
struct log_EST2_s {
float cov[12];
};
/* --- EST3 - ESTIMATOR STATUS --- */
#define LOG_EST3_MSG 35
#define LOG_EST3_MSG 36
struct log_EST3_s {
float cov[16];
};
/* --- TEL0..3 - TELEMETRY STATUS --- */
#define LOG_TEL0_MSG 36
#define LOG_TEL1_MSG 37
#define LOG_TEL2_MSG 38
#define LOG_TEL3_MSG 39
#define LOG_TEL0_MSG 37
#define LOG_TEL1_MSG 38
#define LOG_TEL2_MSG 39
#define LOG_TEL3_MSG 40
struct log_TEL_s {
uint8_t rssi;
uint8_t remote_rssi;
@@ -431,7 +443,7 @@ struct log_TEL_s {
};
/* --- VISN - VISION POSITION --- */
#define LOG_VISN_MSG 40
#define LOG_VISN_MSG 41
struct log_VISN_s {
float x;
float y;
@@ -446,7 +458,7 @@ struct log_VISN_s {
};
/* --- ENCODERS - ENCODER DATA --- */
#define LOG_ENCD_MSG 41
#define LOG_ENCD_MSG 42
struct log_ENCD_s {
int64_t cnt0;
float vel0;
@@ -455,22 +467,22 @@ struct log_ENCD_s {
};
/* --- AIR SPEED SENSORS - DIFF. PRESSURE --- */
#define LOG_AIR1_MSG 42
#define LOG_AIR1_MSG 43
/* --- VTOL - VTOL VEHICLE STATUS */
#define LOG_VTOL_MSG 43
#define LOG_VTOL_MSG 44
struct log_VTOL_s {
float airspeed_tot;
};
/* --- TIMESYNC - TIME SYNCHRONISATION OFFSET */
#define LOG_TSYN_MSG 44
#define LOG_TSYN_MSG 45
struct log_TSYN_s {
uint64_t time_offset;
};
/* --- MACS - MULTIROTOR ATTITUDE CONTROLLER STATUS */
#define LOG_MACS_MSG 45
#define LOG_MACS_MSG 47
struct log_MACS_s {
float roll_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(STAT, "BBBfBBf", "MainState,ArmS,Failsafe,BatRem,BatWarn,Landed,Load"),
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(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
LOG_FORMAT(AIRS, "fff", "IndSpeed,TrueSpeed,AirTemp"),