aboutsummaryrefslogtreecommitdiff
path: root/src/modules/sdlog2
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/sdlog2')
-rw-r--r--src/modules/sdlog2/sdlog2.c97
-rw-r--r--src/modules/sdlog2/sdlog2_messages.h50
2 files changed, 107 insertions, 40 deletions
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index ac6a120fa..5cde94431 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -72,7 +72,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/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_vicon_position.h>
#include <uORB/topics/vehicle_global_velocity_setpoint.h>
@@ -82,6 +82,7 @@
#include <uORB/topics/airspeed.h>
#include <uORB/topics/rc_channels.h>
#include <uORB/topics/esc_status.h>
+#include <uORB/topics/telemetry_status.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
@@ -748,7 +749,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 position_setpoint_triplet_s triplet;
struct vehicle_gps_position_s gps_pos;
struct vehicle_vicon_position_s vicon_pos;
struct optical_flow_s flow;
@@ -758,6 +759,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct esc_status_s esc;
struct vehicle_global_velocity_setpoint_s global_vel_sp;
struct battery_status_s battery;
+ struct telemetry_status_s telemetry;
} buf;
memset(&buf, 0, sizeof(buf));
@@ -774,7 +776,7 @@ 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 triplet_sub;
int gps_pos_sub;
int vicon_pos_sub;
int flow_sub;
@@ -783,6 +785,7 @@ int sdlog2_thread_main(int argc, char *argv[])
int esc_sub;
int global_vel_sp_sub;
int battery_sub;
+ int telemetry_sub;
} subs;
/* log message buffer: header + body */
@@ -810,6 +813,8 @@ int sdlog2_thread_main(int argc, char *argv[])
struct log_ESC_s log_ESC;
struct log_GVSP_s log_GVSP;
struct log_BATT_s log_BATT;
+ struct log_DIST_s log_DIST;
+ struct log_TELE_s log_TELE;
} body;
} log_msg = {
LOG_PACKET_HEADER_INIT(0)
@@ -898,8 +903,8 @@ int sdlog2_thread_main(int argc, char *argv[])
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;
+ subs.triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
+ fds[fdsc_count].fd = subs.triplet_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
@@ -945,6 +950,12 @@ int sdlog2_thread_main(int argc, char *argv[])
fds[fdsc_count].events = POLLIN;
fdsc_count++;
+ /* --- TELEMETRY STATUS --- */
+ subs.telemetry_sub = orb_subscribe(ORB_ID(telemetry_status));
+ fds[fdsc_count].fd = subs.telemetry_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.
@@ -973,6 +984,9 @@ int sdlog2_thread_main(int argc, char *argv[])
hrt_abstime barometer_timestamp = 0;
hrt_abstime differential_pressure_timestamp = 0;
+ /* track changes in distance status */
+ bool dist_bottom_present = false;
+
/* enable logging on start if needed */
if (log_on_start) {
/* check GPS topic to get GPS time */
@@ -989,7 +1003,7 @@ int sdlog2_thread_main(int argc, char *argv[])
/* decide use usleep() or blocking poll() */
bool use_sleep = sleep_delay > 0 && logging_enabled;
- /* poll all topics if logging enabled or only management (first 2) if not */
+ /* poll all topics if logging enabled or only management (first 3) if not */
int poll_ret = poll(fds, logging_enabled ? fdsc_count : 3, use_sleep ? 0 : poll_timeout);
/* handle the poll result */
@@ -1008,6 +1022,7 @@ int sdlog2_thread_main(int argc, char *argv[])
/* --- VEHICLE COMMAND - LOG MANAGEMENT --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd);
+
handle_command(&buf.cmd);
handled_topics++;
}
@@ -1038,7 +1053,7 @@ int sdlog2_thread_main(int argc, char *argv[])
continue;
}
- ifds = 1; // begin from fds[1] again
+ ifds = 1; // begin from VEHICLE STATUS again
pthread_mutex_lock(&logbuffer_mutex);
@@ -1049,10 +1064,9 @@ int sdlog2_thread_main(int argc, char *argv[])
/* --- VEHICLE STATUS --- */
if (fds[ifds++].revents & POLLIN) {
- // Don't orb_copy, it's already done few lines above
+ /* don't orb_copy, it's already done few lines above */
log_msg.msg_type = LOG_STAT_MSG;
log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state;
- log_msg.body.log_STAT.navigation_state = (uint8_t) buf_status.navigation_state;
log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state;
log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining;
log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning;
@@ -1062,7 +1076,7 @@ int sdlog2_thread_main(int argc, char *argv[])
/* --- GPS POSITION --- */
if (fds[ifds++].revents & POLLIN) {
- // Don't orb_copy, it's already done few lines above
+ /* don't orb_copy, it's already done few lines above */
log_msg.msg_type = LOG_GPS_MSG;
log_msg.body.log_GPS.gps_time = buf.gps_pos.time_gps_usec;
log_msg.body.log_GPS.fix_type = buf.gps_pos.fix_type;
@@ -1143,6 +1157,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.gx = buf.att.g_comp[0];
+ 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);
}
@@ -1203,6 +1220,17 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_LPOS.z_flags = (buf.local_pos.z_valid ? 1 : 0) | (buf.local_pos.v_z_valid ? 2 : 0) | (buf.local_pos.z_global ? 8 : 0);
log_msg.body.log_LPOS.landed = buf.local_pos.landed;
LOGBUFFER_WRITE_AND_COUNT(LPOS);
+
+ if (buf.local_pos.dist_bottom_valid) {
+ dist_bottom_present = true;
+ }
+ if (dist_bottom_present) {
+ log_msg.msg_type = LOG_DIST_MSG;
+ log_msg.body.log_DIST.bottom = buf.local_pos.dist_bottom;
+ log_msg.body.log_DIST.bottom_rate = buf.local_pos.dist_bottom_rate;
+ log_msg.body.log_DIST.flags = (buf.local_pos.dist_bottom_valid ? 1 : 0);
+ LOGBUFFER_WRITE_AND_COUNT(DIST);
+ }
}
/* --- LOCAL POSITION SETPOINT --- */
@@ -1220,31 +1248,30 @@ int sdlog2_thread_main(int argc, char *argv[])
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos);
log_msg.msg_type = LOG_GPOS_MSG;
- log_msg.body.log_GPOS.lat = buf.global_pos.lat;
- log_msg.body.log_GPOS.lon = buf.global_pos.lon;
+ log_msg.body.log_GPOS.lat = buf.global_pos.lat * 1e7;
+ log_msg.body.log_GPOS.lon = buf.global_pos.lon * 1e7;
log_msg.body.log_GPOS.alt = buf.global_pos.alt;
- log_msg.body.log_GPOS.vel_n = buf.global_pos.vx;
- log_msg.body.log_GPOS.vel_e = buf.global_pos.vy;
- log_msg.body.log_GPOS.vel_d = buf.global_pos.vz;
+ log_msg.body.log_GPOS.vel_n = buf.global_pos.vel_n;
+ log_msg.body.log_GPOS.vel_e = buf.global_pos.vel_e;
+ log_msg.body.log_GPOS.vel_d = buf.global_pos.vel_d;
+ log_msg.body.log_GPOS.baro_alt = buf.global_pos.baro_alt;
+ log_msg.body.log_GPOS.flags = (buf.global_pos.baro_valid ? 1 : 0) | (buf.global_pos.global_valid ? 2 : 0);
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);
+ orb_copy(ORB_ID(position_setpoint_triplet), subs.triplet_sub, &buf.triplet);
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;
+ log_msg.body.log_GPSP.nav_state = buf.triplet.nav_state;
+ log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7d);
+ log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7d);
+ log_msg.body.log_GPSP.alt = buf.triplet.current.alt;
+ log_msg.body.log_GPSP.yaw = buf.triplet.current.yaw;
+ log_msg.body.log_GPSP.type = buf.triplet.current.type;
+ log_msg.body.log_GPSP.loiter_radius = buf.triplet.current.loiter_radius;
+ log_msg.body.log_GPSP.loiter_direction = buf.triplet.current.loiter_direction;
+ log_msg.body.log_GPSP.pitch_min = buf.triplet.current.pitch_min;
LOGBUFFER_WRITE_AND_COUNT(GPSP);
}
@@ -1330,6 +1357,20 @@ int sdlog2_thread_main(int argc, char *argv[])
LOGBUFFER_WRITE_AND_COUNT(BATT);
}
+ /* --- TELEMETRY --- */
+ if (fds[ifds++].revents & POLLIN) {
+ orb_copy(ORB_ID(telemetry_status), subs.telemetry_sub, &buf.telemetry);
+ log_msg.msg_type = LOG_TELE_MSG;
+ log_msg.body.log_TELE.rssi = buf.telemetry.rssi;
+ log_msg.body.log_TELE.remote_rssi = buf.telemetry.remote_rssi;
+ log_msg.body.log_TELE.noise = buf.telemetry.noise;
+ log_msg.body.log_TELE.remote_noise = buf.telemetry.remote_noise;
+ log_msg.body.log_TELE.rxerrors = buf.telemetry.rxerrors;
+ log_msg.body.log_TELE.fixed = buf.telemetry.fixed;
+ log_msg.body.log_TELE.txbuf = buf.telemetry.txbuf;
+ LOGBUFFER_WRITE_AND_COUNT(TELE);
+ }
+
/* signal the other thread new data, but not yet unlock */
if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) {
/* only request write if several packets can be written at once */
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index a784a1f30..16bfc355d 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -57,6 +57,9 @@ struct log_ATT_s {
float roll_rate;
float pitch_rate;
float yaw_rate;
+ float gx;
+ float gy;
+ float gz;
};
/* --- ATSP - ATTITUDE SET POINT --- */
@@ -146,7 +149,6 @@ struct log_ATTC_s {
#define LOG_STAT_MSG 10
struct log_STAT_s {
uint8_t main_state;
- uint8_t navigation_state;
uint8_t arming_state;
float battery_remaining;
uint8_t battery_warning;
@@ -202,23 +204,22 @@ struct log_GPOS_s {
float vel_n;
float vel_e;
float vel_d;
+ float baro_alt;
+ uint8_t flags;
};
/* --- GPSP - GLOBAL POSITION SETPOINT --- */
#define LOG_GPSP_MSG 17
struct log_GPSP_s {
- uint8_t altitude_is_relative;
+ uint8_t nav_state;
int32_t lat;
int32_t lon;
- float altitude;
+ float alt;
float yaw;
+ uint8_t type;
float loiter_radius;
int8_t loiter_direction;
- uint8_t nav_cmd;
- float param1;
- float param2;
- float param3;
- float param4;
+ float pitch_min;
};
/* --- ESC - ESC STATE --- */
@@ -255,6 +256,28 @@ struct log_BATT_s {
float discharged;
};
+/* --- DIST - DISTANCE TO SURFACE --- */
+#define LOG_DIST_MSG 21
+struct log_DIST_s {
+ float bottom;
+ float bottom_rate;
+ uint8_t flags;
+};
+
+/* --- TELE - TELEMETRY STATUS --- */
+#define LOG_TELE_MSG 22
+struct log_TELE_s {
+ uint8_t rssi;
+ uint8_t remote_rssi;
+ uint8_t noise;
+ uint8_t remote_noise;
+ uint16_t rxerrors;
+ uint16_t fixed;
+ uint8_t txbuf;
+};
+
+/********** SYSTEM MESSAGES, ID > 0x80 **********/
+
/* --- TIME - TIME STAMP --- */
#define LOG_TIME_MSG 129
struct log_TIME_s {
@@ -280,7 +303,7 @@ 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, "ffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate"),
+ LOG_FORMAT(ATT, "fffffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"),
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"),
@@ -288,17 +311,20 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"),
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, "BBBfBB", "MainState,NavState,ArmState,BatRem,BatWarn,Landed"),
+ LOG_FORMAT(STAT, "BBfBB", "MainState,ArmState,BatRem,BatWarn,Landed"),
LOG_FORMAT(RC, "ffffffffB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count"),
LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"),
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(GPOS, "LLfffffB", "Lat,Lon,Alt,VelN,VelE,VelD,BaroAlt,Flags"),
+ LOG_FORMAT(GPSP, "BLLffBfbf", "NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"),
LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"),
LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"),
+ LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"),
+ LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"),
+
/* system-level messages, ID >= 0x80 */
// FMT: don't write format of format message, it's useless
LOG_FORMAT(TIME, "Q", "StartTime"),
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-19 04:22:31 +0000