From 0b47ed86e04bb1930507a74a745ea0b0259dc31f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 15 Jul 2013 13:58:43 +0200 Subject: [PATCH 01/18] Implemented new, simple system boot config and sane default value system based on two parameters evaluated at boot time --- .../init.d/{rc.FMU_quad_x => rc.1_fmu_quad_x} | 35 ++++- .../init.d/{rc.IO_QUAD => rc.2_fmu_io_quad_x} | 29 ++++- .../{rc.PX4IO => rc.30_fmu_io_camflyer} | 25 +++- .../px4fmu_common/init.d/rc.31_fmu_io_phantom | 120 ++++++++++++++++++ .../init.d/{rc.PX4IOAR => rc.fmu_ardrone} | 0 ...AR_PX4FLOW_example => rc.fmu_ardrone_flow} | 0 ROMFS/px4fmu_common/init.d/rcS | 24 ++++ src/modules/systemlib/module.mk | 3 +- src/modules/systemlib/system_params.c | 47 +++++++ src/systemcmds/param/param.c | 73 ++++++++++- 10 files changed, 335 insertions(+), 21 deletions(-) rename ROMFS/px4fmu_common/init.d/{rc.FMU_quad_x => rc.1_fmu_quad_x} (70%) rename ROMFS/px4fmu_common/init.d/{rc.IO_QUAD => rc.2_fmu_io_quad_x} (84%) rename ROMFS/px4fmu_common/init.d/{rc.PX4IO => rc.30_fmu_io_camflyer} (84%) create mode 100644 ROMFS/px4fmu_common/init.d/rc.31_fmu_io_phantom rename ROMFS/px4fmu_common/init.d/{rc.PX4IOAR => rc.fmu_ardrone} (100%) rename ROMFS/px4fmu_common/init.d/{rc.PX4IOAR_PX4FLOW_example => rc.fmu_ardrone_flow} (100%) create mode 100644 src/modules/systemlib/system_params.c diff --git a/ROMFS/px4fmu_common/init.d/rc.FMU_quad_x b/ROMFS/px4fmu_common/init.d/rc.1_fmu_quad_x similarity index 70% rename from ROMFS/px4fmu_common/init.d/rc.FMU_quad_x rename to ROMFS/px4fmu_common/init.d/rc.1_fmu_quad_x index 980197d68e..a72a2a2396 100644 --- a/ROMFS/px4fmu_common/init.d/rc.FMU_quad_x +++ b/ROMFS/px4fmu_common/init.d/rc.1_fmu_quad_x @@ -3,10 +3,8 @@ # Flight startup script for PX4FMU with PWM outputs. # -# Disable the USB interface +# disable USB and autostart set USB no - -# Disable autostarting other apps set MODE custom echo "[init] doing PX4FMU Quad startup..." @@ -25,7 +23,16 @@ if [ -f /fs/microsd/params ] then param load /fs/microsd/params fi - + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set SYS_AUTOCONFIG 0 +fi + # # Force some key parameters to sane values # MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor @@ -48,6 +55,11 @@ sh /etc/init.d/rc.sensors # Start the commander. # commander start + +# +# Start GPS interface (depends on orb) +# +gps start # # Start the attitude estimator @@ -57,11 +69,22 @@ attitude_estimator_ekf start echo "[init] starting PWM output" fmu mode_pwm mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix +pwm -u 400 -m 0xff # # Start attitude control # multirotor_att_control start + +# +# Start logging +# +sdlog2 start -r 50 -a -b 14 -echo "[init] startup done, exiting" -exit \ No newline at end of file +# +# Start system state +# +if blinkm start +then + blinkm systemstate +fi diff --git a/ROMFS/px4fmu_common/init.d/rc.IO_QUAD b/ROMFS/px4fmu_common/init.d/rc.2_fmu_io_quad_x similarity index 84% rename from ROMFS/px4fmu_common/init.d/rc.IO_QUAD rename to ROMFS/px4fmu_common/init.d/rc.2_fmu_io_quad_x index 5f2de0d7e0..8fa87442ba 100644 --- a/ROMFS/px4fmu_common/init.d/rc.IO_QUAD +++ b/ROMFS/px4fmu_common/init.d/rc.2_fmu_io_quad_x @@ -1,8 +1,11 @@ #!nsh +# +# Flight startup script for PX4FMU+PX4IO +# -# Disable USB and autostart +# disable USB and autostart set USB no -set MODE quad +set MODE custom # # Start the ORB (first app to start) @@ -18,6 +21,15 @@ if [ -f /fs/microsd/params ] then param load /fs/microsd/params fi + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set SYS_AUTOCONFIG 0 +fi # # Force some key parameters to sane values @@ -68,6 +80,11 @@ px4io start # Allow PX4IO to recover from midair restarts. # this is very unlikely, but quite safe and robust. px4io recovery + +# +# Disable px4io topic limiting +# +px4io limit 200 # # Start the sensors (depends on orb, px4io) @@ -88,20 +105,18 @@ attitude_estimator_ekf start # Load mixer and start controllers (depends on px4io) # mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix +pwm -u 400 -m 0xff multirotor_att_control start # # Start logging # -#sdlog start -s 4 +sdlog2 start -r 50 -a -b 14 # # Start system state # if blinkm start then - echo "using BlinkM for state indication" blinkm systemstate -else - echo "no BlinkM found, OK." -fi \ No newline at end of file +fi diff --git a/ROMFS/px4fmu_common/init.d/rc.PX4IO b/ROMFS/px4fmu_common/init.d/rc.30_fmu_io_camflyer similarity index 84% rename from ROMFS/px4fmu_common/init.d/rc.PX4IO rename to ROMFS/px4fmu_common/init.d/rc.30_fmu_io_camflyer index 925a5703e7..e04aafe54c 100644 --- a/ROMFS/px4fmu_common/init.d/rc.PX4IO +++ b/ROMFS/px4fmu_common/init.d/rc.30_fmu_io_camflyer @@ -1,8 +1,11 @@ #!nsh +# +# Flight startup script for PX4FMU+PX4IO +# -# Disable USB and autostart +# disable USB and autostart set USB no -set MODE camflyer +set MODE custom # # Start the ORB (first app to start) @@ -18,6 +21,15 @@ if [ -f /fs/microsd/params ] then param load /fs/microsd/params fi + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set SYS_AUTOCONFIG 0 +fi # # Force some key parameters to sane values @@ -68,6 +80,10 @@ px4io start # Allow PX4IO to recover from midair restarts. # this is very unlikely, but quite safe and robust. px4io recovery + +# +# Set actuator limit to 100 Hz update (50 Hz PWM) +px4io limit 100 # # Start the sensors (depends on orb, px4io) @@ -93,15 +109,12 @@ control_demo start # # Start logging # -#sdlog start -s 4 +sdlog2 start -r 50 -a -b 14 # # Start system state # if blinkm start then - echo "using BlinkM for state indication" blinkm systemstate -else - echo "no BlinkM found, OK." fi diff --git a/ROMFS/px4fmu_common/init.d/rc.31_fmu_io_phantom b/ROMFS/px4fmu_common/init.d/rc.31_fmu_io_phantom new file mode 100644 index 0000000000..e04aafe54c --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.31_fmu_io_phantom @@ -0,0 +1,120 @@ +#!nsh +# +# Flight startup script for PX4FMU+PX4IO +# + +# disable USB and autostart +set USB no +set MODE custom + +# +# Start the ORB (first app to start) +# +uorb start + +# +# Load microSD params +# +echo "[init] loading microSD params" +param select /fs/microsd/params +if [ -f /fs/microsd/params ] +then + param load /fs/microsd/params +fi + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set SYS_AUTOCONFIG 0 +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor +# see https://pixhawk.ethz.ch/mavlink/ +# +param set MAV_TYPE 1 + +# +# Check if PX4IO Firmware should be upgraded (from Andrew Tridgell) +# +if [ -f /fs/microsd/px4io.bin ] +then + echo "PX4IO Firmware found. Checking Upgrade.." + if cmp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current + then + echo "No newer version, skipping upgrade." + else + echo "Loading /fs/microsd/px4io.bin" + if px4io update /fs/microsd/px4io.bin > /fs/microsd/px4io_update.log + then + cp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current + echo "Flashed /fs/microsd/px4io.bin OK" >> /fs/microsd/px4io_update.log + else + echo "Failed flashing /fs/microsd/px4io.bin" >> /fs/microsd/px4io_update.log + echo "Failed to upgrade PX4IO firmware - check if PX4IO is in bootloader mode" + fi + fi +fi + +# +# Start MAVLink (depends on orb) +# +mavlink start -d /dev/ttyS1 -b 57600 +usleep 5000 + +# +# Start the commander (depends on orb, mavlink) +# +commander start + +# +# Start PX4IO interface (depends on orb, commander) +# +px4io start + +# +# Allow PX4IO to recover from midair restarts. +# this is very unlikely, but quite safe and robust. +px4io recovery + +# +# Set actuator limit to 100 Hz update (50 Hz PWM) +px4io limit 100 + +# +# Start the sensors (depends on orb, px4io) +# +sh /etc/init.d/rc.sensors + +# +# Start GPS interface (depends on orb) +# +gps start + +# +# Start the attitude estimator (depends on orb) +# +kalman_demo start + +# +# Load mixer and start controllers (depends on px4io) +# +mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix +control_demo start + +# +# Start logging +# +sdlog2 start -r 50 -a -b 14 + +# +# Start system state +# +if blinkm start +then + blinkm systemstate +fi diff --git a/ROMFS/px4fmu_common/init.d/rc.PX4IOAR b/ROMFS/px4fmu_common/init.d/rc.fmu_ardrone similarity index 100% rename from ROMFS/px4fmu_common/init.d/rc.PX4IOAR rename to ROMFS/px4fmu_common/init.d/rc.fmu_ardrone diff --git a/ROMFS/px4fmu_common/init.d/rc.PX4IOAR_PX4FLOW_example b/ROMFS/px4fmu_common/init.d/rc.fmu_ardrone_flow similarity index 100% rename from ROMFS/px4fmu_common/init.d/rc.PX4IOAR_PX4FLOW_example rename to ROMFS/px4fmu_common/init.d/rc.fmu_ardrone_flow diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 22dec87cb0..06c1c2492a 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -24,6 +24,30 @@ else tone_alarm 2 fi +# +# Check if auto-setup from one of the standard scripts is wanted +# SYS_AUTOSTART = 0 means no autostart (default) +# +if param compare SYS_AUTOSTART 1 +then + sh /etc/init.d/rc.1_fmu_quad_x +fi + +if param compare SYS_AUTOSTART 2 +then + sh /etc/init.d/rc.2_fmu_io_quad_x +fi + +if param compare SYS_AUTOSTART 30 +then + sh /etc/init.d/rc.30_fmu_io_camflyer +fi + +if param compare SYS_AUTOSTART 31 +then + sh /etc/init.d/rc.31_fmu_io_phantom +fi + # # Look for an init script on the microSD card. # diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index fd0289c9a4..b470c12271 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -47,4 +47,5 @@ SRCS = err.c \ pid/pid.c \ geo/geo.c \ systemlib.c \ - airspeed.c + airspeed.c \ + system_params.c diff --git a/src/modules/systemlib/system_params.c b/src/modules/systemlib/system_params.c new file mode 100644 index 0000000000..75be090f80 --- /dev/null +++ b/src/modules/systemlib/system_params.c @@ -0,0 +1,47 @@ +/**************************************************************************** + * + * Copyright (c) 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file system_params.c + * + * System wide parameters + */ + +#include +#include + +// Auto-start script with index #n +PARAM_DEFINE_INT32(SYS_AUTOSTART, 0); + +// Automatically configure default values +PARAM_DEFINE_INT32(SYS_AUTOCONFIG, 0); diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index 60e61d07b6..c3fedb958b 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -63,6 +63,7 @@ static void do_import(const char* param_file_name); static void do_show(const char* search_string); static void do_show_print(void *arg, param_t param); static void do_set(const char* name, const char* val); +static void do_compare(const char* name, const char* val); int param_main(int argc, char *argv[]) @@ -117,9 +118,17 @@ param_main(int argc, char *argv[]) errx(1, "not enough arguments.\nTry 'param set PARAM_NAME 3'"); } } + + if (!strcmp(argv[1], "compare")) { + if (argc >= 4) { + do_compare(argv[2], argv[3]); + } else { + errx(1, "not enough arguments.\nTry 'param compare PARAM_NAME 3'"); + } + } } - errx(1, "expected a command, try 'load', 'import', 'show', 'set', 'select' or 'save'"); + errx(1, "expected a command, try 'load', 'import', 'show', 'set', 'compare', 'select' or 'save'"); } static void @@ -295,3 +304,65 @@ do_set(const char* name, const char* val) exit(0); } + +static void +do_compare(const char* name, const char* val) +{ + int32_t i; + float f; + param_t param = param_find(name); + + /* set nothing if parameter cannot be found */ + if (param == PARAM_INVALID) { + /* param not found */ + errx(1, "Error: Parameter %s not found.", name); + } + + /* + * Set parameter if type is known and conversion from string to value turns out fine + */ + + int ret = 1; + + switch (param_type(param)) { + case PARAM_TYPE_INT32: + if (!param_get(param, &i)) { + printf("curr: %d: ", i); + + /* convert string */ + char* end; + int j = strtol(val,&end,10); + if (i == j) { + ret = 0; + } + + } + + break; + + case PARAM_TYPE_FLOAT: + if (!param_get(param, &f)) { + printf("curr: %4.4f: ", (double)f); + + /* convert string */ + char* end; + float g = strtod(val,&end); + if (fabsf(f - g) < 1e-7f) { + ret = 0; + } + } + + break; + + default: + errx(1, "\n", 0 + param_type(param)); + } + + if (ret == 0) { + printf("%c %s: equal\n", + param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'), + param_name(param)); + } + + exit(ret); +} From c46efd3a7b71f725fcc2887d4a40a90864629c02 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 15 Jul 2013 14:03:39 +0200 Subject: [PATCH 02/18] Added saving of default values once loaded --- ROMFS/px4fmu_common/init.d/rc.1_fmu_quad_x | 1 + ROMFS/px4fmu_common/init.d/rc.2_fmu_io_quad_x | 1 + ROMFS/px4fmu_common/init.d/rc.30_fmu_io_camflyer | 1 + ROMFS/px4fmu_common/init.d/rc.31_fmu_io_phantom | 1 + 4 files changed, 4 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.1_fmu_quad_x b/ROMFS/px4fmu_common/init.d/rc.1_fmu_quad_x index a72a2a2396..a9ab1a32e1 100644 --- a/ROMFS/px4fmu_common/init.d/rc.1_fmu_quad_x +++ b/ROMFS/px4fmu_common/init.d/rc.1_fmu_quad_x @@ -31,6 +31,7 @@ if param compare SYS_AUTOCONFIG 1 then # Set all params here, then disable autoconfig param set SYS_AUTOCONFIG 0 + param save /fs/microsd/params fi # diff --git a/ROMFS/px4fmu_common/init.d/rc.2_fmu_io_quad_x b/ROMFS/px4fmu_common/init.d/rc.2_fmu_io_quad_x index 8fa87442ba..200f49d1b0 100644 --- a/ROMFS/px4fmu_common/init.d/rc.2_fmu_io_quad_x +++ b/ROMFS/px4fmu_common/init.d/rc.2_fmu_io_quad_x @@ -29,6 +29,7 @@ if param compare SYS_AUTOCONFIG 1 then # Set all params here, then disable autoconfig param set SYS_AUTOCONFIG 0 + param save /fs/microsd/params fi # diff --git a/ROMFS/px4fmu_common/init.d/rc.30_fmu_io_camflyer b/ROMFS/px4fmu_common/init.d/rc.30_fmu_io_camflyer index e04aafe54c..5090b98a42 100644 --- a/ROMFS/px4fmu_common/init.d/rc.30_fmu_io_camflyer +++ b/ROMFS/px4fmu_common/init.d/rc.30_fmu_io_camflyer @@ -29,6 +29,7 @@ if param compare SYS_AUTOCONFIG 1 then # Set all params here, then disable autoconfig param set SYS_AUTOCONFIG 0 + param save /fs/microsd/params fi # diff --git a/ROMFS/px4fmu_common/init.d/rc.31_fmu_io_phantom b/ROMFS/px4fmu_common/init.d/rc.31_fmu_io_phantom index e04aafe54c..5090b98a42 100644 --- a/ROMFS/px4fmu_common/init.d/rc.31_fmu_io_phantom +++ b/ROMFS/px4fmu_common/init.d/rc.31_fmu_io_phantom @@ -29,6 +29,7 @@ if param compare SYS_AUTOCONFIG 1 then # Set all params here, then disable autoconfig param set SYS_AUTOCONFIG 0 + param save /fs/microsd/params fi # From eb2a9ded6965ef876b578d23916c5b1204cba44d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 15 Jul 2013 14:17:42 +0200 Subject: [PATCH 03/18] Only printing value if equal --- src/systemcmds/param/param.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index c3fedb958b..40a9297a77 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -327,12 +327,12 @@ do_compare(const char* name, const char* val) switch (param_type(param)) { case PARAM_TYPE_INT32: if (!param_get(param, &i)) { - printf("curr: %d: ", i); /* convert string */ char* end; int j = strtol(val,&end,10); if (i == j) { + printf(" %d: ", i); ret = 0; } @@ -342,12 +342,12 @@ do_compare(const char* name, const char* val) case PARAM_TYPE_FLOAT: if (!param_get(param, &f)) { - printf("curr: %4.4f: ", (double)f); /* convert string */ char* end; - float g = strtod(val,&end); + float g = strtod(val, &end); if (fabsf(f - g) < 1e-7f) { + printf(" %4.4f: ", (double)f); ret = 0; } } From 47dd3fdae1d626a28c273946d45ea8c79fb4b76f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 15 Jul 2013 14:35:20 +0200 Subject: [PATCH 04/18] Added default params for F330 --- ROMFS/px4fmu_common/init.d/rc.1_fmu_quad_x | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.1_fmu_quad_x b/ROMFS/px4fmu_common/init.d/rc.1_fmu_quad_x index a9ab1a32e1..58a970eba5 100644 --- a/ROMFS/px4fmu_common/init.d/rc.1_fmu_quad_x +++ b/ROMFS/px4fmu_common/init.d/rc.1_fmu_quad_x @@ -30,6 +30,22 @@ fi if param compare SYS_AUTOCONFIG 1 then # Set all params here, then disable autoconfig + param set MC_ATTRATE_P 0.14 + param set MC_ATTRATE_I 0 + param set MC_ATTRATE_D 0.006 + param set MC_ATT_P 5.5 + param set MC_ATT_I 0 + param set MC_ATT_D 0 + param set MC_YAWPOS_D 0 + param set MC_YAWPOS_I 0 + param set MC_YAWPOS_P 0.6 + param set MC_YAWRATE_D 0 + param set MC_YAWRATE_I 0 + param set MC_YAWRATE_P 0.08 + param set RC_SCALE_PITCH 1 + param set RC_SCALE_ROLL 1 + param set RC_SCALE_YAW 3 + param set SYS_AUTOCONFIG 0 param save /fs/microsd/params fi From 0cd8f2d35b30b5081083ae830409a9090a02c6bc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 18 Jul 2013 10:01:03 +0200 Subject: [PATCH 05/18] HOTFIX: Fix startup order --- ROMFS/px4fmu_common/init.d/rcS | 56 +++++++++++++++++++++------------- 1 file changed, 34 insertions(+), 22 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 22dec87cb0..bbd86a4744 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -7,7 +7,6 @@ # can change this to prevent automatic startup of the flight script. # set MODE autostart -set USB autoconnect # # Try to mount the microSD card. @@ -42,31 +41,44 @@ then sh /fs/microsd/etc/rc.txt fi -# -# Check for USB host -# -if [ $USB != autoconnect ] -then - echo "[init] not connecting USB" -else - if sercon - then - echo "[init] USB interface connected" - else - if [ -f /dev/ttyACM0 ] - echo "[init] NSH via USB" - then - else - echo "[init] No USB connected" - fi - fi -fi - # if this is an APM build then there will be a rc.APM script # from an EXTERNAL_SCRIPTS build option if [ -f /etc/init.d/rc.APM ] then - echo Running rc.APM + if sercon + then + echo "[init] USB interface connected" + fi + + echo "Running rc.APM" # if APM startup is successful then nsh will exit sh /etc/init.d/rc.APM fi + +if [ $MODE == autostart ] +then + +# +# Start the ORB (first app to start) +# +uorb start + +# +# Load microSD params +# +if ramtron start +then + param select /ramtron/params + if [ -f /ramtron/params ] + then + param load /ramtron/params + fi +else + param select /fs/microsd/params + if [ -f /fs/microsd/params ] + then + param load /fs/microsd/params + fi +fi + +fi From 8d1abf4aa441e1c6c886e8af6ecaab2c3ca6e255 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 18 Jul 2013 13:16:34 +0200 Subject: [PATCH 06/18] Lunchtime HOTFIX: Bring back USB console to operational, allow single-USB connection operation via QGC --- ROMFS/px4fmu_common/init.d/rc.sensors | 13 +++---- ROMFS/px4fmu_common/init.d/rc.usb | 27 +++++++++++++- src/drivers/boards/px4fmu/px4fmu_init.c | 2 +- src/drivers/hmc5883/hmc5883.cpp | 3 +- src/drivers/mpu6000/mpu6000.cpp | 3 +- src/drivers/ms5611/ms5611.cpp | 3 +- src/modules/mavlink/mavlink.c | 47 ++++++++++++------------- 7 files changed, 62 insertions(+), 36 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 62c7184b85..5cf1ff3835 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -34,9 +34,10 @@ fi # ALWAYS start this task before the # preflight_check. # -sensors start - -# -# Check sensors - run AFTER 'sensors start' -# -preflight_check \ No newline at end of file +if sensors start +then + # + # Check sensors - run AFTER 'sensors start' + # + preflight_check +fi \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index 31af3991a4..9868219946 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -5,8 +5,33 @@ echo "Starting MAVLink on this USB console" +# Stop tone alarm +tone_alarm stop + # Tell MAVLink that this link is "fast" -mavlink start -b 230400 -d /dev/console +if mavlink stop +then + echo "stopped other MAVLink instance" +fi +mavlink start -b 230400 -d /dev/ttyACM0 + +# Start the commander +commander start + +# Start sensors +sh /etc/init.d/rc.sensors + +# Start one of the estimators +if attitude_estimator_ekf start +then + echo "estimating attitude" +fi + +# Start GPS +if gps start +then + echo "GPS started" +fi echo "MAVLink started, exiting shell.." diff --git a/src/drivers/boards/px4fmu/px4fmu_init.c b/src/drivers/boards/px4fmu/px4fmu_init.c index 212a92cfa8..36af2298c2 100644 --- a/src/drivers/boards/px4fmu/px4fmu_init.c +++ b/src/drivers/boards/px4fmu/px4fmu_init.c @@ -194,7 +194,7 @@ __EXPORT int nsh_archinitialize(void) /* initial LED state */ drv_led_start(); led_off(LED_AMBER); - led_on(LED_BLUE); + led_off(LED_BLUE); /* Configure SPI-based devices */ diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 59e90d86c1..ac3bdc1325 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1221,7 +1221,8 @@ start() int fd; if (g_dev != nullptr) - errx(1, "already started"); + /* if already started, the still command succeeded */ + errx(0, "already started"); /* create the driver, attempt expansion bus first */ g_dev = new HMC5883(PX4_I2C_BUS_EXPANSION); diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 2208425369..8d9054a387 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -1063,7 +1063,8 @@ start() int fd; if (g_dev != nullptr) - errx(1, "already started"); + /* if already started, the still command succeeded */ + errx(0, "already started"); /* create the driver */ g_dev = new MPU6000(1 /* XXX magic number */, (spi_dev_e)PX4_SPIDEV_MPU); diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 59ab5936ee..c13b65d5ae 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -969,7 +969,8 @@ start() int fd; if (g_dev != nullptr) - errx(1, "already started"); + /* if already started, the still command succeeded */ + errx(0, "already started"); /* create the driver */ g_dev = new MS5611(MS5611_BUS); diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 7c1c4b1751..919d01561c 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -420,12 +420,12 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf 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", baud); + warnx("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, baud); + warnx("UART is %s, baudrate is %d\n", uart_name, baud); uart = open(uart_name, O_RDWR | O_NOCTTY); /* Try to set baud rate */ @@ -433,37 +433,35 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf int termios_state; *is_usb = false; - /* make some wild guesses including that USB serial is indicated by either /dev/ttyACM0 or /dev/console */ - if (strcmp(uart_name, "/dev/ttyACM0") != OK && strcmp(uart_name, "/dev/console") != OK) { - /* Back up the original uart configuration to restore it after exit */ - if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) { - fprintf(stderr, "[mavlink] ERROR getting baudrate / termios config for %s: %d\n", uart_name, termios_state); - close(uart); - return -1; - } + /* Back up the original uart configuration to restore it after exit */ + if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) { + warnx("ERROR get termios config %s: %d\n", uart_name, termios_state); + close(uart); + return -1; + } - /* Fill the struct for the new configuration */ - tcgetattr(uart, &uart_config); + /* 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; + /* Clear ONLCR flag (which appends a CR for every LF) */ + uart_config.c_oflag &= ~ONLCR; + + /* USB serial is indicated by /dev/ttyACM0*/ + if (strcmp(uart_name, "/dev/ttyACM0") != OK && strcmp(uart_name, "/dev/ttyACM1") != OK) { /* Set baud rate */ if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { - fprintf(stderr, "[mavlink] ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state); + warnx("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, "[mavlink] ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name); - close(uart); - return -1; - } - - } else { - *is_usb = true; + if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) { + warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name); + close(uart); + return -1; } return uart; @@ -751,8 +749,7 @@ int mavlink_thread_main(int argc, char *argv[]) pthread_join(uorb_receive_thread, NULL); /* Reset the UART flags to original state */ - if (!usb_uart) - tcsetattr(uart, TCSANOW, &uart_config_original); + tcsetattr(uart, TCSANOW, &uart_config_original); thread_running = false; From 7bf2edc3bf9f04d52c6bd9a64d383acbc2071a00 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 18 Jul 2013 14:01:42 +0200 Subject: [PATCH 07/18] Script cleanup, WIP on mavlink logging --- .../init.d/{rc.1_fmu_quad_x => 01_fmu_quad_x} | 0 .../{rc.2_fmu_io_quad_x => 02_io_quad_x} | 0 .../init.d/{rc.fmu_ardrone => 08_ardrone} | 0 .../{rc.fmu_ardrone_flow => 09_ardrone_flow} | 0 .../{rc.30_fmu_io_camflyer => 30_io_camflyer} | 0 .../{rc.31_fmu_io_phantom => 31_io_phantom} | 0 ROMFS/px4fmu_common/init.d/rcS | 63 ++++++++++++------- src/modules/mavlink/mavlink_log.c | 11 ++++ 8 files changed, 50 insertions(+), 24 deletions(-) rename ROMFS/px4fmu_common/init.d/{rc.1_fmu_quad_x => 01_fmu_quad_x} (100%) rename ROMFS/px4fmu_common/init.d/{rc.2_fmu_io_quad_x => 02_io_quad_x} (100%) rename ROMFS/px4fmu_common/init.d/{rc.fmu_ardrone => 08_ardrone} (100%) rename ROMFS/px4fmu_common/init.d/{rc.fmu_ardrone_flow => 09_ardrone_flow} (100%) rename ROMFS/px4fmu_common/init.d/{rc.30_fmu_io_camflyer => 30_io_camflyer} (100%) rename ROMFS/px4fmu_common/init.d/{rc.31_fmu_io_phantom => 31_io_phantom} (100%) diff --git a/ROMFS/px4fmu_common/init.d/rc.1_fmu_quad_x b/ROMFS/px4fmu_common/init.d/01_fmu_quad_x similarity index 100% rename from ROMFS/px4fmu_common/init.d/rc.1_fmu_quad_x rename to ROMFS/px4fmu_common/init.d/01_fmu_quad_x diff --git a/ROMFS/px4fmu_common/init.d/rc.2_fmu_io_quad_x b/ROMFS/px4fmu_common/init.d/02_io_quad_x similarity index 100% rename from ROMFS/px4fmu_common/init.d/rc.2_fmu_io_quad_x rename to ROMFS/px4fmu_common/init.d/02_io_quad_x diff --git a/ROMFS/px4fmu_common/init.d/rc.fmu_ardrone b/ROMFS/px4fmu_common/init.d/08_ardrone similarity index 100% rename from ROMFS/px4fmu_common/init.d/rc.fmu_ardrone rename to ROMFS/px4fmu_common/init.d/08_ardrone diff --git a/ROMFS/px4fmu_common/init.d/rc.fmu_ardrone_flow b/ROMFS/px4fmu_common/init.d/09_ardrone_flow similarity index 100% rename from ROMFS/px4fmu_common/init.d/rc.fmu_ardrone_flow rename to ROMFS/px4fmu_common/init.d/09_ardrone_flow diff --git a/ROMFS/px4fmu_common/init.d/rc.30_fmu_io_camflyer b/ROMFS/px4fmu_common/init.d/30_io_camflyer similarity index 100% rename from ROMFS/px4fmu_common/init.d/rc.30_fmu_io_camflyer rename to ROMFS/px4fmu_common/init.d/30_io_camflyer diff --git a/ROMFS/px4fmu_common/init.d/rc.31_fmu_io_phantom b/ROMFS/px4fmu_common/init.d/31_io_phantom similarity index 100% rename from ROMFS/px4fmu_common/init.d/rc.31_fmu_io_phantom rename to ROMFS/px4fmu_common/init.d/31_io_phantom diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index ee7e840505..b22591f3c3 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -23,30 +23,6 @@ else tone_alarm 2 fi -# -# Check if auto-setup from one of the standard scripts is wanted -# SYS_AUTOSTART = 0 means no autostart (default) -# -if param compare SYS_AUTOSTART 1 -then - sh /etc/init.d/rc.1_fmu_quad_x -fi - -if param compare SYS_AUTOSTART 2 -then - sh /etc/init.d/rc.2_fmu_io_quad_x -fi - -if param compare SYS_AUTOSTART 30 -then - sh /etc/init.d/rc.30_fmu_io_camflyer -fi - -if param compare SYS_AUTOSTART 31 -then - sh /etc/init.d/rc.31_fmu_io_phantom -fi - # # Look for an init script on the microSD card. # @@ -106,3 +82,42 @@ else fi fi + +# +# Check if auto-setup from one of the standard scripts is wanted +# SYS_AUTOSTART = 0 means no autostart (default) +# +if param compare SYS_AUTOSTART 1 +then + sh /etc/init.d/01_fmu_quad_x +fi + +if param compare SYS_AUTOSTART 2 +then + sh /etc/init.d/02_io_quad_x +fi + +if param compare SYS_AUTOSTART 8 +then + sh /etc/init.d/08_ardrone +fi + +if param compare SYS_AUTOSTART 9 +then + sh /etc/init.d/09_ardrone_flow +fi + +if param compare SYS_AUTOSTART 10 +then + sh /etc/init.d/10_io_f330 +fi + +if param compare SYS_AUTOSTART 30 +then + sh /etc/init.d/30_io_camflyer +fi + +if param compare SYS_AUTOSTART 31 +then + sh /etc/init.d/31_io_phantom +fi diff --git a/src/modules/mavlink/mavlink_log.c b/src/modules/mavlink/mavlink_log.c index d9416a08b9..1921958568 100644 --- a/src/modules/mavlink/mavlink_log.c +++ b/src/modules/mavlink/mavlink_log.c @@ -41,16 +41,20 @@ #include #include +#include #include #include +static FILE* text_recorder_fd = NULL; + void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size) { lb->size = size; lb->start = 0; lb->count = 0; lb->elems = (struct mavlink_logmessage *)calloc(lb->size, sizeof(struct mavlink_logmessage)); + text_recorder_fd = fopen("/fs/microsd/text_recorder.txt", "w"); } int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb) @@ -82,6 +86,13 @@ int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessa memcpy(elem, &(lb->elems[lb->start]), sizeof(struct mavlink_logmessage)); lb->start = (lb->start + 1) % lb->size; --lb->count; + + if (text_recorder_fd) { + fwrite(elem->text, 1, strnlen(elem->text, 50), text_recorder_fd); + fputc("\n", text_recorder_fd); + fsync(text_recorder_fd); + } + return 0; } else { From a5c8d8c5f20584f32acaa03e69681a13799fff6d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 18 Jul 2013 14:24:32 +0200 Subject: [PATCH 08/18] Robustified accel cal --- src/modules/commander/accelerometer_calibration.c | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/accelerometer_calibration.c b/src/modules/commander/accelerometer_calibration.c index 48a36ac268..dc653a079d 100644 --- a/src/modules/commander/accelerometer_calibration.c +++ b/src/modules/commander/accelerometer_calibration.c @@ -283,6 +283,9 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { hrt_abstime t = t_start; hrt_abstime t_prev = t_start; hrt_abstime t_still = 0; + + unsigned poll_errcount = 0; + while (true) { /* wait blocking for new data */ int poll_ret = poll(fds, 1, 1000); @@ -327,12 +330,14 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { } } } else if (poll_ret == 0) { - /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "ERROR: poll failure"); - return -3; + poll_errcount++; } if (t > t_timeout) { - mavlink_log_info(mavlink_fd, "ERROR: timeout"); + poll_errcount++; + } + + if (poll_errcount > 1000) { + mavlink_log_info(mavlink_fd, "ERROR: failed reading accel"); return -1; } } From e19d2e94ec5c38c2800a7001a2a04102734012d4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 18 Jul 2013 15:20:36 +0200 Subject: [PATCH 09/18] Hotfix: Ensured there are never two filters running at the same time if auto-magic happens via USB link --- ROMFS/px4fmu_common/init.d/rc.usb | 11 +++++++++-- src/modules/att_pos_estimator_ekf/kalman_main.cpp | 3 ++- .../attitude_estimator_ekf_main.cpp | 6 ++++-- .../attitude_estimator_so3_comp_main.cpp | 6 ++++-- 4 files changed, 19 insertions(+), 7 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index 9868219946..147521fd11 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -22,9 +22,16 @@ commander start sh /etc/init.d/rc.sensors # Start one of the estimators -if attitude_estimator_ekf start +if attitude_estimator_ekf status then - echo "estimating attitude" + echo "multicopter att filter running" +else + if att_pos_estimator_ekf status + then + echo "fixedwing att filter running" + else + attitude_estimator_ekf start + fi fi # Start GPS diff --git a/src/modules/att_pos_estimator_ekf/kalman_main.cpp b/src/modules/att_pos_estimator_ekf/kalman_main.cpp index 4befdc8795..372b2d162c 100644 --- a/src/modules/att_pos_estimator_ekf/kalman_main.cpp +++ b/src/modules/att_pos_estimator_ekf/kalman_main.cpp @@ -121,12 +121,13 @@ int att_pos_estimator_ekf_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { warnx("is running\n"); + exit(0); } else { warnx("not started\n"); + exit(1); } - exit(0); } usage("unrecognized command"); diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index d8b40ac3b5..1eff60e88f 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -139,10 +139,12 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { - printf("\tattitude_estimator_ekf app is running\n"); + warnx("running"); + exit(0); } else { - printf("\tattitude_estimator_ekf app not started\n"); + warnx("not started"); + exit(1); } exit(0); diff --git a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp index 3ca50fb39c..107c2dfb12 100755 --- a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp +++ b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp @@ -139,10 +139,12 @@ int attitude_estimator_so3_comp_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { - printf("\tattitude_estimator_so3_comp app is running\n"); + warnx("running"); + exit(0); } else { - printf("\tattitude_estimator_so3_comp app not started\n"); + warnx("not started"); + exit(1); } exit(0); From f93fbbae238e0d3c303a3553322dfa6a8cc277f2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 18 Jul 2013 15:26:14 +0200 Subject: [PATCH 10/18] Make preflight check optional --- ROMFS/px4fmu_common/init.d/rc.sensors | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 5cf1ff3835..73f40c5039 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -39,5 +39,5 @@ then # # Check sensors - run AFTER 'sensors start' # - preflight_check + preflight_check & fi \ No newline at end of file From 798075c90d0bf1d271ce9758f1649f160b7373ec Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 18 Jul 2013 15:50:07 +0200 Subject: [PATCH 11/18] Work around orb_check fail in sensors app --- src/modules/sensors/sensors.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index ae5a55109a..be87c752d5 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1141,10 +1141,13 @@ Sensors::ppm_poll() { /* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */ - bool rc_updated; - orb_check(_rc_sub, &rc_updated); + struct pollfd fds[1]; + fds[0].fd = _rc_sub; + fds[0].events = POLLIN; + /* check non-blocking for new data */ + int poll_ret = poll(fds, 1, 0); - if (rc_updated) { + if (poll_ret > 0) { struct rc_input_values rc_input; orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); From da54659b5e2c883c504cc48b82a03504cdaae6af Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 18 Jul 2013 15:55:06 +0200 Subject: [PATCH 12/18] Removed wrong dependency check --- src/modules/sensors/sensors.cpp | 8 -------- 1 file changed, 8 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index be87c752d5..e4fb7416fb 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -139,14 +139,12 @@ public: private: static const unsigned _rc_max_chan_count = RC_CHANNELS_MAX; /**< maximum number of r/c channels we handle */ -#if CONFIG_HRT_PPM hrt_abstime _ppm_last_valid; /**< last time we got a valid ppm signal */ /** * Gather and publish PPM input data. */ void ppm_poll(); -#endif /* XXX should not be here - should be own driver */ int _fd_adc; /**< ADC driver handle */ @@ -397,9 +395,7 @@ Sensors *g_sensors; } Sensors::Sensors() : -#ifdef CONFIG_HRT_PPM _ppm_last_valid(0), -#endif _fd_adc(-1), _last_adc(0), @@ -1135,7 +1131,6 @@ Sensors::adc_poll(struct sensor_combined_s &raw) } } -#if CONFIG_HRT_PPM void Sensors::ppm_poll() { @@ -1335,7 +1330,6 @@ Sensors::ppm_poll() } } -#endif void Sensors::task_main_trampoline(int argc, char *argv[]) @@ -1448,10 +1442,8 @@ Sensors::task_main() if (_publishing) orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw); -#ifdef CONFIG_HRT_PPM /* Look for new r/c input data */ ppm_poll(); -#endif perf_end(_loop_perf); } From 7106565e94ab683b3b9d4b6182bb123e018cbb8b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 18 Jul 2013 16:17:47 +0200 Subject: [PATCH 13/18] Simplified USB startup script --- ROMFS/px4fmu_common/init.d/rc.usb | 40 +++++++++++++++++-------------- 1 file changed, 22 insertions(+), 18 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index 147521fd11..c89932bb5d 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -15,29 +15,33 @@ then fi mavlink start -b 230400 -d /dev/ttyACM0 -# Start the commander -commander start - -# Start sensors -sh /etc/init.d/rc.sensors - -# Start one of the estimators -if attitude_estimator_ekf status +if [ $MODE == autostart ] then - echo "multicopter att filter running" -else - if att_pos_estimator_ekf status + + # Start the commander + commander start + + # Start sensors + sh /etc/init.d/rc.sensors + + # Start one of the estimators + if attitude_estimator_ekf status then - echo "fixedwing att filter running" + echo "multicopter att filter running" else - attitude_estimator_ekf start + if att_pos_estimator_ekf status + then + echo "fixedwing att filter running" + else + attitude_estimator_ekf start + fi fi -fi -# Start GPS -if gps start -then - echo "GPS started" + # Start GPS + if gps start + then + echo "GPS started" + fi fi echo "MAVLink started, exiting shell.." From 56805e8378b4c3a23e274e0815d1e11413895533 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 18 Jul 2013 22:49:04 +0200 Subject: [PATCH 14/18] First community review version of autostart --- ROMFS/px4fmu_common/init.d/02_io_quad_x | 2 +- ROMFS/px4fmu_common/init.d/10_io_f330 | 139 ++++++++++++++++++++++++ 2 files changed, 140 insertions(+), 1 deletion(-) create mode 100644 ROMFS/px4fmu_common/init.d/10_io_f330 diff --git a/ROMFS/px4fmu_common/init.d/02_io_quad_x b/ROMFS/px4fmu_common/init.d/02_io_quad_x index 200f49d1b0..131abf8c4e 100644 --- a/ROMFS/px4fmu_common/init.d/02_io_quad_x +++ b/ROMFS/px4fmu_common/init.d/02_io_quad_x @@ -105,7 +105,7 @@ attitude_estimator_ekf start # # Load mixer and start controllers (depends on px4io) # -mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix +mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix pwm -u 400 -m 0xff multirotor_att_control start diff --git a/ROMFS/px4fmu_common/init.d/10_io_f330 b/ROMFS/px4fmu_common/init.d/10_io_f330 new file mode 100644 index 0000000000..4107fab4fa --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/10_io_f330 @@ -0,0 +1,139 @@ +#!nsh +# +# Flight startup script for PX4FMU+PX4IO +# + +# disable USB and autostart +set USB no +set MODE custom + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set SYS_AUTOCONFIG 0 + + param set MC_ATTRATE_D 0.007 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_P 0.13 + param set MC_ATT_D 0.0 + param set MC_ATT_I 0.0 + param set MC_ATT_P 7.0 + param set MC_POS_P 0.1 + param set MC_RCLOSS_THR 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWPOS_I 0.5 + param set MC_YAWPOS_P 1.0 + param set MC_YAWRATE_D 0.0 + param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_P 0.2 + + param save /fs/microsd/params +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor +# see https://pixhawk.ethz.ch/mavlink/ +# +param set MAV_TYPE 2 + +# +# Check if PX4IO Firmware should be upgraded (from Andrew Tridgell) +# +if [ -f /fs/microsd/px4io2.bin ] +then + echo "PX4IO Firmware found. Checking Upgrade.." + if cmp /fs/microsd/px4io2.bin /fs/microsd/px4io2.cur + then + echo "No newer version, skipping upgrade." + else + echo "Loading /fs/microsd/px4io2.bin" + if px4io update /fs/microsd/px4io2.bin > /fs/microsd/px4io2.log + then + cp /fs/microsd/px4io2.bin /fs/microsd/px4io2.cur + echo "Flashed /fs/microsd/px4io2.bin OK" >> /fs/microsd/px4io2.log + else + echo "Failed flashing /fs/microsd/px4io2.bin" >> /fs/microsd/px4io2.log + echo "Failed to upgrade PX4IO2 firmware - check if PX4IO2 is in bootloader mode" + fi + fi +fi + +# +# Start MAVLink (depends on orb) +# +mavlink start +usleep 5000 + +# +# Start PX4IO interface (depends on orb, commander) +# +px4io start +pwm -u 400 -m 0xff + +# +# Allow PX4IO to recover from midair restarts. +# this is very unlikely, but quite safe and robust. +px4io recovery + +# +# Disable px4io topic limiting +# +px4io limit 200 + +# +# This sets a PWM right after startup (regardless of safety button) +# +px4io idle 900 900 900 900 + +# +# The values are for spinning motors when armed using DJI ESCs +# +px4io min 1200 1200 1200 1200 + +# +# Upper limits could be higher, this is on the safe side +# +px4io max 1800 1800 1800 1800 + +# +# Start the sensors (depends on orb, px4io) +# +sh /etc/init.d/rc.sensors + +# +# Start the commander (depends on orb, mavlink) +# +commander start + +# +# Start GPS interface (depends on orb) +# +gps start + +# +# Start the attitude estimator (depends on orb) +# +attitude_estimator_ekf start + +# +# Load mixer and start controllers (depends on px4io) +# +mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix +multirotor_att_control start + +# +# Start logging +# +sdlog2 start -r 20 -a -b 14 + +# +# Start system state +# +if blinkm start +then + blinkm systemstate +fi From 2c31961bb02522543e2f23bca2c21a7aef7669c7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 19 Jul 2013 08:09:35 +0200 Subject: [PATCH 15/18] Minor change to make USB startup more resilient --- src/modules/sensors/sensors.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index ae5a55109a..f7f0a1f48f 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1488,7 +1488,7 @@ int sensors_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { if (sensors::g_sensors != nullptr) - errx(1, "sensors task already running"); + errx(0, "sensors task already running"); sensors::g_sensors = new Sensors; From 4d88b56e38cfcef91890ec3baec16fbda41cee75 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 19 Jul 2013 08:14:44 +0200 Subject: [PATCH 16/18] Handle case of non-present leds in preflight check --- src/systemcmds/preflight_check/preflight_check.c | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c index 7752ffe674..d1dd85d479 100644 --- a/src/systemcmds/preflight_check/preflight_check.c +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -135,6 +135,7 @@ int preflight_check_main(int argc, char *argv[]) close(fd); fd = open(BARO_DEVICE_PATH, 0); + close(fd); /* ---- RC CALIBRATION ---- */ @@ -251,6 +252,11 @@ system_eval: int buzzer = open("/dev/tone_alarm", O_WRONLY); int leds = open(LED_DEVICE_PATH, 0); + if (leds < 0) { + close(buzzer); + errx(1, "failed to open leds, aborting"); + } + /* flip blue led into alternating amber */ led_off(leds, LED_BLUE); led_off(leds, LED_AMBER); From 0f19de53119e5d89b2520f6906ab50fc9d3a3b28 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 21 Jul 2013 11:27:52 +0200 Subject: [PATCH 17/18] Ensured correct pointer init --- src/modules/sensors/sensors.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index b2a6c6a790..5722d2fdf7 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -391,7 +391,7 @@ namespace sensors #endif static const int ERROR = -1; -Sensors *g_sensors; +Sensors *g_sensors = nullptr; } Sensors::Sensors() : From dc5bcdda761e5f8f4f7f26a600f02df007ab1df6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 21 Jul 2013 11:29:10 +0200 Subject: [PATCH 18/18] Hotfix: Made accel calibration a bit more forgiving about motion threshold --- src/modules/commander/accelerometer_calibration.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/accelerometer_calibration.c b/src/modules/commander/accelerometer_calibration.c index dc653a079d..6a304896a3 100644 --- a/src/modules/commander/accelerometer_calibration.c +++ b/src/modules/commander/accelerometer_calibration.c @@ -268,8 +268,8 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { float accel_disp[3] = { 0.0f, 0.0f, 0.0f }; /* EMA time constant in seconds*/ float ema_len = 0.2f; - /* set "still" threshold to 0.1 m/s^2 */ - float still_thr2 = pow(0.1f, 2); + /* set "still" threshold to 0.25 m/s^2 */ + float still_thr2 = pow(0.25f, 2); /* set accel error threshold to 5m/s^2 */ float accel_err_thr = 5.0f; /* still time required in us */