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

Merge branch 'master' into inav_alt_gps. Use GPS for altitude estimation.

Browse Source
This commit is contained in:
Anton Babushkin
2013-10-27 22:03:07 +04:00
parent 7d4981d4b4 59c04adada
commit d309d0203c
73 changed files with 3361 additions and 874 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sdlog2/sdlog2.c
+106 -27
View File
@@ -58,6 +58,7 @@
#include <systemlib/err.h>
#include <unistd.h>
#include <drivers/drv_hrt.h>
#include <math.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
@@ -84,12 +85,14 @@
#include <uORB/topics/esc_status.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include <mavlink/mavlink_log.h>
#include "logbuffer.h"
#include "sdlog2_format.h"
#include "sdlog2_messages.h"
#include "sdlog2_version.h"
#define LOGBUFFER_WRITE_AND_COUNT(_msg) if (logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(_msg))) { \
log_msgs_written++; \
@@ -180,8 +183,17 @@ static void sdlog2_stop_log(void);
/**
* Write a header to log file: list of message formats.
*/
static void write_formats(int fd);
static int write_formats(int fd);
/**
* Write version message to log file.
*/
static int write_version(int fd);
/**
* Write parameters to log file.
*/
static int write_parameters(int fd);
static bool file_exist(const char *filename);
@@ -237,11 +249,11 @@ int sdlog2_main(int argc, char *argv[])
main_thread_should_exit = false;
deamon_task = task_spawn_cmd("sdlog2",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT - 30,
3000,
sdlog2_thread_main,
(const char **)argv);
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT - 30,
3000,
sdlog2_thread_main,
(const char **)argv);
exit(0);
}
@@ -354,10 +366,13 @@ static void *logwriter_thread(void *arg)
struct logbuffer_s *logbuf = (struct logbuffer_s *)arg;
int log_file = open_logfile();
int log_fd = open_logfile();
/* write log messages formats */
write_formats(log_file);
/* write log messages formats, version and parameters */
log_bytes_written += write_formats(log_fd);
log_bytes_written += write_version(log_fd);
log_bytes_written += write_parameters(log_fd);
fsync(log_fd);
int poll_count = 0;
@@ -396,7 +411,7 @@ static void *logwriter_thread(void *arg)
n = available;
}
n = write(log_file, read_ptr, n);
n = write(log_fd, read_ptr, n);
should_wait = (n == available) && !is_part;
@@ -411,21 +426,23 @@ static void *logwriter_thread(void *arg)
} else {
n = 0;
/* exit only with empty buffer */
if (main_thread_should_exit || logwriter_should_exit) {
break;
}
should_wait = true;
}
if (++poll_count == 10) {
fsync(log_file);
fsync(log_fd);
poll_count = 0;
}
}
fsync(log_file);
close(log_file);
fsync(log_fd);
close(log_fd);
return OK;
}
@@ -479,34 +496,92 @@ void sdlog2_stop_log()
/* wait for write thread to return */
int ret;
if ((ret = pthread_join(logwriter_pthread, NULL)) != 0) {
warnx("error joining logwriter thread: %i", ret);
}
logwriter_pthread = 0;
sdlog2_status();
}
void write_formats(int fd)
int write_formats(int fd)
{
/* construct message format packet */
struct {
LOG_PACKET_HEADER;
struct log_format_s body;
} log_format_packet = {
} log_msg_format = {
LOG_PACKET_HEADER_INIT(LOG_FORMAT_MSG),
};
/* fill message format packet for each format and write to log */
int i;
int written = 0;
for (i = 0; i < log_formats_num; i++) {
log_format_packet.body = log_formats[i];
log_bytes_written += write(fd, &log_format_packet, sizeof(log_format_packet));
/* fill message format packet for each format and write it */
for (int i = 0; i < log_formats_num; i++) {
log_msg_format.body = log_formats[i];
written += write(fd, &log_msg_format, sizeof(log_msg_format));
}
fsync(fd);
return written;
}
int write_version(int fd)
{
/* construct version message */
struct {
LOG_PACKET_HEADER;
struct log_VER_s body;
} log_msg_VER = {
LOG_PACKET_HEADER_INIT(LOG_VER_MSG),
};
/* fill version message and write it */
strncpy(log_msg_VER.body.fw_git, FW_GIT, sizeof(log_msg_VER.body.fw_git));
strncpy(log_msg_VER.body.arch, HW_ARCH, sizeof(log_msg_VER.body.arch));
return write(fd, &log_msg_VER, sizeof(log_msg_VER));
}
int write_parameters(int fd)
{
/* construct parameter message */
struct {
LOG_PACKET_HEADER;
struct log_PARM_s body;
} log_msg_PARM = {
LOG_PACKET_HEADER_INIT(LOG_PARM_MSG),
};
int written = 0;
param_t params_cnt = param_count();
for (param_t param = 0; param < params_cnt; param++) {
/* fill parameter message and write it */
strncpy(log_msg_PARM.body.name, param_name(param), sizeof(log_msg_PARM.body.name));
float value = NAN;
switch (param_type(param)) {
case PARAM_TYPE_INT32: {
int32_t i;
param_get(param, &i);
value = i; // cast integer to float
break;
}
case PARAM_TYPE_FLOAT:
param_get(param, &value);
break;
default:
break;
}
log_msg_PARM.body.value = value;
written += write(fd, &log_msg_PARM, sizeof(log_msg_PARM));
}
return written;
}
int sdlog2_thread_main(int argc, char *argv[])
@@ -584,12 +659,13 @@ int sdlog2_thread_main(int argc, char *argv[])
}
const char *converter_in = "/etc/logging/conv.zip";
char* converter_out = malloc(120);
char *converter_out = malloc(120);
sprintf(converter_out, "%s/conv.zip", folder_path);
if (file_copy(converter_in, converter_out)) {
errx(1, "unable to copy conversion scripts, exiting.");
}
free(converter_out);
/* only print logging path, important to find log file later */
@@ -603,6 +679,7 @@ int sdlog2_thread_main(int argc, char *argv[])
}
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! */
@@ -628,6 +705,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct esc_status_s esc;
struct vehicle_global_velocity_setpoint_s global_vel_sp;
} buf;
memset(&buf, 0, sizeof(buf));
struct {
@@ -799,7 +877,7 @@ int sdlog2_thread_main(int argc, char *argv[])
subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed));
fds[fdsc_count].fd = subs.airspeed_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
fdsc_count++;
/* --- ESCs --- */
subs.esc_sub = orb_subscribe(ORB_ID(esc_status));
@@ -890,7 +968,7 @@ int sdlog2_thread_main(int argc, char *argv[])
continue;
}
ifds = 1; // Begin from fds[1] again
ifds = 1; // begin from fds[1] again
pthread_mutex_lock(&logbuffer_mutex);
@@ -1160,8 +1238,8 @@ int sdlog2_thread_main(int argc, char *argv[])
/* --- ESCs --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(esc_status), subs.esc_sub, &buf.esc);
for (uint8_t i=0; i<buf.esc.esc_count; i++)
{
for (uint8_t i = 0; i < buf.esc.esc_count; i++) {
log_msg.msg_type = LOG_ESC_MSG;
log_msg.body.log_ESC.counter = buf.esc.counter;
log_msg.body.log_ESC.esc_count = buf.esc.esc_count;
@@ -1309,6 +1387,7 @@ void handle_status(struct vehicle_status_s *status)
{
// TODO use flag from actuator_armed here?
bool armed = status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR;
if (armed != flag_system_armed) {
flag_system_armed = armed;
src/modules/sdlog2/sdlog2_format.h
+4 -4
View File
@@ -85,10 +85,10 @@ struct log_format_s {
#define LOG_FORMAT(_name, _format, _labels) { \
.type = LOG_##_name##_MSG, \
.length = sizeof(struct log_##_name##_s) + LOG_PACKET_HEADER_LEN, \
.name = #_name, \
.format = _format, \
.labels = _labels \
.length = sizeof(struct log_##_name##_s) + LOG_PACKET_HEADER_LEN, \
.name = #_name, \
.format = _format, \
.labels = _labels \
}
#define LOG_FORMAT_MSG 0x80
src/modules/sdlog2/sdlog2_messages.h
+29 -29
View File
@@ -48,12 +48,6 @@
/* define message formats */
#pragma pack(push, 1)
/* --- TIME - TIME STAMP --- */
#define LOG_TIME_MSG 1
struct log_TIME_s {
uint64_t t;
};
/* --- ATT - ATTITUDE --- */
#define LOG_ATT_MSG 2
struct log_ATT_s {
@@ -253,38 +247,41 @@ struct log_GVSP_s {
float vz;
};
/* --- FWRV - FIRMWARE REVISION --- */
#define LOG_FWRV_MSG 20
struct log_FWRV_s {
char fw_revision[64];
};
/* --- DIST - DISTANCE TO SURFACE --- */
#define LOG_DIST_MSG 21
#define LOG_DIST_MSG 20
struct log_DIST_s {
float bottom;
float bottom_rate;
uint8_t flags;
};
/********** SYSTEM MESSAGES, ID > 0x80 **********/
/* --- TIME - TIME STAMP --- */
#define LOG_TIME_MSG 129
struct log_TIME_s {
uint64_t t;
};
/* --- VER - VERSION --- */
#define LOG_VER_MSG 130
struct log_VER_s {
char arch[16];
char fw_git[64];
};
/* --- PARM - PARAMETER --- */
#define LOG_PARM_MSG 131
struct log_PARM_s {
char name[16];
float value;
};
#pragma pack(pop)
/*
GIT_VERSION is defined at build time via a Makefile call to the
git command line. We create a fake log message format for
the firmware revision "FWRV" that is written to every log
header. This makes it easier to determine which version
of the firmware was running when a log was created.
*/
#define FREEZE_STR(s) #s
#define STRINGIFY(s) FREEZE_STR(s)
#define FW_VERSION_STR STRINGIFY(GIT_VERSION)
/* construct list of all message formats */
static const struct log_format_s log_formats[] = {
LOG_FORMAT(TIME, "Q", "StartTime"),
/* business-level messages, ID < 0x80 */
LOG_FORMAT(ATT, "ffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate"),
LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"),
LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
@@ -303,8 +300,11 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(GPSP, "BLLfffbBffff", "AltRel,Lat,Lon,Alt,Yaw,LoiterR,LoiterDir,NavCmd,P1,P2,P3,P4"),
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(FWRV, "Z", FW_VERSION_STR),
LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"),
/* system-level messages, ID >= 0x80 */
// FMT: don't write format of format message, it's useless
LOG_FORMAT(TIME, "Q", "StartTime"),
LOG_FORMAT(VER, "NZ", "Arch,FwGit"),
LOG_FORMAT(PARM, "Nf", "Name,Value"),
};
static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s);
src/modules/sdlog2/sdlog2_version.h
+62
View File
@@ -0,0 +1,62 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: Anton Babushkin <anton.babushkin@me.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file sdlog2_version.h
*
* Tools for system version detection.
*
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#ifndef SDLOG2_VERSION_H_
#define SDLOG2_VERSION_H_
/*
GIT_VERSION is defined at build time via a Makefile call to the
git command line.
*/
#define FREEZE_STR(s) #s
#define STRINGIFY(s) FREEZE_STR(s)
#define FW_GIT STRINGIFY(GIT_VERSION)
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
#define HW_ARCH "PX4FMU_V1"
#endif
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
#define HW_ARCH "PX4FMU_V2"
#endif
#endif /* SDLOG2_VERSION_H_ */