fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-17 07:13:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' into navigator_new_vector

Browse Source
This commit is contained in:
Anton Babushkin
2014-01-14 15:45:49 +01:00
parent 7bcddd192f b529e112b8
commit 97e4522c76
28 changed files with 984 additions and 273 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sdlog2/sdlog2.c
+24 -6
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
* Anton Babushkin <anton.babushkin@me.com>
*
@@ -704,6 +704,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct airspeed_s airspeed;
struct esc_status_s esc;
struct vehicle_global_velocity_setpoint_s global_vel_sp;
struct battery_status_s battery;
} buf;
memset(&buf, 0, sizeof(buf));
@@ -729,6 +730,7 @@ int sdlog2_thread_main(int argc, char *argv[])
int airspeed_sub;
int esc_sub;
int global_vel_sp_sub;
int battery_sub;
} subs;
/* log message buffer: header + body */
@@ -755,6 +757,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct log_GPSP_s log_GPSP;
struct log_ESC_s log_ESC;
struct log_GVSP_s log_GVSP;
struct log_BATT_s log_BATT;
struct log_DIST_s log_DIST;
} body;
} log_msg = {
@@ -764,9 +767,9 @@ int sdlog2_thread_main(int argc, char *argv[])
memset(&log_msg.body, 0, sizeof(log_msg.body));
/* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
/* number of subscriptions */
const ssize_t fdsc = 20;
/* sanity check variable and index */
/* number of messages */
const ssize_t fdsc = 25;
/* Sanity check variable and index */
ssize_t fdsc_count = 0;
/* file descriptors to wait for */
struct pollfd fds[fdsc];
@@ -891,6 +894,12 @@ int sdlog2_thread_main(int argc, char *argv[])
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- BATTERY --- */
subs.battery_sub = orb_subscribe(ORB_ID(battery_status));
fds[fdsc_count].fd = subs.battery_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.
@@ -986,8 +995,6 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_STAT.main_state = (uint8_t) buf.control_mode.main_state;
log_msg.body.log_STAT.navigation_state = (uint8_t) buf.control_mode.nav_state;
log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state;
log_msg.body.log_STAT.battery_voltage = buf_status.battery_voltage;
log_msg.body.log_STAT.battery_current = buf_status.battery_current;
log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining;
log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning;
log_msg.body.log_STAT.landed = (uint8_t) buf_status.condition_landed;
@@ -1264,6 +1271,17 @@ int sdlog2_thread_main(int argc, char *argv[])
LOGBUFFER_WRITE_AND_COUNT(GVSP);
}
/* --- BATTERY --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(battery_status), subs.battery_sub, &buf.battery);
log_msg.msg_type = LOG_BATT_MSG;
log_msg.body.log_BATT.voltage = buf.battery.voltage_v;
log_msg.body.log_BATT.voltage_filtered = buf.battery.voltage_filtered_v;
log_msg.body.log_BATT.current = buf.battery.current_a;
log_msg.body.log_BATT.discharged = buf.battery.discharged_mah;
LOGBUFFER_WRITE_AND_COUNT(BATT);
}
/* 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 */
src/modules/sdlog2/sdlog2_messages.h
+12 -4
View File
@@ -148,8 +148,6 @@ struct log_STAT_s {
uint8_t main_state;
uint8_t navigation_state;
uint8_t arming_state;
float battery_voltage;
float battery_current;
float battery_remaining;
uint8_t battery_warning;
uint8_t landed;
@@ -247,8 +245,17 @@ struct log_GVSP_s {
float vz;
};
/* --- BATT - BATTERY --- */
#define LOG_BATT_MSG 20
struct log_BATT_s {
float voltage;
float voltage_filtered;
float current;
float discharged;
};
/* --- DIST - DISTANCE TO SURFACE --- */
#define LOG_DIST_MSG 20
#define LOG_DIST_MSG 21
struct log_DIST_s {
float bottom;
float bottom_rate;
@@ -290,7 +297,7 @@ 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, "BBBfffBB", "MainState,NavState,ArmState,BatV,BatC,BatRem,BatWarn,Landed"),
LOG_FORMAT(STAT, "BBBfBB", "MainState,NavState,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"),
@@ -300,6 +307,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(GPSP, "BLLffBfbfff", "AltRel,Lat,Lon,Alt,Yaw,NavCmd,LoitR,LoitDir,AccR,TimeIn,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"),
/* system-level messages, ID >= 0x80 */
// FMT: don't write format of format message, it's useless