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

merged

This commit is contained in:
Lorenz Meier 2013-06-07 21:33:49 +02:00
parent 05e77d9848 5a9e52a287
commit 048967fb6f
5 changed files with 101 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
makefiles/toolchain_gnu-arm-eabi.mk
View File

@ -50,7 +50,7 @@ OBJDUMP = $(CROSSDEV)objdump
# XXX this is pulled pretty directly from the fmu Make.defs - needs cleanup
MAXOPTIMIZATION = -O3
MAXOPTIMIZATION ?= -O3
# Base CPU flags for each of the supported architectures.
#

3
src/modules/sdlog2/logbuffer.c
View File

@ -45,12 +45,13 @@
#include "logbuffer.h"
void logbuffer_init(struct logbuffer_s *lb, int size)
int logbuffer_init(struct logbuffer_s *lb, int size)
{
lb->size = size;
lb->write_ptr = 0;
lb->read_ptr = 0;
lb->data = malloc(lb->size);
return (lb->data == 0) ? ERROR : OK;
}
int logbuffer_count(struct logbuffer_s *lb)

4
src/modules/sdlog2/logbuffer.h
View File

@ -46,14 +46,14 @@
#include <stdbool.h>
struct logbuffer_s {
// all pointers are in bytes
// pointers and size are in bytes
int write_ptr;
int read_ptr;
int size;
char *data;
};
void logbuffer_init(struct logbuffer_s *lb, int size);
int logbuffer_init(struct logbuffer_s *lb, int size);
int logbuffer_count(struct logbuffer_s *lb);

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

@ -107,7 +107,7 @@ static int deamon_task; /**< Handle of deamon task / thread */
static bool logwriter_should_exit = false; /**< Logwriter thread exit flag */
static const int MAX_NO_LOGFOLDER = 999; /**< Maximum number of log folders */
static const int MAX_NO_LOGFILE = 999; /**< Maximum number of log files */
static const int LOG_BUFFER_SIZE = 8192;
static const int LOG_BUFFER_SIZE_DEFAULT = 8192;
static const int MAX_WRITE_CHUNK = 512;
static const int MIN_BYTES_TO_WRITE = 512;
@ -207,8 +207,9 @@ sdlog2_usage(const char *reason)
if (reason)
fprintf(stderr, "%s\n", reason);
errx(1, "usage: sdlog2 {start|stop|status} [-r <log rate>] -e -a\n"
errx(1, "usage: sdlog2 {start|stop|status} [-r <log rate>] [-b <buffer size>] -e -a\n"
"\t-r\tLog rate in Hz, 0 means unlimited rate\n"
"\t-r\tLog buffer size in KBytes, 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");
}
@ -484,7 +485,7 @@ void sdlog2_stop_log()
warnx("stop logging.");
mavlink_log_info(mavlink_fd, "[sdlog2] stop logging");
logging_enabled = true;
logging_enabled = false;
logwriter_should_exit = true;
/* wake up write thread one last time */
@ -529,18 +530,18 @@ int sdlog2_thread_main(int argc, char *argv[])
warnx("failed to open MAVLink log stream, start mavlink app first.");
}
/* log every n'th value (skip three per default) */
int skip_value = 3;
/* log buffer size */
int log_buffer_size = LOG_BUFFER_SIZE_DEFAULT;
/* work around some stupidity in task_create's argv handling */
argc -= 2;
argv += 2;
int ch;
while ((ch = getopt(argc, argv, "r:ea")) != EOF) {
while ((ch = getopt(argc, argv, "r:b:ea")) != EOF) {
switch (ch) {
case 'r': {
unsigned r = strtoul(optarg, NULL, 10);
unsigned long r = strtoul(optarg, NULL, 10);
if (r == 0) {
sleep_delay = 0;
@ -551,6 +552,17 @@ int sdlog2_thread_main(int argc, char *argv[])
}
break;
case 'b': {
unsigned long s = strtoul(optarg, NULL, 10);
if (s < 1) {
s = 1;
}
log_buffer_size = 1024 * s;
}
break;
case 'e':
log_on_start = true;
break;
@ -572,7 +584,7 @@ int sdlog2_thread_main(int argc, char *argv[])
default:
sdlog2_usage("unrecognized flag");
errx(1, "exiting");
errx(1, "exiting.");
}
}
@ -580,12 +592,20 @@ int sdlog2_thread_main(int argc, char *argv[])
errx(1, "logging mount point %s not present, exiting.", mountpoint);
}
if (create_logfolder())
if (create_logfolder()) {
errx(1, "unable to create logging folder, exiting.");
}
/* only print logging path, important to find log file later */
warnx("logging to directory: %s", folder_path);
/* initialize log buffer with specified size */
warnx("log buffer size: %i bytes.", log_buffer_size);
if (OK != logbuffer_init(&lb, log_buffer_size)) {
errx(1, "can't allocate log buffer, exiting.");
}
/* file descriptors to wait for */
struct pollfd fds_control[2];
@ -597,10 +617,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;
@ -652,6 +674,8 @@ int sdlog2_thread_main(int argc, char *argv[])
struct log_LPOS_s log_LPOS;
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)
@ -766,9 +790,6 @@ int sdlog2_thread_main(int argc, char *argv[])
thread_running = true;
/* initialize log buffer with specified size */
logbuffer_init(&lb, LOG_BUFFER_SIZE);
/* initialize thread synchronization */
pthread_mutex_init(&logbuffer_mutex, NULL);
pthread_cond_init(&logbuffer_cond, NULL);
@ -802,25 +823,32 @@ 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);
}
if (!logging_enabled || !check_data) {
handled_topics++;
}
if (!logging_enabled || !check_data || handled_topics >= poll_ret) {
continue;
}
ifds = 1; // Begin from fds[1] again
pthread_mutex_lock(&logbuffer_mutex);
/* write time stamp message */
@ -828,6 +856,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);
@ -930,7 +974,12 @@ int sdlog2_thread_main(int argc, char *argv[])
/* --- ACTUATOR CONTROL --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.act_controls_sub, &buf.act_controls);
// TODO not implemented yet
log_msg.msg_type = LOG_ATTC_MSG;
log_msg.body.log_ATTC.roll = buf.act_controls.control[0];
log_msg.body.log_ATTC.pitch = buf.act_controls.control[1];
log_msg.body.log_ATTC.yaw = buf.act_controls.control[2];
log_msg.body.log_ATTC.thrust = buf.act_controls.control[3];
LOGBUFFER_WRITE_AND_COUNT(ATTC);
}
/* --- ACTUATOR CONTROL EFFECTIVE --- */

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

@ -132,6 +132,29 @@ struct log_GPS_s {
float vel_d;
float cog;
};
/* --- ATTC - ATTITUDE CONTROLS --- */
#define LOG_ATTC_MSG 9
struct log_ATTC_s {
float roll;
float pitch;
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 */
@ -145,6 +168,8 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(LPOS, "fffffffLLf", "X,Y,Z,VX,VY,VZ,Heading,HomeLat,HomeLon,HomeAlt"),
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);