mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-19 07:33:33 +08:00
Merge branch 'master' into mpc_local_pos
This commit is contained in:
Anton Babushkin
+50
-26
@@ -84,6 +84,8 @@
|
||||
#include <uORB/topics/esc_status.h>
|
||||
#include <uORB/topics/telemetry_status.h>
|
||||
#include <uORB/topics/estimator_status.h>
|
||||
#include <uORB/topics/system_power.h>
|
||||
#include <uORB/topics/servorail_status.h>
|
||||
|
||||
#include <systemlib/systemlib.h>
|
||||
#include <systemlib/param/param.h>
|
||||
@@ -224,11 +226,11 @@ sdlog2_usage(const char *reason)
|
||||
}
|
||||
|
||||
errx(1, "usage: sdlog2 {start|stop|status} [-r <log rate>] [-b <buffer size>] -e -a -t\n"
|
||||
"\t-r\tLog rate in Hz, 0 means unlimited rate\n"
|
||||
"\t-b\tLog buffer size in KiB, default is 8\n"
|
||||
"\t-e\tEnable logging by default (if not, can be started by command)\n"
|
||||
"\t-a\tLog only when armed (can be still overriden by command)\n"
|
||||
"\t-t\tUse date/time for naming log directories and files\n");
|
||||
"\t-r\tLog rate in Hz, 0 means unlimited rate\n"
|
||||
"\t-b\tLog buffer size in KiB, default is 8\n"
|
||||
"\t-e\tEnable logging by default (if not, can be started by command)\n"
|
||||
"\t-a\tLog only when armed (can be still overriden by command)\n"
|
||||
"\t-t\tUse date/time for naming log directories and files\n");
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -255,11 +257,11 @@ int sdlog2_main(int argc, char *argv[])
|
||||
|
||||
main_thread_should_exit = false;
|
||||
deamon_task = task_spawn_cmd("sdlog2",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_DEFAULT - 30,
|
||||
3000,
|
||||
sdlog2_thread_main,
|
||||
(const char **)argv);
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_DEFAULT - 30,
|
||||
3000,
|
||||
sdlog2_thread_main,
|
||||
(const char **)argv);
|
||||
exit(0);
|
||||
}
|
||||
|
||||
@@ -796,6 +798,8 @@ int sdlog2_thread_main(int argc, char *argv[])
|
||||
struct telemetry_status_s telemetry;
|
||||
struct range_finder_report range_finder;
|
||||
struct estimator_status_report estimator_status;
|
||||
struct system_power_s system_power;
|
||||
struct servorail_status_s servorail_status;
|
||||
} buf;
|
||||
|
||||
memset(&buf, 0, sizeof(buf));
|
||||
@@ -828,6 +832,8 @@ int sdlog2_thread_main(int argc, char *argv[])
|
||||
struct log_DIST_s log_DIST;
|
||||
struct log_TELE_s log_TELE;
|
||||
struct log_ESTM_s log_ESTM;
|
||||
struct log_PWR_s log_PWR;
|
||||
struct log_VICN_s log_VICN;
|
||||
} body;
|
||||
} log_msg = {
|
||||
LOG_PACKET_HEADER_INIT(0)
|
||||
@@ -859,6 +865,8 @@ int sdlog2_thread_main(int argc, char *argv[])
|
||||
int telemetry_sub;
|
||||
int range_finder_sub;
|
||||
int estimator_status_sub;
|
||||
int system_power_sub;
|
||||
int servorail_status_sub;
|
||||
} subs;
|
||||
|
||||
subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
|
||||
@@ -884,6 +892,8 @@ int sdlog2_thread_main(int argc, char *argv[])
|
||||
subs.telemetry_sub = orb_subscribe(ORB_ID(telemetry_status));
|
||||
subs.range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder));
|
||||
subs.estimator_status_sub = orb_subscribe(ORB_ID(estimator_status));
|
||||
subs.system_power_sub = orb_subscribe(ORB_ID(system_power));
|
||||
subs.servorail_status_sub = orb_subscribe(ORB_ID(servorail_status));
|
||||
|
||||
thread_running = true;
|
||||
|
||||
@@ -898,9 +908,6 @@ 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 */
|
||||
@@ -1089,6 +1096,8 @@ int sdlog2_thread_main(int argc, char *argv[])
|
||||
log_msg.body.log_LPOS.x = buf.local_pos.x;
|
||||
log_msg.body.log_LPOS.y = buf.local_pos.y;
|
||||
log_msg.body.log_LPOS.z = buf.local_pos.z;
|
||||
log_msg.body.log_LPOS.ground_dist = buf.local_pos.dist_bottom;
|
||||
log_msg.body.log_LPOS.ground_dist_rate = buf.local_pos.dist_bottom_rate;
|
||||
log_msg.body.log_LPOS.vx = buf.local_pos.vx;
|
||||
log_msg.body.log_LPOS.vy = buf.local_pos.vy;
|
||||
log_msg.body.log_LPOS.vz = buf.local_pos.vz;
|
||||
@@ -1098,19 +1107,8 @@ int sdlog2_thread_main(int argc, char *argv[])
|
||||
log_msg.body.log_LPOS.xy_flags = (buf.local_pos.xy_valid ? 1 : 0) | (buf.local_pos.v_xy_valid ? 2 : 0) | (buf.local_pos.xy_global ? 8 : 0);
|
||||
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;
|
||||
log_msg.body.log_LPOS.ground_dist_flags = (buf.local_pos.dist_bottom_valid ? 1 : 0);
|
||||
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 --- */
|
||||
@@ -1154,7 +1152,14 @@ int sdlog2_thread_main(int argc, char *argv[])
|
||||
|
||||
/* --- VICON POSITION --- */
|
||||
if (copy_if_updated(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos)) {
|
||||
// TODO not implemented yet
|
||||
log_msg.msg_type = LOG_VICN_MSG;
|
||||
log_msg.body.log_VICN.x = buf.vicon_pos.x;
|
||||
log_msg.body.log_VICN.y = buf.vicon_pos.y;
|
||||
log_msg.body.log_VICN.z = buf.vicon_pos.z;
|
||||
log_msg.body.log_VICN.pitch = buf.vicon_pos.pitch;
|
||||
log_msg.body.log_VICN.roll = buf.vicon_pos.roll;
|
||||
log_msg.body.log_VICN.yaw = buf.vicon_pos.yaw;
|
||||
LOGBUFFER_WRITE_AND_COUNT(VICN);
|
||||
}
|
||||
|
||||
/* --- FLOW --- */
|
||||
@@ -1184,6 +1189,7 @@ int sdlog2_thread_main(int argc, char *argv[])
|
||||
log_msg.msg_type = LOG_AIRS_MSG;
|
||||
log_msg.body.log_AIRS.indicated_airspeed = buf.airspeed.indicated_airspeed_m_s;
|
||||
log_msg.body.log_AIRS.true_airspeed = buf.airspeed.true_airspeed_m_s;
|
||||
log_msg.body.log_AIRS.air_temperature_celsius = buf.airspeed.air_temperature_celsius;
|
||||
LOGBUFFER_WRITE_AND_COUNT(AIRS);
|
||||
}
|
||||
|
||||
@@ -1226,6 +1232,24 @@ int sdlog2_thread_main(int argc, char *argv[])
|
||||
LOGBUFFER_WRITE_AND_COUNT(BATT);
|
||||
}
|
||||
|
||||
/* --- SYSTEM POWER RAILS --- */
|
||||
if (copy_if_updated(ORB_ID(system_power), subs.system_power_sub, &buf.system_power)) {
|
||||
log_msg.msg_type = LOG_PWR_MSG;
|
||||
log_msg.body.log_PWR.peripherals_5v = buf.system_power.voltage5V_v;
|
||||
log_msg.body.log_PWR.usb_ok = buf.system_power.usb_connected;
|
||||
log_msg.body.log_PWR.brick_ok = buf.system_power.brick_valid;
|
||||
log_msg.body.log_PWR.servo_ok = buf.system_power.servo_valid;
|
||||
log_msg.body.log_PWR.low_power_rail_overcurrent = buf.system_power.periph_5V_OC;
|
||||
log_msg.body.log_PWR.high_power_rail_overcurrent = buf.system_power.hipower_5V_OC;
|
||||
|
||||
/* copy servo rail status topic here too */
|
||||
orb_copy(ORB_ID(servorail_status), subs.servorail_status_sub, &buf.servorail_status);
|
||||
log_msg.body.log_PWR.servo_rail_5v = buf.servorail_status.voltage_v;
|
||||
log_msg.body.log_PWR.servo_rssi = buf.servorail_status.rssi_v;
|
||||
|
||||
LOGBUFFER_WRITE_AND_COUNT(PWR);
|
||||
}
|
||||
|
||||
/* --- TELEMETRY --- */
|
||||
if (copy_if_updated(ORB_ID(telemetry_status), subs.telemetry_sub, &buf.telemetry)) {
|
||||
log_msg.msg_type = LOG_TELE_MSG;
|
||||
|
||||
@@ -101,6 +101,8 @@ struct log_LPOS_s {
|
||||
float x;
|
||||
float y;
|
||||
float z;
|
||||
float ground_dist;
|
||||
float ground_dist_rate;
|
||||
float vx;
|
||||
float vy;
|
||||
float vz;
|
||||
@@ -110,6 +112,7 @@ struct log_LPOS_s {
|
||||
uint8_t xy_flags;
|
||||
uint8_t z_flags;
|
||||
uint8_t landed;
|
||||
uint8_t ground_dist_flags;
|
||||
};
|
||||
|
||||
/* --- LPSP - LOCAL POSITION SETPOINT --- */
|
||||
@@ -174,6 +177,7 @@ struct log_OUT0_s {
|
||||
struct log_AIRS_s {
|
||||
float indicated_airspeed;
|
||||
float true_airspeed;
|
||||
float air_temperature_celsius;
|
||||
};
|
||||
|
||||
/* --- ARSP - ATTITUDE RATE SET POINT --- */
|
||||
@@ -277,6 +281,40 @@ struct log_TELE_s {
|
||||
uint8_t txbuf;
|
||||
};
|
||||
|
||||
/* --- ESTM - ESTIMATOR STATUS --- */
|
||||
#define LOG_ESTM_MSG 23
|
||||
struct log_ESTM_s {
|
||||
float s[10];
|
||||
uint8_t n_states;
|
||||
uint8_t states_nan;
|
||||
uint8_t covariance_nan;
|
||||
uint8_t kalman_gain_nan;
|
||||
};
|
||||
|
||||
/* --- PWR - ONBOARD POWER SYSTEM --- */
|
||||
#define LOG_PWR_MSG 24
|
||||
struct log_PWR_s {
|
||||
float peripherals_5v;
|
||||
float servo_rail_5v;
|
||||
float servo_rssi;
|
||||
uint8_t usb_ok;
|
||||
uint8_t brick_ok;
|
||||
uint8_t servo_ok;
|
||||
uint8_t low_power_rail_overcurrent;
|
||||
uint8_t high_power_rail_overcurrent;
|
||||
};
|
||||
|
||||
/* --- VICN - VICON POSITION --- */
|
||||
#define LOG_VICN_MSG 25
|
||||
struct log_VICN_s {
|
||||
float x;
|
||||
float y;
|
||||
float z;
|
||||
float roll;
|
||||
float pitch;
|
||||
float yaw;
|
||||
};
|
||||
|
||||
/********** SYSTEM MESSAGES, ID > 0x80 **********/
|
||||
|
||||
/* --- TIME - TIME STAMP --- */
|
||||
@@ -299,59 +337,43 @@ struct log_PARM_s {
|
||||
float value;
|
||||
};
|
||||
|
||||
/* --- ESTM - ESTIMATOR STATUS --- */
|
||||
#define LOG_ESTM_MSG 132
|
||||
struct log_ESTM_s {
|
||||
float s[10];
|
||||
uint8_t n_states;
|
||||
uint8_t states_nan;
|
||||
uint8_t covariance_nan;
|
||||
uint8_t kalman_gain_nan;
|
||||
};
|
||||
// struct log_ESTM_s {
|
||||
// float s[32];
|
||||
// uint8_t n_states;
|
||||
// uint8_t states_nan;
|
||||
// uint8_t covariance_nan;
|
||||
// uint8_t kalman_gain_nan;
|
||||
// };
|
||||
|
||||
#pragma pack(pop)
|
||||
|
||||
/* 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(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"),
|
||||
LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
|
||||
LOG_FORMAT(SENS, "fffff", "BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"),
|
||||
LOG_FORMAT(LPOS, "ffffffLLfBBB", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt,XYFlags,ZFlags,Landed"),
|
||||
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, "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, "LLffffff", "Lat,Lon,Alt,VelN,VelE,VelD,EPH,EPV"),
|
||||
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"),
|
||||
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, "fffff", "BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"),
|
||||
LOG_FORMAT(LPOS, "ffffffffLLfBBBB", "X,Y,Z,dist,distR,VX,VY,VZ,RLat,RLon,RAlt,XYFlg,ZFlg,LFlg,GFlg"),
|
||||
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, "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, "fff", "IndSpeed,TrueSpeed,AirTemp"),
|
||||
LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"),
|
||||
LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"),
|
||||
LOG_FORMAT(GPOS, "LLffffff", "Lat,Lon,Alt,VelN,VelE,VelD,EPH,EPV"),
|
||||
LOG_FORMAT(GPSP, "BLLffBfbf", "NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"),
|
||||
LOG_FORMAT(ESC, "HBBBHHHHHHfH", "count,nESC,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"),
|
||||
LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,nStat,statNaN,covNaN,kGainNaN"),
|
||||
LOG_FORMAT(PWR, "fffBBBBB", "Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"),
|
||||
LOG_FORMAT(VICN, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"),
|
||||
|
||||
/* system-level messages, ID >= 0x80 */
|
||||
// FMT: don't write format of format message, it's useless
|
||||
/* FMT: don't write format of format message, it's useless */
|
||||
LOG_FORMAT(TIME, "Q", "StartTime"),
|
||||
LOG_FORMAT(VER, "NZ", "Arch,FwGit"),
|
||||
LOG_FORMAT(PARM, "Nf", "Name,Value"),
|
||||
LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,n_states,states_nan,cov_nan,kgain_nan"),
|
||||
//LOG_FORMAT(ESTM, "ffffffffffffffffffffffffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27,s28,s29,s30,s31,n_states,states_nan,cov_nan,kgain_nan"),
|
||||
LOG_FORMAT(PARM, "Nf", "Name,Value")
|
||||
};
|
||||
|
||||
static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s);
|
||||
static const unsigned log_formats_num = sizeof(log_formats) / sizeof(log_formats[0]);
|
||||
|
||||
#endif /* SDLOG2_MESSAGES_H_ */
|
||||
|
||||
Reference in New Issue
Block a user