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

Merge branch 'master' into ms5611_newmath

Browse Source
This commit is contained in:
px4dev
2012-09-03 12:35:36 -07:00
parent eb8e3a2942 79801b1578
commit f92139f53b
179 changed files with 3051 additions and 987 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/ChangeLog.txt
+16 -2
View File
@@ -289,5 +289,19 @@
threds to no more than 3 of each priority. Bad things happen
when the existing logic tried to created several hundred test
treads!
* apps/nshlib/nsh.h: Both CONFIG_LIBC_STRERROR and CONFIG_NSH_STRERROR
must be defined to use strerror() with NSH.
* apps/examples/*/*_main.c, system/i2c/i2c_main.c, and others: Added
configuration variable CONFIG_USER_ENTRYPOINT that may be used to change
the default entry from user_start to some other symbol. Contributed by
Kate.
* apps/netutils/webserver/httpd/c: Fix a typo that as introduced in
version r4402: 'lese' instead of 'else' (Noted by Max Holtzberg).
* tools/mkfsdata.pl: The uIP web server CGI image making perl script was
moved from apps/netutils/webserver/makefsdata to nuttx/tools/mkfsdata.pl
(Part of a larger change submitted by Max Holtzberg).
* apps/netutils/webserver, apps/examples/uip, and apps/include/netutils/httpd.h:
The "canned" version of the uIP web servers content that was at
netutils/webserver/httpd_fsdata.c has been replaced with a dynamically
built configuration located at apps/examples/uip (Contributed by
Max Holtzberg).
apps/README.txt
+1 -1
View File
@@ -118,7 +118,7 @@ the NuttX configuration file:
CONFIG_BUILTIN_APP_START=<application name>
that application shall be invoked immediately after system starts
*instead* of the normal, default "user_start" entry point.
*instead* of the default "user_start" entry point.
Note that <application name> must be provided as: "hello",
will call:
apps/ardrone_interface/ardrone_interface.c
+67 -42
View File
@@ -63,12 +63,18 @@ __EXPORT int ardrone_interface_main(int argc, char *argv[]);
static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int ardrone_interface_task; /**< Handle of deamon task / thread */
static int ardrone_write; /**< UART to write AR.Drone commands to */
/**
* Mainloop of ardrone_interface.
*/
int ardrone_interface_thread_main(int argc, char *argv[]);
/**
* Open the UART connected to the motor controllers
*/
static int ardrone_open_uart(struct termios *uart_config_original);
/**
* Print the correct usage.
*/
@@ -128,63 +134,74 @@ int ardrone_interface_main(int argc, char *argv[])
exit(1);
}
static int ardrone_open_uart(struct termios *uart_config_original)
{
/* baud rate */
int speed = B115200;
int uart;
const char* uart_name = "/dev/ttyS1";
/* open uart */
printf("[ardrone_interface] UART is /dev/ttyS1, baud rate is 115200");
uart = open(uart_name, O_RDWR | O_NOCTTY);
/* Try to set baud rate */
struct termios uart_config;
int termios_state;
/* Back up the original uart configuration to restore it after exit */
if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
fprintf(stderr, "[ardrone_interface] ERROR getting baudrate / termios config for %s: %d\n", uart_name, termios_state);
close(uart);
return -1;
}
/* Fill the struct for the new configuration */
tcgetattr(uart, &uart_config);
/* Clear ONLCR flag (which appends a CR for every LF) */
uart_config.c_oflag &= ~ONLCR;
/* Set baud rate */
if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
fprintf(stderr, "[ardrone_interface] ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
close(uart);
return -1;
}
if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
fprintf(stderr, "[ardrone_interface] ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name);
close(uart);
return -1;
}
return uart;
}
int ardrone_interface_thread_main(int argc, char *argv[])
{
/* welcome user */
printf("[ardrone_interface] Control started, taking over motors\n");
/* default values for arguments */
char *ardrone_uart_name = "/dev/ttyS1";
/* File descriptors */
int ardrone_write;
int gpios;
enum {
CONTROL_MODE_RATES = 0,
CONTROL_MODE_ATTITUDE = 1,
} control_mode = CONTROL_MODE_ATTITUDE;
char *commandline_usage = "\tusage: ardrone_interface -d ardrone-devicename -m mode\n\tmodes are:\n\t\trates\n\t\tattitude\n";
char *commandline_usage = "\tusage: ardrone_interface start|status|stop [-t for motor test (10%% thrust)]\n";
bool motor_test_mode = false;
/* read commandline arguments */
for (int i = 1; i < argc; i++) {
if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { //ardrone set
if (argc > i + 1) {
ardrone_uart_name = argv[i + 1];
} else {
printf(commandline_usage);
return ERROR;
}
} else if (strcmp(argv[i], "-m") == 0 || strcmp(argv[i], "--mode") == 0) {
if (argc > i + 1) {
if (strcmp(argv[i + 1], "rates") == 0) {
control_mode = CONTROL_MODE_RATES;
} else if (strcmp(argv[i + 1], "attitude") == 0) {
control_mode = CONTROL_MODE_ATTITUDE;
} else {
printf(commandline_usage);
return ERROR;
}
} else {
printf(commandline_usage);
return ERROR;
}
} else if (strcmp(argv[i], "-t") == 0 || strcmp(argv[i], "--test") == 0) {
if (strcmp(argv[i], "-t") == 0 || strcmp(argv[i], "--test") == 0) {
motor_test_mode = true;
}
}
/* open uarts */
printf("[ardrone_interface] AR.Drone UART is %s\n", ardrone_uart_name);
ardrone_write = open(ardrone_uart_name, O_RDWR | O_NOCTTY | O_NDELAY);
struct termios uart_config_original;
ardrone_write = ardrone_open_uart(&uart_config_original);
if (ardrone_write < 0) {
fprintf(stderr, "[ardrone_interface] Failed opening AR.Drone UART, exiting.\n");
exit(ERROR);
@@ -279,11 +296,19 @@ int ardrone_interface_thread_main(int argc, char *argv[])
counter++;
}
/* restore old UART config */
int termios_state;
if ((termios_state = tcsetattr(ardrone_write, TCSANOW, &uart_config_original)) < 0) {
fprintf(stderr, "[ardrone_interface] ERROR setting baudrate / termios config for (tcsetattr)\n");
}
printf("[ardrone_interface] Restored original UART config, exiting..\n");
/* close uarts */
close(ardrone_write);
ar_multiplexing_deinit(gpios);
printf("[ardrone_interface] ending now...\n\n");
fflush(stdout);
thread_running = false;
apps/commander/state_machine_helper.c
+6 -1
View File
@@ -43,6 +43,7 @@
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_controls.h>
#include <systemlib/systemlib.h>
#include <arch/board/up_hrt.h>
#include <mavlink/mavlink_log.h>
@@ -121,7 +122,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
if (current_status->state_machine == SYSTEM_STATE_STANDBY
|| current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
/* set system flags according to state */
current_status->flag_system_armed = true;
current_status->flag_system_armed = false;
mavlink_log_critical(mavlink_fd, "[commander] Switched to PREFLIGHT state");
} else {
invalid_state = true;
@@ -198,6 +199,10 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
if (invalid_state == false || old_state != new_state) {
current_status->state_machine = new_state;
state_machine_publish(status_pub, current_status, mavlink_fd);
struct actuator_armed_s armed;
armed.armed = current_status->flag_system_armed;
orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
ret = OK;
}
if (invalid_state) {
apps/examples/README.txt
+8 -4
View File
@@ -1345,10 +1345,14 @@ examples/uip
file in the configuration driver with instruction to build applications
like:
CONFIGURED_APPS += uiplib
CONFIGURED_APPS += dhcpc
CONFIGURED_APPS += resolv
CONFIGURED_APPS += webserver
CONFIGURED_APPS += uiplib
CONFIGURED_APPS += dhcpc
CONFIGURED_APPS += resolv
CONFIGURED_APPS += webserver
NOTE: This example does depend on the perl script at
nuttx/tools/mkfsdata.pl. You must have perl installed on your
development system at /usr/bin/perl.
examples/usbserial
^^^^^^^^^^^^^^^^^^
apps/examples/adc/adc_main.c
+11 -19
View File
@@ -57,14 +57,6 @@
* Pre-processor Definitions
****************************************************************************/
#ifdef CONFIG_NSH_BUILTIN_APPS
# define MAIN_NAME adc_main
# define MAIN_STRING "adc_main: "
#else
# define MAIN_NAME user_start
# define MAIN_STRING "user_start: "
#endif
/****************************************************************************
* Private Types
****************************************************************************/
@@ -223,10 +215,10 @@ static void parse_args(FAR struct adc_state_s *adc, int argc, FAR char **argv)
****************************************************************************/
/****************************************************************************
* Name: user_start/adc_main
* Name: adc_main
****************************************************************************/
int MAIN_NAME(int argc, char *argv[])
int adc_main(int argc, char *argv[])
{
struct adc_msg_s sample[CONFIG_EXAMPLES_ADC_GROUPSIZE];
size_t readsize;
@@ -244,11 +236,11 @@ int MAIN_NAME(int argc, char *argv[])
* this test.
*/
message(MAIN_STRING "Initializing external ADC device\n");
message("adc_main: Initializing external ADC device\n");
ret = adc_devinit();
if (ret != OK)
{
message(MAIN_STRING "adc_devinit failed: %d\n", ret);
message("adc_main: adc_devinit failed: %d\n", ret);
errval = 1;
goto errout;
}
@@ -276,18 +268,18 @@ int MAIN_NAME(int argc, char *argv[])
*/
#if defined(CONFIG_NSH_BUILTIN_APPS) || defined(CONFIG_EXAMPLES_ADC_NSAMPLES)
message(MAIN_STRING "g_adcstate.count: %d\n", g_adcstate.count);
message("adc_main: g_adcstate.count: %d\n", g_adcstate.count);
#endif
/* Open the ADC device for reading */
message(MAIN_STRING "Hardware initialized. Opening the ADC device: %s\n",
message("adc_main: Hardware initialized. Opening the ADC device: %s\n",
g_adcstate.devpath);
fd = open(g_adcstate.devpath, O_RDONLY);
if (fd < 0)
{
message(MAIN_STRING "open %s failed: %d\n", g_adcstate.devpath, errno);
message("adc_main: open %s failed: %d\n", g_adcstate.devpath, errno);
errval = 2;
goto errout_with_dev;
}
@@ -322,17 +314,17 @@ int MAIN_NAME(int argc, char *argv[])
errval = errno;
if (errval != EINTR)
{
message(MAIN_STRING "read %s failed: %d\n",
message("adc_main: read %s failed: %d\n",
g_adcstate.devpath, errval);
errval = 3;
goto errout_with_dev;
}
message(MAIN_STRING "Interrupted read...\n");
message("adc_main: Interrupted read...\n");
}
else if (nbytes == 0)
{
message(MAIN_STRING "No data read, Ignoring\n");
message("adc_main: No data read, Ignoring\n");
}
/* Print the sample data on successful return */
@@ -342,7 +334,7 @@ int MAIN_NAME(int argc, char *argv[])
int nsamples = nbytes / sizeof(struct adc_msg_s);
if (nsamples * sizeof(struct adc_msg_s) != nbytes)
{
message(MAIN_STRING "read size=%d is not a multiple of sample size=%d, Ignoring\n",
message("adc_main: read size=%d is not a multiple of sample size=%d, Ignoring\n",
nbytes, sizeof(struct adc_msg_s));
}
else
apps/examples/buttons/main.c
+2 -12
View File
@@ -130,16 +130,6 @@
#define NUM_BUTTONS (MAX_BUTTON - MIN_BUTTON + 1)
#define BUTTON_INDEX(b) ((b)-MIN_BUTTON)
/* Is this being built as an NSH built-in application? */
#ifdef CONFIG_NSH_BUILTIN_APPS
# define MAIN_NAME buttons_main
# define MAIN_STRING "buttons_main: "
#else
# define MAIN_NAME user_start
# define MAIN_STRING "user_start: "
#endif
/****************************************************************************
* Private Types
****************************************************************************/
@@ -399,10 +389,10 @@ static int button7_handler(int irq, FAR void *context)
****************************************************************************/
/****************************************************************************
* user_start/buttons_main
* buttons_main
****************************************************************************/
int MAIN_NAME(int argc, char *argv[])
int buttons_main(int argc, char *argv[])
{
uint8_t newset;
irqstate_t flags;
apps/examples/can/can_main.c
+8 -16
View File
@@ -76,14 +76,6 @@
# define MAX_ID (1 << 11)
#endif
#ifdef CONFIG_NSH_BUILTIN_APPS
# define MAIN_NAME can_main
# define MAIN_STRING "can_main: "
#else
# define MAIN_NAME user_start
# define MAIN_STRING "user_start: "
#endif
/****************************************************************************
* Private Types
****************************************************************************/
@@ -109,10 +101,10 @@
****************************************************************************/
/****************************************************************************
* Name: user_start/can_main
* Name: can_main
****************************************************************************/
int MAIN_NAME(int argc, char *argv[])
int can_main(int argc, char *argv[])
{
#ifndef CONFIG_EXAMPLES_CAN_READONLY
struct can_msg_s txmsg;
@@ -150,31 +142,31 @@ int MAIN_NAME(int argc, char *argv[])
{
nmsgs = strtol(argv[1], NULL, 10);
}
message(MAIN_STRING "nmsgs: %d\n", nmsgs);
message("can_main: nmsgs: %d\n", nmsgs);
#elif defined(CONFIG_EXAMPLES_CAN_NMSGS)
message(MAIN_STRING "nmsgs: %d\n", CONFIG_EXAMPLES_CAN_NMSGS);
message("can_main: nmsgs: %d\n", CONFIG_EXAMPLES_CAN_NMSGS);
#endif
/* Initialization of the CAN hardware is performed by logic external to
* this test.
*/
message(MAIN_STRING "Initializing external CAN device\n");
message("can_main: Initializing external CAN device\n");
ret = can_devinit();
if (ret != OK)
{
message(MAIN_STRING "can_devinit failed: %d\n", ret);
message("can_main: can_devinit failed: %d\n", ret);
errval = 1;
goto errout;
}
/* Open the CAN device for reading */
message(MAIN_STRING "Hardware initialized. Opening the CAN device\n");
message("can_main: Hardware initialized. Opening the CAN device\n");
fd = open(CONFIG_EXAMPLES_CAN_DEVPATH, CAN_OFLAGS);
if (fd < 0)
{
message(MAIN_STRING "open %s failed: %d\n",
message("can_main: open %s failed: %d\n",
CONFIG_EXAMPLES_CAN_DEVPATH, errno);
errval = 2;
goto errout_with_dev;
apps/examples/hello/main.c
+2 -8
View File
@@ -53,16 +53,10 @@
****************************************************************************/
/****************************************************************************
* user_start/hello_main
* hello_main
****************************************************************************/
#ifdef CONFIG_EXAMPLES_HELLO_BUILTIN
# define MAIN_NAME hello_main
#else
# define MAIN_NAME user_start
#endif
int MAIN_NAME(int argc, char *argv[])
int hello_main(int argc, char *argv[])
{
printf("Hello, World!!\n");
return 0;
apps/examples/helloxx/main.cxx
+5 -18
View File
@@ -124,24 +124,11 @@ static CHelloWorld g_HelloWorld;
// Public Functions
//***************************************************************************
//***************************************************************************
// user_start
//***************************************************************************
/****************************************************************************
* Name: user_start/nxhello_main
* Name: helloxx_main
****************************************************************************/
#ifdef CONFIG_EXAMPLES_HELLOXX_BUILTIN
extern "C" int helloxx_main(int argc, char *argv[]);
# define MAIN_NAME helloxx_main
# define MAIN_STRING "helloxx_main: "
#else
# define MAIN_NAME user_start
# define MAIN_STRING "user_start: "
#endif
int MAIN_NAME(int argc, char *argv[])
int helloxx_main(int argc, char *argv[])
{
// If C++ initialization for static constructors is supported, then do
// that first
@@ -153,7 +140,7 @@ int MAIN_NAME(int argc, char *argv[])
// Exercise an explictly instantiated C++ object
CHelloWorld *pHelloWorld = new CHelloWorld;
printf(MAIN_STRING "Saying hello from the dynamically constructed instance\n");
printf("helloxx_main: Saying hello from the dynamically constructed instance\n");
pHelloWorld->HelloWorld();
// Exercise an C++ object instantiated on the stack
@@ -161,14 +148,14 @@ int MAIN_NAME(int argc, char *argv[])
#ifndef CONFIG_EXAMPLES_HELLOXX_NOSTACKCONST
CHelloWorld HelloWorld;
printf(MAIN_STRING "Saying hello from the instance constructed on the stack\n");
printf("helloxx_main: Saying hello from the instance constructed on the stack\n");
HelloWorld.HelloWorld();
#endif
// Exercise an statically constructed C++ object
#ifdef CONFIG_HAVE_CXXINITIALIZE
printf(MAIN_STRING "Saying hello from the statically constructed instance\n");
printf("helloxx_main: Saying hello from the statically constructed instance\n");
g_HelloWorld.HelloWorld();
#endif
apps/examples/lcdrw/lcdrw_main.c
+2 -8
View File
@@ -152,16 +152,10 @@ static inline int lcdrw_initialize(FAR struct lcdrw_instance_s *inst)
****************************************************************************/
/****************************************************************************
* Name: lcdrw_main/user_start
* Name: lcdrw_main
****************************************************************************/
#ifdef CONFIG_EXAMPLES_LCDRW_BUILTIN
# define MAIN_NAME lcdrw_main
#else
# define MAIN_NAME user_start
#endif
int MAIN_NAME(int argc, char *argv[])
int lcdrw_main(int argc, char *argv[])
{
struct lcdrw_instance_s inst;
nxgl_coord_t row;
apps/examples/mm/mm_main.c
+2 -2
View File
@@ -267,10 +267,10 @@ static void do_frees(void **mem, const int *size, const int *seq, int n)
****************************************************************************/
/****************************************************************************
* Name: user_start
* Name: mm_main
****************************************************************************/
int user_start(int argc, char *argv[])
int mm_main(int argc, char *argv[])
{
mm_showmallinfo();
apps/examples/mount/mount_main.c
+8 -8
View File
@@ -567,10 +567,10 @@ static void succeed_stat(const char *path)
****************************************************************************/
/****************************************************************************
* Name: user_start
* Name: mount_main
****************************************************************************/
int user_start(int argc, char *argv[])
int mount_main(int argc, char *argv[])
{
int ret;
@@ -580,18 +580,18 @@ int user_start(int argc, char *argv[])
ret = create_ramdisk();
if (ret < 0)
{
printf("user_start: ERROR failed to create RAM disk\n");
printf("mount_main: ERROR failed to create RAM disk\n");
return 1;
}
#endif
/* Mount the test file system (see arch/sim/src/up_deviceimage.c */
printf("user_start: mounting %s filesystem at target=%s with source=%s\n",
printf("mount_main: mounting %s filesystem at target=%s with source=%s\n",
g_filesystemtype, g_target, g_source);
ret = mount(g_source, g_target, g_filesystemtype, 0, NULL);
printf("user_start: mount() returned %d\n", ret);
printf("mount_main: mount() returned %d\n", ret);
if (ret == 0)
{
@@ -737,16 +737,16 @@ int user_start(int argc, char *argv[])
/* Unmount the file system */
printf("user_start: Try unmount(%s)\n", g_target);
printf("mount_main: Try unmount(%s)\n", g_target);
ret = umount(g_target);
if (ret != 0)
{
printf("user_start: ERROR umount() failed, errno %d\n", errno);
printf("mount_main: ERROR umount() failed, errno %d\n", errno);
g_nerrors++;
}
printf("user_start: %d errors reported\n", g_nerrors);
printf("mount_main: %d errors reported\n", g_nerrors);
}
fflush(stdout);
apps/examples/nsh/nsh_main.c
+2 -2
View File
@@ -84,10 +84,10 @@
****************************************************************************/
/****************************************************************************
* Name: user_start
* Name: nsh_main
****************************************************************************/
int user_start(int argc, char *argv[])
int nsh_main(int argc, char *argv[])
{
int exitval = 0;
int ret;
apps/examples/null/null_main.c
+2 -2
View File
@@ -58,10 +58,10 @@
****************************************************************************/
/****************************************************************************
* Name: user_start
* Name: null_main
****************************************************************************/
int user_start(int argc, char *argv[])
int null_main(int argc, char *argv[])
{
return 0;
}
apps/examples/ostest/main.c
+12 -20
View File
@@ -409,7 +409,7 @@ static int user_main(int argc, char *argv[])
check_test_memory_usage();
#endif /* CONFIG_PRIORITY_INHERITANCE && !CONFIG_DISABLE_SIGNALS && !CONFIG_DISABLE_PTHREAD */
/* Compare memory usage at time user_start started until
/* Compare memory usage at time ostest_main started until
* user_main exits. These should not be identical, but should
* be similar enough that we can detect any serious OS memory
* leaks.
@@ -458,18 +458,10 @@ static void stdio_test(void)
****************************************************************************/
/****************************************************************************
* user_start/ostest_main
* ostest_main
****************************************************************************/
#ifdef CONFIG_EXAMPLES_OSTEST_BUILTIN
# define MAIN_NAME ostest_main
# define MAIN_STRING "ostest_main: "
#else
# define MAIN_NAME user_start
# define MAIN_STRING "user_start: "
#endif
int MAIN_NAME(int argc, char *argv[])
int ostest_main(int argc, char *argv[])
{
int result;
@@ -492,19 +484,19 @@ int MAIN_NAME(int argc, char *argv[])
/* Set up some environment variables */
#ifndef CONFIG_DISABLE_ENVIRON
printf(MAIN_STRING "putenv(%s)\n", g_putenv_value);
printf("ostest_main: putenv(%s)\n", g_putenv_value);
putenv(g_putenv_value); /* Varaible1=BadValue3 */
printf(MAIN_STRING "setenv(%s, %s, TRUE)\n", g_var1_name, g_var1_value);
printf("ostest_main: setenv(%s, %s, TRUE)\n", g_var1_name, g_var1_value);
setenv(g_var1_name, g_var1_value, TRUE); /* Variable1=GoodValue1 */
printf(MAIN_STRING "setenv(%s, %s, FALSE)\n", g_var2_name, g_bad_value1);
printf("ostest_main: setenv(%s, %s, FALSE)\n", g_var2_name, g_bad_value1);
setenv(g_var2_name, g_bad_value1, FALSE); /* Variable2=BadValue1 */
printf(MAIN_STRING "setenv(%s, %s, TRUE)\n", g_var2_name, g_var2_value);
printf("ostest_main: setenv(%s, %s, TRUE)\n", g_var2_name, g_var2_value);
setenv(g_var2_name, g_var2_value, TRUE); /* Variable2=GoodValue2 */
printf(MAIN_STRING "setenv(%s, %s, FALSE)\n", g_var3_name, g_var3_name);
printf("ostest_main: setenv(%s, %s, FALSE)\n", g_var3_name, g_var3_name);
setenv(g_var3_name, g_var3_value, FALSE); /* Variable3=GoodValue3 */
printf(MAIN_STRING "setenv(%s, %s, FALSE)\n", g_var3_name, g_var3_name);
printf("ostest_main: setenv(%s, %s, FALSE)\n", g_var3_name, g_var3_name);
setenv(g_var3_name, g_bad_value2, FALSE); /* Variable3=GoodValue3 */
show_environment(true, true, true);
#endif
@@ -518,13 +510,13 @@ int MAIN_NAME(int argc, char *argv[])
#endif
if (result == ERROR)
{
printf(MAIN_STRING "ERROR Failed to start user_main\n");
printf("ostest_main: ERROR Failed to start user_main\n");
}
else
{
printf(MAIN_STRING "Started user_main at PID=%d\n", result);
printf("ostest_main: Started user_main at PID=%d\n", result);
}
printf(MAIN_STRING "Exitting\n");
printf("ostest_main: Exitting\n");
return 0;
}
apps/examples/pipe/pipe_main.c
+23 -23
View File
@@ -69,21 +69,21 @@
****************************************************************************/
/****************************************************************************
* Name: user_start
* Name: pipe_main
****************************************************************************/
int user_start(int argc, char *argv[])
int pipe_main(int argc, char *argv[])
{
int filedes[2];
int ret;
/* Test FIFO logic */
printf("\nuser_start: Performing FIFO test\n");
printf("\npipe_main: Performing FIFO test\n");
ret = mkfifo(FIFO_PATH1, 0666);
if (ret < 0)
{
fprintf(stderr, "user_start: mkfifo failed with errno=%d\n", errno);
fprintf(stderr, "pipe_main: mkfifo failed with errno=%d\n", errno);
return 1;
}
@@ -96,7 +96,7 @@ int user_start(int argc, char *argv[])
filedes[1] = open(FIFO_PATH1, O_WRONLY);
if (filedes[1] < 0)
{
fprintf(stderr, "user_start: Failed to open FIFO %s for writing, errno=%d\n",
fprintf(stderr, "pipe_main: Failed to open FIFO %s for writing, errno=%d\n",
FIFO_PATH1, errno);
return 2;
}
@@ -104,11 +104,11 @@ int user_start(int argc, char *argv[])
filedes[0] = open(FIFO_PATH1, O_RDONLY);
if (filedes[0] < 0)
{
fprintf(stderr, "user_start: Failed to open FIFO %s for reading, errno=%d\n",
fprintf(stderr, "pipe_main: Failed to open FIFO %s for reading, errno=%d\n",
FIFO_PATH1, errno);
if (close(filedes[1]) != 0)
{
fprintf(stderr, "user_start: close failed: %d\n", errno);
fprintf(stderr, "pipe_main: close failed: %d\n", errno);
}
return 3;
}
@@ -118,28 +118,28 @@ int user_start(int argc, char *argv[])
ret = transfer_test(filedes[0], filedes[1]);
if (close(filedes[0]) != 0)
{
fprintf(stderr, "user_start: close failed: %d\n", errno);
fprintf(stderr, "pipe_main: close failed: %d\n", errno);
}
if (close(filedes[1]) != 0)
{
fprintf(stderr, "user_start: close failed: %d\n", errno);
fprintf(stderr, "pipe_main: close failed: %d\n", errno);
}
/* unlink(FIFO_PATH1); fails */
if (ret != 0)
{
fprintf(stderr, "user_start: FIFO test FAILED (%d)\n", ret);
fprintf(stderr, "pipe_main: FIFO test FAILED (%d)\n", ret);
return 4;
}
printf("user_start: FIFO test PASSED\n");
printf("pipe_main: FIFO test PASSED\n");
/* Test PIPE logic */
printf("\nuser_start: Performing pipe test\n");
printf("\npipe_main: Performing pipe test\n");
ret = pipe(filedes);
if (ret < 0)
{
fprintf(stderr, "user_start: pipe failed with errno=%d\n", errno);
fprintf(stderr, "pipe_main: pipe failed with errno=%d\n", errno);
return 5;
}
@@ -148,41 +148,41 @@ int user_start(int argc, char *argv[])
ret = transfer_test(filedes[0], filedes[1]);
if (close(filedes[0]) != 0)
{
fprintf(stderr, "user_start: close failed: %d\n", errno);
fprintf(stderr, "pipe_main: close failed: %d\n", errno);
}
if (close(filedes[1]) != 0)
{
fprintf(stderr, "user_start: close failed: %d\n", errno);
fprintf(stderr, "pipe_main: close failed: %d\n", errno);
}
if (ret != 0)
{
fprintf(stderr, "user_start: PIPE test FAILED (%d)\n", ret);
fprintf(stderr, "pipe_main: PIPE test FAILED (%d)\n", ret);
return 6;
}
printf("user_start: PIPE test PASSED\n");
printf("pipe_main: PIPE test PASSED\n");
/* Perform the FIFO interlock test */
printf("\nuser_start: Performing pipe interlock test\n");
printf("\npipe_main: Performing pipe interlock test\n");
ret = interlock_test();
if (ret != 0)
{
fprintf(stderr, "user_start: FIFO interlock test FAILED (%d)\n", ret);
fprintf(stderr, "pipe_main: FIFO interlock test FAILED (%d)\n", ret);
return 7;
}
printf("user_start: PIPE interlock test PASSED\n");
printf("pipe_main: PIPE interlock test PASSED\n");
/* Perform the pipe redirection test */
printf("\nuser_start: Performing redirection test\n");
printf("\npipe_main: Performing redirection test\n");
ret = redirection_test();
if (ret != 0)
{
fprintf(stderr, "user_start: FIFO redirection test FAILED (%d)\n", ret);
fprintf(stderr, "pipe_main: FIFO redirection test FAILED (%d)\n", ret);
return 7;
}
printf("user_start: PIPE redirection test PASSED\n");
printf("pipe_main: PIPE redirection test PASSED\n");
fflush(stdout);
return 0;
apps/examples/pipe/redirect_test.c
+3 -3
View File
@@ -301,16 +301,16 @@ int redirection_test(void)
if (close(filedes[0]) != 0)
{
fprintf(stderr, "user_start: close failed: %d\n", errno);
fprintf(stderr, "pipe_main: close failed: %d\n", errno);
}
if (close(filedes[1]) != 0)
{
fprintf(stderr, "user_start: close failed: %d\n", errno);
fprintf(stderr, "pipe_main: close failed: %d\n", errno);
}
if (ret != 0)
{
fprintf(stderr, "user_start: PIPE test FAILED (%d)\n", ret);
fprintf(stderr, "pipe_main: PIPE test FAILED (%d)\n", ret);
return 6;
}
apps/examples/poll/poll_main.c
+18 -18
View File
@@ -74,10 +74,10 @@
****************************************************************************/
/****************************************************************************
* Name: user_start
* Name: poll_main
****************************************************************************/
int user_start(int argc, char *argv[])
int poll_main(int argc, char *argv[])
{
char buffer[64];
ssize_t nbytes;
@@ -94,20 +94,20 @@ int user_start(int argc, char *argv[])
/* Open FIFOs */
message("\nuser_start: Creating FIFO %s\n", FIFO_PATH1);
message("\npoll_main: Creating FIFO %s\n", FIFO_PATH1);
ret = mkfifo(FIFO_PATH1, 0666);
if (ret < 0)
{
message("user_start: mkfifo failed: %d\n", errno);
message("poll_main: mkfifo failed: %d\n", errno);
exitcode = 1;
goto errout;
}
message("\nuser_start: Creating FIFO %s\n", FIFO_PATH2);
message("\npoll_main: Creating FIFO %s\n", FIFO_PATH2);
ret = mkfifo(FIFO_PATH2, 0666);
if (ret < 0)
{
message("user_start: mkfifo failed: %d\n", errno);
message("poll_main: mkfifo failed: %d\n", errno);
exitcode = 2;
goto errout;
}
@@ -117,7 +117,7 @@ int user_start(int argc, char *argv[])
fd1 = open(FIFO_PATH1, O_WRONLY);
if (fd1 < 0)
{
message("user_start: Failed to open FIFO %s for writing, errno=%d\n",
message("poll_main: Failed to open FIFO %s for writing, errno=%d\n",
FIFO_PATH1, errno);
exitcode = 3;
goto errout;
@@ -126,7 +126,7 @@ int user_start(int argc, char *argv[])
fd2 = open(FIFO_PATH2, O_WRONLY);
if (fd2 < 0)
{
message("user_start: Failed to open FIFO %s for writing, errno=%d\n",
message("poll_main: Failed to open FIFO %s for writing, errno=%d\n",
FIFO_PATH2, errno);
exitcode = 4;
goto errout;
@@ -134,39 +134,39 @@ int user_start(int argc, char *argv[])
/* Start the listeners */
message("user_start: Starting poll_listener thread\n");
message("poll_main: Starting poll_listener thread\n");
ret = pthread_create(&tid1, NULL, poll_listener, NULL);
if (ret != 0)
{
message("user_start: Failed to create poll_listener thread: %d\n", ret);
message("poll_main: Failed to create poll_listener thread: %d\n", ret);
exitcode = 5;
goto errout;
}
message("user_start: Starting select_listener thread\n");
message("poll_main: Starting select_listener thread\n");
ret = pthread_create(&tid2, NULL, select_listener, NULL);
if (ret != 0)
{
message("user_start: Failed to create select_listener thread: %d\n", ret);
message("poll_main: Failed to create select_listener thread: %d\n", ret);
exitcode = 6;
goto errout;
}
#ifdef HAVE_NETPOLL
#ifdef CONFIG_NET_TCPBACKLOG
message("user_start: Starting net_listener thread\n");
message("poll_main: Starting net_listener thread\n");
ret = pthread_create(&tid3, NULL, net_listener, NULL);
#else
message("user_start: Starting net_reader thread\n");
message("poll_main: Starting net_reader thread\n");
ret = pthread_create(&tid3, NULL, net_reader, NULL);
#endif
if (ret != 0)
{
message("user_start: Failed to create net_listener thread: %d\n", ret);
message("poll_main: Failed to create net_listener thread: %d\n", ret);
}
#endif
@@ -182,7 +182,7 @@ int user_start(int argc, char *argv[])
nbytes = write(fd1, buffer, strlen(buffer));
if (nbytes < 0)
{
message("user_start: Write to fd1 failed: %d\n", errno);
message("poll_main: Write to fd1 failed: %d\n", errno);
exitcode = 7;
goto errout;
}
@@ -190,12 +190,12 @@ int user_start(int argc, char *argv[])
nbytes = write(fd2, buffer, strlen(buffer));
if (nbytes < 0)
{
message("user_start: Write fd2 failed: %d\n", errno);
message("poll_main: Write fd2 failed: %d\n", errno);
exitcode = 8;
goto errout;
}
message("\nuser_start: Sent '%s' (%d bytes)\n", buffer, nbytes);
message("\npoll_main: Sent '%s' (%d bytes)\n", buffer, nbytes);
msgflush();
/* Wait awhile. This delay should be long enough that the
apps/examples/pwm/pwm_main.c
+1 -1
View File
@@ -269,7 +269,7 @@ static void parse_args(FAR struct pwm_state_s *pwm, int argc, FAR char **argv)
****************************************************************************/
/****************************************************************************
* Name: user_start/pwm_main
* Name: pwm_main
****************************************************************************/
int pwm_main(int argc, char *argv[])
apps/examples/qencoder/qe_main.c
+12 -20
View File
@@ -59,14 +59,6 @@
* Pre-processor Definitions
****************************************************************************/
#ifdef CONFIG_NSH_BUILTIN_APPS
# define MAIN_NAME qe_main
# define MAIN_STRING "qe_main: "
#else
# define MAIN_NAME user_start
# define MAIN_STRING "user_start: "
#endif
/****************************************************************************
* Private Types
****************************************************************************/
@@ -245,10 +237,10 @@ static void parse_args(int argc, FAR char **argv)
****************************************************************************/
/****************************************************************************
* Name: user_start/qe_main
* Name: qe_main
****************************************************************************/
int MAIN_NAME(int argc, char *argv[])
int qe_main(int argc, char *argv[])
{
int32_t position;
int fd;
@@ -266,11 +258,11 @@ int MAIN_NAME(int argc, char *argv[])
* this test.
*/
message(MAIN_STRING "Initializing external encoder(s)\n");
message("qe_main: Initializing external encoder(s)\n");
ret = qe_devinit();
if (ret != OK)
{
message(MAIN_STRING "qe_devinit failed: %d\n", ret);
message("qe_main: qe_devinit failed: %d\n", ret);
exitval = EXIT_FAILURE;
goto errout;
}
@@ -289,13 +281,13 @@ int MAIN_NAME(int argc, char *argv[])
/* Open the encoder device for reading */
message(MAIN_STRING "Hardware initialized. Opening the encoder device: %s\n",
message("qe_main: Hardware initialized. Opening the encoder device: %s\n",
g_qeexample.devpath);
fd = open(g_qeexample.devpath, O_RDONLY);
if (fd < 0)
{
message(MAIN_STRING "open %s failed: %d\n", g_qeexample.devpath, errno);
message("qe_main: open %s failed: %d\n", g_qeexample.devpath, errno);
exitval = EXIT_FAILURE;
goto errout_with_dev;
}
@@ -304,11 +296,11 @@ int MAIN_NAME(int argc, char *argv[])
if (g_qeexample.reset)
{
message(MAIN_STRING "Resetting the count...\n");
message("qe_main: Resetting the count...\n");
ret = ioctl(fd, QEIOC_RESET, 0);
if (ret < 0)
{
message(MAIN_STRING "ioctl(QEIOC_RESET) failed: %d\n", errno);
message("qe_main: ioctl(QEIOC_RESET) failed: %d\n", errno);
exitval = EXIT_FAILURE;
goto errout_with_dev;
}
@@ -319,10 +311,10 @@ int MAIN_NAME(int argc, char *argv[])
*/
#if defined(CONFIG_NSH_BUILTIN_APPS)
message(MAIN_STRING "Number of samples: %d\n", g_qeexample.nloops);
message("qe_main: Number of samples: %d\n", g_qeexample.nloops);
for (nloops = 0; nloops < g_qeexample.nloops; nloops++)
#elif defined(CONFIG_EXAMPLES_QENCODER_NSAMPLES)
message(MAIN_STRING "Number of samples: %d\n", CONFIG_EXAMPLES_QENCODER_NSAMPLES);
message("qe_main: Number of samples: %d\n", CONFIG_EXAMPLES_QENCODER_NSAMPLES);
for (nloops = 0; nloops < CONFIG_EXAMPLES_QENCODER_NSAMPLES; nloops++)
#else
for (;;)
@@ -339,7 +331,7 @@ int MAIN_NAME(int argc, char *argv[])
ret = ioctl(fd, QEIOC_POSITION, (unsigned long)((uintptr_t)&position));
if (ret < 0)
{
message(MAIN_STRING "ioctl(QEIOC_POSITION) failed: %d\n", errno);
message("qe_main: ioctl(QEIOC_POSITION) failed: %d\n", errno);
exitval = EXIT_FAILURE;
goto errout_with_dev;
}
@@ -348,7 +340,7 @@ int MAIN_NAME(int argc, char *argv[])
else
{
message(MAIN_STRING "%3d. %d\n", nloops+1, position);
message("qe_main: %3d. %d\n", nloops+1, position);
}
/* Delay a little bit */
apps/examples/romfs/romfs_main.c
+2 -2
View File
@@ -452,10 +452,10 @@ static void checkdirectories(struct node_s *entry)
****************************************************************************/
/****************************************************************************
* Name: user_start
* Name: romfs_main
****************************************************************************/
int user_start(int argc, char *argv[])
int romfs_main(int argc, char *argv[])
{
int ret;
apps/examples/serloop/main.c
+2 -2
View File
@@ -56,10 +56,10 @@
****************************************************************************/
/****************************************************************************
* user_start
* serloop_main
****************************************************************************/
int user_start(int argc, char *argv[])
int serloop_main(int argc, char *argv[])
{
#ifdef CONFIG_EXAMPLES_SERLOOP_BUFIO
int ch;
apps/examples/watchdog/watchdog_main.c
+1 -1
View File
@@ -217,7 +217,7 @@ static void parse_args(FAR struct wdog_example_s *wdog, int argc, FAR char **arg
****************************************************************************/
/****************************************************************************
* Name: user_start/wdog_main
* Name: wdog_main
****************************************************************************/
int wdog_main(int argc, char *argv[])
apps/interpreters/README.txt
+1 -1
View File
@@ -58,7 +58,7 @@ pcode
Pascal P-Code at runtime. To use this example, place the following in
your appconfig file"
# Path to example in apps/examples containing the user_start entry point
# Path to example in apps/examples containing the passhello_main entry point
CONFIGURED_APPS += examples/pashello
apps/mavlink/mavlink.c
+173 -136
View File
@@ -62,7 +62,6 @@
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/rc_channels.h>
#include <uORB/topics/ardrone_control.h>
#include <uORB/topics/fixedwing_control.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_global_position.h>
@@ -74,6 +73,7 @@
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <systemlib/param/param.h>
@@ -126,16 +126,15 @@ static struct vehicle_attitude_s hil_attitude;
static struct vehicle_global_position_s hil_global_pos;
static struct fixedwing_control_s fw_control;
static struct ardrone_motors_setpoint_s ardrone_motors;
static struct vehicle_command_s vcmd;
static struct actuator_armed_s armed;
static orb_advert_t pub_hil_global_pos = -1;
static orb_advert_t ardrone_motors_pub = -1;
static orb_advert_t cmd_pub = -1;
static int local_pos_sub = -1;
static orb_advert_t flow_pub = -1;
static orb_advert_t global_position_setpoint_pub = -1;
static orb_advert_t local_position_setpoint_pub = -1;
@@ -143,6 +142,8 @@ static bool mavlink_hil_enabled = false;
static char mavlink_message_string[51] = {0};
static int baudrate = 57600;
/* interface mode */
static enum {
MAVLINK_INTERFACE_MODE_OFFBOARD,
@@ -159,6 +160,12 @@ static struct mavlink_subscriptions {
int act_3_sub;
int gps_sub;
int man_control_sp_sub;
int armed_sub;
int actuators_sub;
int local_pos_sub;
int spa_sub;
int spl_sub;
int spg_sub;
bool initialized;
} mavlink_subs = {
.sensor_sub = 0,
@@ -170,6 +177,12 @@ static struct mavlink_subscriptions {
.act_3_sub = 0,
.gps_sub = 0,
.man_control_sp_sub = 0,
.armed_sub = 0,
.actuators_sub = 0,
.local_pos_sub = 0,
.spa_sub = 0,
.spl_sub = 0,
.spg_sub = 0,
.initialized = false
};
@@ -192,7 +205,7 @@ int set_hil_on_off(bool hil_enabled);
/**
* Translate the custom state into standard mavlink modes and state.
*/
void get_mavlink_mode_and_state(const struct vehicle_status_s *c_status, uint8_t *mavlink_state, uint8_t *mavlink_mode);
void get_mavlink_mode_and_state(const struct vehicle_status_s *c_status, const struct actuator_armed_s *actuator, uint8_t *mavlink_state, uint8_t *mavlink_mode);
int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb);
@@ -345,16 +358,34 @@ int set_hil_on_off(bool hil_enabled)
printf("\n pub_hil_attitude :%i\n", pub_hil_attitude);
printf("\n pub_hil_global_pos :%i\n", pub_hil_global_pos);
if (pub_hil_attitude > 0 && pub_hil_global_pos > 0) {
mavlink_hil_enabled = true;
mavlink_hil_enabled = true;
/* ramp up some HIL-related subscriptions */
unsigned hil_rate_interval;
if (baudrate < 19200) {
/* 10 Hz */
hil_rate_interval = 100;
} else if (baudrate < 38400) {
/* 10 Hz */
hil_rate_interval = 100;
} else if (baudrate < 115200) {
/* 20 Hz */
hil_rate_interval = 50;
} else if (baudrate < 460800) {
/* 50 Hz */
hil_rate_interval = 20;
} else {
ret = ERROR;
/* 100 Hz */
hil_rate_interval = 10;
}
orb_set_interval(mavlink_subs.spa_sub, hil_rate_interval);
}
if (!hil_enabled && mavlink_hil_enabled) {
mavlink_hil_enabled = false;
orb_set_interval(mavlink_subs.spa_sub, 200);
(void)close(pub_hil_attitude);
(void)close(pub_hil_global_pos);
@@ -365,7 +396,7 @@ int set_hil_on_off(bool hil_enabled)
return ret;
}
void get_mavlink_mode_and_state(const struct vehicle_status_s *c_status, uint8_t *mavlink_state, uint8_t *mavlink_mode)
void get_mavlink_mode_and_state(const struct vehicle_status_s *c_status, const struct actuator_armed_s *actuator, uint8_t *mavlink_state, uint8_t *mavlink_mode)
{
/* reset MAVLink mode bitfield */
*mavlink_mode = 0;
@@ -375,70 +406,65 @@ void get_mavlink_mode_and_state(const struct vehicle_status_s *c_status, uint8_t
*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
}
/* set arming state */
if (actuator->armed) {
*mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
} else {
*mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
}
switch (c_status->state_machine) {
case SYSTEM_STATE_PREFLIGHT:
if (c_status->flag_preflight_gyro_calibration ||
c_status->flag_preflight_mag_calibration ||
c_status->flag_preflight_accel_calibration) {
*mavlink_state = MAV_STATE_CALIBRATING;
*mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
} else {
*mavlink_state = MAV_STATE_UNINIT;
*mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
}
break;
case SYSTEM_STATE_STANDBY:
*mavlink_state = MAV_STATE_STANDBY;
*mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
break;
case SYSTEM_STATE_GROUND_READY:
*mavlink_state = MAV_STATE_ACTIVE;
*mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
break;
case SYSTEM_STATE_MANUAL:
*mavlink_state = MAV_STATE_ACTIVE;
*mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
break;
case SYSTEM_STATE_STABILIZED:
*mavlink_state = MAV_STATE_ACTIVE;
*mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
*mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
break;
case SYSTEM_STATE_AUTO:
*mavlink_state = MAV_STATE_ACTIVE;
*mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
*mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
break;
case SYSTEM_STATE_MISSION_ABORT:
*mavlink_state = MAV_STATE_EMERGENCY;
*mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
break;
case SYSTEM_STATE_EMCY_LANDING:
*mavlink_state = MAV_STATE_EMERGENCY;
*mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
break;
case SYSTEM_STATE_EMCY_CUTOFF:
*mavlink_state = MAV_STATE_EMERGENCY;
*mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
break;
case SYSTEM_STATE_GROUND_ERROR:
*mavlink_state = MAV_STATE_EMERGENCY;
*mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
break;
case SYSTEM_STATE_REBOOT:
*mavlink_state = MAV_STATE_POWEROFF;
*mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
break;
}
@@ -495,6 +521,10 @@ static int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int ma
/* senser sub triggers scaled IMU */
if (subs->sensor_sub) orb_set_interval(subs->sensor_sub, min_interval);
break;
case MAVLINK_MSG_ID_HIGHRES_IMU:
/* senser sub triggers highres IMU */
if (subs->sensor_sub) orb_set_interval(subs->sensor_sub, min_interval);
break;
case MAVLINK_MSG_ID_RAW_IMU:
/* senser sub triggers RAW IMU */
if (subs->sensor_sub) orb_set_interval(subs->sensor_sub, min_interval);
@@ -557,6 +587,7 @@ static void *uorb_receiveloop(void *arg)
struct vehicle_attitude_setpoint_s att_sp;
struct actuator_outputs_s act_outputs;
struct manual_control_setpoint_s man_control;
struct actuator_controls_s actuators;
} buf;
/* --- SENSORS RAW VALUE --- */
@@ -582,14 +613,6 @@ static void *uorb_receiveloop(void *arg)
fds[fdsc_count].events = POLLIN;
fdsc_count++;
// /* --- ARDRONE CONTROL --- */
// /* subscribe to ORB for AR.Drone controller outputs */
// int ar_sub = orb_subscribe(ORB_ID(ardrone_control));
// orb_set_interval(ar_sub, 200); /* 5Hz updates */
// fds[fdsc_count].fd = ar_sub;
// fds[fdsc_count].events = POLLIN;
// fdsc_count++;
/* --- SYSTEM STATE --- */
/* struct already globally allocated */
/* subscribe to topic */
@@ -608,15 +631,6 @@ static void *uorb_receiveloop(void *arg)
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- FIXED WING CONTROL VALUE --- */
/* struct already globally allocated */
/* subscribe to ORB for fixed wing control */
int fw_sub = orb_subscribe(ORB_ID(fixedwing_control));
orb_set_interval(fw_sub, 50); /* 20 Hz updates */
fds[fdsc_count].fd = fw_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- GLOBAL POS VALUE --- */
/* struct already globally allocated and topic already subscribed */
fds[fdsc_count].fd = subs->global_pos_sub;
@@ -625,34 +639,34 @@ static void *uorb_receiveloop(void *arg)
/* --- LOCAL POS VALUE --- */
/* struct and topic already globally subscribed */
fds[fdsc_count].fd = local_pos_sub;
fds[fdsc_count].fd = subs->local_pos_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- GLOBAL SETPOINT VALUE --- */
/* subscribe to ORB for local setpoint */
/* struct already allocated */
int spg_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
orb_set_interval(spg_sub, 2000); /* 0.5 Hz updates */
fds[fdsc_count].fd = spg_sub;
subs->spg_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
orb_set_interval(subs->spg_sub, 2000); /* 0.5 Hz updates */
fds[fdsc_count].fd = subs->spg_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- LOCAL SETPOINT VALUE --- */
/* subscribe to ORB for local setpoint */
/* struct already allocated */
int spl_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
orb_set_interval(spl_sub, 2000); /* 0.5 Hz updates */
fds[fdsc_count].fd = spl_sub;
subs->spl_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
orb_set_interval(subs->spl_sub, 2000); /* 0.5 Hz updates */
fds[fdsc_count].fd = subs->spl_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- ATTITUDE SETPOINT VALUE --- */
/* subscribe to ORB for attitude setpoint */
/* struct already allocated */
int spa_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
orb_set_interval(spa_sub, 2000); /* 0.5 Hz updates */
fds[fdsc_count].fd = spa_sub;
subs->spa_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
orb_set_interval(subs->spa_sub, 2000); /* 0.5 Hz updates */
fds[fdsc_count].fd = subs->spa_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
@@ -680,6 +694,21 @@ static void *uorb_receiveloop(void *arg)
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- ACTUATOR ARMED VALUE --- */
/* subscribe to ORB for actuator armed */
subs->armed_sub = orb_subscribe(ORB_ID(actuator_armed));
fds[fdsc_count].fd = subs->armed_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- ACTUATOR CONTROL VALUE --- */
/* subscribe to ORB for actuator control */
subs->actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
orb_set_interval(subs->actuators_sub, 2000); /* 0.5 Hz updates */
fds[fdsc_count].fd = subs->actuators_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* all subscriptions initialized, return success */
subs->initialized = true;
@@ -724,9 +753,18 @@ static void *uorb_receiveloop(void *arg)
/* send raw imu data */
mavlink_msg_raw_imu_send(MAVLINK_COMM_0, buf.raw.timestamp, buf.raw.accelerometer_raw[0], buf.raw.accelerometer_raw[1], buf.raw.accelerometer_raw[2], buf.raw.gyro_raw[0], buf.raw.gyro_raw[1], buf.raw.gyro_raw[2], buf.raw.magnetometer_raw[0], buf.raw.magnetometer_raw[1], buf.raw.magnetometer_raw[2]);
/* send scaled imu data (m/s^2 accelerations scaled back to milli-g) */
mavlink_msg_scaled_imu_send(MAVLINK_COMM_0, buf.raw.timestamp, buf.raw.accelerometer_m_s2[0] * 101.936799f, buf.raw.accelerometer_m_s2[1] * 101.936799f, buf.raw.accelerometer_m_s2[2] * 101.936799f, buf.raw.gyro_rad_s[0] * 1000, buf.raw.gyro_rad_s[1] * 1000, buf.raw.gyro_rad_s[2] * 1000, buf.raw.magnetometer_ga[0] * 1000, buf.raw.magnetometer_ga[1] * 1000, buf.raw.magnetometer_ga[2] * 1000);
//mavlink_msg_scaled_imu_send(MAVLINK_COMM_0, buf.raw.timestamp, buf.raw.accelerometer_m_s2[0] * 101.936799f, buf.raw.accelerometer_m_s2[1] * 101.936799f, buf.raw.accelerometer_m_s2[2] * 101.936799f, buf.raw.gyro_rad_s[0] * 1000, buf.raw.gyro_rad_s[1] * 1000, buf.raw.gyro_rad_s[2] * 1000, buf.raw.magnetometer_ga[0] * 1000, buf.raw.magnetometer_ga[1] * 1000, buf.raw.magnetometer_ga[2] * 1000);
/* send scaled imu data */
mavlink_msg_highres_imu_send(MAVLINK_COMM_0, buf.raw.timestamp,
buf.raw.accelerometer_m_s2[0], buf.raw.accelerometer_m_s2[1],
buf.raw.accelerometer_m_s2[2], buf.raw.gyro_rad_s[0],
buf.raw.gyro_rad_s[1], buf.raw.gyro_rad_s[2],
buf.raw.magnetometer_ga[0],
buf.raw.magnetometer_ga[1],buf.raw.magnetometer_ga[2],
buf.raw.baro_pres_mbar, 0 /* no diff pressure yet */,
buf.raw.baro_alt_meter, buf.raw.baro_temp_celcius);
/* send pressure */
mavlink_msg_scaled_pressure_send(MAVLINK_COMM_0, buf.raw.timestamp / 1000, buf.raw.baro_pres_mbar, buf.raw.baro_alt_meter, buf.raw.baro_temp_celcius * 100);
//mavlink_msg_scaled_pressure_send(MAVLINK_COMM_0, buf.raw.timestamp / 1000, buf.raw.baro_pres_mbar, buf.raw.baro_alt_meter, buf.raw.baro_temp_celcius * 100);
sensors_raw_counter++;
}
@@ -781,13 +819,14 @@ static void *uorb_receiveloop(void *arg)
if (fds[ifds++].revents & POLLIN) {
/* immediately communicate state changes back to user */
orb_copy(ORB_ID(vehicle_status), status_sub, &v_status);
orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed);
/* enable or disable HIL */
set_hil_on_off(v_status.flag_hil_enabled);
/* translate the current syste state to mavlink state and mode */
uint8_t mavlink_state = 0;
uint8_t mavlink_mode = 0;
get_mavlink_mode_and_state(&v_status, &mavlink_state, &mavlink_mode);
get_mavlink_mode_and_state(&v_status, &armed, &mavlink_state, &mavlink_mode);
/* send heartbeat */
mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.state_machine, mavlink_state);
@@ -801,71 +840,6 @@ static void *uorb_receiveloop(void *arg)
// TODO decide where to send channels
}
/* --- FIXED WING CONTROL CHANNELS --- */
if (fds[ifds++].revents & POLLIN) {
/* copy fixed wing control into local buffer */
orb_copy(ORB_ID(fixedwing_control), fw_sub, &fw_control);
/* send control output via MAVLink */
mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(MAVLINK_COMM_0, fw_control.timestamp / 1000, fw_control.attitude_control_output[0],
fw_control.attitude_control_output[1], fw_control.attitude_control_output[2],
fw_control.attitude_control_output[3]);
/* Only send in HIL mode */
if (v_status.flag_hil_enabled) {
/* Send the desired attitude from RC or from the autonomous controller */
// XXX it should not depend on a RC setting, but on a system_state value
float roll_ail, pitch_elev, throttle, yaw_rudd;
if (rc.chan[rc.function[OVERRIDE]].scale < 2000) {
//orb_copy(ORB_ID(fixedwing_control), fixed_wing_control_sub, &fixed_wing_control);
roll_ail = fw_control.attitude_control_output[ROLL];
pitch_elev = fw_control.attitude_control_output[PITCH];
throttle = fw_control.attitude_control_output[THROTTLE];
yaw_rudd = fw_control.attitude_control_output[YAW];
} else {
roll_ail = rc.chan[rc.function[ROLL]].scale;
pitch_elev = rc.chan[rc.function[PITCH]].scale;
throttle = rc.chan[rc.function[THROTTLE]].scale;
yaw_rudd = rc.chan[rc.function[YAW]].scale;
}
/* hacked HIL implementation in order for the APM Planner to work
* (correct cmd: mavlink_msg_hil_controls_send())
*/
mavlink_msg_rc_channels_scaled_send(chan,
hrt_absolute_time(),
0, // port 0
roll_ail,
pitch_elev,
throttle,
yaw_rudd,
0,
0,
0,
0,
1 /*rssi=1*/);
/* correct command duplicate */
mavlink_msg_hil_controls_send(chan,
hrt_absolute_time(),
roll_ail,
pitch_elev,
yaw_rudd,
throttle,
0,
0,
0,
0,
32, /* HIL_MODE */
0);
}
}
/* --- VEHICLE GLOBAL POSITION --- */
if (fds[ifds++].revents & POLLIN) {
/* copy global position data into local buffer */
@@ -881,37 +855,82 @@ static void *uorb_receiveloop(void *arg)
/* heading in degrees * 10, from 0 to 36.000) */
uint16_t hdg = (global_pos.hdg / M_PI_F) * (180.0f * 10.0f) + (180.0f * 10.0f);
mavlink_msg_global_position_int_send(MAVLINK_COMM_0, timestamp / 1000, lat, lon, alt, relative_alt, vx, vy, vz, hdg);
mavlink_msg_global_position_int_send(MAVLINK_COMM_0, timestamp / 1000, lat, lon, alt,
relative_alt, vx, vy, vz, hdg);
}
/* --- VEHICLE LOCAL POSITION --- */
if (fds[ifds++].revents & POLLIN) {
/* copy local position data into local buffer */
orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos);
mavlink_msg_local_position_ned_send(MAVLINK_COMM_0, local_pos.timestamp / 1000, local_pos.x, local_pos.y, local_pos.z, local_pos.vx, local_pos.vy, local_pos.vz);
orb_copy(ORB_ID(vehicle_local_position), subs->local_pos_sub, &local_pos);
mavlink_msg_local_position_ned_send(MAVLINK_COMM_0, local_pos.timestamp / 1000, local_pos.x,
local_pos.y, local_pos.z, local_pos.vx, local_pos.vy, local_pos.vz);
}
/* --- VEHICLE GLOBAL SETPOINT --- */
if (fds[ifds++].revents & POLLIN) {
/* copy local position data into local buffer */
orb_copy(ORB_ID(vehicle_global_position_setpoint), spg_sub, &buf.global_sp);
orb_copy(ORB_ID(vehicle_global_position_setpoint), subs->spg_sub, &buf.global_sp);
uint8_t coordinate_frame = MAV_FRAME_GLOBAL;
if (buf.global_sp.altitude_is_relative) coordinate_frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
mavlink_msg_global_position_setpoint_int_send(MAVLINK_COMM_0, coordinate_frame, buf.global_sp.lat, buf.global_sp.lon, buf.global_sp.altitude, buf.global_sp.yaw);
mavlink_msg_global_position_setpoint_int_send(MAVLINK_COMM_0, coordinate_frame, buf.global_sp.lat,
buf.global_sp.lon, buf.global_sp.altitude, buf.global_sp.yaw);
}
/* --- VEHICLE LOCAL SETPOINT --- */
if (fds[ifds++].revents & POLLIN) {
/* copy local position data into local buffer */
orb_copy(ORB_ID(vehicle_local_position_setpoint), spl_sub, &buf.local_sp);
mavlink_msg_local_position_setpoint_send(MAVLINK_COMM_0, MAV_FRAME_LOCAL_NED, buf.local_sp.x, buf.local_sp.y, buf.local_sp.z, buf.local_sp.yaw);
orb_copy(ORB_ID(vehicle_local_position_setpoint), subs->spl_sub, &buf.local_sp);
mavlink_msg_local_position_setpoint_send(MAVLINK_COMM_0, MAV_FRAME_LOCAL_NED, buf.local_sp.x,
buf.local_sp.y, buf.local_sp.z, buf.local_sp.yaw);
}
/* --- VEHICLE ATTITUDE SETPOINT --- */
if (fds[ifds++].revents & POLLIN) {
/* copy local position data into local buffer */
orb_copy(ORB_ID(vehicle_attitude_setpoint), spa_sub, &buf.att_sp);
mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(MAVLINK_COMM_0, buf.att_sp.timestamp/1000, buf.att_sp.roll_body, buf.att_sp.pitch_body, buf.att_sp.yaw_body, buf.att_sp.thrust);
orb_copy(ORB_ID(vehicle_attitude_setpoint), subs->spa_sub, &buf.att_sp);
mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(MAVLINK_COMM_0, buf.att_sp.timestamp/1000,
buf.att_sp.roll_body, buf.att_sp.pitch_body, buf.att_sp.yaw_body, buf.att_sp.thrust);
/* Only send in HIL mode */
if (mavlink_hil_enabled) {
/* hacked HIL implementation in order for the APM Planner to work
* (correct cmd: mavlink_msg_hil_controls_send())
*/
mavlink_msg_rc_channels_scaled_send(chan,
hrt_absolute_time(),
0, // port 0
buf.att_sp.roll_body,
buf.att_sp.pitch_body,
buf.att_sp.thrust,
buf.att_sp.yaw_body,
0,
0,
0,
0,
1 /*rssi=1*/);
/* translate the current syste state to mavlink state and mode */
uint8_t mavlink_state = 0;
uint8_t mavlink_mode = 0;
get_mavlink_mode_and_state(&v_status, &armed, &mavlink_state, &mavlink_mode);
/* correct HIL message as per MAVLink spec */
mavlink_msg_hil_controls_send(chan,
hrt_absolute_time(),
buf.att_sp.roll_body,
buf.att_sp.pitch_body,
buf.att_sp.yaw_body,
buf.att_sp.thrust,
0,
0,
0,
0,
mavlink_mode,
0);
}
}
/* --- ACTUATOR OUTPUTS 0 --- */
@@ -1033,6 +1052,19 @@ static void *uorb_receiveloop(void *arg)
mavlink_msg_manual_control_send(MAVLINK_COMM_0, mavlink_system.sysid, buf.man_control.roll,
buf.man_control.pitch, buf.man_control.yaw, buf.man_control.throttle, 1, 1, 1, 1);
}
/* --- ACTUATOR ARMED --- */
if (fds[ifds++].revents & POLLIN) {
}
/* --- ACTUATOR CONTROL --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs->actuators_sub, &buf.actuators);
mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, "ctrl0", buf.actuators.control[0]);
mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, "ctrl1", buf.actuators.control[1]);
mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, "ctrl2", buf.actuators.control[2]);
mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, "ctrl3", buf.actuators.control[3]);
}
}
}
@@ -1252,12 +1284,12 @@ void handleMessage(mavlink_message_t *msg)
}
}
int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb)
int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb)
{
/* process baud rate */
int speed;
switch (baudrate) {
switch (baud) {
case 0: speed = B0; break;
case 50: speed = B50; break;
@@ -1301,12 +1333,12 @@ int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_
case 921600: speed = B921600; break;
default:
fprintf(stderr, "[mavlink] ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600\n\n", baudrate);
fprintf(stderr, "[mavlink] ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600\n\n", baud);
return -EINVAL;
}
/* open uart */
printf("[mavlink] UART is %s, baudrate is %d\n", uart_name, baudrate);
printf("[mavlink] UART is %s, baudrate is %d\n", uart_name, baud);
uart = open(uart_name, O_RDWR | O_NOCTTY);
/* Try to set baud rate */
@@ -1363,7 +1395,6 @@ int mavlink_thread_main(int argc, char *argv[])
memset(&rc, 0, sizeof(rc));
memset(&hil_attitude, 0, sizeof(hil_attitude));
memset(&hil_global_pos, 0, sizeof(hil_global_pos));
memset(&fw_control, 0, sizeof(fw_control));
memset(&ardrone_motors, 0, sizeof(ardrone_motors));
memset(&vcmd, 0, sizeof(vcmd));
@@ -1375,7 +1406,7 @@ int mavlink_thread_main(int argc, char *argv[])
/* default values for arguments */
char *uart_name = "/dev/ttyS1";
int baudrate = 57600;
baudrate = 57600;
/* read program arguments */
int i;
@@ -1435,8 +1466,8 @@ int mavlink_thread_main(int argc, char *argv[])
mavlink_subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
orb_set_interval(mavlink_subs.global_pos_sub, 1000); /* 1Hz active updates */
/* subscribe to ORB for local position */
local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
orb_set_interval(local_pos_sub, 1000); /* 1Hz active updates */
mavlink_subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
orb_set_interval(mavlink_subs.local_pos_sub, 1000); /* 1Hz active updates */
pthread_attr_t receiveloop_attr;
@@ -1467,6 +1498,7 @@ int mavlink_thread_main(int argc, char *argv[])
/* 500 Hz / 2 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 2);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 2);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 2);
/* 200 Hz / 5 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 5);
/* 5 Hz */
@@ -1475,6 +1507,7 @@ int mavlink_thread_main(int argc, char *argv[])
/* 250 Hz / 4 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 5);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 5);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 5);
/* 50 Hz / 20 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 20);
/* 2 Hz */
@@ -1483,6 +1516,7 @@ int mavlink_thread_main(int argc, char *argv[])
/* 50 Hz / 20 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 50);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 50);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 50);
/* 10 Hz / 100 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 100);
/* 1 Hz */
@@ -1491,6 +1525,7 @@ int mavlink_thread_main(int argc, char *argv[])
/* 10 Hz / 100 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 100);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 100);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 100);
/* 5 Hz / 200 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200);
/* 0.2 Hz */
@@ -1499,6 +1534,7 @@ int mavlink_thread_main(int argc, char *argv[])
/* very low baud rate, limit to 1 Hz / 1000 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 1000);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 1000);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 1000);
/* 0.5 Hz */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 2000);
/* 0.1 Hz */
@@ -1511,7 +1547,8 @@ int mavlink_thread_main(int argc, char *argv[])
/* get local and global position */
orb_copy(ORB_ID(vehicle_global_position), mavlink_subs.global_pos_sub, &global_pos);
orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos);
orb_copy(ORB_ID(vehicle_local_position), mavlink_subs.local_pos_sub, &local_pos);
orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed);
/* 1 Hz */
if (lowspeed_counter == 10) {
@@ -1537,7 +1574,7 @@ int mavlink_thread_main(int argc, char *argv[])
/* translate the current syste state to mavlink state and mode */
uint8_t mavlink_state = 0;
uint8_t mavlink_mode = 0;
get_mavlink_mode_and_state(&v_status, &mavlink_state, &mavlink_mode);
get_mavlink_mode_and_state(&v_status, &armed, &mavlink_state, &mavlink_mode);
/* send heartbeat */
mavlink_msg_heartbeat_send(chan, system_type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.state_machine, mavlink_state);
@@ -1607,7 +1644,7 @@ exit_cleanup:
/* close subscriptions */
close(mavlink_subs.global_pos_sub);
close(local_pos_sub);
close(mavlink_subs.local_pos_sub);
fflush(stdout);
fflush(stderr);
apps/multirotor_att_control/multirotor_att_control_main.c
+2 -18
View File
@@ -117,7 +117,6 @@ mc_thread_main(int argc, char *argv[])
actuators.control[i] = 0.0f;
orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
armed.armed = false;
orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
/* register the perf counter */
@@ -151,25 +150,13 @@ mc_thread_main(int argc, char *argv[])
att_sp.yaw_body = 0.0f;
att_sp.thrust = 0.1f;
} else {
if (state.flag_system_armed) {
att_sp.thrust = manual.throttle;
armed.armed = true;
} else if (state.state_machine == SYSTEM_STATE_EMCY_CUTOFF) {
/* immediately cut off motors */
armed.armed = false;
} else {
/* limit motor throttle to zero for an unknown mode */
armed.armed = false;
}
att_sp.thrust = manual.throttle;
}
multirotor_control_attitude(&att_sp, &att, &state, &actuators, motor_test_mode);
multirotor_control_attitude(&att_sp, &att, &actuators);
/* publish the result */
orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
@@ -179,8 +166,6 @@ mc_thread_main(int argc, char *argv[])
printf("[multirotor att control] stopping, disarming motors.\n");
/* kill all outputs */
armed.armed = false;
orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
actuators.control[i] = 0.0f;
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
@@ -190,7 +175,6 @@ mc_thread_main(int argc, char *argv[])
close(state_sub);
close(manual_sub);
close(actuator_pub);
close(armed_pub);
close(att_sp_pub);
perf_print_counter(mc_loop_perf);
apps/multirotor_att_control/multirotor_attitude_control.c
+2 -3
View File
@@ -188,8 +188,7 @@ static int parameters_update(const struct mc_att_control_param_handles *h, struc
}
void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
const struct vehicle_attitude_s *att, const struct vehicle_status_s *status,
struct actuator_controls_s *actuators, bool verbose)
const struct vehicle_attitude_s *att, struct actuator_controls_s *actuators)
{
static uint64_t last_run = 0;
const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
@@ -218,7 +217,7 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
PID_MODE_DERIVATIV_CALC, 155);
pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, p.att_awu,
PID_MODE_DERIVATIV_SET, 156);
pid_init(&roll_controller, p.att_d, p.att_i, p.att_d, p.att_awu,
pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, p.att_awu,
PID_MODE_DERIVATIV_SET, 157);
initialized = true;
apps/multirotor_att_control/multirotor_attitude_control.h
+1 -3
View File
@@ -54,8 +54,6 @@
#include <uORB/topics/actuator_controls.h>
void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att,
const struct vehicle_status_s *status, struct actuator_controls_s *actuators, bool verbose);
void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls_s *actuators, bool verbose);
struct actuator_controls_s *actuators);
#endif /* MULTIROTOR_ATTITUDE_CONTROL_H_ */
apps/nshlib/Kconfig
+3
View File
@@ -148,9 +148,12 @@ config NSH_FILEIOSIZE
config NSH_STRERROR
bool "Use strerror()"
default n
depends on LIBC_STRERROR
---help---
strerror(errno) makes more readable output but strerror() is
very large and will not be used unless this setting is 'y'
This setting depends upon the strerror() having been enabled
with LIBC_STRERROR.
config NSH_LINELEN
int "Max command line length"
apps/nshlib/README.txt
+3 -1
View File
@@ -915,7 +915,9 @@ NSH-Specific Configuration Settings
* CONFIG_NSH_STRERROR
strerror(errno) makes more readable output but strerror() is
very large and will not be used unless this setting is 'y'
very large and will not be used unless this setting is 'y'.
This setting depends upon the strerror() having been enabled
with CONFIG_LIBC_STRERROR.
* CONFIG_NSH_LINELEN
The maximum length of one command line and of one output line.
apps/nshlib/nsh.h
+6 -1
View File
@@ -238,9 +238,14 @@
#define NSH_MAX_ARGUMENTS 6
/* strerror() produces much nicer output but is, however, quite large and
* will only be used if CONFIG_NSH_STRERROR is defined.
* will only be used if CONFIG_NSH_STRERROR is defined. Note that the strerror
* interface must also have been enabled with CONFIG_LIBC_STRERROR.
*/
#ifndef CONFIG_LIBC_STRERROR
# undef CONFIG_NSH_STRERROR
#endif
#ifdef CONFIG_NSH_STRERROR
# define NSH_ERRNO strerror(errno)
# define NSH_ERRNO_OF(err) strerror(err)
apps/px4/fmu/fmu.cpp
+1 -1
View File
@@ -552,7 +552,7 @@ void
fake(int argc, char *argv[])
{
if (argc < 5) {
puts("fmu fake <roll> <pitch> <yaw> <thrust> (values -100 - 100)");
puts("fmu fake <roll> <pitch> <yaw> <thrust> (values -100 .. 100)");
exit(1);
}
apps/system/i2c/i2c_main.c
+1 -9
View File
@@ -351,15 +351,7 @@ static void i2c_teardown(FAR struct i2ctool_s *i2ctool)
* Name: i2c_main
****************************************************************************/
#ifdef CONFIG_I2CTOOL_BUILTIN
# define MAIN_NAME i2c_main
# define MAIN_NAME_STRING "i2c_main"
#else
# define MAIN_NAME user_start
# define MAIN_NAME_STRING "user_start"
#endif
int MAIN_NAME(int argc, char *argv[])
int i2c_main(int argc, char *argv[])
{
/* Verify settings */
apps/systemlib/mixer/mixer_multirotor.cpp
+36 -36
View File
@@ -64,52 +64,52 @@ namespace
* These tables automatically generated by multi_tables - do not edit.
*/
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.00 },
{ 0.707107, -0.707107, -1.00 },
{ 0.707107, 0.707107, 1.00 },
{ -0.707107, -0.707107, 1.00 },
};
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.00 },
{ 1.000000, 0.000000, -1.00 },
{ 0.000000, 1.000000, 1.00 },
{ -0.000000, -1.000000, 1.00 },
};
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.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 },
};
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.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 },
};
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.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 },
};
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.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 },
};
const MultirotorMixer::Rotor *_config_index[MultirotorMixer::Geometry::MAX_GEOMETRY] = {
&_config_quad_x[0],
apps/systemlib/mixer/multi_tables
+1 -1
View File
@@ -62,7 +62,7 @@ set octa_plus {
set tables {quad_x quad_plus hex_x hex_plus octa_x octa_plus}
proc factors {a d} { puts [format "\t{ %9.6f, %9.6f, %5.2f }," [rcos [expr $a + 90]] [rcos $a] $d]}
proc factors {a d} { puts [format "\t{ %9.6f, %9.6f, %5.2f }," [rcos [expr $a + 90]] [rcos $a] [expr -$d]]}
foreach table $tables {
puts [format "const MultirotorMixer::Rotor _config_%s\[\] = {" $table]