forked from Archive/PX4-Autopilot
Merge branch 'master' of github.com:PX4/Firmware
This commit is contained in:
Lorenz Meier
commit
dc5a006285
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
|
|
@ -35,7 +35,7 @@
|
|||
* @file mavlink_log.h
|
||||
* MAVLink text logging.
|
||||
*
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
* @author Lorenz Meier <lorenz@px4.io>
|
||||
*/
|
||||
|
||||
#ifndef MAVLINK_LOG
|
||||
|
|
@ -99,6 +99,38 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...);
|
|||
*/
|
||||
#define mavlink_log_info(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_INFO, _text, ##__VA_ARGS__);
|
||||
|
||||
/**
|
||||
* Send a mavlink emergency message and print to console.
|
||||
*
|
||||
* @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0);
|
||||
* @param _text The text to log;
|
||||
*/
|
||||
#define mavlink_and_console_log_emergency(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, _text, ##__VA_ARGS__); \
|
||||
fprintf(stderr, "telem> "); \
|
||||
fprintf(stderr, _text, ##__VA_ARGS__); \
|
||||
fprintf(stderr, "\n");
|
||||
|
||||
/**
|
||||
* Send a mavlink critical message and print to console.
|
||||
*
|
||||
* @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0);
|
||||
* @param _text The text to log;
|
||||
*/
|
||||
#define mavlink_and_console_log_critical(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, _text, ##__VA_ARGS__); \
|
||||
fprintf(stderr, "telem> "); \
|
||||
fprintf(stderr, _text, ##__VA_ARGS__); \
|
||||
fprintf(stderr, "\n");
|
||||
|
||||
/**
|
||||
* Send a mavlink emergency message and print to console.
|
||||
*
|
||||
* @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0);
|
||||
* @param _text The text to log;
|
||||
*/
|
||||
#define mavlink_and_console_log_info(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_INFO, _text, ##__VA_ARGS__); \
|
||||
fprintf(stderr, "telem> "); \
|
||||
fprintf(stderr, _text, ##__VA_ARGS__); \
|
||||
fprintf(stderr, "\n");
|
||||
|
||||
struct mavlink_logmessage {
|
||||
char text[MAVLINK_LOG_MAXLEN + 1];
|
||||
|
|
|
|||
|
|
@ -342,6 +342,8 @@ private:
|
|||
MavlinkStreamStatustext(MavlinkStreamStatustext &);
|
||||
MavlinkStreamStatustext& operator = (const MavlinkStreamStatustext &);
|
||||
FILE *fp = nullptr;
|
||||
unsigned write_err_count = 0;
|
||||
static const unsigned write_err_threshold = 5;
|
||||
|
||||
protected:
|
||||
explicit MavlinkStreamStatustext(Mavlink *mavlink) : MavlinkStream(mavlink)
|
||||
|
|
@ -370,10 +372,21 @@ protected:
|
|||
/* write log messages in first instance to disk */
|
||||
if (_mavlink->get_instance_id() == 0) {
|
||||
if (fp) {
|
||||
fputs(msg.text, fp);
|
||||
fputs("\n", fp);
|
||||
fsync(fileno(fp));
|
||||
} else {
|
||||
if (EOF == fputs(msg.text, fp)) {
|
||||
write_err_count++;
|
||||
} else {
|
||||
write_err_count = 0;
|
||||
}
|
||||
|
||||
if (write_err_count >= write_err_threshold) {
|
||||
(void)fclose(fp);
|
||||
fp = nullptr;
|
||||
} else {
|
||||
(void)fputs("\n", fp);
|
||||
(void)fsync(fileno(fp));
|
||||
}
|
||||
|
||||
} else if (write_err_count < write_err_threshold) {
|
||||
/* string to hold the path to the log */
|
||||
char log_file_name[32] = "";
|
||||
char log_file_path[64] = "";
|
||||
|
|
@ -389,6 +402,10 @@ protected:
|
|||
strftime(log_file_name, sizeof(log_file_name), "msgs_%Y_%m_%d_%H_%M_%S.txt", &tt);
|
||||
snprintf(log_file_path, sizeof(log_file_path), "/fs/microsd/%s", log_file_name);
|
||||
fp = fopen(log_file_path, "ab");
|
||||
|
||||
/* write first message */
|
||||
fputs(msg.text, fp);
|
||||
fputs("\n", fp);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
|
|
@ -39,12 +39,14 @@
|
|||
*
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
* @author Anton Babushkin <anton.babushkin@me.com>
|
||||
* @author Ban Siesta <bansiesta@gmail.com>
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
#include <sys/types.h>
|
||||
#include <sys/stat.h>
|
||||
#include <sys/prctl.h>
|
||||
#include <sys/statfs.h>
|
||||
#include <fcntl.h>
|
||||
#include <errno.h>
|
||||
#include <unistd.h>
|
||||
|
|
@ -161,7 +163,9 @@ static const int MIN_BYTES_TO_WRITE = 512;
|
|||
|
||||
static bool _extended_logging = false;
|
||||
|
||||
static const char *log_root = "/fs/microsd/log";
|
||||
#define MOUNTPOINT "/fs/microsd"
|
||||
static const char *mountpoint = MOUNTPOINT;
|
||||
static const char *log_root = MOUNTPOINT "/log";
|
||||
static int mavlink_fd = -1;
|
||||
struct logbuffer_s lb;
|
||||
|
||||
|
|
@ -174,6 +178,7 @@ static char log_dir[32];
|
|||
/* statistics counters */
|
||||
static uint64_t start_time = 0;
|
||||
static unsigned long log_bytes_written = 0;
|
||||
static unsigned long last_checked_bytes_written = 0;
|
||||
static unsigned long log_msgs_written = 0;
|
||||
static unsigned long log_msgs_skipped = 0;
|
||||
|
||||
|
|
@ -188,6 +193,9 @@ static bool log_name_timestamp = false;
|
|||
/* helper flag to track system state changes */
|
||||
static bool flag_system_armed = false;
|
||||
|
||||
/* flag if warning about MicroSD card being almost full has already been sent */
|
||||
static bool space_warning_sent = false;
|
||||
|
||||
static pthread_t logwriter_pthread = 0;
|
||||
static pthread_attr_t logwriter_attr;
|
||||
|
||||
|
|
@ -247,6 +255,11 @@ static bool file_exist(const char *filename);
|
|||
|
||||
static int file_copy(const char *file_old, const char *file_new);
|
||||
|
||||
/**
|
||||
* Check if there is still free space available
|
||||
*/
|
||||
static int check_free_space(void);
|
||||
|
||||
static void handle_command(struct vehicle_command_s *cmd);
|
||||
|
||||
static void handle_status(struct vehicle_status_s *cmd);
|
||||
|
|
@ -390,8 +403,7 @@ int create_log_dir()
|
|||
}
|
||||
|
||||
/* print logging path, important to find log file later */
|
||||
warnx("log dir: %s", log_dir);
|
||||
mavlink_log_info(mavlink_fd, "[sdlog2] log dir: %s", log_dir);
|
||||
mavlink_and_console_log_info(mavlink_fd, "[sdlog2] log dir: %s", log_dir);
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
|
@ -431,7 +443,7 @@ int open_log_file()
|
|||
|
||||
if (file_number > MAX_NO_LOGFILE) {
|
||||
/* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */
|
||||
mavlink_log_critical(mavlink_fd, "[sdlog2] ERR: max files %d", MAX_NO_LOGFILE);
|
||||
mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] ERR: max files %d", MAX_NO_LOGFILE);
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
|
@ -439,12 +451,10 @@ int open_log_file()
|
|||
int fd = open(log_file_path, O_CREAT | O_WRONLY | O_DSYNC);
|
||||
|
||||
if (fd < 0) {
|
||||
warn("failed opening log: %s", log_file_name);
|
||||
mavlink_log_critical(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name);
|
||||
mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name);
|
||||
|
||||
} else {
|
||||
warnx("log file: %s", log_file_name);
|
||||
mavlink_log_info(mavlink_fd, "[sdlog2] log file: %s", log_file_name);
|
||||
mavlink_and_console_log_info(mavlink_fd, "[sdlog2] log file: %s", log_file_name);
|
||||
}
|
||||
|
||||
return fd;
|
||||
|
|
@ -485,7 +495,7 @@ int open_perf_file(const char* str)
|
|||
|
||||
if (file_number > MAX_NO_LOGFILE) {
|
||||
/* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */
|
||||
mavlink_log_critical(mavlink_fd, "[sdlog2] ERR: max files %d", MAX_NO_LOGFILE);
|
||||
mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] ERR: max files %d", MAX_NO_LOGFILE);
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
|
@ -493,12 +503,8 @@ int open_perf_file(const char* str)
|
|||
int fd = open(log_file_path, O_CREAT | O_WRONLY | O_DSYNC);
|
||||
|
||||
if (fd < 0) {
|
||||
warn("failed opening log: %s", log_file_name);
|
||||
mavlink_log_critical(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name);
|
||||
mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] failed opening: %s", log_file_name);
|
||||
|
||||
} else {
|
||||
warnx("log file: %s", log_file_name);
|
||||
mavlink_log_info(mavlink_fd, "[sdlog2] log file: %s", log_file_name);
|
||||
}
|
||||
|
||||
return fd;
|
||||
|
|
@ -560,6 +566,7 @@ static void *logwriter_thread(void *arg)
|
|||
pthread_mutex_unlock(&logbuffer_mutex);
|
||||
|
||||
if (available > 0) {
|
||||
|
||||
/* do heavy IO here */
|
||||
if (available > MAX_WRITE_CHUNK) {
|
||||
n = MAX_WRITE_CHUNK;
|
||||
|
|
@ -597,6 +604,16 @@ static void *logwriter_thread(void *arg)
|
|||
if (++poll_count == 10) {
|
||||
fsync(log_fd);
|
||||
poll_count = 0;
|
||||
|
||||
}
|
||||
|
||||
if (log_bytes_written - last_checked_bytes_written > 20*1024*1024) {
|
||||
/* check if space is available, if not stop everything */
|
||||
if (check_free_space() != OK) {
|
||||
logwriter_should_exit = true;
|
||||
main_thread_should_exit = true;
|
||||
}
|
||||
last_checked_bytes_written = log_bytes_written;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -611,15 +628,15 @@ static void *logwriter_thread(void *arg)
|
|||
|
||||
void sdlog2_start_log()
|
||||
{
|
||||
warnx("start logging");
|
||||
mavlink_log_info(mavlink_fd, "[sdlog2] start logging");
|
||||
mavlink_and_console_log_info(mavlink_fd, "[sdlog2] start logging");
|
||||
|
||||
/* create log dir if needed */
|
||||
if (create_log_dir() != 0) {
|
||||
mavlink_log_critical(mavlink_fd, "[sdlog2] error creating log dir");
|
||||
errx(1, "error creating log dir");
|
||||
mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] error creating log dir");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
|
||||
/* initialize statistics counter */
|
||||
log_bytes_written = 0;
|
||||
start_time = hrt_absolute_time();
|
||||
|
|
@ -657,8 +674,7 @@ void sdlog2_start_log()
|
|||
|
||||
void sdlog2_stop_log()
|
||||
{
|
||||
warnx("stop logging");
|
||||
mavlink_log_info(mavlink_fd, "[sdlog2] stop logging");
|
||||
mavlink_and_console_log_info(mavlink_fd, "[sdlog2] stop logging");
|
||||
|
||||
logging_enabled = false;
|
||||
|
||||
|
|
@ -784,7 +800,7 @@ int sdlog2_thread_main(int argc, char *argv[])
|
|||
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
|
||||
|
||||
if (mavlink_fd < 0) {
|
||||
warnx("failed to open MAVLink log stream, start mavlink app first");
|
||||
warnx("ERR: log stream, start mavlink app first");
|
||||
}
|
||||
|
||||
/* delay = 1 / rate (rate defined by -r option), default log rate: 50 Hz */
|
||||
|
|
@ -912,11 +928,17 @@ int sdlog2_thread_main(int argc, char *argv[])
|
|||
|
||||
}
|
||||
|
||||
|
||||
if (check_free_space() != OK) {
|
||||
errx(1, "ERR: MicroSD almost full");
|
||||
}
|
||||
|
||||
|
||||
/* create log root dir */
|
||||
int mkdir_ret = mkdir(log_root, S_IRWXU | S_IRWXG | S_IRWXO);
|
||||
|
||||
if (mkdir_ret != 0 && errno != EEXIST) {
|
||||
err(1, "failed creating log root dir: %s", log_root);
|
||||
err(1, "ERR: failed creating log dir: %s", log_root);
|
||||
}
|
||||
|
||||
/* copy conversion scripts */
|
||||
|
|
@ -1768,8 +1790,6 @@ int sdlog2_thread_main(int argc, char *argv[])
|
|||
|
||||
free(lb.data);
|
||||
|
||||
warnx("exiting");
|
||||
|
||||
thread_running = false;
|
||||
|
||||
return 0;
|
||||
|
|
@ -1802,7 +1822,7 @@ int file_copy(const char *file_old, const char *file_new)
|
|||
int ret = 0;
|
||||
|
||||
if (source == NULL) {
|
||||
warnx("failed opening input file to copy");
|
||||
warnx("ERR: open in");
|
||||
return 1;
|
||||
}
|
||||
|
||||
|
|
@ -1810,7 +1830,7 @@ int file_copy(const char *file_old, const char *file_new)
|
|||
|
||||
if (target == NULL) {
|
||||
fclose(source);
|
||||
warnx("failed to open output file to copy");
|
||||
warnx("ERR: open out");
|
||||
return 1;
|
||||
}
|
||||
|
||||
|
|
@ -1835,6 +1855,34 @@ int file_copy(const char *file_old, const char *file_new)
|
|||
return OK;
|
||||
}
|
||||
|
||||
int check_free_space()
|
||||
{
|
||||
/* use statfs to determine the number of blocks left */
|
||||
FAR struct statfs statfs_buf;
|
||||
if (statfs(mountpoint, &statfs_buf) != OK) {
|
||||
errx(ERROR, "ERR: statfs");
|
||||
}
|
||||
|
||||
/* use a threshold of 50 MiB */
|
||||
if (statfs_buf.f_bavail < (int)(50 * 1024 * 1024 / statfs_buf.f_bsize)) {
|
||||
mavlink_and_console_log_critical(mavlink_fd,
|
||||
"[sdlog2] no space on MicroSD: %u MiB",
|
||||
(unsigned int)(statfs_buf.f_bavail * statfs_buf.f_bsize) / (1024U * 1024U));
|
||||
/* we do not need a flag to remember that we sent this warning because we will exit anyway */
|
||||
return ERROR;
|
||||
|
||||
/* use a threshold of 100 MiB to send a warning */
|
||||
} else if (!space_warning_sent && statfs_buf.f_bavail < (int)(100 * 1024 * 1024 / statfs_buf.f_bsize)) {
|
||||
mavlink_and_console_log_critical(mavlink_fd,
|
||||
"[sdlog2] space on MicroSD low: %u MiB",
|
||||
(unsigned int)(statfs_buf.f_bavail * statfs_buf.f_bsize) / (1024U * 1024U));
|
||||
/* we don't want to flood the user with warnings */
|
||||
space_warning_sent = true;
|
||||
}
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
void handle_command(struct vehicle_command_s *cmd)
|
||||
{
|
||||
int param;
|
||||
|
|
|
|||
|
|
@ -78,89 +78,87 @@ float constrain(float val, float min, float max)
|
|||
*/
|
||||
|
||||
const MultirotorMixer::Rotor _config_quad_x[] = {
|
||||
{ -0.707107, 0.707107, 1.00 },
|
||||
{ 0.707107, -0.707107, 1.00 },
|
||||
{ 0.707107, 0.707107, -1.00 },
|
||||
{ -0.707107, -0.707107, -1.00 },
|
||||
{ -0.707107, 0.707107, 1.000000 },
|
||||
{ 0.707107, -0.707107, 1.000000 },
|
||||
{ 0.707107, 0.707107, -1.000000 },
|
||||
{ -0.707107, -0.707107, -1.000000 },
|
||||
};
|
||||
const MultirotorMixer::Rotor _config_quad_plus[] = {
|
||||
{ -1.000000, 0.000000, 1.00 },
|
||||
{ 1.000000, 0.000000, 1.00 },
|
||||
{ 0.000000, 1.000000, -1.00 },
|
||||
{ -0.000000, -1.000000, -1.00 },
|
||||
{ -1.000000, 0.000000, 1.000000 },
|
||||
{ 1.000000, 0.000000, 1.000000 },
|
||||
{ 0.000000, 1.000000, -1.000000 },
|
||||
{ -0.000000, -1.000000, -1.000000 },
|
||||
};
|
||||
//Add table for quad in V configuration, which is not generated by multi_tables!
|
||||
const MultirotorMixer::Rotor _config_quad_v[] = {
|
||||
{ -0.3223, 0.9466, 0.4242 },
|
||||
{ 0.3223, -0.9466, 1.0000 },
|
||||
{ 0.3223, 0.9466, -0.4242 },
|
||||
{ -0.3223, -0.9466, -1.0000 },
|
||||
{ -0.322266, 0.946649, 0.424200 },
|
||||
{ 0.322266, 0.946649, 1.000000 },
|
||||
{ 0.322266, 0.946649, -0.424200 },
|
||||
{ -0.322266, 0.946649, -1.000000 },
|
||||
};
|
||||
const MultirotorMixer::Rotor _config_quad_wide[] = {
|
||||
{ -0.927184, 0.374607, 1.00 },
|
||||
{ 0.777146, -0.629320, 1.00 },
|
||||
{ 0.927184, 0.374607, -1.00 },
|
||||
{ -0.777146, -0.629320, -1.00 },
|
||||
{ -0.927184, 0.374607, 1.000000 },
|
||||
{ 0.777146, -0.629320, 1.000000 },
|
||||
{ 0.927184, 0.374607, -1.000000 },
|
||||
{ -0.777146, -0.629320, -1.000000 },
|
||||
};
|
||||
const MultirotorMixer::Rotor _config_hex_x[] = {
|
||||
{ -1.000000, 0.000000, -1.00 },
|
||||
{ 1.000000, 0.000000, 1.00 },
|
||||
{ 0.500000, 0.866025, -1.00 },
|
||||
{ -0.500000, -0.866025, 1.00 },
|
||||
{ -0.500000, 0.866025, 1.00 },
|
||||
{ 0.500000, -0.866025, -1.00 },
|
||||
{ -1.000000, 0.000000, -1.000000 },
|
||||
{ 1.000000, 0.000000, 1.000000 },
|
||||
{ 0.500000, 0.866025, -1.000000 },
|
||||
{ -0.500000, -0.866025, 1.000000 },
|
||||
{ -0.500000, 0.866025, 1.000000 },
|
||||
{ 0.500000, -0.866025, -1.000000 },
|
||||
};
|
||||
const MultirotorMixer::Rotor _config_hex_plus[] = {
|
||||
{ 0.000000, 1.000000, -1.00 },
|
||||
{ -0.000000, -1.000000, 1.00 },
|
||||
{ 0.866025, -0.500000, -1.00 },
|
||||
{ -0.866025, 0.500000, 1.00 },
|
||||
{ 0.866025, 0.500000, 1.00 },
|
||||
{ -0.866025, -0.500000, -1.00 },
|
||||
{ 0.000000, 1.000000, -1.000000 },
|
||||
{ -0.000000, -1.000000, 1.000000 },
|
||||
{ 0.866025, -0.500000, -1.000000 },
|
||||
{ -0.866025, 0.500000, 1.000000 },
|
||||
{ 0.866025, 0.500000, 1.000000 },
|
||||
{ -0.866025, -0.500000, -1.000000 },
|
||||
};
|
||||
const MultirotorMixer::Rotor _config_hex_cox[] = {
|
||||
{ -0.866025, 0.500000, -1.00 },
|
||||
{ -0.866025, 0.500000, 1.00 },
|
||||
{ -0.000000, -1.000000, -1.00 },
|
||||
{ -0.000000, -1.000000, 1.00 },
|
||||
{ 0.866025, 0.500000, -1.00 },
|
||||
{ 0.866025, 0.500000, 1.00 },
|
||||
{ -0.866025, 0.500000, -1.000000 },
|
||||
{ -0.866025, 0.500000, 1.000000 },
|
||||
{ -0.000000, -1.000000, -1.000000 },
|
||||
{ -0.000000, -1.000000, 1.000000 },
|
||||
{ 0.866025, 0.500000, -1.000000 },
|
||||
{ 0.866025, 0.500000, 1.000000 },
|
||||
};
|
||||
const MultirotorMixer::Rotor _config_octa_x[] = {
|
||||
{ -0.382683, 0.923880, -1.00 },
|
||||
{ 0.382683, -0.923880, -1.00 },
|
||||
{ -0.923880, 0.382683, 1.00 },
|
||||
{ -0.382683, -0.923880, 1.00 },
|
||||
{ 0.382683, 0.923880, 1.00 },
|
||||
{ 0.923880, -0.382683, 1.00 },
|
||||
{ 0.923880, 0.382683, -1.00 },
|
||||
{ -0.923880, -0.382683, -1.00 },
|
||||
{ -0.382683, 0.923880, -1.000000 },
|
||||
{ 0.382683, -0.923880, -1.000000 },
|
||||
{ -0.923880, 0.382683, 1.000000 },
|
||||
{ -0.382683, -0.923880, 1.000000 },
|
||||
{ 0.382683, 0.923880, 1.000000 },
|
||||
{ 0.923880, -0.382683, 1.000000 },
|
||||
{ 0.923880, 0.382683, -1.000000 },
|
||||
{ -0.923880, -0.382683, -1.000000 },
|
||||
};
|
||||
const MultirotorMixer::Rotor _config_octa_plus[] = {
|
||||
{ 0.000000, 1.000000, -1.00 },
|
||||
{ -0.000000, -1.000000, -1.00 },
|
||||
{ -0.707107, 0.707107, 1.00 },
|
||||
{ -0.707107, -0.707107, 1.00 },
|
||||
{ 0.707107, 0.707107, 1.00 },
|
||||
{ 0.707107, -0.707107, 1.00 },
|
||||
{ 1.000000, 0.000000, -1.00 },
|
||||
{ -1.000000, 0.000000, -1.00 },
|
||||
{ 0.000000, 1.000000, -1.000000 },
|
||||
{ -0.000000, -1.000000, -1.000000 },
|
||||
{ -0.707107, 0.707107, 1.000000 },
|
||||
{ -0.707107, -0.707107, 1.000000 },
|
||||
{ 0.707107, 0.707107, 1.000000 },
|
||||
{ 0.707107, -0.707107, 1.000000 },
|
||||
{ 1.000000, 0.000000, -1.000000 },
|
||||
{ -1.000000, 0.000000, -1.000000 },
|
||||
};
|
||||
const MultirotorMixer::Rotor _config_octa_cox[] = {
|
||||
{ -0.707107, 0.707107, 1.00 },
|
||||
{ 0.707107, 0.707107, -1.00 },
|
||||
{ 0.707107, -0.707107, 1.00 },
|
||||
{ -0.707107, -0.707107, -1.00 },
|
||||
{ 0.707107, 0.707107, 1.00 },
|
||||
{ -0.707107, 0.707107, -1.00 },
|
||||
{ -0.707107, -0.707107, 1.00 },
|
||||
{ 0.707107, -0.707107, -1.00 },
|
||||
{ -0.707107, 0.707107, 1.000000 },
|
||||
{ 0.707107, 0.707107, -1.000000 },
|
||||
{ 0.707107, -0.707107, 1.000000 },
|
||||
{ -0.707107, -0.707107, -1.000000 },
|
||||
{ 0.707107, 0.707107, 1.000000 },
|
||||
{ -0.707107, 0.707107, -1.000000 },
|
||||
{ -0.707107, -0.707107, 1.000000 },
|
||||
{ 0.707107, -0.707107, -1.000000 },
|
||||
};
|
||||
const MultirotorMixer::Rotor _config_duorotor[] = {
|
||||
{ -1.000000, 0.000000, 0.00 },
|
||||
{ 1.000000, 0.000000, 0.00 },
|
||||
const MultirotorMixer::Rotor _config_twin_engine[] = {
|
||||
{ -1.000000, 0.000000, 0.000000 },
|
||||
{ 1.000000, 0.000000, 0.000000 },
|
||||
};
|
||||
|
||||
const MultirotorMixer::Rotor *_config_index[MultirotorMixer::MAX_GEOMETRY] = {
|
||||
&_config_quad_x[0],
|
||||
&_config_quad_plus[0],
|
||||
|
|
@ -172,7 +170,7 @@ const MultirotorMixer::Rotor *_config_index[MultirotorMixer::MAX_GEOMETRY] = {
|
|||
&_config_octa_x[0],
|
||||
&_config_octa_plus[0],
|
||||
&_config_octa_cox[0],
|
||||
&_config_duorotor[0],
|
||||
&_config_twin_engine[0],
|
||||
};
|
||||
const unsigned _config_rotor_count[MultirotorMixer::MAX_GEOMETRY] = {
|
||||
4, /* quad_x */
|
||||
|
|
|
|||
|
|
@ -21,6 +21,12 @@ set quad_plus {
|
|||
180 CW
|
||||
}
|
||||
|
||||
set quad_v {
|
||||
18.8 0.4242
|
||||
-18.8 1.0
|
||||
-18.8 -0.4242
|
||||
18.8 -1.0
|
||||
}
|
||||
|
||||
set quad_wide {
|
||||
68 CCW
|
||||
|
|
@ -89,11 +95,14 @@ set octa_cox {
|
|||
-135 CW
|
||||
}
|
||||
|
||||
set twin_engine {
|
||||
90 0.0
|
||||
-90 0.0
|
||||
}
|
||||
|
||||
set tables {quad_x quad_plus quad_wide hex_x hex_plus hex_cox octa_x octa_plus octa_cox}
|
||||
set tables {quad_x quad_plus quad_v quad_wide hex_x hex_plus hex_cox octa_x octa_plus octa_cox twin_engine}
|
||||
|
||||
|
||||
proc factors {a d} { puts [format "\t{ %9.6f, %9.6f, %5.2f }," [rcos [expr $a + 90]] [rcos $a] [expr -$d]]}
|
||||
proc factors {a d} { puts [format "\t{ %9.6f, %9.6f, %9.6f }," [rcos [expr $a + 90]] [rcos $a] [expr $d]]}
|
||||
|
||||
foreach table $tables {
|
||||
puts [format "const MultirotorMixer::Rotor _config_%s\[\] = {" $table]
|
||||
|
|
@ -101,9 +110,11 @@ foreach table $tables {
|
|||
upvar #0 $table angles
|
||||
foreach {angle dir} $angles {
|
||||
if {$dir == "CW"} {
|
||||
set dd -1.0
|
||||
} elseif {$dir == "CCW"} {
|
||||
set dd 1.0
|
||||
} else {
|
||||
set dd -1.0
|
||||
set dd $dir
|
||||
}
|
||||
factors $angle $dd
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue