mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-21 15:23:34 +08:00
Merged beta into paul_estimator
This commit is contained in:
Lorenz Meier
+69
-28
@@ -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 */
|
||||
|
||||
@@ -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"),
|
||||
|
||||
Reference in New Issue
Block a user