dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

sdlog2: STAT (vehicle state) log message added, minor optimizations

This commit is contained in:
Anton Babushkin 2013-06-07 13:36:15 +04:00
parent 4052652a28
commit 5bad186916
2 changed files with 43 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
src/modules/sdlog2/sdlog2.c
View File

@ -597,10 +597,12 @@ int sdlog2_thread_main(int argc, char *argv[])
/* file descriptors to wait for */
struct pollfd fds[fdsc];
struct vehicle_status_s buf_status;
memset(&buf_status, 0, sizeof(buf_status));
/* warning! using union here to save memory, elements should be used separately! */
union {
struct vehicle_command_s cmd;
struct vehicle_status_s status;
struct sensor_combined_s sensor;
struct vehicle_attitude_s att;
struct vehicle_attitude_setpoint_s att_sp;
@ -653,6 +655,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct log_LPSP_s log_LPSP;
struct log_GPS_s log_GPS;
struct log_ATTC_s log_ATTC;
struct log_STAT_s log_STAT;
} body;
} log_msg = {
LOG_PACKET_HEADER_INIT(0)
@ -803,22 +806,25 @@ int sdlog2_thread_main(int argc, char *argv[])
* logging_enabled can be changed while checking vehicle_command and vehicle_status */
bool check_data = logging_enabled;
int ifds = 0;
int handled_topics = 0;
/* --- VEHICLE COMMAND --- */
/* --- 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++;
}
/* --- VEHICLE STATUS --- */
/* --- VEHICLE STATUS - LOG MANAGEMENT --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_status), subs.status_sub, &buf.status);
orb_copy(ORB_ID(vehicle_status), subs.status_sub, &buf_status);
if (log_when_armed) {
handle_status(&buf.status);
handle_status(&buf_status);
}
handled_topics++;
}
if (!logging_enabled || !check_data) {
if (!logging_enabled || !check_data || handled_topics >= poll_ret) {
continue;
}
@ -829,6 +835,22 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_TIME.t = hrt_absolute_time();
LOGBUFFER_WRITE_AND_COUNT(TIME);
/* --- VEHICLE STATUS --- */
if (fds[ifds++].revents & POLLIN) {
// Don't orb_copy, it's already done few lines above
log_msg.msg_type = LOG_STAT_MSG;
log_msg.body.log_STAT.state = (unsigned char) buf_status.state_machine;
log_msg.body.log_STAT.flight_mode = (unsigned char) buf_status.flight_mode;
log_msg.body.log_STAT.manual_control_mode = (unsigned char) buf_status.manual_control_mode;
log_msg.body.log_STAT.manual_sas_mode = (unsigned char) buf_status.manual_sas_mode;
log_msg.body.log_STAT.armed = (unsigned char) buf_status.flag_system_armed;
log_msg.body.log_STAT.battery_voltage = buf_status.voltage_battery;
log_msg.body.log_STAT.battery_current = buf_status.current_battery;
log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining;
log_msg.body.log_STAT.battery_warning = (unsigned char) buf_status.battery_warning;
LOGBUFFER_WRITE_AND_COUNT(STAT);
}
/* --- GPS POSITION --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos);

15
src/modules/sdlog2/sdlog2_messages.h
View File

@ -141,6 +141,20 @@ struct log_ATTC_s {
float yaw;
float thrust;
};
/* --- STAT - VEHICLE STATE --- */
#define LOG_STAT_MSG 10
struct log_STAT_s {
unsigned char state;
unsigned char flight_mode;
unsigned char manual_control_mode;
unsigned char manual_sas_mode;
unsigned char armed;
float battery_voltage;
float battery_current;
float battery_remaining;
unsigned char battery_warning;
};
#pragma pack(pop)
/* construct list of all message formats */
@ -155,6 +169,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, "BBBBBfffB", "State,FlightMode,CtlMode,SASMode,Armed,BatV,BatC,BatRem,BatWarn"),
};
static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s);