fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-30 05:43:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

log secondary attitude and fixed wing controls for VTOL

Browse Source
This commit is contained in:
Roman Bapst
2014-12-02 10:33:43 +01:00
parent 35c985e523
commit ad204bbb5d
2 changed files with 37 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sdlog2/sdlog2.c
+25
View File
@@ -941,6 +941,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct vehicle_rates_setpoint_s rates_sp;
struct actuator_outputs_s act_outputs;
struct actuator_controls_s act_controls;
struct actuator_controls_s act_controls1;
struct vehicle_local_position_s local_pos;
struct vehicle_local_position_setpoint_s local_pos_sp;
struct vehicle_global_position_s global_pos;
@@ -981,6 +982,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct log_LPSP_s log_LPSP;
struct log_GPS_s log_GPS;
struct log_ATTC_s log_ATTC;
struct log_FWC_s log_FWC;
struct log_STAT_s log_STAT;
struct log_RC_s log_RC;
struct log_OUT0_s log_OUT0;
@@ -1022,6 +1024,7 @@ int sdlog2_thread_main(int argc, char *argv[])
int rates_sp_sub;
int act_outputs_sub;
int act_controls_sub;
int act_controls_1_sub;
int local_pos_sub;
int local_pos_sp_sub;
int global_pos_sub;
@@ -1055,6 +1058,7 @@ int sdlog2_thread_main(int argc, char *argv[])
subs.rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
subs.act_outputs_sub = orb_subscribe(ORB_ID_VEHICLE_CONTROLS);
subs.act_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
subs.act_controls_1_sub = orb_subscribe(ORB_ID(actuator_controls_1));
subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
subs.local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
@@ -1375,6 +1379,18 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_ATT.gy = buf.att.g_comp[1];
log_msg.body.log_ATT.gz = buf.att.g_comp[2];
LOGBUFFER_WRITE_AND_COUNT(ATT);
// secondary attitude
log_msg.msg_type = LOG_ATT2_MSG;
log_msg.body.log_ATT.roll = buf.att.roll_sec;
log_msg.body.log_ATT.pitch = buf.att.pitch_sec;
log_msg.body.log_ATT.yaw = buf.att.yaw_sec;
log_msg.body.log_ATT.roll_rate = buf.att.rollspeed_sec;
log_msg.body.log_ATT.pitch_rate = buf.att.pitchspeed_sec;
log_msg.body.log_ATT.yaw_rate = buf.att.yawspeed_sec;
log_msg.body.log_ATT.gx = buf.att.g_comp_sec[0];
log_msg.body.log_ATT.gy = buf.att.g_comp_sec[1];
log_msg.body.log_ATT.gz = buf.att.g_comp_sec[2];
LOGBUFFER_WRITE_AND_COUNT(ATT);
}
/* --- ATTITUDE SETPOINT --- */
@@ -1413,6 +1429,15 @@ int sdlog2_thread_main(int argc, char *argv[])
LOGBUFFER_WRITE_AND_COUNT(ATTC);
}
if(copy_if_updated(ORB_ID(actuator_controls_1),subs.act_controls_1_sub,&buf.act_controls1)) {
log_msg.msg_type = LOG_FWC_MSG;
log_msg.body.log_FWC.roll = buf.act_controls1.control[0];
log_msg.body.log_FWC.pitch = buf.act_controls1.control[1];
log_msg.body.log_FWC.yaw = buf.act_controls1.control[2];
log_msg.body.log_FWC.thrust = buf.act_controls1.control[3];
LOGBUFFER_WRITE_AND_COUNT(FWC);
}
/* --- LOCAL POSITION --- */
if (copy_if_updated(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos)) {
log_msg.msg_type = LOG_LPOS_MSG;