From ad204bbb5d2bab4431b15281fde693e6bf03a0fd Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 2 Dec 2014 10:33:43 +0100 Subject: log secondary attitude and fixed wing controls for VTOL --- src/modules/sdlog2/sdlog2.c | 25 +++++++++++++++++++++++++ src/modules/sdlog2/sdlog2_messages.h | 13 ++++++++++++- 2 files changed, 37 insertions(+), 1 deletion(-) (limited to 'src/modules/sdlog2') diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 0b6861d2a..8f87d897a 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -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; diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 30491036a..447b3fb9c 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -50,6 +50,7 @@ #pragma pack(push, 1) /* --- ATT - ATTITUDE --- */ #define LOG_ATT_MSG 2 +#define LOG_ATT2_MSG 41 struct log_ATT_s { float roll; float pitch; @@ -422,6 +423,14 @@ struct log_ENCD_s { float vel1; }; +/* ATTITUDE CONTROLS (ACTUATOR_1 CONTROLS) */ +#define LOG_FWC_MSG 40 +struct log_FWC_s { + float roll; + float pitch; + float yaw; + float thrust; +}; /********** SYSTEM MESSAGES, ID > 0x80 **********/ @@ -450,7 +459,8 @@ struct log_PARM_s { /* construct list of all message formats */ static const struct log_format_s log_formats[] = { /* business-level messages, ID < 0x80 */ - LOG_FORMAT(ATT, "fffffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"), + LOG_FORMAT_S(ATT, ATT, "fffffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"), + LOG_FORMAT_S(ATT2, ATT, "fffffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"), LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), LOG_FORMAT_S(IMU, IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), LOG_FORMAT_S(IMU1, IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), @@ -460,6 +470,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), LOG_FORMAT(GPS, "QBffLLfffffBHHH", "GPSTime,Fix,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog,nSat,SNR,N,J"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), + LOG_FORMAT(FWC,"ffff", "Roll,Pitch,Yaw,Thrust"), LOG_FORMAT(STAT, "BBBfBB", "MainState,ArmState,FailsafeState,BatRem,BatWarn,Landed"), 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"), -- cgit v1.2.3