Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-20 20:03:54 +08:00
Code Issues Actions 8 Packages Projects Releases Wiki Activity

Merge branch 'master' into sdlog2

Browse Source
This commit is contained in:
Anton Babushkin
2013-06-07 19:29:14 +04:00
parent 5bad186916 5c74809dac
commit 03357f89fd
19 changed files with 1156 additions and 91 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/mixers/IO_pass.mix
+38
View File
@@ -0,0 +1,38 @@
Passthrough mixer for PX4IO
============================
This file defines passthrough mixers suitable for testing.
Channel group 0, channels 0-7 are passed directly through to the outputs.
M: 1
O: 10000 10000 0 -10000 10000
S: 0 0 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 1 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 2 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 3 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 4 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 5 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 6 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 7 10000 10000 0 -10000 10000
makefiles/config_px4fmu_default.mk
+2 -1
View File
@@ -62,7 +62,8 @@ MODULES += modules/gpio_led
# Estimation modules (EKF / other filters)
#
MODULES += modules/attitude_estimator_ekf
MODULES += modules/position_estimator_mc
MODULES += modules/attitude_estimator_so3_comp
#MODULES += modules/position_estimator_mc
MODULES += modules/position_estimator
MODULES += modules/att_pos_estimator_ekf
makefiles/firmware.mk
+6 -6
View File
@@ -176,6 +176,12 @@ GLOBAL_DEPS += $(MAKEFILE_LIST)
#
EXTRA_CLEANS =
################################################################################
# NuttX libraries and paths
################################################################################
include $(PX4_MK_DIR)/nuttx.mk
################################################################################
# Modules
################################################################################
@@ -296,12 +302,6 @@ $(LIBRARY_CLEANS):
LIBRARY_MK=$(mkfile) \
clean
################################################################################
# NuttX libraries and paths
################################################################################
include $(PX4_MK_DIR)/nuttx.mk
################################################################################
# ROMFS generation
################################################################################
makefiles/nuttx.mk
+5 -1
View File
@@ -69,10 +69,14 @@ INCLUDE_DIRS += $(NUTTX_EXPORT_DIR)include \
LIB_DIRS += $(NUTTX_EXPORT_DIR)libs
LIBS += -lapps -lnuttx
LINK_DEPS += $(NUTTX_EXPORT_DIR)libs/libapps.a \
NUTTX_LIBS = $(NUTTX_EXPORT_DIR)libs/libapps.a \
$(NUTTX_EXPORT_DIR)libs/libnuttx.a
LINK_DEPS += $(NUTTX_LIBS)
$(NUTTX_CONFIG_HEADER): $(NUTTX_ARCHIVE)
@$(ECHO) %% Unpacking $(NUTTX_ARCHIVE)
$(Q) $(UNZIP_CMD) -q -o -d $(WORK_DIR) $(NUTTX_ARCHIVE)
$(Q) $(TOUCH) $@
$(LDSCRIPT): $(NUTTX_CONFIG_HEADER)
$(NUTTX_LIBS): $(NUTTX_CONFIG_HEADER)
makefiles/toolchain_gnu-arm-eabi.mk
+10 -2
View File
@@ -70,6 +70,14 @@ ARCHCPUFLAGS_CORTEXM3 = -mcpu=cortex-m3 \
-march=armv7-m \
-mfloat-abi=soft
ARCHINSTRUMENTATIONDEFINES_CORTEXM4F = -finstrument-functions \
-ffixed-r10
ARCHINSTRUMENTATIONDEFINES_CORTEXM4 = -finstrument-functions \
-ffixed-r10
ARCHINSTRUMENTATIONDEFINES_CORTEXM3 =
# Pick the right set of flags for the architecture.
#
ARCHCPUFLAGS = $(ARCHCPUFLAGS_$(CONFIG_ARCH))
@@ -91,8 +99,8 @@ ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \
# enable precise stack overflow tracking
# note - requires corresponding support in NuttX
INSTRUMENTATIONDEFINES = -finstrument-functions \
-ffixed-r10
INSTRUMENTATIONDEFINES = $(ARCHINSTRUMENTATIONDEFINES_$(CONFIG_ARCH))
# Language-specific flags
#
ARCHCFLAGS = -std=gnu99
nuttx/configs/px4fmu/nsh/defconfig
+4 -4
View File
@@ -248,7 +248,7 @@ CONFIG_SERIAL_TERMIOS=y
CONFIG_SERIAL_CONSOLE_REINIT=y
CONFIG_STANDARD_SERIAL=y
CONFIG_USART1_SERIAL_CONSOLE=y
CONFIG_USART1_SERIAL_CONSOLE=n
CONFIG_USART2_SERIAL_CONSOLE=n
CONFIG_USART3_SERIAL_CONSOLE=n
CONFIG_UART4_SERIAL_CONSOLE=n
@@ -561,7 +561,7 @@ CONFIG_START_MONTH=1
CONFIG_START_DAY=1
CONFIG_GREGORIAN_TIME=n
CONFIG_JULIAN_TIME=n
CONFIG_DEV_CONSOLE=y
CONFIG_DEV_CONSOLE=n
CONFIG_DEV_LOWCONSOLE=n
CONFIG_MUTEX_TYPES=n
CONFIG_PRIORITY_INHERITANCE=y
@@ -717,7 +717,7 @@ CONFIG_ARCH_BZERO=n
# zero for all dynamic allocations.
#
CONFIG_MAX_TASKS=32
CONFIG_MAX_TASK_ARGS=8
CONFIG_MAX_TASK_ARGS=10
CONFIG_NPTHREAD_KEYS=4
CONFIG_NFILE_DESCRIPTORS=32
CONFIG_NFILE_STREAMS=25
@@ -925,7 +925,7 @@ CONFIG_USBDEV_TRACE_NRECORDS=512
# Size of the serial receive/transmit buffers. Default 256.
#
CONFIG_CDCACM=y
CONFIG_CDCACM_CONSOLE=n
CONFIG_CDCACM_CONSOLE=y
#CONFIG_CDCACM_EP0MAXPACKET
CONFIG_CDCACM_EPINTIN=1
#CONFIG_CDCACM_EPINTIN_FSSIZE
src/drivers/hott_telemetry/hott_telemetry_main.c
+10 -4
View File
@@ -133,15 +133,14 @@ recv_req_id(int uart, uint8_t *id)
if (poll(fds, 1, timeout_ms) > 0) {
/* Get the mode: binary or text */
read(uart, &mode, sizeof(mode));
/* Read the device ID being polled */
read(uart, id, sizeof(*id));
/* if we have a binary mode request */
if (mode != BINARY_MODE_REQUEST_ID) {
warnx("Non binary request ID detected: %d", mode);
return ERROR;
}
/* Read the device ID being polled */
read(uart, id, sizeof(*id));
} else {
warnx("UART timeout on TX/RX port");
return ERROR;
@@ -216,9 +215,15 @@ hott_telemetry_thread_main(int argc, char *argv[])
uint8_t buffer[MESSAGE_BUFFER_SIZE];
size_t size = 0;
uint8_t id = 0;
bool connected = true;
while (!thread_should_exit) {
if (recv_req_id(uart, &id) == OK) {
if (!connected) {
connected = true;
warnx("OK");
}
switch (id) {
case EAM_SENSOR_ID:
build_eam_response(buffer, &size);
@@ -234,7 +239,8 @@ hott_telemetry_thread_main(int argc, char *argv[])
send_data(uart, buffer, size);
} else {
warnx("NOK");
connected = false;
warnx("syncing");
}
}
src/drivers/px4io/px4io.cpp
+19 -2
View File
@@ -1302,7 +1302,7 @@ PX4IO::print_status()
io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VBATT),
io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT),
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE));
printf("amp_per_volt %.3f amp_offset %.3f mAhDischarged %.3f\n",
printf("amp_per_volt %.3f amp_offset %.3f mAh discharged %.3f\n",
(double)_battery_amp_per_volt,
(double)_battery_amp_bias,
(double)_battery_mamphour_total);
@@ -1496,7 +1496,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
case MIXERIOCLOADBUF: {
const char *buf = (const char *)arg;
ret = mixer_send(buf, strnlen(buf, 1024));
ret = mixer_send(buf, strnlen(buf, 2048));
break;
}
@@ -1637,6 +1637,13 @@ test(void)
if (ioctl(fd, PWM_SERVO_ARM, 0))
err(1, "failed to arm servos");
/* Open console directly to grab CTRL-C signal */
int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY);
if (!console)
err(1, "failed opening console");
warnx("Press CTRL-C or 'c' to abort.");
for (;;) {
/* sweep all servos between 1000..2000 */
@@ -1671,6 +1678,16 @@ test(void)
if (value != servos[i])
errx(1, "servo %d readback error, got %u expected %u", i, value, servos[i]);
}
/* Check if user wants to quit */
char c;
if (read(console, &c, 1) == 1) {
if (c == 0x03 || c == 0x63) {
warnx("User abort\n");
close(console);
exit(0);
}
}
}
}
src/examples/px4_daemon_app/px4_daemon_app.c
+29 -25
View File
@@ -1,7 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Example User <mail@example.com>
* Copyright (c) 2012, 2013 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
@@ -33,27 +32,33 @@
****************************************************************************/
/**
* @file px4_deamon_app.c
* Deamon application example for PX4 autopilot
* @file px4_daemon_app.c
* daemon application example for PX4 autopilot
*
* @author Example User <mail@example.com>
*/
#include <nuttx/config.h>
#include <nuttx/sched.h>
#include <unistd.h>
#include <stdio.h>
static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int deamon_task; /**< Handle of deamon task / thread */
#include <systemlib/systemlib.h>
#include <systemlib/err.h>
static bool thread_should_exit = false; /**< daemon exit flag */
static bool thread_running = false; /**< daemon status flag */
static int daemon_task; /**< Handle of daemon task / thread */
/**
* Deamon management function.
* daemon management function.
*/
__EXPORT int px4_deamon_app_main(int argc, char *argv[]);
__EXPORT int px4_daemon_app_main(int argc, char *argv[]);
/**
* Mainloop of deamon.
* Mainloop of daemon.
*/
int px4_deamon_thread_main(int argc, char *argv[]);
int px4_daemon_thread_main(int argc, char *argv[]);
/**
* Print the correct usage.
@@ -64,20 +69,19 @@ static void
usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: deamon {start|stop|status} [-p <additional params>]\n\n");
exit(1);
warnx("%s\n", reason);
errx(1, "usage: daemon {start|stop|status} [-p <additional params>]\n\n");
}
/**
* The deamon app only briefly exists to start
* The daemon app only briefly exists to start
* the background job. The stack size assigned in the
* Makefile does only apply to this management task.
*
* The actual stack size should be set in the call
* to task_create().
*/
int px4_deamon_app_main(int argc, char *argv[])
int px4_daemon_app_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
@@ -85,17 +89,17 @@ int px4_deamon_app_main(int argc, char *argv[])
if (!strcmp(argv[1], "start")) {
if (thread_running) {
printf("deamon already running\n");
warnx("daemon already running\n");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
deamon_task = task_spawn("deamon",
daemon_task = task_spawn("daemon",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
4096,
px4_deamon_thread_main,
px4_daemon_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
exit(0);
}
@@ -107,9 +111,9 @@ int px4_deamon_app_main(int argc, char *argv[])
if (!strcmp(argv[1], "status")) {
if (thread_running) {
printf("\tdeamon app is running\n");
warnx("\trunning\n");
} else {
printf("\tdeamon app not started\n");
warnx("\tnot started\n");
}
exit(0);
}
@@ -118,18 +122,18 @@ int px4_deamon_app_main(int argc, char *argv[])
exit(1);
}
int px4_deamon_thread_main(int argc, char *argv[]) {
int px4_daemon_thread_main(int argc, char *argv[]) {
printf("[deamon] starting\n");
warnx("[daemon] starting\n");
thread_running = true;
while (!thread_should_exit) {
printf("Hello Deamon!\n");
warnx("Hello daemon!\n");
sleep(10);
}
printf("[deamon] exiting.\n");
warnx("[daemon] exiting.\n");
thread_running = false;
src/modules/att_pos_estimator_ekf/kalman_main.cpp
+8 -7
View File
@@ -44,6 +44,7 @@
#include <string.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include <math.h>
#include "KalmanNav.hpp"
@@ -73,7 +74,7 @@ usage(const char *reason)
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: kalman_demo {start|stop|status} [-p <additional params>]\n\n");
warnx("usage: att_pos_estimator_ekf {start|stop|status} [-p <additional params>]");
exit(1);
}
@@ -94,13 +95,13 @@ int att_pos_estimator_ekf_main(int argc, char *argv[])
if (!strcmp(argv[1], "start")) {
if (thread_running) {
printf("kalman_demo already running\n");
warnx("already running");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
deamon_task = task_spawn("kalman_demo",
deamon_task = task_spawn("att_pos_estimator_ekf",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
4096,
@@ -116,10 +117,10 @@ int att_pos_estimator_ekf_main(int argc, char *argv[])
if (!strcmp(argv[1], "status")) {
if (thread_running) {
printf("\tkalman_demo app is running\n");
warnx("is running\n");
} else {
printf("\tkalman_demo app not started\n");
warnx("not started\n");
}
exit(0);
@@ -132,7 +133,7 @@ int att_pos_estimator_ekf_main(int argc, char *argv[])
int kalman_demo_thread_main(int argc, char *argv[])
{
printf("[kalman_demo] starting\n");
warnx("starting\n");
using namespace math;
@@ -144,7 +145,7 @@ int kalman_demo_thread_main(int argc, char *argv[])
nav.update();
}
printf("[kalman_demo] exiting.\n");
printf("exiting.\n");
thread_running = false;
src/modules/attitude_estimator_so3_comp/README
+5
View File
@@ -0,0 +1,5 @@
Synopsis
nsh> attitude_estimator_so3_comp start -d /dev/ttyS1 -b 115200
Option -d is for debugging packet. See code for detailed packet structure.
src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp
Executable
+833
View File
File diff suppressed because it is too large Load Diff
src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.c
Executable
+63
View File
@@ -0,0 +1,63 @@
/*
* Author: Hyon Lim <limhyon@gmail.com, hyonlim@snu.ac.kr>
*
* @file attitude_estimator_so3_comp_params.c
*
* Implementation of nonlinear complementary filters on the SO(3).
* This code performs attitude estimation by using accelerometer, gyroscopes and magnetometer.
* Result is provided as quaternion, 1-2-3 Euler angle and rotation matrix.
*
* Theory of nonlinear complementary filters on the SO(3) is based on [1].
* Quaternion realization of [1] is based on [2].
* Optmized quaternion update code is based on Sebastian Madgwick's implementation.
*
* References
* [1] Mahony, R.; Hamel, T.; Pflimlin, Jean-Michel, "Nonlinear Complementary Filters on the Special Orthogonal Group," Automatic Control, IEEE Transactions on , vol.53, no.5, pp.1203,1218, June 2008
* [2] Euston, M.; Coote, P.; Mahony, R.; Jonghyuk Kim; Hamel, T., "A complementary filter for attitude estimation of a fixed-wing UAV," Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on , vol., no., pp.340,345, 22-26 Sept. 2008
*/
#include "attitude_estimator_so3_comp_params.h"
/* This is filter gain for nonlinear SO3 complementary filter */
/* NOTE : How to tune the gain? First of all, stick with this default gain. And let the quad in stable place.
Log the steady state reponse of filter. If it is too slow, increase SO3_COMP_KP.
If you are flying from ground to high altitude in short amount of time, please increase SO3_COMP_KI which
will compensate gyro bias which depends on temperature and vibration of your vehicle */
PARAM_DEFINE_FLOAT(SO3_COMP_KP, 1.0f); //! This parameter will give you about 15 seconds convergence time.
//! You can set this gain higher if you want more fast response.
//! But note that higher gain will give you also higher overshoot.
PARAM_DEFINE_FLOAT(SO3_COMP_KI, 0.05f); //! This gain will incorporate slow time-varying bias (e.g., temperature change)
//! This gain is depend on your vehicle status.
/* offsets in roll, pitch and yaw of sensor plane and body */
PARAM_DEFINE_FLOAT(ATT_ROLL_OFFS, 0.0f);
PARAM_DEFINE_FLOAT(ATT_PITCH_OFFS, 0.0f);
PARAM_DEFINE_FLOAT(ATT_YAW_OFFS, 0.0f);
int parameters_init(struct attitude_estimator_so3_comp_param_handles *h)
{
/* Filter gain parameters */
h->Kp = param_find("SO3_COMP_KP");
h->Ki = param_find("SO3_COMP_KI");
/* Attitude offset (WARNING: Do not change if you do not know what exactly this variable wil lchange) */
h->roll_off = param_find("ATT_ROLL_OFFS");
h->pitch_off = param_find("ATT_PITCH_OFFS");
h->yaw_off = param_find("ATT_YAW_OFFS");
return OK;
}
int parameters_update(const struct attitude_estimator_so3_comp_param_handles *h, struct attitude_estimator_so3_comp_params *p)
{
/* Update filter gain */
param_get(h->Kp, &(p->Kp));
param_get(h->Ki, &(p->Ki));
/* Update attitude offset */
param_get(h->roll_off, &(p->roll_off));
param_get(h->pitch_off, &(p->pitch_off));
param_get(h->yaw_off, &(p->yaw_off));
return OK;
}
src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.h
Executable
+44
View File
@@ -0,0 +1,44 @@
/*
* Author: Hyon Lim <limhyon@gmail.com, hyonlim@snu.ac.kr>
*
* @file attitude_estimator_so3_comp_params.h
*
* Implementation of nonlinear complementary filters on the SO(3).
* This code performs attitude estimation by using accelerometer, gyroscopes and magnetometer.
* Result is provided as quaternion, 1-2-3 Euler angle and rotation matrix.
*
* Theory of nonlinear complementary filters on the SO(3) is based on [1].
* Quaternion realization of [1] is based on [2].
* Optmized quaternion update code is based on Sebastian Madgwick's implementation.
*
* References
* [1] Mahony, R.; Hamel, T.; Pflimlin, Jean-Michel, "Nonlinear Complementary Filters on the Special Orthogonal Group," Automatic Control, IEEE Transactions on , vol.53, no.5, pp.1203,1218, June 2008
* [2] Euston, M.; Coote, P.; Mahony, R.; Jonghyuk Kim; Hamel, T., "A complementary filter for attitude estimation of a fixed-wing UAV," Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on , vol., no., pp.340,345, 22-26 Sept. 2008
*/
#include <systemlib/param/param.h>
struct attitude_estimator_so3_comp_params {
float Kp;
float Ki;
float roll_off;
float pitch_off;
float yaw_off;
};
struct attitude_estimator_so3_comp_param_handles {
param_t Kp, Ki;
param_t roll_off, pitch_off, yaw_off;
};
/**
* Initialize all parameter handles and values
*
*/
int parameters_init(struct attitude_estimator_so3_comp_param_handles *h);
/**
* Update all parameters
*
*/
int parameters_update(const struct attitude_estimator_so3_comp_param_handles *h, struct attitude_estimator_so3_comp_params *p);
src/modules/attitude_estimator_so3_comp/module.mk
+8
View File
@@ -0,0 +1,8 @@
#
# Attitude estimator (Nonlinear SO3 complementary Filter)
#
MODULE_COMMAND = attitude_estimator_so3_comp
SRCS = attitude_estimator_so3_comp_main.cpp \
attitude_estimator_so3_comp_params.c
src/modules/fixedwing_backside/fixedwing_backside_main.cpp
+9 -8
View File
@@ -47,6 +47,7 @@
#include <systemlib/systemlib.h>
#include <controllib/fixedwing.hpp>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include <math.h>
@@ -80,7 +81,7 @@ usage(const char *reason)
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: control_demo {start|stop|status} [-p <additional params>]\n\n");
fprintf(stderr, "usage: fixedwing_backside {start|stop|status} [-p <additional params>]\n\n");
exit(1);
}
@@ -101,13 +102,13 @@ int fixedwing_backside_main(int argc, char *argv[])
if (!strcmp(argv[1], "start")) {
if (thread_running) {
printf("control_demo already running\n");
warnx("already running");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
deamon_task = task_spawn("control_demo",
deamon_task = task_spawn("fixedwing_backside",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 10,
5120,
@@ -128,10 +129,10 @@ int fixedwing_backside_main(int argc, char *argv[])
if (!strcmp(argv[1], "status")) {
if (thread_running) {
printf("\tcontrol_demo app is running\n");
warnx("is running");
} else {
printf("\tcontrol_demo app not started\n");
warnx("not started");
}
exit(0);
@@ -144,7 +145,7 @@ int fixedwing_backside_main(int argc, char *argv[])
int control_demo_thread_main(int argc, char *argv[])
{
printf("[control_Demo] starting\n");
warnx("starting");
using namespace control;
@@ -156,7 +157,7 @@ int control_demo_thread_main(int argc, char *argv[])
autopilot.update();
}
printf("[control_demo] exiting.\n");
warnx("exiting.");
thread_running = false;
@@ -165,6 +166,6 @@ int control_demo_thread_main(int argc, char *argv[])
void test()
{
printf("beginning control lib test\n");
warnx("beginning control lib test");
control::basicBlocksTest();
}
src/modules/gpio_led/gpio_led.c
+49 -23
View File
@@ -48,6 +48,7 @@
#include <nuttx/wqueue.h>
#include <nuttx/clock.h>
#include <systemlib/systemlib.h>
#include <systemlib/err.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <poll.h>
@@ -64,6 +65,7 @@ struct gpio_led_s {
};
static struct gpio_led_s gpio_led_data;
static bool gpio_led_started = false;
__EXPORT int gpio_led_main(int argc, char *argv[]);
@@ -75,31 +77,54 @@ int gpio_led_main(int argc, char *argv[])
{
int pin = GPIO_EXT_1;
if (argc > 1) {
if (!strcmp(argv[1], "-p")) {
if (!strcmp(argv[2], "1")) {
pin = GPIO_EXT_1;
if (argc < 2) {
errx(1, "no argument provided. Try 'start' or 'stop' [-p 1/2]");
} else if (!strcmp(argv[2], "2")) {
pin = GPIO_EXT_2;
} else {
/* START COMMAND HANDLING */
if (!strcmp(argv[1], "start")) {
if (argc > 2) {
if (!strcmp(argv[1], "-p")) {
if (!strcmp(argv[2], "1")) {
pin = GPIO_EXT_1;
} else if (!strcmp(argv[2], "2")) {
pin = GPIO_EXT_2;
} else {
warnx("[gpio_led] Unsupported pin: %s\n", argv[2]);
exit(1);
}
}
}
memset(&gpio_led_data, 0, sizeof(gpio_led_data));
gpio_led_data.pin = pin;
int ret = work_queue(LPWORK, &gpio_led_data.work, gpio_led_start, &gpio_led_data, 0);
if (ret != 0) {
warnx("[gpio_led] Failed to queue work: %d\n", ret);
exit(1);
} else {
printf("[gpio_led] Unsupported pin: %s\n", argv[2]);
exit(1);
gpio_led_started = true;
}
exit(0);
/* STOP COMMAND HANDLING */
} else if (!strcmp(argv[1], "stop")) {
gpio_led_started = false;
/* INVALID COMMAND */
} else {
errx(1, "unrecognized command '%s', only supporting 'start' or 'stop'", argv[1]);
}
}
memset(&gpio_led_data, 0, sizeof(gpio_led_data));
gpio_led_data.pin = pin;
int ret = work_queue(LPWORK, &gpio_led_data.work, gpio_led_start, &gpio_led_data, 0);
if (ret != 0) {
printf("[gpio_led] Failed to queue work: %d\n", ret);
exit(1);
}
exit(0);
}
void gpio_led_start(FAR void *arg)
@@ -110,7 +135,7 @@ void gpio_led_start(FAR void *arg)
priv->gpio_fd = open(GPIO_DEVICE_PATH, 0);
if (priv->gpio_fd < 0) {
printf("[gpio_led] GPIO: open fail\n");
warnx("[gpio_led] GPIO: open fail\n");
return;
}
@@ -125,11 +150,11 @@ void gpio_led_start(FAR void *arg)
int ret = work_queue(LPWORK, &priv->work, gpio_led_cycle, priv, 0);
if (ret != 0) {
printf("[gpio_led] Failed to queue work: %d\n", ret);
warnx("[gpio_led] Failed to queue work: %d\n", ret);
return;
}
printf("[gpio_led] Started, using pin GPIO_EXT%i\n", priv->pin);
warnx("[gpio_led] Started, using pin GPIO_EXT%i\n", priv->pin);
}
void gpio_led_cycle(FAR void *arg)
@@ -187,5 +212,6 @@ void gpio_led_cycle(FAR void *arg)
priv->counter = 0;
/* repeat cycle at 5 Hz*/
work_queue(LPWORK, &priv->work, gpio_led_cycle, priv, USEC2TICK(200000));
if (gpio_led_started)
work_queue(LPWORK, &priv->work, gpio_led_cycle, priv, USEC2TICK(200000));
}
src/modules/px4iofirmware/mixer.cpp
+12 -6
View File
@@ -294,8 +294,7 @@ mixer_handle_text(const void *buffer, size_t length)
case F2I_MIXER_ACTION_APPEND:
isr_debug(2, "append %d", length);
/* check for overflow - this is really fatal */
/* XXX could add just what will fit & try to parse, then repeat... */
/* check for overflow - this would be really fatal */
if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) {
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
return;
@@ -314,8 +313,13 @@ mixer_handle_text(const void *buffer, size_t length)
/* if anything was parsed */
if (resid != mixer_text_length) {
/* ideally, this should test resid == 0 ? */
r_status_flags |= PX4IO_P_STATUS_FLAGS_MIXER_OK;
/* only set mixer ok if no residual is left over */
if (resid == 0) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_MIXER_OK;
} else {
/* not yet reached the end of the mixer, set as not ok */
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
}
isr_debug(2, "used %u", mixer_text_length - resid);
@@ -338,11 +342,13 @@ mixer_set_failsafe()
{
/*
* Check if a custom failsafe value has been written,
* else use the opportunity to set decent defaults.
* or if the mixer is not ok and bail out.
*/
if (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)
if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ||
!(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK))
return;
/* set failsafe defaults to the values for all inputs = 0 */
float outputs[IO_SERVO_COUNT];
unsigned mixed;
src/systemcmds/mixer/mixer.c
+2 -2
View File
@@ -88,8 +88,8 @@ load(const char *devname, const char *fname)
{
int dev;
FILE *fp;
char line[80];
char buf[512];
char line[120];
char buf[2048];
/* open the device */
if ((dev = open(devname, 0)) < 0)