aboutsummaryrefslogtreecommitdiff
path: root/src/modules/sdlog2
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/sdlog2')
-rw-r--r--src/modules/sdlog2/sdlog2.c98
-rw-r--r--src/modules/sdlog2/sdlog2_messages.h33
2 files changed, 107 insertions, 24 deletions
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index ba7cdd91c..fefa539f9 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -60,6 +60,7 @@
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
+#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
@@ -75,6 +76,7 @@
#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/vehicle_control_debug.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/differential_pressure.h>
@@ -192,7 +194,7 @@ static int file_copy(const char *file_old, const char *file_new);
static void handle_command(struct vehicle_command_s *cmd);
-static void handle_status(struct vehicle_status_s *cmd);
+static void handle_status(struct actuator_armed_s *armed);
/**
* Create folder for current logging session. Store folder name in 'log_folder'.
@@ -623,9 +625,10 @@ int sdlog2_thread_main(int argc, char *argv[])
errx(1, "can't allocate log buffer, exiting.");
}
- /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
- /* number of messages */
- const ssize_t fdsc = 19;
+ /* --- IMPORTANT: DEFINE MAX NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
+ /* max number of messages */
+ const ssize_t fdsc = 21;
+
/* Sanity check variable and index */
ssize_t fdsc_count = 0;
/* file descriptors to wait for */
@@ -634,6 +637,9 @@ int sdlog2_thread_main(int argc, char *argv[])
struct vehicle_status_s buf_status;
memset(&buf_status, 0, sizeof(buf_status));
+ struct actuator_armed_s buf_armed;
+ memset(&buf_armed, 0, sizeof(buf_armed));
+
/* warning! using union here to save memory, elements should be used separately! */
union {
struct vehicle_command_s cmd;
@@ -650,6 +656,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct vehicle_global_position_setpoint_s global_pos_sp;
struct vehicle_gps_position_s gps_pos;
struct vehicle_vicon_position_s vicon_pos;
+ struct vehicle_control_debug_s control_debug;
struct optical_flow_s flow;
struct rc_channels_s rc;
struct differential_pressure_s diff_pres;
@@ -661,6 +668,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct {
int cmd_sub;
int status_sub;
+ int armed_sub;
int sensor_sub;
int att_sub;
int att_sp_sub;
@@ -674,6 +682,7 @@ int sdlog2_thread_main(int argc, char *argv[])
int global_pos_sp_sub;
int gps_pos_sub;
int vicon_pos_sub;
+ int control_debug_sub;
int flow_sub;
int rc_sub;
int airspeed_sub;
@@ -695,6 +704,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct log_GPS_s log_GPS;
struct log_ATTC_s log_ATTC;
struct log_STAT_s log_STAT;
+ struct log_CTRL_s log_CTRL;
struct log_RC_s log_RC;
struct log_OUT0_s log_OUT0;
struct log_AIRS_s log_AIRS;
@@ -716,6 +726,12 @@ int sdlog2_thread_main(int argc, char *argv[])
fds[fdsc_count].events = POLLIN;
fdsc_count++;
+ /* --- ACTUATOR ARMED --- */
+ subs.armed_sub = orb_subscribe(ORB_ID(actuator_armed));
+ fds[fdsc_count].fd = subs.armed_sub;
+ fds[fdsc_count].events = POLLIN;
+ fdsc_count++;
+
/* --- VEHICLE STATUS --- */
subs.status_sub = orb_subscribe(ORB_ID(vehicle_status));
fds[fdsc_count].fd = subs.status_sub;
@@ -800,6 +816,12 @@ int sdlog2_thread_main(int argc, char *argv[])
fds[fdsc_count].events = POLLIN;
fdsc_count++;
+ /* --- CONTROL DEBUG --- */
+ subs.control_debug_sub = orb_subscribe(ORB_ID(vehicle_control_debug));
+ fds[fdsc_count].fd = subs.control_debug_sub;
+ fds[fdsc_count].events = POLLIN;
+ fdsc_count++;
+
/* --- OPTICAL FLOW --- */
subs.flow_sub = orb_subscribe(ORB_ID(optical_flow));
fds[fdsc_count].fd = subs.flow_sub;
@@ -883,22 +905,33 @@ int sdlog2_thread_main(int argc, char *argv[])
handled_topics++;
}
- /* --- VEHICLE STATUS - LOG MANAGEMENT --- */
+ /* --- ARMED- LOG MANAGEMENT --- */
if (fds[ifds++].revents & POLLIN) {
- orb_copy(ORB_ID(vehicle_status), subs.status_sub, &buf_status);
+ orb_copy(ORB_ID(actuator_armed), subs.armed_sub, &buf_armed);
if (log_when_armed) {
- handle_status(&buf_status);
+ handle_status(&buf_armed);
}
handled_topics++;
}
+ /* --- VEHICLE STATUS - LOG MANAGEMENT --- */
+ if (fds[ifds++].revents & POLLIN) {
+ orb_copy(ORB_ID(vehicle_status), subs.status_sub, &buf_status);
+
+ //if (log_when_armed) {
+ // handle_status(&buf_armed);
+ //}
+
+ //handled_topics++;
+ }
+
if (!logging_enabled || !check_data || handled_topics >= poll_ret) {
continue;
}
- ifds = 1; // Begin from fds[1] again
+ ifds = 2; // Begin from fds[2] again
pthread_mutex_lock(&logbuffer_mutex);
@@ -911,13 +944,18 @@ int sdlog2_thread_main(int argc, char *argv[])
if (fds[ifds++].revents & POLLIN) {
// Don't orb_copy, it's already done few lines above
log_msg.msg_type = LOG_STAT_MSG;
- log_msg.body.log_STAT.state = (unsigned char) buf_status.state_machine;
- log_msg.body.log_STAT.flight_mode = (unsigned char) buf_status.flight_mode;
- log_msg.body.log_STAT.manual_control_mode = (unsigned char) buf_status.manual_control_mode;
- log_msg.body.log_STAT.manual_sas_mode = (unsigned char) buf_status.manual_sas_mode;
- log_msg.body.log_STAT.armed = (unsigned char) buf_status.flag_system_armed;
- log_msg.body.log_STAT.battery_voltage = buf_status.voltage_battery;
- log_msg.body.log_STAT.battery_current = buf_status.current_battery;
+ // XXX fix this
+ // log_msg.body.log_STAT.state = (unsigned char) buf_status.state_machine;
+ // log_msg.body.log_STAT.flight_mode = (unsigned char) buf_status.flight_mode;
+ // log_msg.body.log_STAT.manual_control_mode = (unsigned char) buf_status.manual_control_mode;
+ // log_msg.body.log_STAT.manual_sas_mode = (unsigned char) buf_status.manual_sas_mode;
+ log_msg.body.log_STAT.state = 0;
+ log_msg.body.log_STAT.flight_mode = 0;
+ log_msg.body.log_STAT.manual_control_mode = 0;
+ log_msg.body.log_STAT.manual_sas_mode = 0;
+ log_msg.body.log_STAT.armed = (unsigned char) buf_armed.armed; /* XXX fmu armed correct? */
+ log_msg.body.log_STAT.battery_voltage = buf_status.battery_voltage;
+ log_msg.body.log_STAT.battery_current = buf_status.battery_current;
log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining;
log_msg.body.log_STAT.battery_warning = (unsigned char) buf_status.battery_warning;
LOGBUFFER_WRITE_AND_COUNT(STAT);
@@ -1006,6 +1044,9 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_ATT.roll_rate = buf.att.rollspeed;
log_msg.body.log_ATT.pitch_rate = buf.att.pitchspeed;
log_msg.body.log_ATT.yaw_rate = buf.att.yawspeed;
+ log_msg.body.log_ATT.roll_acc = buf.att.rollacc;
+ log_msg.body.log_ATT.pitch_acc = buf.att.pitchacc;
+ log_msg.body.log_ATT.yaw_acc = buf.att.yawacc;
LOGBUFFER_WRITE_AND_COUNT(ATT);
}
@@ -1121,6 +1162,27 @@ int sdlog2_thread_main(int argc, char *argv[])
// TODO not implemented yet
}
+ /* --- CONTROL DEBUG --- */
+ if (fds[ifds++].revents & POLLIN) {
+ orb_copy(ORB_ID(vehicle_control_debug), subs.control_debug_sub, &buf.control_debug);
+
+ log_msg.msg_type = LOG_CTRL_MSG;
+ log_msg.body.log_CTRL.roll_p = buf.control_debug.roll_p;
+ log_msg.body.log_CTRL.roll_i = buf.control_debug.roll_i;
+ log_msg.body.log_CTRL.roll_d = buf.control_debug.roll_d;
+ log_msg.body.log_CTRL.roll_rate_p = buf.control_debug.roll_rate_p;
+ log_msg.body.log_CTRL.roll_rate_i = buf.control_debug.roll_rate_i;
+ log_msg.body.log_CTRL.roll_rate_d = buf.control_debug.roll_rate_d;
+ log_msg.body.log_CTRL.pitch_p = buf.control_debug.pitch_p;
+ log_msg.body.log_CTRL.pitch_i = buf.control_debug.pitch_i;
+ log_msg.body.log_CTRL.pitch_d = buf.control_debug.pitch_d;
+ log_msg.body.log_CTRL.pitch_rate_p = buf.control_debug.pitch_rate_p;
+ log_msg.body.log_CTRL.pitch_rate_i = buf.control_debug.pitch_rate_i;
+ log_msg.body.log_CTRL.pitch_rate_d = buf.control_debug.pitch_rate_d;
+
+ LOGBUFFER_WRITE_AND_COUNT(CTRL);
+ }
+
/* --- FLOW --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(optical_flow), subs.flow_sub, &buf.flow);
@@ -1295,10 +1357,10 @@ void handle_command(struct vehicle_command_s *cmd)
}
}
-void handle_status(struct vehicle_status_s *status)
+void handle_status(struct actuator_armed_s *armed)
{
- if (status->flag_system_armed != flag_system_armed) {
- flag_system_armed = status->flag_system_armed;
+ if (armed->armed != flag_system_armed) {
+ flag_system_armed = armed->armed;
if (flag_system_armed) {
sdlog2_start_log();
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index 934e4dec8..9be96a62e 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -63,6 +63,9 @@ struct log_ATT_s {
float roll_rate;
float pitch_rate;
float yaw_rate;
+ float roll_acc;
+ float pitch_acc;
+ float yaw_acc;
};
/* --- ATSP - ATTITUDE SET POINT --- */
@@ -160,27 +163,44 @@ struct log_STAT_s {
uint8_t battery_warning;
};
+/* --- CTRL - CONTROL DEBUG --- */
+#define LOG_CTRL_MSG 11
+struct log_CTRL_s {
+ float roll_p;
+ float roll_i;
+ float roll_d;
+ float roll_rate_p;
+ float roll_rate_i;
+ float roll_rate_d;
+ float pitch_p;
+ float pitch_i;
+ float pitch_d;
+ float pitch_rate_p;
+ float pitch_rate_i;
+ float pitch_rate_d;
+};
+
/* --- RC - RC INPUT CHANNELS --- */
-#define LOG_RC_MSG 11
+#define LOG_RC_MSG 12
struct log_RC_s {
float channel[8];
};
/* --- 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;
};
/* --- 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;
@@ -188,7 +208,7 @@ struct log_ARSP_s {
};
/* --- FLOW - OPTICAL FLOW --- */
-#define LOG_FLOW_MSG 15
+#define LOG_FLOW_MSG 16
struct log_FLOW_s {
int16_t flow_raw_x;
int16_t flow_raw_y;
@@ -250,7 +270,7 @@ struct log_ESC_s {
static const struct log_format_s log_formats[] = {
LOG_FORMAT(TIME, "Q", "StartTime"),
- LOG_FORMAT(ATT, "ffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate"),
+ LOG_FORMAT(ATT, "fffffffff", "Roll,Pitch,Yaw,RollR,PitchR,YawR,RollA,PitchA,YawA"),
LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"),
LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
LOG_FORMAT(SENS, "ffff", "BaroPres,BaroAlt,BaroTemp,DiffPres"),
@@ -259,6 +279,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"),
LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
LOG_FORMAT(STAT, "BBBBBfffB", "State,FlightMode,CtlMode,SASMode,Armed,BatV,BatC,BatRem,BatWarn"),
+ LOG_FORMAT(CTRL, "ffffffffffff", "RP,RI,RD,RRP,RRI,RRD,PP,PI,PD,PRP,PRI,PRD"),
LOG_FORMAT(RC, "ffffffff", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7"),
LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"),
generated by cgit v1.2.3 (git 2.39.1) at 2024-08-03 15:12:27 +0000