aboutsummaryrefslogtreecommitdiff
path: root/src/modules/sdlog2
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/sdlog2')
-rw-r--r--src/modules/sdlog2/sdlog2.c67
-rw-r--r--src/modules/sdlog2/sdlog2_messages.h49
2 files changed, 107 insertions, 9 deletions
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 31da81f5c..80ee3adee 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -73,6 +73,7 @@
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_global_position.h>
+#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>
@@ -81,6 +82,7 @@
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/rc_channels.h>
+#include <uORB/topics/esc_status.h>
#include <systemlib/systemlib.h>
@@ -614,9 +616,9 @@ 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 = 20;
+ /* --- 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;
@@ -642,6 +644,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct vehicle_local_position_s local_pos;
struct vehicle_local_position_setpoint_s local_pos_sp;
struct vehicle_global_position_s global_pos;
+ 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;
@@ -649,6 +652,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct rc_channels_s rc;
struct differential_pressure_s diff_pres;
struct airspeed_s airspeed;
+ struct esc_status_s esc;
} buf;
memset(&buf, 0, sizeof(buf));
@@ -666,12 +670,14 @@ int sdlog2_thread_main(int argc, char *argv[])
int local_pos_sub;
int local_pos_sp_sub;
int global_pos_sub;
+ 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;
+ int esc_sub;
} subs;
/* log message buffer: header + body */
@@ -696,6 +702,8 @@ int sdlog2_thread_main(int argc, char *argv[])
struct log_ARSP_s log_ARSP;
struct log_FLOW_s log_FLOW;
struct log_GPOS_s log_GPOS;
+ struct log_GPSP_s log_GPSP;
+ struct log_ESC_s log_ESC;
} body;
} log_msg = {
LOG_PACKET_HEADER_INIT(0)
@@ -787,6 +795,12 @@ int sdlog2_thread_main(int argc, char *argv[])
fds[fdsc_count].events = POLLIN;
fdsc_count++;
+ /* --- GLOBAL POSITION SETPOINT--- */
+ subs.global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
+ fds[fdsc_count].fd = subs.global_pos_sp_sub;
+ fds[fdsc_count].events = POLLIN;
+ fdsc_count++;
+
/* --- VICON POSITION --- */
subs.vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position));
fds[fdsc_count].fd = subs.vicon_pos_sub;
@@ -817,6 +831,12 @@ int sdlog2_thread_main(int argc, char *argv[])
fds[fdsc_count].events = POLLIN;
fdsc_count++;
+ /* --- ESCs --- */
+ subs.esc_sub = orb_subscribe(ORB_ID(esc_status));
+ fds[fdsc_count].fd = subs.esc_sub;
+ fds[fdsc_count].events = POLLIN;
+ fdsc_count++;
+
/* WARNING: If you get the error message below,
* then the number of registered messages (fdsc)
* differs from the number of messages in the above list.
@@ -1108,6 +1128,25 @@ int sdlog2_thread_main(int argc, char *argv[])
LOGBUFFER_WRITE_AND_COUNT(GPOS);
}
+ /* --- GLOBAL POSITION SETPOINT --- */
+ if (fds[ifds++].revents & POLLIN) {
+ orb_copy(ORB_ID(vehicle_global_position_setpoint), subs.global_pos_sp_sub, &buf.global_pos_sp);
+ log_msg.msg_type = LOG_GPSP_MSG;
+ log_msg.body.log_GPSP.altitude_is_relative = buf.global_pos_sp.altitude_is_relative;
+ log_msg.body.log_GPSP.lat = buf.global_pos_sp.lat;
+ log_msg.body.log_GPSP.lon = buf.global_pos_sp.lon;
+ log_msg.body.log_GPSP.altitude = buf.global_pos_sp.altitude;
+ log_msg.body.log_GPSP.yaw = buf.global_pos_sp.yaw;
+ log_msg.body.log_GPSP.loiter_radius = buf.global_pos_sp.loiter_radius;
+ log_msg.body.log_GPSP.loiter_direction = buf.global_pos_sp.loiter_direction;
+ log_msg.body.log_GPSP.nav_cmd = buf.global_pos_sp.nav_cmd;
+ log_msg.body.log_GPSP.param1 = buf.global_pos_sp.param1;
+ log_msg.body.log_GPSP.param2 = buf.global_pos_sp.param2;
+ log_msg.body.log_GPSP.param3 = buf.global_pos_sp.param3;
+ log_msg.body.log_GPSP.param4 = buf.global_pos_sp.param4;
+ LOGBUFFER_WRITE_AND_COUNT(GPSP);
+ }
+
/* --- VICON POSITION --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos);
@@ -1167,6 +1206,28 @@ int sdlog2_thread_main(int argc, char *argv[])
LOGBUFFER_WRITE_AND_COUNT(AIRS);
}
+ /* --- ESCs --- */
+ if (fds[ifds++].revents & POLLIN) {
+ orb_copy(ORB_ID(esc_status), subs.esc_sub, &buf.esc);
+ for (uint8_t i=0; i<buf.esc.esc_count; i++)
+ {
+ log_msg.msg_type = LOG_ESC_MSG;
+ log_msg.body.log_ESC.counter = buf.esc.counter;
+ log_msg.body.log_ESC.esc_count = buf.esc.esc_count;
+ log_msg.body.log_ESC.esc_connectiontype = buf.esc.esc_connectiontype;
+ log_msg.body.log_ESC.esc_num = i;
+ log_msg.body.log_ESC.esc_address = buf.esc.esc[i].esc_address;
+ log_msg.body.log_ESC.esc_version = buf.esc.esc[i].esc_version;
+ log_msg.body.log_ESC.esc_voltage = buf.esc.esc[i].esc_voltage;
+ log_msg.body.log_ESC.esc_current = buf.esc.esc[i].esc_current;
+ log_msg.body.log_ESC.esc_rpm = buf.esc.esc[i].esc_rpm;
+ log_msg.body.log_ESC.esc_temperature = buf.esc.esc[i].esc_temperature;
+ log_msg.body.log_ESC.esc_setpoint = buf.esc.esc[i].esc_setpoint;
+ log_msg.body.log_ESC.esc_setpoint_raw = buf.esc.esc[i].esc_setpoint_raw;
+ LOGBUFFER_WRITE_AND_COUNT(ESC);
+ }
+ }
+
#ifdef SDLOG2_DEBUG
printf("fill rp=%i wp=%i count=%i\n", lb.read_ptr, lb.write_ptr, logbuffer_count(&lb));
#endif
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index 358637f93..9be96a62e 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -152,15 +152,15 @@ struct log_ATTC_s {
/* --- STAT - VEHICLE STATE --- */
#define LOG_STAT_MSG 10
struct log_STAT_s {
- unsigned char state;
- unsigned char flight_mode;
- unsigned char manual_control_mode;
- unsigned char manual_sas_mode;
- unsigned char armed;
+ uint8_t state;
+ uint8_t flight_mode;
+ uint8_t manual_control_mode;
+ uint8_t manual_sas_mode;
+ uint8_t armed;
float battery_voltage;
float battery_current;
float battery_remaining;
- unsigned char battery_warning;
+ uint8_t battery_warning;
};
/* --- CTRL - CONTROL DEBUG --- */
@@ -229,6 +229,41 @@ struct log_GPOS_s {
float vel_e;
float vel_d;
};
+
+/* --- GPSP - GLOBAL POSITION SETPOINT --- */
+#define LOG_GPSP_MSG 17
+struct log_GPSP_s {
+ uint8_t altitude_is_relative;
+ int32_t lat;
+ int32_t lon;
+ float altitude;
+ float yaw;
+ float loiter_radius;
+ int8_t loiter_direction;
+ uint8_t nav_cmd;
+ float param1;
+ float param2;
+ float param3;
+ float param4;
+};
+
+/* --- ESC - ESC STATE --- */
+#define LOG_ESC_MSG 18
+struct log_ESC_s {
+ uint16_t counter;
+ uint8_t esc_count;
+ uint8_t esc_connectiontype;
+ uint8_t esc_num;
+ uint16_t esc_address;
+ uint16_t esc_version;
+ uint16_t esc_voltage;
+ uint16_t esc_current;
+ uint16_t esc_rpm;
+ uint16_t esc_temperature;
+ float esc_setpoint;
+ uint16_t esc_setpoint_raw;
+};
+
#pragma pack(pop)
/* construct list of all message formats */
@@ -251,6 +286,8 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"),
LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"),
LOG_FORMAT(GPOS, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"),
+ LOG_FORMAT(GPSP, "BLLfffbBffff", "AltRel,Lat,Lon,Alt,Yaw,LoiterR,LoiterDir,NavCmd,P1,P2,P3,P4"),
+ LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,No,Version,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
};
static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s);
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-11 22:27:48 +0000