fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-27 09:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added support for battery voltage and differential pressure to logging and plot script

Browse Source
This commit is contained in:
Lorenz Meier
2013-01-09 16:37:45 +01:00
parent e1a6f1b910
commit 16e49c447d
11 changed files with 388 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/sdlog/sdlog.c
+29 -3
View File
@@ -66,6 +66,8 @@
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_vicon_position.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/differential_pressure.h>
#include <systemlib/systemlib.h>
@@ -383,6 +385,8 @@ int sdlog_thread_main(int argc, char *argv[])
struct vehicle_gps_position_s gps_pos;
struct vehicle_vicon_position_s vicon_pos;
struct optical_flow_s flow;
struct battery_status_s batt;
struct differential_pressure_s diff_pressure;
} buf;
memset(&buf, 0, sizeof(buf));
@@ -399,6 +403,8 @@ int sdlog_thread_main(int argc, char *argv[])
int gps_pos_sub;
int vicon_pos_sub;
int flow_sub;
int batt_sub;
int diff_pressure_sub;
} subs;
/* --- MANAGEMENT - LOGGING COMMAND --- */
@@ -487,6 +493,20 @@ int sdlog_thread_main(int argc, char *argv[])
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- BATTERY STATUS --- */
/* subscribe to ORB for flow measurements */
subs.batt_sub = orb_subscribe(ORB_ID(battery_status));
fds[fdsc_count].fd = subs.batt_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- DIFFERENTIAL PRESSURE --- */
/* subscribe to ORB for flow measurements */
subs.diff_pressure_sub = orb_subscribe(ORB_ID(differential_pressure));
fds[fdsc_count].fd = subs.diff_pressure_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.
@@ -597,6 +617,8 @@ int sdlog_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att);
orb_copy(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos);
orb_copy(ORB_ID(optical_flow), subs.flow_sub, &buf.flow);
orb_copy(ORB_ID(differential_pressure), subs.diff_pressure_sub, &buf.diff_pressure);
orb_copy(ORB_ID(battery_status), subs.batt_sub, &buf.batt);
struct sdlog_sysvector sysvect = {
.timestamp = buf.raw.timestamp,
@@ -611,7 +633,9 @@ int sdlog_thread_main(int argc, char *argv[])
buf.act_outputs.output[0], buf.act_outputs.output[1], buf.act_outputs.output[2], buf.act_outputs.output[3],
buf.act_outputs.output[4], buf.act_outputs.output[5], buf.act_outputs.output[6], buf.act_outputs.output[7]
},
.vbat = 0.0f, /* XXX use battery_status uORB topic */
.vbat = buf.batt.voltage_v,
.bat_current = buf.batt.current_a,
.bat_discharged = buf.batt.discharged_mah,
.adc = {buf.raw.adc_voltage_v[0], buf.raw.adc_voltage_v[1], buf.raw.adc_voltage_v[2]},
.local_position = {buf.local_pos.x, buf.local_pos.y, buf.local_pos.z},
.gps_raw_position = {buf.gps_pos.lat, buf.gps_pos.lon, buf.gps_pos.alt},
@@ -619,8 +643,10 @@ int sdlog_thread_main(int argc, char *argv[])
.rotMatrix = {buf.att.R[0][0], buf.att.R[0][1], buf.att.R[0][2], buf.att.R[1][0], buf.att.R[1][1], buf.att.R[1][2], buf.att.R[2][0], buf.att.R[2][1], buf.att.R[2][2]},
.vicon = {buf.vicon_pos.x, buf.vicon_pos.y, buf.vicon_pos.z, buf.vicon_pos.roll, buf.vicon_pos.pitch, buf.vicon_pos.yaw},
.control_effective = {buf.act_controls_effective.control_effective[0], buf.act_controls_effective.control_effective[1], buf.act_controls_effective.control_effective[2], buf.act_controls_effective.control_effective[3]},
.flow = {buf.flow.flow_raw_x, buf.flow.flow_raw_y, buf.flow.flow_comp_x_m, buf.flow.flow_comp_y_m, buf.flow.ground_distance_m, buf.flow.quality}
/* XXX add calculated airspeed */
.flow = {buf.flow.flow_raw_x, buf.flow.flow_raw_y, buf.flow.flow_comp_x_m, buf.flow.flow_comp_y_m, buf.flow.ground_distance_m, buf.flow.quality},
.diff_pressure = buf.diff_pressure.differential_pressure_mbar,
.ind_airspeed = buf.diff_pressure.indicated_airspeed_m_s,
.true_airspeed = buf.diff_pressure.true_airspeed_m_s
};
/* put into buffer for later IO */
apps/sdlog/sdlog_ringbuffer.h
+5
View File
@@ -54,6 +54,8 @@ struct sdlog_sysvector {
float control[4]; /**< roll, pitch, yaw [-1..1], thrust [0..1] */
float actuators[8]; /**< motor 1-8, in motor units (PWM: 1000-2000,AR.Drone: 0-512) */
float vbat; /**< battery voltage in [volt] */
float bat_current; /**< battery discharge current */
float bat_discharged; /**< discharged energy in mAh */
float adc[3]; /**< remaining auxiliary ADC ports [volt] */
float local_position[3]; /**< tangent plane mapping into x,y,z [m] */
int32_t gps_raw_position[3]; /**< latitude [degrees] north, longitude [degrees] east, altitude above MSL [millimeter] */
@@ -62,6 +64,9 @@ struct sdlog_sysvector {
float vicon[6]; /**< Vicon ground truth x, y, z and roll, pitch, yaw */
float control_effective[4]; /**< roll, pitch, yaw [-1..1], thrust [0..1] */
float flow[6]; /**< flow raw x, y, flow metric x, y, flow ground dist, flow quality */
float diff_pressure; /**< differential pressure */
float ind_airspeed; /**< indicated airspeed */
float true_airspeed; /**< true airspeed */
};
#pragma pack(pop)