From 0b47ed86e04bb1930507a74a745ea0b0259dc31f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 15 Jul 2013 13:58:43 +0200 Subject: [PATCH 01/10] 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/10] 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/10] 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/10] 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 7bf2edc3bf9f04d52c6bd9a64d383acbc2071a00 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 18 Jul 2013 14:01:42 +0200 Subject: [PATCH 05/10] 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 56805e8378b4c3a23e274e0815d1e11413895533 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 18 Jul 2013 22:49:04 +0200 Subject: [PATCH 06/10] 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 07/10] 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 08/10] 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 09/10] 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 10/10] 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 */