From 255d91d8d49ce06f065b6a0269bdfabeaa40fae4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 7 Jan 2014 21:56:35 +0100 Subject: [PATCH 001/108] hw_ver app added for hardware version checking --- ROMFS/px4fmu_common/init.d/1001_rc_quad.hil | 7 +- ROMFS/px4fmu_common/init.d/800_sdlogger | 4 +- ROMFS/px4fmu_common/init.d/rc.io | 2 +- ROMFS/px4fmu_common/init.d/rc.logging | 4 +- ROMFS/px4fmu_common/init.d/rc.sensors | 4 +- ROMFS/px4fmu_common/init.d/rcS | 9 ++- makefiles/config_px4fmu-v1_backside.mk | 1 + makefiles/config_px4fmu-v1_default.mk | 1 + makefiles/config_px4fmu-v2_default.mk | 1 + makefiles/config_px4fmu-v2_test.mk | 1 + .../version/version.h} | 8 +- src/modules/sdlog2/sdlog2.c | 2 +- src/systemcmds/hw_ver/hw_ver.c | 73 +++++++++++++++++++ src/systemcmds/hw_ver/module.mk | 43 +++++++++++ 14 files changed, 144 insertions(+), 16 deletions(-) rename src/{modules/sdlog2/sdlog2_version.h => lib/version/version.h} (95%) create mode 100644 src/systemcmds/hw_ver/hw_ver.c create mode 100644 src/systemcmds/hw_ver/module.mk diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil index 9b664d63ed..25ea25ae88 100644 --- a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil +++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil @@ -67,8 +67,11 @@ if px4io start then echo "IO started" else - fmu mode_serial - echo "FMU started" + if hw_ver compare PX4FMU_V1 + then + fmu mode_serial + echo "FMU started" + fi fi # diff --git a/ROMFS/px4fmu_common/init.d/800_sdlogger b/ROMFS/px4fmu_common/init.d/800_sdlogger index 9b90cbdd04..2d2c3737bd 100644 --- a/ROMFS/px4fmu_common/init.d/800_sdlogger +++ b/ROMFS/px4fmu_common/init.d/800_sdlogger @@ -42,10 +42,12 @@ position_estimator_inav start if [ -d /fs/microsd ] then - if [ $BOARD == fmuv1 ] + if hw_ver compare PX4FMU_V1 then + echo "Start sdlog2 at 50Hz" sdlog2 start -r 50 -e -b 16 else + echo "Start sdlog2 at 200Hz" sdlog2 start -r 200 -e -b 16 fi fi diff --git a/ROMFS/px4fmu_common/init.d/rc.io b/ROMFS/px4fmu_common/init.d/rc.io index aaf91b3166..24784610c1 100644 --- a/ROMFS/px4fmu_common/init.d/rc.io +++ b/ROMFS/px4fmu_common/init.d/rc.io @@ -11,7 +11,7 @@ then # # Disable px4io topic limiting # - if [ $BOARD == fmuv1 ] + if hw_ver compare PX4FMU_V1 then px4io limit 200 else diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging index dc4be8055a..1791acbee4 100644 --- a/ROMFS/px4fmu_common/init.d/rc.logging +++ b/ROMFS/px4fmu_common/init.d/rc.logging @@ -5,10 +5,12 @@ if [ -d /fs/microsd ] then - if [ $BOARD == fmuv1 ] + if hw_ver compare PX4FMU_V1 then + echo "Start sdlog2 at 50Hz" sdlog2 start -r 50 -a -b 16 else + echo "Start sdlog2 at 200Hz" sdlog2 start -r 200 -a -b 16 fi fi diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 070a4e7e35..a2517135f3 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -28,9 +28,7 @@ fi if lsm303d start then - set BOARD fmuv2 -else - set BOARD fmuv1 + echo "using LSM303D" fi # Start airspeed sensors diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 66cb3f2375..8801d11267 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -185,9 +185,12 @@ then else echo "PX4IO restart failed" echo "PX4IO restart failed" >> $logfile - tone_alarm MNGGG - sleep 10 - reboot + if hw_ver compare PX4FMU_V2 + then + tone_alarm MNGGG + sleep 10 + reboot + fi fi else echo "PX4IO update failed" diff --git a/makefiles/config_px4fmu-v1_backside.mk b/makefiles/config_px4fmu-v1_backside.mk index c86beaccae..a37aa8f962 100644 --- a/makefiles/config_px4fmu-v1_backside.mk +++ b/makefiles/config_px4fmu-v1_backside.mk @@ -54,6 +54,7 @@ MODULES += systemcmds/top MODULES += systemcmds/tests MODULES += systemcmds/config MODULES += systemcmds/nshterm +MODULES += systemcmds/hw_ver # # General system control diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index d0733ec53d..33c0b99e24 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -58,6 +58,7 @@ MODULES += systemcmds/top MODULES += systemcmds/tests MODULES += systemcmds/config MODULES += systemcmds/nshterm +MODULES += systemcmds/hw_ver # # General system control diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index afe072b644..475cbae010 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -60,6 +60,7 @@ MODULES += systemcmds/top MODULES += systemcmds/tests MODULES += systemcmds/config MODULES += systemcmds/nshterm +MODULES += systemcmds/hw_ver # # General system control diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index 0f60e88b5e..750c0e7c33 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -16,6 +16,7 @@ MODULES += drivers/led MODULES += drivers/boards/px4fmu-v2 MODULES += systemcmds/perf MODULES += systemcmds/reboot +MODULES += systemcmds/hw_ver # # Library modules diff --git a/src/modules/sdlog2/sdlog2_version.h b/src/lib/version/version.h similarity index 95% rename from src/modules/sdlog2/sdlog2_version.h rename to src/lib/version/version.h index c6a9ba6387..af733aaf06 100644 --- a/src/modules/sdlog2/sdlog2_version.h +++ b/src/lib/version/version.h @@ -33,15 +33,15 @@ ****************************************************************************/ /** - * @file sdlog2_version.h + * @file version.h * * Tools for system version detection. * * @author Anton Babushkin */ -#ifndef SDLOG2_VERSION_H_ -#define SDLOG2_VERSION_H_ +#ifndef VERSION_H_ +#define VERSION_H_ /* GIT_VERSION is defined at build time via a Makefile call to the @@ -59,4 +59,4 @@ #define HW_ARCH "PX4FMU_V2" #endif -#endif /* SDLOG2_VERSION_H_ */ +#endif /* VERSION_H_ */ diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index e94b1e13cb..658c6779ce 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -85,13 +85,13 @@ #include #include +#include #include #include "logbuffer.h" #include "sdlog2_format.h" #include "sdlog2_messages.h" -#include "sdlog2_version.h" #define LOGBUFFER_WRITE_AND_COUNT(_msg) if (logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(_msg))) { \ log_msgs_written++; \ diff --git a/src/systemcmds/hw_ver/hw_ver.c b/src/systemcmds/hw_ver/hw_ver.c new file mode 100644 index 0000000000..4b84523cc1 --- /dev/null +++ b/src/systemcmds/hw_ver/hw_ver.c @@ -0,0 +1,73 @@ +/**************************************************************************** + * + * Copyright (c) 2014 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 hw_ver.c + * + * Show and test hardware version. + */ + +#include + +#include +#include +#include +#include + +__EXPORT int hw_ver_main(int argc, char *argv[]); + +int +hw_ver_main(int argc, char *argv[]) +{ + if (argc >= 2) { + if (!strcmp(argv[1], "show")) { + printf(HW_ARCH "\n"); + exit(0); + } + + if (!strcmp(argv[1], "compare")) { + if (argc >= 3) { + int ret = strcmp(HW_ARCH, argv[2]) != 0; + if (ret == 0) { + printf("hw_ver match: %s\n", HW_ARCH); + } + exit(ret); + + } else { + errx(1, "not enough arguments, try 'compare PX4FMU_1'"); + } + } + } + + errx(1, "expected a command, try 'show' or 'compare'"); +} diff --git a/src/systemcmds/hw_ver/module.mk b/src/systemcmds/hw_ver/module.mk new file mode 100644 index 0000000000..3cc08b6a1a --- /dev/null +++ b/src/systemcmds/hw_ver/module.mk @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2014 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. +# +############################################################################ + +# +# Show and test hardware version +# + +MODULE_COMMAND = hw_ver +SRCS = hw_ver.c + +MODULE_STACKSIZE = 1024 + +MAXOPTIMIZATION = -Os From 4cffd99db940a9f0cda7643842ccf17d8a3f1b48 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 8 Jan 2014 20:55:12 +0100 Subject: [PATCH 002/108] Major autostart rewrite --- ROMFS/px4fmu_common/init.d/1001_rc_quad.hil | 138 ++----- ROMFS/px4fmu_common/init.d/4010_dji_f330 | 67 +-- ROMFS/px4fmu_common/init.d/4011_dji_f450 | 66 +-- ROMFS/px4fmu_common/init.d/rc.autostart | 180 ++++++++ ROMFS/px4fmu_common/init.d/rc.autostart_hil | 34 ++ ROMFS/px4fmu_common/init.d/rc.io | 32 +- .../init.d/{rc.multirotor => rc.mc_apps} | 15 - ROMFS/px4fmu_common/init.d/rc.mc_interface | 23 +- ROMFS/px4fmu_common/init.d/rcS | 385 +++++++----------- 9 files changed, 454 insertions(+), 486 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/rc.autostart create mode 100644 ROMFS/px4fmu_common/init.d/rc.autostart_hil rename ROMFS/px4fmu_common/init.d/{rc.multirotor => rc.mc_apps} (64%) diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil index 25ea25ae88..8732673f70 100644 --- a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil +++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil @@ -1,108 +1,42 @@ #!nsh -# -# USB HIL start -# -echo "[HIL] HILS quadrotor starting.." +echo "[init] PX4FMU v1, v2 with or without IO on HIL Quad" -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 +if [ $DO_AUTOCONFIG == yes ] then - # Set all params here, then disable autoconfig - param set SYS_AUTOCONFIG 0 - - param set MC_ATTRATE_D 0.0 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.05 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 - param set MC_ATT_P 3.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_P 2.1 - param set MC_YAWRATE_D 0.0 - param set MC_YAWRATE_I 0.0 - param set MC_YAWRATE_P 0.05 - param set NAV_TAKEOFF_ALT 3.0 - param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.5 - param set MPC_THR_MIN 0.1 - param set MPC_XY_D 0 - param set MPC_XY_P 0.5 - param set MPC_XY_VEL_D 0 - param set MPC_XY_VEL_I 0 - param set MPC_XY_VEL_MAX 3 - param set MPC_XY_VEL_P 0.2 - param set MPC_Z_D 0 - param set MPC_Z_P 1 - param set MPC_Z_VEL_D 0 - param set MPC_Z_VEL_I 0.1 - param set MPC_Z_VEL_MAX 2 - param set MPC_Z_VEL_P 0.20 - - param save + # + # Default parameters for this platform + # + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 + param set MC_ATT_P 7.0 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 + param set MC_YAWPOS_P 2.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + param set MPC_TILT_MAX 0.5 + param set MPC_THR_MAX 0.8 + param set MPC_THR_MIN 0.2 + param set MPC_XY_D 0 + param set MPC_XY_P 0.5 + param set MPC_XY_VEL_D 0 + param set MPC_XY_VEL_I 0 + param set MPC_XY_VEL_MAX 3 + param set MPC_XY_VEL_P 0.2 + param set MPC_Z_D 0 + param set MPC_Z_P 1 + param set MPC_Z_VEL_D 0 + param set MPC_Z_VEL_I 0.1 + param set MPC_Z_VEL_MAX 2 + param set MPC_Z_VEL_P 0.20 fi -# Allow USB some time to come up -sleep 1 -# Tell MAVLink that this link is "fast" -mavlink start -b 230400 -d /dev/ttyACM0 - -# Create a fake HIL /dev/pwm_output interface -hil mode_pwm - -# -# 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 we got an IO -# -if px4io start -then - echo "IO started" -else - if hw_ver compare PX4FMU_V1 - then - fmu mode_serial - echo "FMU started" - fi -fi - -# -# Start the sensors (depends on orb, px4io) -# -sh /etc/init.d/rc.sensors - -# -# Start the attitude estimator (depends on orb) -# -att_pos_estimator_ekf start - -# -# Load mixer and start controllers (depends on px4io) -# -mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix - -# -# Start position estimator -# -position_estimator_inav start - -# -# Start attitude control -# -multirotor_att_control start - -# -# Start position control -# -multirotor_pos_control start - -echo "[HIL] setup done, running" - +set FRAME_TYPE mc +set FRAME_GEOMETRY x +set FRAME_COUNT 4 diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330 index 7054210e24..58c6d99bb4 100644 --- a/ROMFS/px4fmu_common/init.d/4010_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330 @@ -2,30 +2,27 @@ echo "[init] PX4FMU v1, v2 with or without IO on DJI F330" -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 +if [ $DO_AUTOCONFIG == yes ] then - # Set all params here, then disable autoconfig - param set SYS_AUTOCONFIG 0 - - param set MC_ATTRATE_D 0.004 - param set MC_ATTRATE_I 0.0 + # + # Default parameters for this platform + # param set MC_ATTRATE_P 0.12 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 param set MC_ATT_P 7.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.0 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 param set MC_YAWPOS_P 2.0 - param set MC_YAWRATE_D 0.005 - param set MC_YAWRATE_I 0.2 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_D 0.0 param set MC_YAWRATE_P 0.3 - param set NAV_TAKEOFF_ALT 3.0 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.7 - param set MPC_THR_MIN 0.3 + param set MPC_THR_MAX 0.8 + param set MPC_THR_MIN 0.2 param set MPC_XY_D 0 param set MPC_XY_P 0.5 param set MPC_XY_VEL_D 0 @@ -38,32 +35,12 @@ then param set MPC_Z_VEL_I 0.1 param set MPC_Z_VEL_MAX 2 param set MPC_Z_VEL_P 0.20 - - param save fi -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - usleep 5000 - - sh /etc/init.d/rc.io -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - usleep 5000 - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -sh /etc/init.d/rc.mc_interface - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor +set FRAME_TYPE mc +set FRAME_GEOMETRY x +set FRAME_COUNT 4 +set PWM_RATE 400 +set PWM_MIN 1200 +set PWM_MAX 1900 +set PWM_DISARMED 900 diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450 index a1d253191d..ca055dfcb0 100644 --- a/ROMFS/px4fmu_common/init.d/4011_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450 @@ -2,14 +2,11 @@ echo "[init] PX4FMU v1, v2 with or without IO on DJI F450" -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 +if [ $DO_AUTOCONFIG == yes ] then - # Set all params here, then disable autoconfig - param set SYS_AUTOCONFIG 0 - + # + # Default parameters for this platform + # param set MC_ATTRATE_D 0.004 param set MC_ATTRATE_I 0.0 param set MC_ATTRATE_P 0.12 @@ -22,53 +19,12 @@ then param set MC_YAWRATE_D 0.005 param set MC_YAWRATE_I 0.2 param set MC_YAWRATE_P 0.3 - - param save fi -# -# Force some key parameters to sane values -# MAV_TYPE 2 = quadrotor -# -param set MAV_TYPE 2 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - usleep 5000 - - sh /etc/init.d/rc.io -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - usleep 5000 - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -# -# Load mixer -# -mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix - -# -# Set PWM output frequency -# -pwm rate -c 1234 -r 400 - -# -# Set disarmed, min and max PWM signals (for DJI ESCs) -# -pwm disarmed -c 1234 -p 900 -pwm min -c 1234 -p 1200 -pwm max -c 1234 -p 1800 - -# -# Start common multirotor apps -# -sh /etc/init.d/rc.multirotor +set FRAME_TYPE mc +set FRAME_GEOMETRY x +set FRAME_COUNT 4 +set PWM_RATE 400 +set PWM_MIN 1200 +set PWM_MAX 1900 +set PWM_DISARMED 900 diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart new file mode 100644 index 0000000000..10b7bc424e --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -0,0 +1,180 @@ +# +# Check if auto-setup from one of the standard scripts is wanted +# SYS_AUTOSTART = 0 means no autostart (default) +# +# AUTOSTART PARTITION: +# 0 .. 999 Reserved (historical) +# 1000 .. 1999 Simulation setups +# 2000 .. 2999 Standard planes +# 3000 .. 3999 Flying wing +# 4000 .. 4999 Quad X +# 5000 .. 5999 Quad + +# 6000 .. 6999 Hexa X +# 7000 .. 7999 Hexa + +# 8000 .. 8999 Octo X +# 9000 .. 9999 Octo + +# 10000 .. 10999 Wide arm / H frame +# 11000 .. 11999 Hexa Cox +# 12000 .. 12999 Octo Cox + +if param compare SYS_AUTOSTART 4008 8 +then + #sh /etc/init.d/4008_ardrone +fi + +if param compare SYS_AUTOSTART 4009 9 +then + #sh /etc/init.d/4009_ardrone_flow +fi + +if param compare SYS_AUTOSTART 4010 10 +then + sh /etc/init.d/4010_dji_f330 +fi + +if param compare SYS_AUTOSTART 4011 11 +then + sh /etc/init.d/4011_dji_f450 +fi + +if param compare SYS_AUTOSTART 4012 +then + #sh /etc/init.d/666_fmu_q_x550 +fi + +if param compare SYS_AUTOSTART 6012 12 +then + #set MIXER /etc/mixers/FMU_hex_x.mix + #sh /etc/init.d/rc.hexa +fi + +if param compare SYS_AUTOSTART 7013 13 +then + #set MIXER /etc/mixers/FMU_hex_+.mix + #sh /etc/init.d/rc.hexa +fi + +if param compare SYS_AUTOSTART 8001 +then + #set MIXER /etc/mixers/FMU_octo_x.mix + #sh /etc/init.d/rc.octo +fi + +if param compare SYS_AUTOSTART 9001 +then + #set MIXER /etc/mixers/FMU_octo_+.mix + #sh /etc/init.d/rc.octo +fi + +if param compare SYS_AUTOSTART 12001 +then + #set MIXER /etc/mixers/FMU_octo_cox.mix + #sh /etc/init.d/rc.octo +fi + +if param compare SYS_AUTOSTART 10015 15 +then + #sh /etc/init.d/10015_tbs_discovery +fi + +if param compare SYS_AUTOSTART 10016 16 +then + #sh /etc/init.d/10016_3dr_iris +fi + +if param compare SYS_AUTOSTART 4017 17 +then + #set MKBLCTRL_MODE no + #set MKBLCTRL_FRAME x + #sh /etc/init.d/rc.custom_dji_f330_mkblctrl +fi + +if param compare SYS_AUTOSTART 5018 18 +then + #set MKBLCTRL_MODE no + #set MKBLCTRL_FRAME + + #sh /etc/init.d/rc.custom_dji_f330_mkblctrl +fi + +if param compare SYS_AUTOSTART 4019 19 +then + #set MKBLCTRL_MODE yes + #set MKBLCTRL_FRAME x + #sh /etc/init.d/rc.custom_dji_f330_mkblctrl +fi + +if param compare SYS_AUTOSTART 5020 20 +then + #set MKBLCTRL_MODE yes + #set MKBLCTRL_FRAME + + #sh /etc/init.d/rc.custom_dji_f330_mkblctrl +fi + +if param compare SYS_AUTOSTART 4021 21 +then + #set FRAME_GEOMETRY x + #set ESC_MAKER afro + #sh /etc/init.d/rc.custom_io_esc +fi + +if param compare SYS_AUTOSTART 10022 22 +then + #set FRAME_GEOMETRY w + #sh /etc/init.d/rc.custom_io_esc +fi + +if param compare SYS_AUTOSTART 3030 30 +then + #sh /etc/init.d/3030_io_camflyer +fi + +if param compare SYS_AUTOSTART 3031 31 +then + #sh /etc/init.d/3031_io_phantom +fi + +if param compare SYS_AUTOSTART 3032 32 +then + #sh /etc/init.d/3032_skywalker_x5 +fi + +if param compare SYS_AUTOSTART 3033 33 +then + #sh /etc/init.d/3033_io_wingwing +fi + +if param compare SYS_AUTOSTART 3034 34 +then + #sh /etc/init.d/3034_io_fx79 + #set MODE custom +fi + +if param compare SYS_AUTOSTART 40 +then + #sh /etc/init.d/40_io_segway + #set MODE custom +fi + +if param compare SYS_AUTOSTART 2100 100 +then + #sh /etc/init.d/2100_mpx_easystar + #set MODE custom +fi + +if param compare SYS_AUTOSTART 2101 101 +then + #sh /etc/init.d/2101_hk_bixler + #set MODE custom +fi + +if param compare SYS_AUTOSTART 2102 102 +then + #sh /etc/init.d/2102_3dr_skywalker + #set MODE custom +fi + +if param compare SYS_AUTOSTART 800 +then + #sh /etc/init.d/800_sdlogger + #set MODE custom +fi diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart_hil b/ROMFS/px4fmu_common/init.d/rc.autostart_hil new file mode 100644 index 0000000000..d5fc5eb087 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.autostart_hil @@ -0,0 +1,34 @@ +# +# Check if auto-setup from one of the standard scripts for HIL is wanted +# + +if param compare SYS_AUTOSTART 1000 +then + #sh /etc/init.d/1000_rc_fw_easystar.hil + set MODE hil +fi + +if param compare SYS_AUTOSTART 1001 +then + sh /etc/init.d/1001_rc_quad.hil + set MODE hil +fi + +if param compare SYS_AUTOSTART 1002 +then + #sh /etc/init.d/1002_rc_fw_state.hil + set MODE hil +fi + +if param compare SYS_AUTOSTART 1003 +then + #sh /etc/init.d/1003_rc_quad_+.hil + set MODE hil +fi + +if param compare SYS_AUTOSTART 1004 +then + #sh /etc/init.d/1004_rc_fw_Rascal110.hil + set MODE hil +fi + diff --git a/ROMFS/px4fmu_common/init.d/rc.io b/ROMFS/px4fmu_common/init.d/rc.io index 24784610c1..5a010cc9b0 100644 --- a/ROMFS/px4fmu_common/init.d/rc.io +++ b/ROMFS/px4fmu_common/init.d/rc.io @@ -1,23 +1,19 @@ # -# Start PX4IO interface (depends on orb, commander) +# Init PX4IO interface # -if px4io start -then - # - # Allow PX4IO to recover from midair restarts. - # this is very unlikely, but quite safe and robust. - px4io recovery - # - # Disable px4io topic limiting - # - if hw_ver compare PX4FMU_V1 - then - px4io limit 200 - else - px4io limit 400 - fi +# +# Allow PX4IO to recover from midair restarts. +# this is very unlikely, but quite safe and robust. +# +px4io recovery + +# +# Adjust px4io topic limiting +# +if hw_ver compare PX4FMU_V1 +then + px4io limit 200 else - # SOS - tone_alarm error + px4io limit 400 fi diff --git a/ROMFS/px4fmu_common/init.d/rc.multirotor b/ROMFS/px4fmu_common/init.d/rc.mc_apps similarity index 64% rename from ROMFS/px4fmu_common/init.d/rc.multirotor rename to ROMFS/px4fmu_common/init.d/rc.mc_apps index bc550ac5a1..16a7a33c6b 100644 --- a/ROMFS/px4fmu_common/init.d/rc.multirotor +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -3,21 +3,6 @@ # Standard everything needed for multirotors except mixer, actuator output and mavlink # -# -# Start the sensors and test them. -# -sh /etc/init.d/rc.sensors - -# -# Start logging (depends on sensors) -# -sh /etc/init.d/rc.logging - -# -# Start GPS interface (depends on orb) -# -gps start - # # Start the attitude estimator # diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_interface b/ROMFS/px4fmu_common/init.d/rc.mc_interface index 6bb2e84ecf..2a05012a60 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_interface +++ b/ROMFS/px4fmu_common/init.d/rc.mc_interface @@ -35,15 +35,26 @@ else fi fi - # # Set PWM output frequency # -pwm rate -c $OUTPUTS -r $PWM_RATE +if [ $PWM_RATE != none ] +then + pwm rate -c $OUTPUTS -r $PWM_RATE +fi # -# Set disarmed, min and max PWM signals (for DJI ESCs) +# Set disarmed, min and max PWM values # -pwm disarmed -c $OUTPUTS -p $PWM_DISARMED -pwm min -c $OUTPUTS -p $PWM_MIN -pwm max -c $OUTPUTS -p $PWM_MAX +if [ $PWM_DISARMED != none ] +then + pwm disarmed -c $OUTPUTS -p $PWM_DISARMED +fi +if [ $PWM_MIN != none ] +then + pwm min -c $OUTPUTS -p $PWM_MIN +fi +if [ $PWM_MAX != none ] +then + pwm max -c $OUTPUTS -p $PWM_MAX +fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 8801d11267..4dd9af4070 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -60,12 +60,9 @@ fi if [ $MODE == autostart ] then # - # Start terminal + # Start CDC/ACM serial driver # - if sercon - then - echo "USB connected" - fi + sercon # # Start the ORB (first app to start) @@ -107,52 +104,79 @@ then blinkm systemstate fi fi + + set USE_IO no + set FRAME_TYPE none + set PWM_RATE none + set PWM_DISARMED none + set PWM_MIN none + set PWM_MAX none + + if param compare SYS_AUTOCONFIG 1 + then + set DO_AUTOCONFIG yes + else + set DO_AUTOCONFIG no + fi # # Start the Commander (needs to be this early for in-air-restarts) # commander start + + # + # Set parameters and env variables for selected AUTOSTART (HIL setups) + # + sh /etc/init.d/rc.autostart_hil + + if [ $MODE == hil ] + then + # + # Do common HIL setup depending on env variables + # + # Allow USB some time to come up + sleep 1 - if param compare SYS_AUTOSTART 1000 - then - sh /etc/init.d/1000_rc_fw_easystar.hil - set MODE custom - fi + # Start MAVLink on USB port + mavlink start -b 230400 -d /dev/ttyACM0 + usleep 5000 + + # Create a fake HIL /dev/pwm_output interface + hil mode_pwm + + # Sensors + echo "Start sensors" + sh /etc/init.d/rc.sensors + + # + # Fixed wing setup + # + if [ $FRAME_TYPE == fw ] + then + echo "Setup FIXED WING" + fi - if param compare SYS_AUTOSTART 1001 - then - sh /etc/init.d/1001_rc_quad.hil - set MODE custom - fi - - if param compare SYS_AUTOSTART 1002 - then - sh /etc/init.d/1002_rc_fw_state.hil - set MODE custom - fi - - if param compare SYS_AUTOSTART 1003 - then - sh /etc/init.d/1003_rc_quad_+.hil - set MODE custom - fi - - if param compare SYS_AUTOSTART 1004 - then - sh /etc/init.d/1004_rc_fw_Rascal110.hil - set MODE custom - fi - - if [ $MODE != custom ] - then - # Try to get an USB console + # + # Multicopters setup + # + if [ $FRAME_TYPE == mc ] + then + echo "Setup MULTICOPTER" + + # Load mixer and configure outputs + sh /etc/init.d/rc.mc_interface + + # Start common multicopter apps + sh /etc/init.d/rc.mc_apps + fi + else + # Try to get an USB console if not in HIL mode nshterm /dev/ttyACM0 & fi - + # # Upgrade PX4IO firmware # - if [ -f /etc/extras/px4io-v2_default.bin ] then set io_file /etc/extras/px4io-v2_default.bin @@ -170,6 +194,7 @@ then then echo "PX4IO CRC OK" echo "PX4IO CRC OK" >> $logfile + set USE_IO yes else echo "PX4IO CRC failure" echo "PX4IO CRC failure" >> $logfile @@ -182,6 +207,7 @@ then echo "PX4IO restart OK" echo "PX4IO restart OK" >> $logfile tone_alarm MSPAA + set USE_IO yes else echo "PX4IO restart failed" echo "PX4IO restart failed" >> $logfile @@ -200,222 +226,91 @@ then fi set EXIT_ON_END no + set USE_LOGGING yes + set USE_GPS yes # - # Check if auto-setup from one of the standard scripts is wanted - # SYS_AUTOSTART = 0 means no autostart (default) + # Set parameters and env variables for selected AUTOSTART # - # AUTOSTART PARTITION: - # 0 .. 999 Reserved (historical) - # 1000 .. 1999 Simulation setups - # 2000 .. 2999 Standard planes - # 3000 .. 3999 Flying wing - # 4000 .. 4999 Quad X - # 5000 .. 5999 Quad + - # 6000 .. 6999 Hexa X - # 7000 .. 7999 Hexa + - # 8000 .. 8999 Octo X - # 9000 .. 9999 Octo + - # 10000 .. 10999 Wide arm / H frame - # 11000 .. 11999 Hexa Cox - # 12000 .. 12999 Octo Cox + sh /etc/init.d/rc.autostart - if param compare SYS_AUTOSTART 4008 8 + # + # If autoconfig parameter was set, reset it and save parameters + # + if [ $DO_AUTOCONFIG == yes ] then - sh /etc/init.d/4008_ardrone - set MODE custom + param set SYS_AUTOCONFIG 0 + param save fi - if param compare SYS_AUTOSTART 4009 9 - then - sh /etc/init.d/4009_ardrone_flow - set MODE custom - fi + if [ $MODE == autostart ] + then + # + # Do common setup depending on env variables + # + if [ $USE_IO == yes ] + then + echo "Use IO" + + # Start MAVLink on default port: ttyS1 + mavlink start + usleep 5000 + + sh /etc/init.d/rc.io + else + echo "Don't use IO" + + # Start MAVLink on ttyS0 + mavlink start -d /dev/ttyS0 + usleep 5000 + + # Configure FMU for PWM outputs + fmu mode_pwm + + # Exit from nsh to free port for mavlink + set EXIT_ON_END yes + fi + + # Sensors + echo "Start sensors" + sh /etc/init.d/rc.sensors + + # Logging + if [ $USE_LOGGING == yes ] + then + sh /etc/init.d/rc.logging + fi + + # GPS interface + if [ $USE_GPS == yes ] + then + gps start + fi + + # + # Fixed wing setup + # + if [ $FRAME_TYPE == fw ] + then + echo "Setup FIXED WING" + fi - if param compare SYS_AUTOSTART 4010 10 - then - set FRAME_GEOMETRY x - set FRAME_COUNT 4 - set PWM_MIN 1200 - set PWM_MAX 1900 - set PWM_DISARMED 900 - sh /etc/init.d/4010_dji_f330 - set MODE custom + # + # Multicopters setup + # + if [ $FRAME_TYPE == mc ] + then + echo "Setup MULTICOPTER" + + # Load mixer and configure outputs + sh /etc/init.d/rc.mc_interface + + # Start common multicopter apps + sh /etc/init.d/rc.mc_apps + fi fi - if param compare SYS_AUTOSTART 4011 11 - then - sh /etc/init.d/4011_dji_f450 - set MODE custom - fi - - if param compare SYS_AUTOSTART 4012 - then - sh /etc/init.d/666_fmu_q_x550 - set MODE custom - fi - - if param compare SYS_AUTOSTART 6012 12 - then - set MIXER /etc/mixers/FMU_hex_x.mix - sh /etc/init.d/rc.hexa - set MODE custom - fi - - if param compare SYS_AUTOSTART 7013 13 - then - set MIXER /etc/mixers/FMU_hex_+.mix - sh /etc/init.d/rc.hexa - set MODE custom - fi - - if param compare SYS_AUTOSTART 8001 - then - set MIXER /etc/mixers/FMU_octo_x.mix - sh /etc/init.d/rc.octo - set MODE custom - fi - - if param compare SYS_AUTOSTART 9001 - then - set MIXER /etc/mixers/FMU_octo_+.mix - sh /etc/init.d/rc.octo - set MODE custom - fi - - if param compare SYS_AUTOSTART 12001 - then - set MIXER /etc/mixers/FMU_octo_cox.mix - sh /etc/init.d/rc.octo - set MODE custom - fi - - if param compare SYS_AUTOSTART 10015 15 - then - sh /etc/init.d/10015_tbs_discovery - set MODE custom - fi - - if param compare SYS_AUTOSTART 10016 16 - then - sh /etc/init.d/10016_3dr_iris - set MODE custom - fi - - # PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 X Frame - if param compare SYS_AUTOSTART 4017 17 - then - set MKBLCTRL_MODE no - set MKBLCTRL_FRAME x - sh /etc/init.d/rc.custom_dji_f330_mkblctrl - set MODE custom - fi - - # PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame - if param compare SYS_AUTOSTART 5018 18 - then - set MKBLCTRL_MODE no - set MKBLCTRL_FRAME + - sh /etc/init.d/rc.custom_dji_f330_mkblctrl - set MODE custom - fi - - # PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 X Frame Mikrokopter-Addressing - if param compare SYS_AUTOSTART 4019 19 - then - set MKBLCTRL_MODE yes - set MKBLCTRL_FRAME x - sh /etc/init.d/rc.custom_dji_f330_mkblctrl - set MODE custom - fi - - # PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame Mikrokopter-Addressing - if param compare SYS_AUTOSTART 5020 20 - then - set MKBLCTRL_MODE yes - set MKBLCTRL_FRAME + - sh /etc/init.d/rc.custom_dji_f330_mkblctrl - set MODE custom - fi - - # PX4FMU v1 with IO + PPM-based ESCs on Quad X-shape frame - if param compare SYS_AUTOSTART 4021 21 - then - set FRAME_GEOMETRY x - set ESC_MAKER afro - sh /etc/init.d/rc.custom_io_esc - set MODE custom - fi - - # PX4FMU v1 with IO + PPM-based ESCs on Quad X-shape frame - if param compare SYS_AUTOSTART 10022 22 - then - set FRAME_GEOMETRY w - sh /etc/init.d/rc.custom_io_esc - set MODE custom - fi - - if param compare SYS_AUTOSTART 3030 30 - then - sh /etc/init.d/3030_io_camflyer - set MODE custom - fi - - if param compare SYS_AUTOSTART 3031 31 - then - sh /etc/init.d/3031_io_phantom - set MODE custom - fi - - if param compare SYS_AUTOSTART 3032 32 - then - sh /etc/init.d/3032_skywalker_x5 - set MODE custom - fi - - if param compare SYS_AUTOSTART 3033 33 - then - sh /etc/init.d/3033_io_wingwing - set MODE custom - fi - - if param compare SYS_AUTOSTART 3034 34 - then - sh /etc/init.d/3034_io_fx79 - set MODE custom - fi - - if param compare SYS_AUTOSTART 40 - then - sh /etc/init.d/40_io_segway - set MODE custom - fi - - if param compare SYS_AUTOSTART 2100 100 - then - sh /etc/init.d/2100_mpx_easystar - set MODE custom - fi - - if param compare SYS_AUTOSTART 2101 101 - then - sh /etc/init.d/2101_hk_bixler - set MODE custom - fi - - if param compare SYS_AUTOSTART 2102 102 - then - sh /etc/init.d/2102_3dr_skywalker - set MODE custom - fi - - if param compare SYS_AUTOSTART 800 - then - sh /etc/init.d/800_sdlogger - set MODE custom - fi - - # Start any custom extensions that might be missing + # Start any custom extensions if [ -f /fs/microsd/etc/rc.local ] then sh /fs/microsd/etc/rc.local From 9a5ef700709b50d57201e77bc80f11c47b25f548 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 8 Jan 2014 23:31:49 +0100 Subject: [PATCH 003/108] init: USE_LOGGING and USE_GPS env vars removed --- ROMFS/px4fmu_common/init.d/rcS | 12 ++---------- 1 file changed, 2 insertions(+), 10 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 4dd9af4070..7d38736dee 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -226,8 +226,6 @@ then fi set EXIT_ON_END no - set USE_LOGGING yes - set USE_GPS yes # # Set parameters and env variables for selected AUTOSTART @@ -276,16 +274,10 @@ then sh /etc/init.d/rc.sensors # Logging - if [ $USE_LOGGING == yes ] - then - sh /etc/init.d/rc.logging - fi + sh /etc/init.d/rc.logging # GPS interface - if [ $USE_GPS == yes ] - then - gps start - fi + gps start # # Fixed wing setup From 532c4c771e3da9d0b371101a056c29d0f417cd09 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 9 Jan 2014 10:05:24 +0100 Subject: [PATCH 004/108] Autostart: generic quad, hexa and octo added, WIP --- ROMFS/px4fmu_common/init.d/4001_quad_x_pwm | 17 ++++ ROMFS/px4fmu_common/init.d/4010_dji_f330 | 26 +++--- ROMFS/px4fmu_common/init.d/4011_dji_f450 | 32 ++++---- ROMFS/px4fmu_common/init.d/4012_hk_x550 | 27 +++++++ ROMFS/px4fmu_common/init.d/5001_quad_+_pwm | 17 ++++ ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm | 17 ++++ ROMFS/px4fmu_common/init.d/666_fmu_q_x550 | 76 ----------------- ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm | 17 ++++ ROMFS/px4fmu_common/init.d/8001_octo_x_pwm | 17 ++++ ROMFS/px4fmu_common/init.d/9001_octo_+_pwm | 17 ++++ ROMFS/px4fmu_common/init.d/rc.autostart | 31 +++---- ROMFS/px4fmu_common/init.d/rc.hexa | 94 ---------------------- ROMFS/px4fmu_common/init.d/rc.mc_interface | 52 ++++++------ 13 files changed, 201 insertions(+), 239 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/4001_quad_x_pwm create mode 100644 ROMFS/px4fmu_common/init.d/4012_hk_x550 create mode 100644 ROMFS/px4fmu_common/init.d/5001_quad_+_pwm create mode 100644 ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm delete mode 100644 ROMFS/px4fmu_common/init.d/666_fmu_q_x550 create mode 100644 ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm create mode 100644 ROMFS/px4fmu_common/init.d/8001_octo_x_pwm create mode 100644 ROMFS/px4fmu_common/init.d/9001_octo_+_pwm delete mode 100644 ROMFS/px4fmu_common/init.d/rc.hexa diff --git a/ROMFS/px4fmu_common/init.d/4001_quad_x_pwm b/ROMFS/px4fmu_common/init.d/4001_quad_x_pwm new file mode 100644 index 0000000000..9bee414dc7 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/4001_quad_x_pwm @@ -0,0 +1,17 @@ +#!nsh + +# Maintainers: Anton Babushkin , Lorenz Meier + +echo "[init] PX4FMU v1, v2 with or without IO on generic quadcopter X with PWM ESCs" + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # +fi + +set FRAME_TYPE mc +set FRAME_GEOMETRY x +set FRAME_COUNT 4 +set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330 index 58c6d99bb4..93edb00059 100644 --- a/ROMFS/px4fmu_common/init.d/4010_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330 @@ -1,24 +1,29 @@ #!nsh +# Maintainers: Anton Babushkin + echo "[init] PX4FMU v1, v2 with or without IO on DJI F330" +# Use generic quad X PWM as base +sh /etc/init.d/4001_quad_x_pwm + if [ $DO_AUTOCONFIG == yes ] then # # Default parameters for this platform # - param set MC_ATTRATE_P 0.12 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_D 0.004 param set MC_ATT_P 7.0 param set MC_ATT_I 0.0 param set MC_ATT_D 0.0 - param set MC_YAWPOS_P 2.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 + param set MC_YAWPOS_P 2.8 param set MC_YAWPOS_I 0.0 param set MC_YAWPOS_D 0.0 - param set MC_YAWRATE_P 0.3 - param set MC_YAWRATE_I 0.2 - param set MC_YAWRATE_D 0.005 + param set MC_YAWRATE_P 0.2 + param set MC_YAWRATE_I 0.05 + param set MC_YAWRATE_D 0.0 param set MPC_TILT_MAX 0.5 param set MPC_THR_MAX 0.8 @@ -37,10 +42,7 @@ then param set MPC_Z_VEL_P 0.20 fi -set FRAME_TYPE mc -set FRAME_GEOMETRY x -set FRAME_COUNT 4 -set PWM_RATE 400 +# DJI ESC range +set PWM_DISARMED 900 set PWM_MIN 1200 set PWM_MAX 1900 -set PWM_DISARMED 900 diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450 index ca055dfcb0..cc0ddccc89 100644 --- a/ROMFS/px4fmu_common/init.d/4011_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450 @@ -1,30 +1,34 @@ #!nsh +# Maintainers: Lorenz Meier + echo "[init] PX4FMU v1, v2 with or without IO on DJI F450" +# Use generic quad X PWM as base +sh /etc/init.d/4001_quad_x_pwm + if [ $DO_AUTOCONFIG == yes ] then # # Default parameters for this platform # - param set MC_ATTRATE_D 0.004 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.12 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 param set MC_ATT_P 7.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.0 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 param set MC_YAWPOS_P 2.0 - param set MC_YAWRATE_D 0.005 - param set MC_YAWRATE_I 0.2 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_D 0.0 param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + # TODO add default MPC parameters fi -set FRAME_TYPE mc -set FRAME_GEOMETRY x -set FRAME_COUNT 4 -set PWM_RATE 400 +# DJI ESC range +set PWM_DISARMED 900 set PWM_MIN 1200 set PWM_MAX 1900 -set PWM_DISARMED 900 diff --git a/ROMFS/px4fmu_common/init.d/4012_hk_x550 b/ROMFS/px4fmu_common/init.d/4012_hk_x550 new file mode 100644 index 0000000000..a749b7699c --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/4012_hk_x550 @@ -0,0 +1,27 @@ +#!nsh + +# Maintainers: Todd Stellanova + +echo "[init] PX4FMU v1, v2 with or without IO on HobbyKing X550" + +# Use generic quad X PWM as base +sh /etc/init.d/4001_quad_x_pwm + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set MC_ATT_P 5.5 + param set MC_ATT_I 0 + param set MC_ATT_D 0 + param set MC_ATTRATE_P 0.14 + param set MC_ATTRATE_I 0 + param set MC_ATTRATE_D 0.006 + param set MC_YAWPOS_P 0.6 + param set MC_YAWPOS_I 0 + param set MC_YAWPOS_D 0 + param set MC_YAWRATE_P 0.08 + param set MC_YAWRATE_I 0 + param set MC_YAWRATE_D 0 +fi diff --git a/ROMFS/px4fmu_common/init.d/5001_quad_+_pwm b/ROMFS/px4fmu_common/init.d/5001_quad_+_pwm new file mode 100644 index 0000000000..7d07486685 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/5001_quad_+_pwm @@ -0,0 +1,17 @@ +#!nsh + +# Maintainers: Anton Babushkin , Lorenz Meier + +echo "[init] PX4FMU v1, v2 with or without IO on generic quadcopter + with PWM ESCs" + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # +fi + +set FRAME_TYPE mc +set FRAME_GEOMETRY + +set FRAME_COUNT 4 +set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm b/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm new file mode 100644 index 0000000000..3ea5479cbd --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm @@ -0,0 +1,17 @@ +#!nsh + +# Maintainers: Anton Babushkin , Lorenz Meier + +echo "[init] PX4FMU v1, v2 with or without IO on generic hexacopter X with PWM ESCs" + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # +fi + +set FRAME_TYPE mc +set FRAME_GEOMETRY x +set FRAME_COUNT 6 +set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 b/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 deleted file mode 100644 index acd8027fb5..0000000000 --- a/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 +++ /dev/null @@ -1,76 +0,0 @@ -#!nsh - -echo "[init] 666_fmu_q_x550: PX4FMU Quad X550 with or without IO" - -# -# Load default params for this platform -# -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 -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 2 = quadrotor -# -param set MAV_TYPE 2 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - usleep 5000 - - sh /etc/init.d/rc.io -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - usleep 5000 - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -# -# Load mixer -# -mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix - -# -# Set PWM output frequency -# -pwm rate -c 1234 -r 400 - -# -# Set disarmed, min and max PWM signals -# -pwm disarmed -c 1234 -p 900 -pwm min -c 1234 -p 1100 -pwm max -c 1234 -p 1900 - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor diff --git a/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm b/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm new file mode 100644 index 0000000000..f747618b8d --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm @@ -0,0 +1,17 @@ +#!nsh + +# Maintainers: Anton Babushkin , Lorenz Meier + +echo "[init] PX4FMU v1, v2 with or without IO on generic hexacopter + with PWM ESCs" + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # +fi + +set FRAME_TYPE mc +set FRAME_GEOMETRY + +set FRAME_COUNT 4 +set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm b/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm new file mode 100644 index 0000000000..f8f4591856 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm @@ -0,0 +1,17 @@ +#!nsh + +# Maintainers: Anton Babushkin , Lorenz Meier + +echo "[init] PX4FMU v1, v2 with or without IO on generic octocopter X with PWM ESCs" + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # +fi + +set FRAME_TYPE mc +set FRAME_GEOMETRY x +set FRAME_COUNT 8 +set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm b/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm new file mode 100644 index 0000000000..f8bcd13a9e --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm @@ -0,0 +1,17 @@ +#!nsh + +# Maintainers: Anton Babushkin , Lorenz Meier + +echo "[init] PX4FMU v1, v2 with or without IO on generic octocopter + with PWM ESCs" + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # +fi + +set FRAME_TYPE mc +set FRAME_GEOMETRY + +set FRAME_COUNT 8 +set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 10b7bc424e..153fbb66bb 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -17,53 +17,54 @@ # 11000 .. 11999 Hexa Cox # 12000 .. 12999 Octo Cox -if param compare SYS_AUTOSTART 4008 8 +if param compare SYS_AUTOSTART 4001 +then + sh /etc/init.d/4001_quad_x_pwm +fi + +if param compare SYS_AUTOSTART 4008 then #sh /etc/init.d/4008_ardrone fi -if param compare SYS_AUTOSTART 4009 9 +if param compare SYS_AUTOSTART 4009 then #sh /etc/init.d/4009_ardrone_flow fi -if param compare SYS_AUTOSTART 4010 10 +if param compare SYS_AUTOSTART 4010 then sh /etc/init.d/4010_dji_f330 fi -if param compare SYS_AUTOSTART 4011 11 +if param compare SYS_AUTOSTART 4011 then sh /etc/init.d/4011_dji_f450 fi if param compare SYS_AUTOSTART 4012 then - #sh /etc/init.d/666_fmu_q_x550 + sh /etc/init.d/4012_hk_x550 fi -if param compare SYS_AUTOSTART 6012 12 +if param compare SYS_AUTOSTART 6001 then - #set MIXER /etc/mixers/FMU_hex_x.mix - #sh /etc/init.d/rc.hexa + sh /etc/init.d/6001_hexa_x_pwm fi -if param compare SYS_AUTOSTART 7013 13 +if param compare SYS_AUTOSTART 7001 then - #set MIXER /etc/mixers/FMU_hex_+.mix - #sh /etc/init.d/rc.hexa + sh /etc/init.d/7001_hexa_+_pwm fi if param compare SYS_AUTOSTART 8001 then - #set MIXER /etc/mixers/FMU_octo_x.mix - #sh /etc/init.d/rc.octo + sh /etc/init.d/8001_octo_x_pwm fi if param compare SYS_AUTOSTART 9001 then - #set MIXER /etc/mixers/FMU_octo_+.mix - #sh /etc/init.d/rc.octo + sh /etc/init.d/9001_octo_+_pwm fi if param compare SYS_AUTOSTART 12001 diff --git a/ROMFS/px4fmu_common/init.d/rc.hexa b/ROMFS/px4fmu_common/init.d/rc.hexa deleted file mode 100644 index 097db28e48..0000000000 --- a/ROMFS/px4fmu_common/init.d/rc.hexa +++ /dev/null @@ -1,94 +0,0 @@ -#!nsh - -echo "[init] PX4FMU v1, v2 with or without IO on Hex" - -# -# 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.004 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.12 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 - param set MC_ATT_P 7.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_P 2.0 - param set MC_YAWRATE_D 0.005 - param set MC_YAWRATE_I 0.2 - param set MC_YAWRATE_P 0.3 - param set NAV_TAKEOFF_ALT 3.0 - param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.7 - param set MPC_THR_MIN 0.3 - param set MPC_XY_D 0 - param set MPC_XY_P 0.5 - param set MPC_XY_VEL_D 0 - param set MPC_XY_VEL_I 0 - param set MPC_XY_VEL_MAX 3 - param set MPC_XY_VEL_P 0.2 - param set MPC_Z_D 0 - param set MPC_Z_P 1 - param set MPC_Z_VEL_D 0 - param set MPC_Z_VEL_I 0.1 - param set MPC_Z_VEL_MAX 2 - param set MPC_Z_VEL_P 0.20 - - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE list: https://pixhawk.ethz.ch/mavlink/ -# 13 = hexarotor -# -param set MAV_TYPE 13 - -set EXIT_ON_END no - -# -# Start and configure PX4IO interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - usleep 5000 - - sh /etc/init.d/rc.io -else - # This is not possible on a hexa - tone_alarm error -fi - -# -# Load mixer -# -mixer load /dev/pwm_output $MIXER - -# -# Set PWM output frequency to 400 Hz -# -pwm rate -a -r 400 - -# -# Set disarmed, min and max PWM signals -# -pwm disarmed -c 123456 -p 900 -pwm min -c 123456 -p 1100 -pwm max -c 123456 -p 1900 - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor - -if [ $EXIT_ON_END == yes ] -then - exit -fi diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_interface b/ROMFS/px4fmu_common/init.d/rc.mc_interface index 2a05012a60..9e3d678bf3 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_interface +++ b/ROMFS/px4fmu_common/init.d/rc.mc_interface @@ -3,37 +3,33 @@ # Script to set PWM min / max limits and mixer # +echo "Rotors count: $FRAME_COUNT" +if [ $FRAME_COUNT == 4 ] +then + set FRAME_COUNT_STR quad + set OUTPUTS 1234 + param set MAV_TYPE 2 +fi +if [ $FRAME_COUNT == 6 ] +then + set FRAME_COUNT_STR hex + set OUTPUTS 123456 + param set MAV_TYPE 13 +fi +if [ $FRAME_COUNT == 8 ] +then + set FRAME_COUNT_STR octo + set OUTPUTS 12345678 + param set MAV_TYPE 14 +fi + # # Load mixer # -if [ $FRAME_GEOMETRY == x ] -then - echo "Frame geometry X" - mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix -else - if [ $FRAME_GEOMETRY == w ] - then - echo "Frame geometry W" - mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix - else - echo "Frame geometry +" - mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix - fi -fi - -if [ $FRAME_COUNT == 4 ] -then - set OUTPUTS 1234 - param set MAV_TYPE 2 -else - if [ $FRAME_COUNT == 6 ] - then - set OUTPUTS 123456 - param set MAV_TYPE 13 - else - set OUTPUTS 12345678 - fi -fi +echo "Frame geometry: ${FRAME_GEOMETRY}" +set MIXER /etc/mixers/FMU_${FRAME_GEOMETRY_STR}_${FRAME_GEOMETRY}.mix +echo "Loading mixer: ${MIXER}" +mixer load /dev/pwm_output ${MIXER} # # Set PWM output frequency From b5d56523bc100d7bf95a6dfbac95c1afc89e345e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 10 Jan 2014 13:18:34 +0100 Subject: [PATCH 005/108] Init scripts cleanup, WIP --- .../px4fmu_common/init.d/10015_tbs_discovery | 85 ++--- ROMFS/px4fmu_common/init.d/10016_3dr_iris | 98 ++--- ROMFS/px4fmu_common/init.d/1001_rc_quad.hil | 7 +- ROMFS/px4fmu_common/init.d/2100_mpx_easystar | 58 +-- ROMFS/px4fmu_common/init.d/4001_quad_x_pwm | 17 - ROMFS/px4fmu_common/init.d/4010_dji_f330 | 10 +- ROMFS/px4fmu_common/init.d/4011_dji_f450 | 12 +- ROMFS/px4fmu_common/init.d/4012_hk_x550 | 8 +- ROMFS/px4fmu_common/init.d/40_io_segway | 51 --- ROMFS/px4fmu_common/init.d/5001_quad_+_pwm | 17 - ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm | 17 - ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm | 17 - ROMFS/px4fmu_common/init.d/8001_octo_x_pwm | 17 - ROMFS/px4fmu_common/init.d/800_sdlogger | 53 --- ROMFS/px4fmu_common/init.d/9001_octo_+_pwm | 17 - ROMFS/px4fmu_common/init.d/rc.autostart | 218 +++++------ ROMFS/px4fmu_common/init.d/rc.autostart_hil | 34 -- .../init.d/rc.custom_dji_f330_mkblctrl | 113 ------ ROMFS/px4fmu_common/init.d/rc.custom_io_esc | 120 ------ ROMFS/px4fmu_common/init.d/rc.fixedwing | 34 -- ROMFS/px4fmu_common/init.d/rc.fw_apps | 19 + ROMFS/px4fmu_common/init.d/rc.fw_interface | 18 + ROMFS/px4fmu_common/init.d/rc.mc_apps | 2 +- ROMFS/px4fmu_common/init.d/rc.mc_interface | 70 ++-- ROMFS/px4fmu_common/init.d/rc.octo | 94 ----- ROMFS/px4fmu_common/init.d/rc.standalone | 13 - ROMFS/px4fmu_common/init.d/rc.usb | 33 -- ROMFS/px4fmu_common/init.d/rcS | 342 +++++++++--------- 28 files changed, 422 insertions(+), 1172 deletions(-) delete mode 100644 ROMFS/px4fmu_common/init.d/4001_quad_x_pwm delete mode 100644 ROMFS/px4fmu_common/init.d/40_io_segway delete mode 100644 ROMFS/px4fmu_common/init.d/5001_quad_+_pwm delete mode 100644 ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm delete mode 100644 ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm delete mode 100644 ROMFS/px4fmu_common/init.d/8001_octo_x_pwm delete mode 100644 ROMFS/px4fmu_common/init.d/800_sdlogger delete mode 100644 ROMFS/px4fmu_common/init.d/9001_octo_+_pwm delete mode 100644 ROMFS/px4fmu_common/init.d/rc.autostart_hil delete mode 100644 ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl delete mode 100644 ROMFS/px4fmu_common/init.d/rc.custom_io_esc delete mode 100644 ROMFS/px4fmu_common/init.d/rc.fixedwing create mode 100644 ROMFS/px4fmu_common/init.d/rc.fw_apps create mode 100644 ROMFS/px4fmu_common/init.d/rc.fw_interface delete mode 100644 ROMFS/px4fmu_common/init.d/rc.octo delete mode 100644 ROMFS/px4fmu_common/init.d/rc.standalone diff --git a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery index 81d4b5d570..610d482c1f 100644 --- a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery @@ -1,74 +1,31 @@ #!nsh +# Maintainers: Anton Babushkin + echo "[init] Team Blacksheep Discovery Quad" -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 +if [ $DO_AUTOCONFIG == yes ] then - # Set all params here, then disable autoconfig - param set SYS_AUTOCONFIG 0 - - param set MC_ATTRATE_D 0.006 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.17 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 + # + # Default parameters for this platform + # param set MC_ATT_P 5.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.15 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 + param set MC_ATTRATE_P 0.17 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.006 param set MC_YAWPOS_P 0.5 - param set MC_YAWRATE_D 0.0 - param set MC_YAWRATE_I 0.0 + param set MC_YAWPOS_I 0.15 + param set MC_YAWPOS_D 0.0 param set MC_YAWRATE_P 0.2 - - param save + param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_D 0.0 fi -# -# Force some key parameters to sane values -# MAV_TYPE 2 = quadrotor -# -param set MAV_TYPE 2 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - usleep 5000 - - sh /etc/init.d/rc.io -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - usleep 5000 - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -# -# Load the mixer for a quad with wide arms -# -mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix - -# -# Set PWM output frequency -# -pwm rate -c 1234 -r 400 - -# -# Set disarmed, min and max PWM signals -# -pwm disarmed -c 1234 -p 900 -pwm min -c 1234 -p 1100 -pwm max -c 1234 -p 1900 - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor +set FRAME_TYPE mc +set FRAME_GEOMETRY quad_w +set PWM_RATE 400 +set PWM_DISARMED 900 +set PWM_MIN 1100 +set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris index b0f4eda79e..de50280527 100644 --- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris @@ -1,73 +1,43 @@ #!nsh +# Maintainers: Anton Babushkin + echo "[init] 3DR Iris Quad" -# -# 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 +# Use generic wide arm quad X PWM as base +sh /etc/init.d/10001_quad_w - param set MC_ATTRATE_D 0.004 - 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 +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # param set MC_ATT_P 9.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.15 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 + param set MC_ATTRATE_P 0.13 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 param set MC_YAWPOS_P 0.5 - param set MC_YAWRATE_D 0.0 - param set MC_YAWRATE_I 0.0 + param set MC_YAWPOS_I 0.15 + param set MC_YAWPOS_D 0.0 param set MC_YAWRATE_P 0.2 - - param save + param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_D 0.0 + + param set MPC_TILT_MAX 0.5 + param set MPC_THR_MAX 0.8 + param set MPC_THR_MIN 0.2 + param set MPC_XY_D 0 + param set MPC_XY_P 0.5 + param set MPC_XY_VEL_D 0 + param set MPC_XY_VEL_I 0 + param set MPC_XY_VEL_MAX 3 + param set MPC_XY_VEL_P 0.2 + param set MPC_Z_D 0 + param set MPC_Z_P 1 + param set MPC_Z_VEL_D 0 + param set MPC_Z_VEL_I 0.1 + param set MPC_Z_VEL_MAX 2 + param set MPC_Z_VEL_P 0.20 fi - -# -# Force some key parameters to sane values -# MAV_TYPE 2 = quadrotor -# -param set MAV_TYPE 2 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - usleep 5000 - - sh /etc/init.d/rc.io -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - usleep 5000 - fmu mode_pwm - param set BAT_V_SCALING 0.0098 - set EXIT_ON_END yes -fi - -# -# Load the mixer for a quad with wide arms -# -mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix - -# -# Set PWM output frequency -# -pwm rate -c 1234 -r 400 - -# -# Set disarmed, min and max PWM signals -# -pwm disarmed -c 1234 -p 900 -pwm min -c 1234 -p 1050 - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil index 8732673f70..7d21176f22 100644 --- a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil +++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil @@ -37,6 +37,7 @@ then param set MPC_Z_VEL_P 0.20 fi -set FRAME_TYPE mc -set FRAME_GEOMETRY x -set FRAME_COUNT 4 +set HIL yes + +set VEHICLE_TYPE mc +set FRAME_GEOMETRY quad_x diff --git a/ROMFS/px4fmu_common/init.d/2100_mpx_easystar b/ROMFS/px4fmu_common/init.d/2100_mpx_easystar index 97c2d7c903..9bf01c60ce 100644 --- a/ROMFS/px4fmu_common/init.d/2100_mpx_easystar +++ b/ROMFS/px4fmu_common/init.d/2100_mpx_easystar @@ -1,13 +1,12 @@ #!nsh -echo "[init] PX4FMU v1, v2 with or without IO on EasyStar" +echo "[init] EasyStar" -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 +if [ $DO_AUTOCONFIG == yes ] then - # Set all params here, then disable autoconfig + # + # Default parameters for this platform + # param set FW_P_D 0 param set FW_P_I 0 param set FW_P_IMAX 15 @@ -31,50 +30,7 @@ then param set FW_L1_PERIOD 16 param set RC_SCALE_ROLL 1.0 param set RC_SCALE_PITCH 1.0 - - param set SYS_AUTOCONFIG 0 - param save fi -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing -# -param set MAV_TYPE 1 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - - sh /etc/init.d/rc.io - # Limit to 100 Hz updates and (implicit) 50 Hz PWM - px4io limit 100 -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -# -# Load mixer and start controllers (depends on px4io) -# -if [ -f /fs/microsd/etc/mixers/FMU_RET.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_RET.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_RET.mix -else - echo "Using /etc/mixers/FMU_RET.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_RET.mix -fi - -# -# Start common fixedwing apps -# -sh /etc/init.d/rc.fixedwing +set FRAME_TYPE fw +set FRAME_GEOMETRY RET diff --git a/ROMFS/px4fmu_common/init.d/4001_quad_x_pwm b/ROMFS/px4fmu_common/init.d/4001_quad_x_pwm deleted file mode 100644 index 9bee414dc7..0000000000 --- a/ROMFS/px4fmu_common/init.d/4001_quad_x_pwm +++ /dev/null @@ -1,17 +0,0 @@ -#!nsh - -# Maintainers: Anton Babushkin , Lorenz Meier - -echo "[init] PX4FMU v1, v2 with or without IO on generic quadcopter X with PWM ESCs" - -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # -fi - -set FRAME_TYPE mc -set FRAME_GEOMETRY x -set FRAME_COUNT 4 -set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330 index 93edb00059..0cfe68b27d 100644 --- a/ROMFS/px4fmu_common/init.d/4010_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330 @@ -2,10 +2,7 @@ # Maintainers: Anton Babushkin -echo "[init] PX4FMU v1, v2 with or without IO on DJI F330" - -# Use generic quad X PWM as base -sh /etc/init.d/4001_quad_x_pwm +echo "[init] DJI F330" if [ $DO_AUTOCONFIG == yes ] then @@ -42,6 +39,11 @@ then param set MPC_Z_VEL_P 0.20 fi +set VEHICLE_TYPE mc +set FRAME_GEOMETRY quad_x + +set PWM_RATE 400 + # DJI ESC range set PWM_DISARMED 900 set PWM_MIN 1200 diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450 index cc0ddccc89..4a0953a2ed 100644 --- a/ROMFS/px4fmu_common/init.d/4011_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450 @@ -2,10 +2,7 @@ # Maintainers: Lorenz Meier -echo "[init] PX4FMU v1, v2 with or without IO on DJI F450" - -# Use generic quad X PWM as base -sh /etc/init.d/4001_quad_x_pwm +echo "[init] DJI F450" if [ $DO_AUTOCONFIG == yes ] then @@ -27,7 +24,12 @@ then # TODO add default MPC parameters fi - + +set VEHICLE_TYPE mc +set FRAME_GEOMETRY quad_x + +set PWM_RATE 400 + # DJI ESC range set PWM_DISARMED 900 set PWM_MIN 1200 diff --git a/ROMFS/px4fmu_common/init.d/4012_hk_x550 b/ROMFS/px4fmu_common/init.d/4012_hk_x550 index a749b7699c..396349d9c0 100644 --- a/ROMFS/px4fmu_common/init.d/4012_hk_x550 +++ b/ROMFS/px4fmu_common/init.d/4012_hk_x550 @@ -4,9 +4,6 @@ echo "[init] PX4FMU v1, v2 with or without IO on HobbyKing X550" -# Use generic quad X PWM as base -sh /etc/init.d/4001_quad_x_pwm - if [ $DO_AUTOCONFIG == yes ] then # @@ -25,3 +22,8 @@ then param set MC_YAWRATE_I 0 param set MC_YAWRATE_D 0 fi + +set VEHICLE_TYPE mc +set FRAME_GEOMETRY quad_x + +set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/40_io_segway b/ROMFS/px4fmu_common/init.d/40_io_segway deleted file mode 100644 index ad455b4403..0000000000 --- a/ROMFS/px4fmu_common/init.d/40_io_segway +++ /dev/null @@ -1,51 +0,0 @@ -#!nsh - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - # TODO - - param set SYS_AUTOCONFIG 0 - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 10 = ground rover -# -param set MAV_TYPE 10 - -# -# Start MAVLink (depends on orb) -# -mavlink start -d /dev/ttyS1 -b 57600 -usleep 5000 - -# -# Start and configure PX4IO interface -# -sh /etc/init.d/rc.io - -# -# 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) -# -attitude_estimator_ekf start - -# -# Load mixer and start controllers (depends on px4io) -# -roboclaw start /dev/ttyS2 128 1200 -segway start diff --git a/ROMFS/px4fmu_common/init.d/5001_quad_+_pwm b/ROMFS/px4fmu_common/init.d/5001_quad_+_pwm deleted file mode 100644 index 7d07486685..0000000000 --- a/ROMFS/px4fmu_common/init.d/5001_quad_+_pwm +++ /dev/null @@ -1,17 +0,0 @@ -#!nsh - -# Maintainers: Anton Babushkin , Lorenz Meier - -echo "[init] PX4FMU v1, v2 with or without IO on generic quadcopter + with PWM ESCs" - -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # -fi - -set FRAME_TYPE mc -set FRAME_GEOMETRY + -set FRAME_COUNT 4 -set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm b/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm deleted file mode 100644 index 3ea5479cbd..0000000000 --- a/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm +++ /dev/null @@ -1,17 +0,0 @@ -#!nsh - -# Maintainers: Anton Babushkin , Lorenz Meier - -echo "[init] PX4FMU v1, v2 with or without IO on generic hexacopter X with PWM ESCs" - -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # -fi - -set FRAME_TYPE mc -set FRAME_GEOMETRY x -set FRAME_COUNT 6 -set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm b/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm deleted file mode 100644 index f747618b8d..0000000000 --- a/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm +++ /dev/null @@ -1,17 +0,0 @@ -#!nsh - -# Maintainers: Anton Babushkin , Lorenz Meier - -echo "[init] PX4FMU v1, v2 with or without IO on generic hexacopter + with PWM ESCs" - -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # -fi - -set FRAME_TYPE mc -set FRAME_GEOMETRY + -set FRAME_COUNT 4 -set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm b/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm deleted file mode 100644 index f8f4591856..0000000000 --- a/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm +++ /dev/null @@ -1,17 +0,0 @@ -#!nsh - -# Maintainers: Anton Babushkin , Lorenz Meier - -echo "[init] PX4FMU v1, v2 with or without IO on generic octocopter X with PWM ESCs" - -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # -fi - -set FRAME_TYPE mc -set FRAME_GEOMETRY x -set FRAME_COUNT 8 -set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/800_sdlogger b/ROMFS/px4fmu_common/init.d/800_sdlogger deleted file mode 100644 index 2d2c3737bd..0000000000 --- a/ROMFS/px4fmu_common/init.d/800_sdlogger +++ /dev/null @@ -1,53 +0,0 @@ -#!nsh - -echo "[init] PX4FMU v1, v2 init to log only - -# -# 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 save -fi - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - usleep 5000 - - sh /etc/init.d/rc.io - # Set PWM values for DJI ESCs -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - usleep 5000 - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -sh /etc/init.d/rc.sensors - -gps start - -attitude_estimator_ekf start - -position_estimator_inav start - -if [ -d /fs/microsd ] -then - if hw_ver compare PX4FMU_V1 - then - echo "Start sdlog2 at 50Hz" - sdlog2 start -r 50 -e -b 16 - else - echo "Start sdlog2 at 200Hz" - sdlog2 start -r 200 -e -b 16 - fi -fi diff --git a/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm b/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm deleted file mode 100644 index f8bcd13a9e..0000000000 --- a/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm +++ /dev/null @@ -1,17 +0,0 @@ -#!nsh - -# Maintainers: Anton Babushkin , Lorenz Meier - -echo "[init] PX4FMU v1, v2 with or without IO on generic octocopter + with PWM ESCs" - -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # -fi - -set FRAME_TYPE mc -set FRAME_GEOMETRY + -set FRAME_COUNT 8 -set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 153fbb66bb..9da0135b30 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -17,11 +17,90 @@ # 11000 .. 11999 Hexa Cox # 12000 .. 12999 Octo Cox -if param compare SYS_AUTOSTART 4001 +# +# Simulation setups +# + +if param compare SYS_AUTOSTART 1000 then - sh /etc/init.d/4001_quad_x_pwm + #sh /etc/init.d/1000_rc_fw_easystar.hil fi +if param compare SYS_AUTOSTART 1001 +then + sh /etc/init.d/1001_rc_quad.hil +fi + +if param compare SYS_AUTOSTART 1002 +then + #sh /etc/init.d/1002_rc_fw_state.hil +fi + +if param compare SYS_AUTOSTART 1003 +then + #sh /etc/init.d/1003_rc_quad_+.hil +fi + +if param compare SYS_AUTOSTART 1004 +then + #sh /etc/init.d/1004_rc_fw_Rascal110.hil +fi + +# +# Standard plane +# + +if param compare SYS_AUTOSTART 2100 100 +then + #sh /etc/init.d/2100_mpx_easystar + #set MODE custom +fi + +if param compare SYS_AUTOSTART 2101 101 +then + #sh /etc/init.d/2101_hk_bixler + #set MODE custom +fi + +if param compare SYS_AUTOSTART 2102 102 +then + #sh /etc/init.d/2102_3dr_skywalker + #set MODE custom +fi + +# +# Flying wing +# + +if param compare SYS_AUTOSTART 3030 +then + #sh /etc/init.d/3030_io_camflyer +fi + +if param compare SYS_AUTOSTART 3031 31 +then + #sh /etc/init.d/3031_io_phantom +fi + +if param compare SYS_AUTOSTART 3032 32 +then + #sh /etc/init.d/3032_skywalker_x5 +fi + +if param compare SYS_AUTOSTART 3033 33 +then + #sh /etc/init.d/3033_io_wingwing +fi + +if param compare SYS_AUTOSTART 3034 34 +then + #sh /etc/init.d/3034_io_fx79 +fi + +# +# Quad X +# + if param compare SYS_AUTOSTART 4008 then #sh /etc/init.d/4008_ardrone @@ -47,135 +126,16 @@ then sh /etc/init.d/4012_hk_x550 fi -if param compare SYS_AUTOSTART 6001 +# +# Wide arm / H frame +# + +if param compare SYS_AUTOSTART 10015 then - sh /etc/init.d/6001_hexa_x_pwm + sh /etc/init.d/10015_tbs_discovery fi -if param compare SYS_AUTOSTART 7001 +if param compare SYS_AUTOSTART 10016 then - sh /etc/init.d/7001_hexa_+_pwm -fi - -if param compare SYS_AUTOSTART 8001 -then - sh /etc/init.d/8001_octo_x_pwm -fi - -if param compare SYS_AUTOSTART 9001 -then - sh /etc/init.d/9001_octo_+_pwm -fi - -if param compare SYS_AUTOSTART 12001 -then - #set MIXER /etc/mixers/FMU_octo_cox.mix - #sh /etc/init.d/rc.octo -fi - -if param compare SYS_AUTOSTART 10015 15 -then - #sh /etc/init.d/10015_tbs_discovery -fi - -if param compare SYS_AUTOSTART 10016 16 -then - #sh /etc/init.d/10016_3dr_iris -fi - -if param compare SYS_AUTOSTART 4017 17 -then - #set MKBLCTRL_MODE no - #set MKBLCTRL_FRAME x - #sh /etc/init.d/rc.custom_dji_f330_mkblctrl -fi - -if param compare SYS_AUTOSTART 5018 18 -then - #set MKBLCTRL_MODE no - #set MKBLCTRL_FRAME + - #sh /etc/init.d/rc.custom_dji_f330_mkblctrl -fi - -if param compare SYS_AUTOSTART 4019 19 -then - #set MKBLCTRL_MODE yes - #set MKBLCTRL_FRAME x - #sh /etc/init.d/rc.custom_dji_f330_mkblctrl -fi - -if param compare SYS_AUTOSTART 5020 20 -then - #set MKBLCTRL_MODE yes - #set MKBLCTRL_FRAME + - #sh /etc/init.d/rc.custom_dji_f330_mkblctrl -fi - -if param compare SYS_AUTOSTART 4021 21 -then - #set FRAME_GEOMETRY x - #set ESC_MAKER afro - #sh /etc/init.d/rc.custom_io_esc -fi - -if param compare SYS_AUTOSTART 10022 22 -then - #set FRAME_GEOMETRY w - #sh /etc/init.d/rc.custom_io_esc -fi - -if param compare SYS_AUTOSTART 3030 30 -then - #sh /etc/init.d/3030_io_camflyer -fi - -if param compare SYS_AUTOSTART 3031 31 -then - #sh /etc/init.d/3031_io_phantom -fi - -if param compare SYS_AUTOSTART 3032 32 -then - #sh /etc/init.d/3032_skywalker_x5 -fi - -if param compare SYS_AUTOSTART 3033 33 -then - #sh /etc/init.d/3033_io_wingwing -fi - -if param compare SYS_AUTOSTART 3034 34 -then - #sh /etc/init.d/3034_io_fx79 - #set MODE custom -fi - -if param compare SYS_AUTOSTART 40 -then - #sh /etc/init.d/40_io_segway - #set MODE custom -fi - -if param compare SYS_AUTOSTART 2100 100 -then - #sh /etc/init.d/2100_mpx_easystar - #set MODE custom -fi - -if param compare SYS_AUTOSTART 2101 101 -then - #sh /etc/init.d/2101_hk_bixler - #set MODE custom -fi - -if param compare SYS_AUTOSTART 2102 102 -then - #sh /etc/init.d/2102_3dr_skywalker - #set MODE custom -fi - -if param compare SYS_AUTOSTART 800 -then - #sh /etc/init.d/800_sdlogger - #set MODE custom + sh /etc/init.d/10016_3dr_iris fi diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart_hil b/ROMFS/px4fmu_common/init.d/rc.autostart_hil deleted file mode 100644 index d5fc5eb087..0000000000 --- a/ROMFS/px4fmu_common/init.d/rc.autostart_hil +++ /dev/null @@ -1,34 +0,0 @@ -# -# Check if auto-setup from one of the standard scripts for HIL is wanted -# - -if param compare SYS_AUTOSTART 1000 -then - #sh /etc/init.d/1000_rc_fw_easystar.hil - set MODE hil -fi - -if param compare SYS_AUTOSTART 1001 -then - sh /etc/init.d/1001_rc_quad.hil - set MODE hil -fi - -if param compare SYS_AUTOSTART 1002 -then - #sh /etc/init.d/1002_rc_fw_state.hil - set MODE hil -fi - -if param compare SYS_AUTOSTART 1003 -then - #sh /etc/init.d/1003_rc_quad_+.hil - set MODE hil -fi - -if param compare SYS_AUTOSTART 1004 -then - #sh /etc/init.d/1004_rc_fw_Rascal110.hil - set MODE hil -fi - diff --git a/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl b/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl deleted file mode 100644 index 40b2ee68bd..0000000000 --- a/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl +++ /dev/null @@ -1,113 +0,0 @@ -#!nsh - -# -# 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.002 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.09 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 - param set MC_ATT_P 6.8 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_P 2.0 - param set MC_YAWRATE_D 0.005 - param set MC_YAWRATE_I 0.2 - param set MC_YAWRATE_P 0.3 - param set NAV_TAKEOFF_ALT 3.0 - param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.7 - param set MPC_THR_MIN 0.3 - param set MPC_XY_D 0 - param set MPC_XY_P 0.5 - param set MPC_XY_VEL_D 0 - param set MPC_XY_VEL_I 0 - param set MPC_XY_VEL_MAX 3 - param set MPC_XY_VEL_P 0.2 - param set MPC_Z_D 0 - param set MPC_Z_P 1 - param set MPC_Z_VEL_D 0 - param set MPC_Z_VEL_I 0.1 - param set MPC_Z_VEL_MAX 2 - param set MPC_Z_VEL_P 0.20 - - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 2 = quadrotor -# -param set MAV_TYPE 2 - -set EXIT_ON_END no - -# -# Start the Mikrokopter ESC driver -# -if [ $MKBLCTRL_MODE == yes ] -then - if [ $MKBLCTRL_FRAME == x ] - then - echo "[init] PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 X Frame Mikrokopter-Addressing" - mkblctrl -mkmode x - else - echo "[init] PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame Mikrokopter-Addressing" - mkblctrl -mkmode + - fi -else - if [ $MKBLCTRL_FRAME == x ] - then - echo "[init] PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 X Frame" - else - echo "[init] PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame" - fi - mkblctrl -fi - -usleep 10000 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - usleep 5000 - - sh /etc/init.d/rc.io -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - usleep 5000 - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -# -# Load mixer -# -if [ $MKBLCTRL_FRAME == x ] -then - mixer load /dev/mkblctrl /etc/mixers/FMU_quad_x.mix -else - mixer load /dev/mkblctrl /etc/mixers/FMU_quad_+.mix -fi - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor - -if [ $EXIT_ON_END == yes ] -then - exit -fi diff --git a/ROMFS/px4fmu_common/init.d/rc.custom_io_esc b/ROMFS/px4fmu_common/init.d/rc.custom_io_esc deleted file mode 100644 index 045e41e529..0000000000 --- a/ROMFS/px4fmu_common/init.d/rc.custom_io_esc +++ /dev/null @@ -1,120 +0,0 @@ -#!nsh - -# -# 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.002 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.09 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 - param set MC_ATT_P 6.8 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_P 2.0 - param set MC_YAWRATE_D 0.005 - param set MC_YAWRATE_I 0.2 - param set MC_YAWRATE_P 0.3 - param set NAV_TAKEOFF_ALT 3.0 - param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.7 - param set MPC_THR_MIN 0.3 - param set MPC_XY_D 0 - param set MPC_XY_P 0.5 - param set MPC_XY_VEL_D 0 - param set MPC_XY_VEL_I 0 - param set MPC_XY_VEL_MAX 3 - param set MPC_XY_VEL_P 0.2 - param set MPC_Z_D 0 - param set MPC_Z_P 1 - param set MPC_Z_VEL_D 0 - param set MPC_Z_VEL_I 0.1 - param set MPC_Z_VEL_MAX 2 - param set MPC_Z_VEL_P 0.20 - - param save -fi - -echo "RC script for PX4FMU + PX4IO + PPM-ESCs running" - -# -# Force some key parameters to sane values -# MAV_TYPE 2 = quadrotor -# -param set MAV_TYPE 2 - -set EXIT_ON_END no - -usleep 10000 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start -d /dev/ttyS1 -b 57600 - usleep 5000 - - sh /etc/init.d/rc.io -else - fmu mode_pwm - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS1 -b 57600 - usleep 5000 - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -if [ $ESC_MAKER = afro ] -then - # Set PWM values for Afro ESCs - pwm disarmed -c 1234 -p 1050 - pwm min -c 1234 -p 1080 - pwm max -c 1234 -p 1860 -else - # Set PWM values for typical ESCs - pwm disarmed -c 1234 -p 900 - pwm min -c 1234 -p 980 - pwm max -c 1234 -p 1800 -fi - -# -# Load mixer -# -if [ $FRAME_GEOMETRY == x ] -then - echo "Frame geometry X" - mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix -else - if [ $FRAME_GEOMETRY == w ] - then - echo "Frame geometry W" - mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix - else - echo "Frame geometry +" - mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix - fi -fi - -# -# Set PWM output frequency -# -pwm rate -r 400 -c 1234 - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor - -if [ $EXIT_ON_END == yes ] -then - exit -fi - -echo "Script end" diff --git a/ROMFS/px4fmu_common/init.d/rc.fixedwing b/ROMFS/px4fmu_common/init.d/rc.fixedwing deleted file mode 100644 index f028510064..0000000000 --- a/ROMFS/px4fmu_common/init.d/rc.fixedwing +++ /dev/null @@ -1,34 +0,0 @@ -#!nsh -# -# Standard everything needed for fixedwing except mixer, actuator output and mavlink -# - -# -# Start the sensors and test them. -# -sh /etc/init.d/rc.sensors - -# -# Start logging (depends on sensors) -# -sh /etc/init.d/rc.logging - -# -# Start GPS interface (depends on orb) -# -gps start - -# -# Start the attitude and position estimator -# -att_pos_estimator_ekf start - -# -# Start attitude controller -# -fw_att_control start - -# -# Start the position controller -# -fw_pos_control_l1 start diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps new file mode 100644 index 0000000000..d354fb06fa --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps @@ -0,0 +1,19 @@ +#!nsh +# +# Standard apps for fixed wing +# + +# +# Start the attitude and position estimator +# +att_pos_estimator_ekf start + +# +# Start attitude controller +# +fw_att_control start + +# +# Start the position controller +# +fw_pos_control_l1 start diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_interface b/ROMFS/px4fmu_common/init.d/rc.fw_interface new file mode 100644 index 0000000000..f9517f4220 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.fw_interface @@ -0,0 +1,18 @@ +#!nsh +# +# Script to load fixedwing mixer and set rate/disarmed/min/max values for PWM output +# + +# +# Force some key parameters to sane values +# MAV_TYPE 1 = fixed wing +# +param set MAV_TYPE 1 + +# +# Load mixer +# +echo "Frame geometry: ${FRAME_GEOMETRY}" +set MIXER /etc/mixers/FMU_${FRAME_GEOMETRY}.mix +echo "Loading mixer: ${MIXER}" +mixer load /dev/pwm_output ${MIXER} diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index 16a7a33c6b..8b51d57e51 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -1,6 +1,6 @@ #!nsh # -# Standard everything needed for multirotors except mixer, actuator output and mavlink +# Standard apps for multirotors # # diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_interface b/ROMFS/px4fmu_common/init.d/rc.mc_interface index 9e3d678bf3..003aee81b6 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_interface +++ b/ROMFS/px4fmu_common/init.d/rc.mc_interface @@ -1,24 +1,25 @@ #!nsh # -# Script to set PWM min / max limits and mixer +# Script to load multicopter mixer and set rate/disarmed/min/max values for PWM output # -echo "Rotors count: $FRAME_COUNT" -if [ $FRAME_COUNT == 4 ] +if [ $FRAME_GEOMETRY == quad_x -o $FRAME_GEOMETRY == quad_+ ] then - set FRAME_COUNT_STR quad set OUTPUTS 1234 param set MAV_TYPE 2 fi -if [ $FRAME_COUNT == 6 ] +if [ $FRAME_GEOMETRY == quad_w -o $FRAME_GEOMETRY == quad_v ] +then + set OUTPUTS 1234 + param set MAV_TYPE 2 +fi +if [ $FRAME_GEOMETRY == hex_x -o $FRAME_GEOMETRY == hex_+ ] then - set FRAME_COUNT_STR hex set OUTPUTS 123456 param set MAV_TYPE 13 fi -if [ $FRAME_COUNT == 8 ] +if [ $FRAME_GEOMETRY == octo_x -o $FRAME_GEOMETRY == octo_+ ] then - set FRAME_COUNT_STR octo set OUTPUTS 12345678 param set MAV_TYPE 14 fi @@ -26,31 +27,34 @@ fi # # Load mixer # -echo "Frame geometry: ${FRAME_GEOMETRY}" -set MIXER /etc/mixers/FMU_${FRAME_GEOMETRY_STR}_${FRAME_GEOMETRY}.mix -echo "Loading mixer: ${MIXER}" -mixer load /dev/pwm_output ${MIXER} +echo "Frame geometry: $FRAME_GEOMETRY" +set MIXER /etc/mixers/FMU_${FRAME_GEOMETRY}.mix +echo "Loading mixer: $MIXER" +mixer load /dev/pwm_output $MIXER -# -# Set PWM output frequency -# -if [ $PWM_RATE != none ] +if [ $OUTPUT_MODE == fmu_pwm -o $OUTPUT_MODE == io_pwm ] then - pwm rate -c $OUTPUTS -r $PWM_RATE -fi - -# -# Set disarmed, min and max PWM values -# -if [ $PWM_DISARMED != none ] -then - pwm disarmed -c $OUTPUTS -p $PWM_DISARMED -fi -if [ $PWM_MIN != none ] -then - pwm min -c $OUTPUTS -p $PWM_MIN -fi -if [ $PWM_MAX != none ] -then - pwm max -c $OUTPUTS -p $PWM_MAX + # + # Set PWM output frequency + # + if [ $PWM_RATE != none ] + then + pwm rate -c $OUTPUTS -r $PWM_RATE + fi + + # + # Set disarmed, min and max PWM values + # + if [ $PWM_DISARMED != none ] + then + pwm disarmed -c $OUTPUTS -p $PWM_DISARMED + fi + if [ $PWM_MIN != none ] + then + pwm min -c $OUTPUTS -p $PWM_MIN + fi + if [ $PWM_MAX != none ] + then + pwm max -c $OUTPUTS -p $PWM_MAX + fi fi diff --git a/ROMFS/px4fmu_common/init.d/rc.octo b/ROMFS/px4fmu_common/init.d/rc.octo deleted file mode 100644 index ecb12e96e5..0000000000 --- a/ROMFS/px4fmu_common/init.d/rc.octo +++ /dev/null @@ -1,94 +0,0 @@ -#!nsh - -echo "[init] Octorotor startup" - -# -# 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.004 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.12 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 - param set MC_ATT_P 7.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_P 2.0 - param set MC_YAWRATE_D 0.005 - param set MC_YAWRATE_I 0.2 - param set MC_YAWRATE_P 0.3 - param set NAV_TAKEOFF_ALT 3.0 - param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.7 - param set MPC_THR_MIN 0.3 - param set MPC_XY_D 0 - param set MPC_XY_P 0.5 - param set MPC_XY_VEL_D 0 - param set MPC_XY_VEL_I 0 - param set MPC_XY_VEL_MAX 3 - param set MPC_XY_VEL_P 0.2 - param set MPC_Z_D 0 - param set MPC_Z_P 1 - param set MPC_Z_VEL_D 0 - param set MPC_Z_VEL_I 0.1 - param set MPC_Z_VEL_MAX 2 - param set MPC_Z_VEL_P 0.20 - - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE list: https://pixhawk.ethz.ch/mavlink/ -# 14 = octorotor -# -param set MAV_TYPE 14 - -set EXIT_ON_END no - -# -# Start and configure PX4IO interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - usleep 5000 - - sh /etc/init.d/rc.io -else - # This is not possible on an octo - tone_alarm error -fi - -# -# Load mixer -# -mixer load /dev/pwm_output $MIXER - -# -# Set PWM output frequency to 400 Hz -# -pwm rate -a -r 400 - -# -# Set disarmed, min and max PWM signals -# -pwm disarmed -c 12345678 -p 900 -pwm min -c 12345678 -p 1100 -pwm max -c 12345678 -p 1900 - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor - -if [ $EXIT_ON_END == yes ] -then - exit -fi diff --git a/ROMFS/px4fmu_common/init.d/rc.standalone b/ROMFS/px4fmu_common/init.d/rc.standalone deleted file mode 100644 index 67e95215b9..0000000000 --- a/ROMFS/px4fmu_common/init.d/rc.standalone +++ /dev/null @@ -1,13 +0,0 @@ -#!nsh -# -# Flight startup script for PX4FMU standalone configuration. -# - -echo "[init] doing standalone PX4FMU startup..." - -# -# Start the ORB -# -uorb start - -echo "[init] startup done" diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index ccf2cd47ea..0cd8a0e042 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -36,39 +36,6 @@ then echo "Commander started" fi -# Start px4io if present -if px4io start -then - echo "PX4IO driver started" -else - if fmu mode_serial - then - echo "FMU driver started" - fi -fi - -# Start sensors -sh /etc/init.d/rc.sensors - -# Start one of the estimators -if attitude_estimator_ekf status -then - 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 -if gps start -then - echo "GPS started" -fi - echo "MAVLink started, exiting shell.." # Exit shell to make it available to MAVLink diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 7d38736dee..09d66cf415 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -52,7 +52,7 @@ then echo "[init] USB interface connected" fi - echo "Running rc.APM" + echo "[init] Running rc.APM" # if APM startup is successful then nsh will exit sh /etc/init.d/rc.APM fi @@ -85,9 +85,9 @@ then then if param load /fs/microsd/params then - echo "Parameters loaded" + echo "[init] Parameters loaded" else - echo "Parameter file corrupt - ignoring" + echo "[init] Parameter file corrupt - ignoring" fi fi #fi @@ -97,7 +97,7 @@ then # if rgbled start then - echo "Using external RGB Led" + echo "[init] Using external RGB Led" else if blinkm start then @@ -105,13 +105,75 @@ then fi fi - set USE_IO no + # Use FMU PWM output by default + set OUTPUT_MODE fmu_pwm + set IO_PRESENT no + + # + # Upgrade PX4IO firmware + # + if [ -f /etc/extras/px4io-v2_default.bin ] + then + set io_file /etc/extras/px4io-v2_default.bin + else + set io_file /etc/extras/px4io-v1_default.bin + fi + + if px4io checkcrc $io_file + then + echo "[init] PX4IO CRC OK" + echo "PX4IO CRC OK" >> $logfile + + set IO_PRESENT yes + + # If PX4IO present, use it as primary PWM output by default + set OUTPUT_MODE io_pwm + else + echo "[init] PX4IO CRC failure" + echo "PX4IO CRC failure" >> $logfile + tone_alarm MBABGP + if px4io forceupdate 14662 $io_file + then + usleep 500000 + if px4io start + then + echo "[init] PX4IO restart OK" + echo "PX4IO restart OK" >> $logfile + tone_alarm MSPAA + + set IO_PRESENT yes + + # If PX4IO present, use it as primary PWM output by default + set OUTPUT_MODE io_pwm + else + echo "[init] PX4IO restart failed" + echo "PX4IO restart failed" >> $logfile + if hw_ver compare PX4FMU_V2 + then + tone_alarm MNGGG + sleep 10 + reboot + fi + fi + else + echo "[init] PX4IO update failed" + echo "PX4IO update failed" >> $logfile + if hw_ver compare PX4FMU_V2 + then + tone_alarm MNGGG + fi + fi + fi + + set HIL no set FRAME_TYPE none set PWM_RATE none set PWM_DISARMED none set PWM_MIN none set PWM_MAX none + set EXIT_ON_END no + if param compare SYS_AUTOCONFIG 1 then set DO_AUTOCONFIG yes @@ -125,112 +187,23 @@ then commander start # - # Set parameters and env variables for selected AUTOSTART (HIL setups) + # Set parameters and env variables for selected AUTOSTART # - sh /etc/init.d/rc.autostart_hil - - if [ $MODE == hil ] - then - # - # Do common HIL setup depending on env variables - # - # Allow USB some time to come up - sleep 1 + sh /etc/init.d/rc.autostart - # Start MAVLink on USB port - mavlink start -b 230400 -d /dev/ttyACM0 - usleep 5000 - - # Create a fake HIL /dev/pwm_output interface - hil mode_pwm - - # Sensors - echo "Start sensors" - sh /etc/init.d/rc.sensors - - # - # Fixed wing setup - # - if [ $FRAME_TYPE == fw ] - then - echo "Setup FIXED WING" - fi - - # - # Multicopters setup - # - if [ $FRAME_TYPE == mc ] - then - echo "Setup MULTICOPTER" - - # Load mixer and configure outputs - sh /etc/init.d/rc.mc_interface - - # Start common multicopter apps - sh /etc/init.d/rc.mc_apps - fi + # Custom configuration + if [ -f /fs/microsd/etc/rc.conf ] + then + sh /fs/microsd/etc/rc.conf + fi + + if [ $HIL == yes ] + then + set OUTPUT_MODE hil else # Try to get an USB console if not in HIL mode nshterm /dev/ttyACM0 & fi - - # - # Upgrade PX4IO firmware - # - if [ -f /etc/extras/px4io-v2_default.bin ] - then - set io_file /etc/extras/px4io-v2_default.bin - else - set io_file /etc/extras/px4io-v1_default.bin - fi - - if px4io start - then - echo "PX4IO OK" - echo "PX4IO OK" >> $logfile - fi - - if px4io checkcrc $io_file - then - echo "PX4IO CRC OK" - echo "PX4IO CRC OK" >> $logfile - set USE_IO yes - else - echo "PX4IO CRC failure" - echo "PX4IO CRC failure" >> $logfile - tone_alarm MBABGP - if px4io forceupdate 14662 $io_file - then - usleep 500000 - if px4io start - then - echo "PX4IO restart OK" - echo "PX4IO restart OK" >> $logfile - tone_alarm MSPAA - set USE_IO yes - else - echo "PX4IO restart failed" - echo "PX4IO restart failed" >> $logfile - if hw_ver compare PX4FMU_V2 - then - tone_alarm MNGGG - sleep 10 - reboot - fi - fi - else - echo "PX4IO update failed" - echo "PX4IO update failed" >> $logfile - tone_alarm MNGGG - fi - fi - - set EXIT_ON_END no - - # - # Set parameters and env variables for selected AUTOSTART - # - sh /etc/init.d/rc.autostart # # If autoconfig parameter was set, reset it and save parameters @@ -240,66 +213,132 @@ then param set SYS_AUTOCONFIG 0 param save fi - + if [ $MODE == autostart ] then # - # Do common setup depending on env variables + # Start primary output # - if [ $USE_IO == yes ] + if [ $OUTPUT_MODE == io_pwm ] then - echo "Use IO" - - # Start MAVLink on default port: ttyS1 - mavlink start - usleep 5000 - - sh /etc/init.d/rc.io - else - echo "Don't use IO" - - # Start MAVLink on ttyS0 + echo "[init] Use PX4IO PWM as primary output" + if px4io start + then + echo "[init] PX4IO OK" + echo "PX4IO OK" >> $logfile + sh /etc/init.d/rc.io + else + echo "[init] PX4IO start error" + echo "PX4IO start error" >> $logfile + tone_alarm MNGGG + fi + fi + if [ $OUTPUT_MODE == fmu_pwm ] + then + echo "[init] Use PX4FMU PWM as primary output" + fmu mode_pwm + fi + if [ $OUTPUT_MODE == mkblctrl ] + then + echo "[init] Use MKBLCTRL as primary output" + mkblctrl + fi + if [ $OUTPUT_MODE == hil ] + then + echo "[init] Use HIL as primary output" + hil mode_pwm + fi + + # + # Start PX4IO as secondary output if needed + # + if [ $IO_PRESENT == yes -a $OUTPUT_MODE != io_pwm ] + then + echo "[init] Use PX4IO PWM as secondary output" + if px4io start + then + echo "[init] PX4IO OK" + echo "PX4IO OK" >> $logfile + sh /etc/init.d/rc.io + else + echo "[init] PX4IO start error" + echo "PX4IO start error" >> $logfile + tone_alarm MNGGG + fi + fi + + # + # MAVLink + # + if [ $OUTPUT_MODE == fmu_pwm -a hw_ver compare PX4FMU_V1 ] + then + # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as PWM output mavlink start -d /dev/ttyS0 usleep 5000 - # Configure FMU for PWM outputs - fmu mode_pwm - # Exit from nsh to free port for mavlink set EXIT_ON_END yes + else + # Start MAVLink on default port: ttyS1 + mavlink start + usleep 5000 fi - # Sensors - echo "Start sensors" + # + # Sensors, Logging, GPS + # + echo "[init] Start sensors" sh /etc/init.d/rc.sensors - - # Logging - sh /etc/init.d/rc.logging - - # GPS interface - gps start + + if [ $HIL == no ] + then + echo "[init] Start logging" + sh /etc/init.d/rc.logging + fi + + if [ $HIL == no ] + then + echo "[init] Start GPS" + gps start + fi # # Fixed wing setup # - if [ $FRAME_TYPE == fw ] + if [ $VEHICLE_TYPE == fw ] then - echo "Setup FIXED WING" + echo "[init] Vehicle type: FIXED WING" + + # Load mixer and configure outputs + sh /etc/init.d/rc.fw_interface + + # Start standard fixedwing apps + sh /etc/init.d/rc.mc_apps fi # # Multicopters setup # - if [ $FRAME_TYPE == mc ] + if [ $VEHICLE_TYPE == mc ] then - echo "Setup MULTICOPTER" + echo "[init] Vehicle type: MULTICOPTER" # Load mixer and configure outputs sh /etc/init.d/rc.mc_interface - # Start common multicopter apps + # Start standard multicopter apps sh /etc/init.d/rc.mc_apps fi + + # + # Generic setup (autostart ID not found) + # + if [ $VEHICLE_TYPE == none ] + then + echo "[init] Vehicle type: GENERIC" + attitude_estimator_ekf start + position_estimator_inav start + fi fi # Start any custom extensions @@ -307,39 +346,6 @@ then then sh /fs/microsd/etc/rc.local fi - - # If none of the autostart scripts triggered, get a minimal setup - if [ $MODE == autostart ] - then - # Telemetry port is on both FMU boards ttyS1 - # but the AR.Drone motors can be get 'flashed' - # if starting MAVLink on them - so do not - # start it as default (default link: USB) - - # Start commander - commander start - - # Start px4io if present - if px4io detect - then - px4io start - else - if fmu mode_serial - then - echo "FMU driver (no PWM) started" - fi - fi - - # Start sensors - sh /etc/init.d/rc.sensors - - # Start one of the estimators - attitude_estimator_ekf start - - # Start GPS - gps start - - fi if [ $EXIT_ON_END == yes ] then From 6e609845569722367b5d38bc508edb69dfa8d47f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 10 Jan 2014 22:04:56 +0100 Subject: [PATCH 006/108] rcS and autostart scripts cleanup, WIP --- .../px4fmu_common/init.d/10015_tbs_discovery | 9 +- ROMFS/px4fmu_common/init.d/10016_3dr_iris | 15 +- .../{1001_rc_quad.hil => 1001_rc_quad_x.hil} | 7 +- ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil | 135 ++++-------- ROMFS/px4fmu_common/init.d/4010_dji_f330 | 7 +- ROMFS/px4fmu_common/init.d/4011_dji_f450 | 7 +- ROMFS/px4fmu_common/init.d/4012_hk_x550 | 9 +- ROMFS/px4fmu_common/init.d/rc.autostart | 4 +- ROMFS/px4fmu_common/init.d/rcS | 199 ++++++++++-------- 9 files changed, 183 insertions(+), 209 deletions(-) rename ROMFS/px4fmu_common/init.d/{1001_rc_quad.hil => 1001_rc_quad_x.hil} (91%) diff --git a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery index 610d482c1f..24f1099d3f 100644 --- a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery @@ -1,8 +1,9 @@ #!nsh - -# Maintainers: Anton Babushkin - -echo "[init] Team Blacksheep Discovery Quad" +# +# Team Blacksheep Discovery Quadcopter +# +# Maintainers: Simon Wilks +# if [ $DO_AUTOCONFIG == yes ] then diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris index de50280527..b8fc5e606f 100644 --- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris @@ -1,11 +1,9 @@ #!nsh - +# +# 3DR Iris Quadcopter +# # Maintainers: Anton Babushkin - -echo "[init] 3DR Iris Quad" - -# Use generic wide arm quad X PWM as base -sh /etc/init.d/10001_quad_w +# if [ $DO_AUTOCONFIG == yes ] then @@ -41,3 +39,8 @@ then param set MPC_Z_VEL_MAX 2 param set MPC_Z_VEL_P 0.20 fi + +set VEHICLE_TYPE mc +set FRAME_GEOMETRY quad_w + +set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil similarity index 91% rename from ROMFS/px4fmu_common/init.d/1001_rc_quad.hil rename to ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil index 7d21176f22..c5b92d7d4d 100644 --- a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil +++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil @@ -1,6 +1,9 @@ #!nsh - -echo "[init] PX4FMU v1, v2 with or without IO on HIL Quad" +# +# HIL Quadcopter X +# +# Maintainers: Anton Babushkin +# if [ $DO_AUTOCONFIG == yes ] then diff --git a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil index 0cc07ad343..5ec70043a0 100644 --- a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil +++ b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil @@ -1,105 +1,46 @@ #!nsh # -# USB HIL start +# HIL Quadcopter + +# +# Maintainers: Anton Babushkin # -echo "[HIL] HILS quadrotor + starting.." - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 +if [ $DO_AUTOCONFIG == yes ] then - # Set all params here, then disable autoconfig - param set SYS_AUTOCONFIG 0 - - param set MC_ATTRATE_D 0.0 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.05 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 - param set MC_ATT_P 3.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_P 2.1 - param set MC_YAWRATE_D 0.0 - param set MC_YAWRATE_I 0.0 - param set MC_YAWRATE_P 0.05 - param set NAV_TAKEOFF_ALT 3.0 - param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.5 - param set MPC_THR_MIN 0.1 - param set MPC_XY_D 0 - param set MPC_XY_P 0.5 - param set MPC_XY_VEL_D 0 - param set MPC_XY_VEL_I 0 - param set MPC_XY_VEL_MAX 3 - param set MPC_XY_VEL_P 0.2 - param set MPC_Z_D 0 - param set MPC_Z_P 1 - param set MPC_Z_VEL_D 0 - param set MPC_Z_VEL_I 0.1 - param set MPC_Z_VEL_MAX 2 - param set MPC_Z_VEL_P 0.20 - - param save + # + # Default parameters for this platform + # + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 + param set MC_ATT_P 7.0 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 + param set MC_YAWPOS_P 2.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + param set MPC_TILT_MAX 0.5 + param set MPC_THR_MAX 0.8 + param set MPC_THR_MIN 0.2 + param set MPC_XY_D 0 + param set MPC_XY_P 0.5 + param set MPC_XY_VEL_D 0 + param set MPC_XY_VEL_I 0 + param set MPC_XY_VEL_MAX 3 + param set MPC_XY_VEL_P 0.2 + param set MPC_Z_D 0 + param set MPC_Z_P 1 + param set MPC_Z_VEL_D 0 + param set MPC_Z_VEL_I 0.1 + param set MPC_Z_VEL_MAX 2 + param set MPC_Z_VEL_P 0.20 fi -# Allow USB some time to come up -sleep 1 -# Tell MAVLink that this link is "fast" -mavlink start -b 230400 -d /dev/ttyACM0 - -# Create a fake HIL /dev/pwm_output interface -hil mode_pwm - -# -# 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 we got an IO -# -if px4io start -then - echo "IO started" -else - fmu mode_serial - echo "FMU started" -fi - -# -# Start the sensors (depends on orb, px4io) -# -sh /etc/init.d/rc.sensors - -# -# Start the attitude estimator (depends on orb) -# -att_pos_estimator_ekf start - -# -# Load mixer and start controllers (depends on px4io) -# -mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix - -# -# Start position estimator -# -position_estimator_inav start - -# -# Start attitude control -# -multirotor_att_control start - -# -# Start position control -# -multirotor_pos_control start - -echo "[HIL] setup done, running" +set HIL yes +set VEHICLE_TYPE mc +set FRAME_GEOMETRY quad_+ diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330 index 0cfe68b27d..94afe91ae9 100644 --- a/ROMFS/px4fmu_common/init.d/4010_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330 @@ -1,8 +1,9 @@ #!nsh - +# +# DJI Flame Wheel F330 Quadcopter +# # Maintainers: Anton Babushkin - -echo "[init] DJI F330" +# if [ $DO_AUTOCONFIG == yes ] then diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450 index 4a0953a2ed..21b3088d32 100644 --- a/ROMFS/px4fmu_common/init.d/4011_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450 @@ -1,8 +1,9 @@ #!nsh - +# +# DJI Flame Wheel F450 Quadcopter +# # Maintainers: Lorenz Meier - -echo "[init] DJI F450" +# if [ $DO_AUTOCONFIG == yes ] then diff --git a/ROMFS/px4fmu_common/init.d/4012_hk_x550 b/ROMFS/px4fmu_common/init.d/4012_hk_x550 index 396349d9c0..27f73471d9 100644 --- a/ROMFS/px4fmu_common/init.d/4012_hk_x550 +++ b/ROMFS/px4fmu_common/init.d/4012_hk_x550 @@ -1,8 +1,9 @@ #!nsh - +# +# HobbyKing X550 Quadcopter +# # Maintainers: Todd Stellanova - -echo "[init] PX4FMU v1, v2 with or without IO on HobbyKing X550" +# if [ $DO_AUTOCONFIG == yes ] then @@ -21,6 +22,8 @@ then param set MC_YAWRATE_P 0.08 param set MC_YAWRATE_I 0 param set MC_YAWRATE_D 0 + + # TODO add default MPC parameters fi set VEHICLE_TYPE mc diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 9da0135b30..352a916ac1 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -28,7 +28,7 @@ fi if param compare SYS_AUTOSTART 1001 then - sh /etc/init.d/1001_rc_quad.hil + sh /etc/init.d/1001_rc_quad_x.hil fi if param compare SYS_AUTOSTART 1002 @@ -38,7 +38,7 @@ fi if param compare SYS_AUTOSTART 1003 then - #sh /etc/init.d/1003_rc_quad_+.hil + sh /etc/init.d/1003_rc_quad_+.hil fi if param compare SYS_AUTOSTART 1004 diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 09d66cf415..d6d6ec352a 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -13,18 +13,31 @@ set logfile /fs/microsd/bootlog.txt # # Try to mount the microSD card. # -echo "[init] looking for microSD..." +echo "[init] Looking for microSD..." if mount -t vfat /dev/mmcsd0 /fs/microsd then - echo "[init] card mounted at /fs/microsd" + echo "[init] microSD card mounted at /fs/microsd" # Start playing the startup tune tone_alarm start else - echo "[init] no microSD card found" + echo "[init] No microSD card found" # Play SOS tone_alarm error fi +# +# Set default values here, can be overriden in rc.txt from SD card +# +set HIL no +set VEHICLE_TYPE none +set FRAME_GEOMETRY none +set OUTPUT_MODE none +set VEHICLE_TYPE none +set PWM_RATE none +set PWM_DISARMED none +set PWM_MIN none +set PWM_MAX none + # # Look for an init script on the microSD card. # @@ -70,27 +83,18 @@ then uorb start # - # Load microSD params + # Load parameters # - #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 ] + param select /fs/microsd/params + if [ -f /fs/microsd/params ] + then + if param load /fs/microsd/params then - if param load /fs/microsd/params - then - echo "[init] Parameters loaded" - else - echo "[init] Parameter file corrupt - ignoring" - fi + echo "[init] Parameters loaded" + else + echo "[init] Parameter file corrupt - ignoring" fi - #fi + fi # # Start system state indicator @@ -104,13 +108,9 @@ then blinkm systemstate fi fi - - # Use FMU PWM output by default - set OUTPUT_MODE fmu_pwm - set IO_PRESENT no # - # Upgrade PX4IO firmware + # Check if PX4IO present and update firmware if needed # if [ -f /etc/extras/px4io-v2_default.bin ] then @@ -119,15 +119,14 @@ then set io_file /etc/extras/px4io-v1_default.bin fi + set IO_PRESENT no + if px4io checkcrc $io_file then echo "[init] PX4IO CRC OK" echo "PX4IO CRC OK" >> $logfile set IO_PRESENT yes - - # If PX4IO present, use it as primary PWM output by default - set OUTPUT_MODE io_pwm else echo "[init] PX4IO CRC failure" echo "PX4IO CRC failure" >> $logfile @@ -142,9 +141,6 @@ then tone_alarm MSPAA set IO_PRESENT yes - - # If PX4IO present, use it as primary PWM output by default - set OUTPUT_MODE io_pwm else echo "[init] PX4IO restart failed" echo "PX4IO restart failed" >> $logfile @@ -164,16 +160,27 @@ then fi fi fi - - set HIL no - set FRAME_TYPE none - set PWM_RATE none - set PWM_DISARMED none - set PWM_MIN none - set PWM_MAX none + + # + # Set default output if it was not defined in rc.txt + # + if [ $OUTPUT_MODE == none ] + then + if [ $IO_PRESENT == yes ] + then + # If PX4IO present, use it as primary PWM output by default + set OUTPUT_MODE io_pwm + else + # Else use PX4FMU PWM output + set OUTPUT_MODE fmu_pwm + fi + fi set EXIT_ON_END no + # + # Set DO_AUTOCONFIG flag to use it in AUTOSTART scripts + # if param compare SYS_AUTOCONFIG 1 then set DO_AUTOCONFIG yes @@ -181,41 +188,35 @@ then set DO_AUTOCONFIG no fi - # - # Start the Commander (needs to be this early for in-air-restarts) - # - commander start - # # Set parameters and env variables for selected AUTOSTART # sh /etc/init.d/rc.autostart - # Custom configuration - if [ -f /fs/microsd/etc/rc.conf ] - then - sh /fs/microsd/etc/rc.conf - fi - - if [ $HIL == yes ] - then - set OUTPUT_MODE hil - else - # Try to get an USB console if not in HIL mode - nshterm /dev/ttyACM0 & - fi - - # - # If autoconfig parameter was set, reset it and save parameters - # - if [ $DO_AUTOCONFIG == yes ] - then - param set SYS_AUTOCONFIG 0 - param save - fi - if [ $MODE == autostart ] then + if [ $HIL == yes ] + then + set OUTPUT_MODE hil + else + # Try to get an USB console if not in HIL mode + nshterm /dev/ttyACM0 & + fi + + # + # If autoconfig parameter was set, reset it and save parameters + # + if [ $DO_AUTOCONFIG == yes ] + then + param set SYS_AUTOCONFIG 0 + param save + fi + + # + # Start the Commander (needs to be this early for in-air-restarts) + # + commander start + # # Start primary output # @@ -224,29 +225,47 @@ then echo "[init] Use PX4IO PWM as primary output" if px4io start then - echo "[init] PX4IO OK" - echo "PX4IO OK" >> $logfile + echo "[init] PX4IO started" sh /etc/init.d/rc.io else echo "[init] PX4IO start error" - echo "PX4IO start error" >> $logfile tone_alarm MNGGG fi fi if [ $OUTPUT_MODE == fmu_pwm ] then echo "[init] Use PX4FMU PWM as primary output" - fmu mode_pwm + if fmu mode_pwm + then + echo "[init] PX4FMU PWM output started" + sh /etc/init.d/rc.io + else + echo "[init] PX4FMU PWM output start error" + tone_alarm MNGGG + fi fi if [ $OUTPUT_MODE == mkblctrl ] then echo "[init] Use MKBLCTRL as primary output" - mkblctrl + if mkblctrl + then + echo "[init] MKBLCTRL started" + else + echo "[init] MKBLCTRL start error" + tone_alarm MNGGG + fi + fi if [ $OUTPUT_MODE == hil ] then echo "[init] Use HIL as primary output" - hil mode_pwm + if hil mode_pwm + then + echo "[init] HIL output started" + else + echo "[init] HIL output error" + tone_alarm MNGGG + fi fi # @@ -257,12 +276,10 @@ then echo "[init] Use PX4IO PWM as secondary output" if px4io start then - echo "[init] PX4IO OK" - echo "PX4IO OK" >> $logfile + echo "[init] PX4IO started" sh /etc/init.d/rc.io else echo "[init] PX4IO start error" - echo "PX4IO start error" >> $logfile tone_alarm MNGGG fi fi @@ -270,18 +287,24 @@ then # # MAVLink # - if [ $OUTPUT_MODE == fmu_pwm -a hw_ver compare PX4FMU_V1 ] + if [ $HIL == yes ] then - # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as PWM output - mavlink start -d /dev/ttyS0 + mavlink start -b 230400 -d /dev/ttyACM0 usleep 5000 - - # Exit from nsh to free port for mavlink - set EXIT_ON_END yes else - # Start MAVLink on default port: ttyS1 - mavlink start - usleep 5000 + if [ $OUTPUT_MODE == fmu_pwm -a hw_ver compare PX4FMU_V1 ] + then + # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as PWM output + mavlink start -d /dev/ttyS0 + usleep 5000 + + # Exit from nsh to free port for mavlink + set EXIT_ON_END yes + else + # Start MAVLink on default port: ttyS1 + mavlink start + usleep 5000 + fi fi # @@ -294,10 +317,7 @@ then then echo "[init] Start logging" sh /etc/init.d/rc.logging - fi - - if [ $HIL == no ] - then + echo "[init] Start GPS" gps start fi @@ -336,6 +356,7 @@ then if [ $VEHICLE_TYPE == none ] then echo "[init] Vehicle type: GENERIC" + attitude_estimator_ekf start position_estimator_inav start fi From f3a3d62cf94e85d69243715de4d5e0cf70d2dbfc Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 10 Jan 2014 23:14:29 +0100 Subject: [PATCH 007/108] Use rc.txt, config.txt, extras.txt files, minor boot messages fixes --- ROMFS/px4fmu_common/init.d/rc.fw_interface | 10 +- ROMFS/px4fmu_common/init.d/rc.mc_interface | 12 +- ROMFS/px4fmu_common/init.d/rcS | 458 +++++++++++---------- 3 files changed, 251 insertions(+), 229 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_interface b/ROMFS/px4fmu_common/init.d/rc.fw_interface index f9517f4220..c864e7c618 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_interface +++ b/ROMFS/px4fmu_common/init.d/rc.fw_interface @@ -1,6 +1,6 @@ #!nsh # -# Script to load fixedwing mixer and set rate/disarmed/min/max values for PWM output +# Script to configure fixedwing control interface # # @@ -12,7 +12,7 @@ param set MAV_TYPE 1 # # Load mixer # -echo "Frame geometry: ${FRAME_GEOMETRY}" -set MIXER /etc/mixers/FMU_${FRAME_GEOMETRY}.mix -echo "Loading mixer: ${MIXER}" -mixer load /dev/pwm_output ${MIXER} +echo "[init] Frame geometry: $FRAME_GEOMETRY" +set MIXER /etc/mixers/FMU_$FRAME_GEOMETRY.mix +echo "[init] Loading mixer: $MIXER" +mixer load /dev/pwm_output $MIXER diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_interface b/ROMFS/px4fmu_common/init.d/rc.mc_interface index 003aee81b6..7fbcbd2821 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_interface +++ b/ROMFS/px4fmu_common/init.d/rc.mc_interface @@ -1,6 +1,6 @@ #!nsh # -# Script to load multicopter mixer and set rate/disarmed/min/max values for PWM output +# Script to configure multicopter control interface # if [ $FRAME_GEOMETRY == quad_x -o $FRAME_GEOMETRY == quad_+ ] @@ -27,9 +27,9 @@ fi # # Load mixer # -echo "Frame geometry: $FRAME_GEOMETRY" -set MIXER /etc/mixers/FMU_${FRAME_GEOMETRY}.mix -echo "Loading mixer: $MIXER" +echo "[init] Frame geometry: $FRAME_GEOMETRY" +set MIXER /etc/mixers/FMU_$FRAME_GEOMETRY.mix +echo "[init] Loading mixer: $MIXER" mixer load /dev/pwm_output $MIXER if [ $OUTPUT_MODE == fmu_pwm -o $OUTPUT_MODE == io_pwm ] @@ -39,6 +39,7 @@ then # if [ $PWM_RATE != none ] then + echo "[init] Set PWM rate: $PWM_RATE" pwm rate -c $OUTPUTS -r $PWM_RATE fi @@ -47,14 +48,17 @@ then # if [ $PWM_DISARMED != none ] then + echo "[init] Set PWM disarmed: $PWM_DISARMED" pwm disarmed -c $OUTPUTS -p $PWM_DISARMED fi if [ $PWM_MIN != none ] then + echo "[init] Set PWM min: $PWM_MIN" pwm min -c $OUTPUTS -p $PWM_MIN fi if [ $PWM_MAX != none ] then + echo "[init] Set PWM max: $PWM_MAX" pwm max -c $OUTPUTS -p $PWM_MAX fi fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index d6d6ec352a..576fc2d949 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -8,7 +8,10 @@ # set MODE autostart -set logfile /fs/microsd/bootlog.txt +set LOG_FILE /fs/microsd/bootlog.txt +set RC_FILE /fs/microsd/etc/rc.txt +set CONFIG_FILE /fs/microsd/etc/config.txt +set EXTRAS_FILE /fs/microsd/etc/extras.txt # # Try to mount the microSD card. @@ -25,35 +28,17 @@ else tone_alarm error fi -# -# Set default values here, can be overriden in rc.txt from SD card -# -set HIL no -set VEHICLE_TYPE none -set FRAME_GEOMETRY none -set OUTPUT_MODE none -set VEHICLE_TYPE none -set PWM_RATE none -set PWM_DISARMED none -set PWM_MIN none -set PWM_MAX none - # # Look for an init script on the microSD card. +# Disable autostart if the script found. # -# To prevent automatic startup in the current flight mode, -# the script should set MODE to some other value. -# -if [ -f /fs/microsd/etc/rc ] +if [ -f $RC_FILE ] then - echo "[init] reading /fs/microsd/etc/rc" - sh /fs/microsd/etc/rc -fi -# Also consider rc.txt files -if [ -f /fs/microsd/etc/rc.txt ] -then - echo "[init] reading /fs/microsd/etc/rc.txt" - sh /fs/microsd/etc/rc.txt + echo "[init] Executing init script: $RC_FILE" + sh $RC_FILE + set MODE custom +else + echo "[init] Init script not found: $RC_FILE" fi # if this is an APM build then there will be a rc.APM script @@ -72,15 +57,7 @@ fi if [ $MODE == autostart ] then - # - # Start CDC/ACM serial driver - # - sercon - - # - # Start the ORB (first app to start) - # - uorb start + echo "[init] AUTOSTART mode" # # Load parameters @@ -95,6 +72,16 @@ then echo "[init] Parameter file corrupt - ignoring" fi fi + + # + # Start CDC/ACM serial driver + # + sercon + + # + # Start the ORB (first app to start) + # + uorb start # # Start system state indicator @@ -114,36 +101,36 @@ then # if [ -f /etc/extras/px4io-v2_default.bin ] then - set io_file /etc/extras/px4io-v2_default.bin + set IO_FILE /etc/extras/px4io-v2_default.bin else - set io_file /etc/extras/px4io-v1_default.bin + set IO_FILE /etc/extras/px4io-v1_default.bin fi set IO_PRESENT no - if px4io checkcrc $io_file + if px4io checkcrc $IO_FILE then echo "[init] PX4IO CRC OK" - echo "PX4IO CRC OK" >> $logfile + echo "PX4IO CRC OK" >> $LOG_FILE set IO_PRESENT yes else echo "[init] PX4IO CRC failure" - echo "PX4IO CRC failure" >> $logfile + echo "PX4IO CRC failure" >> $LOG_FILE tone_alarm MBABGP - if px4io forceupdate 14662 $io_file + if px4io forceupdate 14662 $IO_FILE then usleep 500000 if px4io start then echo "[init] PX4IO restart OK" - echo "PX4IO restart OK" >> $logfile + echo "PX4IO restart OK" >> $LOG_FILE tone_alarm MSPAA set IO_PRESENT yes else echo "[init] PX4IO restart failed" - echo "PX4IO restart failed" >> $logfile + echo "PX4IO restart failed" >> $LOG_FILE if hw_ver compare PX4FMU_V2 then tone_alarm MNGGG @@ -153,27 +140,35 @@ then fi else echo "[init] PX4IO update failed" - echo "PX4IO update failed" >> $logfile + echo "PX4IO update failed" >> $LOG_FILE if hw_ver compare PX4FMU_V2 then tone_alarm MNGGG fi fi fi - + # - # Set default output if it was not defined in rc.txt + # Set default values # - if [ $OUTPUT_MODE == none ] + set HIL no + set VEHICLE_TYPE none + set FRAME_GEOMETRY none + set PWM_RATE none + set PWM_DISARMED none + set PWM_MIN none + set PWM_MAX none + + # + # Set default output + # + if [ $IO_PRESENT == yes ] then - if [ $IO_PRESENT == yes ] - then - # If PX4IO present, use it as primary PWM output by default - set OUTPUT_MODE io_pwm - else - # Else use PX4FMU PWM output - set OUTPUT_MODE fmu_pwm - fi + # If PX4IO present, use it as primary PWM output by default + set OUTPUT_MODE io_pwm + else + # Else use PX4FMU PWM output + set OUTPUT_MODE fmu_pwm fi set EXIT_ON_END no @@ -193,179 +188,202 @@ then # sh /etc/init.d/rc.autostart - if [ $MODE == autostart ] - then - if [ $HIL == yes ] - then - set OUTPUT_MODE hil - else - # Try to get an USB console if not in HIL mode - nshterm /dev/ttyACM0 & - fi - - # - # If autoconfig parameter was set, reset it and save parameters - # - if [ $DO_AUTOCONFIG == yes ] - then - param set SYS_AUTOCONFIG 0 - param save - fi + # + # Override parameters from user configuration file + # + if [ -f $CONFIG_FILE ] + then + echo "[init] Reading config: $CONFIG_FILE" + sh $CONFIG_FILE + else + echo "[init] Config file not found: $CONFIG_FILE" + fi - # - # Start the Commander (needs to be this early for in-air-restarts) - # - commander start - - # - # Start primary output - # - if [ $OUTPUT_MODE == io_pwm ] - then - echo "[init] Use PX4IO PWM as primary output" - if px4io start - then - echo "[init] PX4IO started" - sh /etc/init.d/rc.io - else - echo "[init] PX4IO start error" - tone_alarm MNGGG - fi - fi - if [ $OUTPUT_MODE == fmu_pwm ] - then - echo "[init] Use PX4FMU PWM as primary output" - if fmu mode_pwm - then - echo "[init] PX4FMU PWM output started" - sh /etc/init.d/rc.io - else - echo "[init] PX4FMU PWM output start error" - tone_alarm MNGGG - fi - fi - if [ $OUTPUT_MODE == mkblctrl ] - then - echo "[init] Use MKBLCTRL as primary output" - if mkblctrl - then - echo "[init] MKBLCTRL started" - else - echo "[init] MKBLCTRL start error" - tone_alarm MNGGG - fi - - fi - if [ $OUTPUT_MODE == hil ] - then - echo "[init] Use HIL as primary output" - if hil mode_pwm - then - echo "[init] HIL output started" - else - echo "[init] HIL output error" - tone_alarm MNGGG - fi - fi - - # - # Start PX4IO as secondary output if needed - # - if [ $IO_PRESENT == yes -a $OUTPUT_MODE != io_pwm ] - then - echo "[init] Use PX4IO PWM as secondary output" - if px4io start - then - echo "[init] PX4IO started" - sh /etc/init.d/rc.io - else - echo "[init] PX4IO start error" - tone_alarm MNGGG - fi - fi - - # - # MAVLink - # - if [ $HIL == yes ] - then - mavlink start -b 230400 -d /dev/ttyACM0 - usleep 5000 - else - if [ $OUTPUT_MODE == fmu_pwm -a hw_ver compare PX4FMU_V1 ] - then - # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as PWM output - mavlink start -d /dev/ttyS0 - usleep 5000 - - # Exit from nsh to free port for mavlink - set EXIT_ON_END yes - else - # Start MAVLink on default port: ttyS1 - mavlink start - usleep 5000 - fi - fi - - # - # Sensors, Logging, GPS - # - echo "[init] Start sensors" - sh /etc/init.d/rc.sensors - - if [ $HIL == no ] - then - echo "[init] Start logging" - sh /etc/init.d/rc.logging - - echo "[init] Start GPS" - gps start - fi - - # - # Fixed wing setup - # - if [ $VEHICLE_TYPE == fw ] - then - echo "[init] Vehicle type: FIXED WING" - - # Load mixer and configure outputs - sh /etc/init.d/rc.fw_interface - - # Start standard fixedwing apps - sh /etc/init.d/rc.mc_apps - fi + if [ $HIL == yes ] + then + set OUTPUT_MODE hil + else + # Try to get an USB console if not in HIL mode + nshterm /dev/ttyACM0 & + fi - # - # Multicopters setup - # - if [ $VEHICLE_TYPE == mc ] - then - echo "[init] Vehicle type: MULTICOPTER" - - # Load mixer and configure outputs - sh /etc/init.d/rc.mc_interface - - # Start standard multicopter apps - sh /etc/init.d/rc.mc_apps - fi + # + # If autoconfig parameter was set, reset it and save parameters + # + if [ $DO_AUTOCONFIG == yes ] + then + param set SYS_AUTOCONFIG 0 + param save + fi - # - # Generic setup (autostart ID not found) - # - if [ $VEHICLE_TYPE == none ] + # + # Start the Commander (needs to be this early for in-air-restarts) + # + commander start + + # + # Start primary output + # + if [ $OUTPUT_MODE == io_pwm ] + then + echo "[init] Use PX4IO PWM as primary output" + if px4io start then - echo "[init] Vehicle type: GENERIC" - - attitude_estimator_ekf start - position_estimator_inav start + echo "[init] PX4IO started" + sh /etc/init.d/rc.io + else + echo "[init] PX4IO start error" + tone_alarm MNGGG + fi + fi + if [ $OUTPUT_MODE == fmu_pwm ] + then + echo "[init] Use PX4FMU PWM as primary output" + if fmu mode_pwm + then + echo "[init] PX4FMU PWM output started" + sh /etc/init.d/rc.io + else + echo "[init] PX4FMU PWM output start error" + tone_alarm MNGGG + fi + fi + if [ $OUTPUT_MODE == mkblctrl ] + then + echo "[init] Use MKBLCTRL as primary output" + if mkblctrl + then + echo "[init] MKBLCTRL started" + else + echo "[init] MKBLCTRL start error" + tone_alarm MNGGG + fi + + fi + if [ $OUTPUT_MODE == hil ] + then + echo "[init] Use HIL as primary output" + if hil mode_pwm + then + echo "[init] HIL output started" + else + echo "[init] HIL output error" + tone_alarm MNGGG + fi + fi + + # + # Start PX4IO as secondary output if needed + # + if [ $IO_PRESENT == yes -a $OUTPUT_MODE != io_pwm ] + then + echo "[init] Use PX4IO PWM as secondary output" + if px4io start + then + echo "[init] PX4IO started" + sh /etc/init.d/rc.io + else + echo "[init] PX4IO start error" + tone_alarm MNGGG fi fi - # Start any custom extensions - if [ -f /fs/microsd/etc/rc.local ] + # + # MAVLink + # + if [ $HIL == yes ] then - sh /fs/microsd/etc/rc.local + mavlink start -b 230400 -d /dev/ttyACM0 + usleep 5000 + else + if [ $OUTPUT_MODE == fmu_pwm -a hw_ver compare PX4FMU_V1 ] + then + # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as PWM output + mavlink start -d /dev/ttyS0 + usleep 5000 + + # Exit from nsh to free port for mavlink + set EXIT_ON_END yes + else + # Start MAVLink on default port: ttyS1 + mavlink start + usleep 5000 + fi + fi + + # + # Sensors, Logging, GPS + # + echo "[init] Start sensors" + sh /etc/init.d/rc.sensors + + if [ $HIL == no ] + then + echo "[init] Start logging" + sh /etc/init.d/rc.logging + + echo "[init] Start GPS" + gps start + fi + + # + # Fixed wing setup + # + if [ $VEHICLE_TYPE == fw ] + then + echo "[init] Vehicle type: FIXED WING" + + if [ $FRAME_GEOMETRY == none ] + then + # Set default frame geometry for fixed wing + set FRAME_GEOMETRY AERT + fi + + # Load mixer and configure outputs + sh /etc/init.d/rc.fw_interface + + # Start standard fixedwing apps + sh /etc/init.d/rc.mc_apps + fi + + # + # Multicopters setup + # + if [ $VEHICLE_TYPE == mc ] + then + echo "[init] Vehicle type: MULTICOPTER" + + if [ $FRAME_GEOMETRY == none ] + then + # Set default frame geometry for multicopter + set FRAME_GEOMETRY quad_x + fi + + # Load mixer and configure outputs + sh /etc/init.d/rc.mc_interface + + # Start standard multicopter apps + sh /etc/init.d/rc.mc_apps + fi + + # + # Generic setup (autostart ID not found) + # + if [ $VEHICLE_TYPE == none ] + then + echo "[init] Vehicle type: GENERIC" + + attitude_estimator_ekf start + position_estimator_inav start + fi + + # Start any custom addons + if [ -f $EXTRAS_FILE ] + then + echo "[init] Starting addons script: $EXTRAS_FILE" + sh $EXTRAS_FILE + else + echo "[init] Addons script not found: $EXTRAS_FILE" fi if [ $EXIT_ON_END == yes ] From 2e1199299219baf487b4c31ebf4453ffaf971fbf Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 10 Jan 2014 23:41:03 +0100 Subject: [PATCH 008/108] =?UTF-8?q?Don=E2=80=99t=20try=20to=20find=20autos?= =?UTF-8?q?tart=20script=20if=20SYS=5FAUTOSTART=20=3D=200?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ROMFS/px4fmu_common/init.d/rcS | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 576fc2d949..178ed18124 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -186,7 +186,12 @@ then # # Set parameters and env variables for selected AUTOSTART # - sh /etc/init.d/rc.autostart + if param compare SYS_AUTOSTART 0 + then + echo "[init] Don't try to find autostart script" + else + sh /etc/init.d/rc.autostart + fi # # Override parameters from user configuration file From e301bdcf99271a6c95bbe6b83ab7b24ba682b211 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 11 Jan 2014 00:34:46 +0100 Subject: [PATCH 009/108] Autostart: some fixed wing setups added --- .../px4fmu_common/init.d/10015_tbs_discovery | 6 +- ROMFS/px4fmu_common/init.d/2100_mpx_easystar | 9 +- ROMFS/px4fmu_common/init.d/3031_io_phantom | 85 ------------------- ROMFS/px4fmu_common/init.d/3031_phantom | 44 ++++++++++ ROMFS/px4fmu_common/init.d/3033_io_wingwing | 84 ------------------ ROMFS/px4fmu_common/init.d/3033_wingwing | 43 ++++++++++ ROMFS/px4fmu_common/init.d/3034_fx79 | 43 ++++++++++ ROMFS/px4fmu_common/init.d/3034_io_fx79 | 84 ------------------ ROMFS/px4fmu_common/init.d/rc.autostart | 14 +-- 9 files changed, 145 insertions(+), 267 deletions(-) delete mode 100644 ROMFS/px4fmu_common/init.d/3031_io_phantom create mode 100644 ROMFS/px4fmu_common/init.d/3031_phantom delete mode 100644 ROMFS/px4fmu_common/init.d/3033_io_wingwing create mode 100644 ROMFS/px4fmu_common/init.d/3033_wingwing create mode 100644 ROMFS/px4fmu_common/init.d/3034_fx79 delete mode 100644 ROMFS/px4fmu_common/init.d/3034_io_fx79 diff --git a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery index 24f1099d3f..53140caff3 100644 --- a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery @@ -24,9 +24,7 @@ then param set MC_YAWRATE_D 0.0 fi -set FRAME_TYPE mc +set VEHICLE_TYPE mc set FRAME_GEOMETRY quad_w + set PWM_RATE 400 -set PWM_DISARMED 900 -set PWM_MIN 1100 -set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/2100_mpx_easystar b/ROMFS/px4fmu_common/init.d/2100_mpx_easystar index 9bf01c60ce..43f911a781 100644 --- a/ROMFS/px4fmu_common/init.d/2100_mpx_easystar +++ b/ROMFS/px4fmu_common/init.d/2100_mpx_easystar @@ -1,6 +1,9 @@ #!nsh - -echo "[init] EasyStar" +# +# MPX EasyStar Plane +# +# Maintainers: ??? +# if [ $DO_AUTOCONFIG == yes ] then @@ -32,5 +35,5 @@ then param set RC_SCALE_PITCH 1.0 fi -set FRAME_TYPE fw +set VEHICLE_TYPE fw set FRAME_GEOMETRY RET diff --git a/ROMFS/px4fmu_common/init.d/3031_io_phantom b/ROMFS/px4fmu_common/init.d/3031_io_phantom deleted file mode 100644 index 0cf6ee39a5..0000000000 --- a/ROMFS/px4fmu_common/init.d/3031_io_phantom +++ /dev/null @@ -1,85 +0,0 @@ -#!nsh - -echo "[init] PX4FMU v1, v2 with or without IO on Phantom FPV" - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set FW_AIRSPD_MIN 11.4 - param set FW_AIRSPD_TRIM 14 - param set FW_AIRSPD_MAX 22 - param set FW_L1_PERIOD 15 - param set FW_P_D 0 - param set FW_P_I 0 - param set FW_P_IMAX 15 - param set FW_P_LIM_MAX 45 - param set FW_P_LIM_MIN -45 - param set FW_P_P 60 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 - param set FW_P_ROLLFF 2 - param set FW_R_D 0 - param set FW_R_I 5 - param set FW_R_IMAX 15 - param set FW_R_P 80 - param set FW_R_RMAX 60 - param set FW_THR_CRUISE 0.8 - param set FW_THR_LND_MAX 0 - param set FW_THR_MAX 1 - param set FW_THR_MIN 0.5 - param set FW_T_SINK_MAX 5.0 - param set FW_T_SINK_MIN 2.0 - param set FW_Y_ROLLFF 1.0 - param set RC_SCALE_ROLL 0.6 - param set RC_SCALE_PITCH 0.6 - param set TRIM_PITCH 0.1 - - param set SYS_AUTOCONFIG 0 - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing -# -param set MAV_TYPE 1 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - - sh /etc/init.d/rc.io - # Limit to 100 Hz updates and (implicit) 50 Hz PWM - px4io limit 100 -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -# -# Load mixer and start controllers (depends on px4io) -# -if [ -f /fs/microsd/etc/mixers/FMU_Q.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix -else - echo "Using /etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix -fi - -# -# Start common fixedwing apps -# -sh /etc/init.d/rc.fixedwing diff --git a/ROMFS/px4fmu_common/init.d/3031_phantom b/ROMFS/px4fmu_common/init.d/3031_phantom new file mode 100644 index 0000000000..29af48ec65 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/3031_phantom @@ -0,0 +1,44 @@ +#!nsh +# +# Phantom FPV Flying Wing +# +# Maintainers: Simon Wilks +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set FW_AIRSPD_MIN 11.4 + param set FW_AIRSPD_TRIM 14 + param set FW_AIRSPD_MAX 22 + param set FW_L1_PERIOD 15 + param set FW_P_D 0 + param set FW_P_I 0 + param set FW_P_IMAX 15 + param set FW_P_LIM_MAX 45 + param set FW_P_LIM_MIN -45 + param set FW_P_P 60 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 2 + param set FW_R_D 0 + param set FW_R_I 5 + param set FW_R_IMAX 15 + param set FW_R_P 80 + param set FW_R_RMAX 60 + param set FW_THR_CRUISE 0.8 + param set FW_THR_LND_MAX 0 + param set FW_THR_MAX 1 + param set FW_THR_MIN 0.5 + param set FW_T_SINK_MAX 5.0 + param set FW_T_SINK_MIN 2.0 + param set FW_Y_ROLLFF 1.0 + param set RC_SCALE_ROLL 0.6 + param set RC_SCALE_PITCH 0.6 + param set TRIM_PITCH 0.1 +fi + +set VEHICLE_TYPE fw +set FRAME_GEOMETRY Q diff --git a/ROMFS/px4fmu_common/init.d/3033_io_wingwing b/ROMFS/px4fmu_common/init.d/3033_io_wingwing deleted file mode 100644 index 82ff425e6d..0000000000 --- a/ROMFS/px4fmu_common/init.d/3033_io_wingwing +++ /dev/null @@ -1,84 +0,0 @@ -#!nsh - -echo "[init] PX4FMU v1, v2 with or without IO on the Wing Wing (aka Z-84)" - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set FW_AIRSPD_MIN 7 - param set FW_AIRSPD_TRIM 9 - param set FW_AIRSPD_MAX 14 - param set FW_L1_PERIOD 10 - param set FW_P_D 0 - param set FW_P_I 0 - param set FW_P_IMAX 20 - param set FW_P_LIM_MAX 30 - param set FW_P_LIM_MIN -20 - param set FW_P_P 30 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 - param set FW_P_ROLLFF 2 - param set FW_R_D 0 - param set FW_R_I 5 - param set FW_R_IMAX 20 - param set FW_R_P 60 - param set FW_R_RMAX 60 - param set FW_THR_CRUISE 0.65 - param set FW_THR_MAX 0.7 - param set FW_THR_MIN 0 - param set FW_T_SINK_MAX 5 - param set FW_T_SINK_MIN 2 - param set FW_T_TIME_CONST 9 - param set FW_Y_ROLLFF 2.0 - param set RC_SCALE_ROLL 1.0 - param set RC_SCALE_PITCH 1.0 - - param set SYS_AUTOCONFIG 0 - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing -# -param set MAV_TYPE 1 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - - sh /etc/init.d/rc.io - # Limit to 100 Hz updates and (implicit) 50 Hz PWM - px4io limit 100 -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -# -# Load mixer and start controllers (depends on px4io) -# -if [ -f /fs/microsd/etc/mixers/FMU_Q.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix -else - echo "Using /etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix -fi - -# -# Start common fixedwing apps -# -sh /etc/init.d/rc.fixedwing diff --git a/ROMFS/px4fmu_common/init.d/3033_wingwing b/ROMFS/px4fmu_common/init.d/3033_wingwing new file mode 100644 index 0000000000..e0340a8d9a --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/3033_wingwing @@ -0,0 +1,43 @@ +#!nsh +# +# Wing Wing (aka Z-84) Flying Wing +# +# Maintainers: Simon Wilks +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set FW_AIRSPD_MIN 7 + param set FW_AIRSPD_TRIM 9 + param set FW_AIRSPD_MAX 14 + param set FW_L1_PERIOD 10 + param set FW_P_D 0 + param set FW_P_I 0 + param set FW_P_IMAX 20 + param set FW_P_LIM_MAX 30 + param set FW_P_LIM_MIN -20 + param set FW_P_P 30 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 2 + param set FW_R_D 0 + param set FW_R_I 5 + param set FW_R_IMAX 20 + param set FW_R_P 60 + param set FW_R_RMAX 60 + param set FW_THR_CRUISE 0.65 + param set FW_THR_MAX 0.7 + param set FW_THR_MIN 0 + param set FW_T_SINK_MAX 5 + param set FW_T_SINK_MIN 2 + param set FW_T_TIME_CONST 9 + param set FW_Y_ROLLFF 2.0 + param set RC_SCALE_ROLL 1.0 + param set RC_SCALE_PITCH 1.0 +fi + +set VEHICLE_TYPE fw +set FRAME_GEOMETRY Q diff --git a/ROMFS/px4fmu_common/init.d/3034_fx79 b/ROMFS/px4fmu_common/init.d/3034_fx79 new file mode 100644 index 0000000000..c4dab7ba66 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/3034_fx79 @@ -0,0 +1,43 @@ +#!nsh +# +# FX-79 Buffalo Flying Wing +# +# Maintainers: Simon Wilks +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set FW_AIRSPD_MAX 20 + param set FW_AIRSPD_TRIM 12 + param set FW_AIRSPD_MIN 15 + param set FW_L1_PERIOD 12 + param set FW_P_D 0 + param set FW_P_I 0 + param set FW_P_IMAX 15 + param set FW_P_LIM_MAX 50 + param set FW_P_LIM_MIN -50 + param set FW_P_P 60 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 1.1 + param set FW_R_D 0 + param set FW_R_I 5 + param set FW_R_IMAX 20 + param set FW_R_P 80 + param set FW_R_RMAX 100 + param set FW_THR_CRUISE 0.75 + param set FW_THR_MAX 1 + param set FW_THR_MIN 0 + param set FW_T_SINK_MAX 5.0 + param set FW_T_SINK_MIN 4.0 + param set FW_T_TIME_CONST 9 + param set FW_Y_ROLLFF 1.1 + param set RC_SCALE_ROLL 1.0 + param set RC_SCALE_PITCH 1.0 +fi + +set VEHICLE_TYPE fw +set FRAME_GEOMETRY Q diff --git a/ROMFS/px4fmu_common/init.d/3034_io_fx79 b/ROMFS/px4fmu_common/init.d/3034_io_fx79 deleted file mode 100644 index 759c17bb44..0000000000 --- a/ROMFS/px4fmu_common/init.d/3034_io_fx79 +++ /dev/null @@ -1,84 +0,0 @@ -#!nsh - -echo "[init] PX4FMU v1, v2 with or without IO on FX-79 Buffalo" - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set FW_AIRSPD_MAX 20 - param set FW_AIRSPD_TRIM 12 - param set FW_AIRSPD_MIN 15 - param set FW_L1_PERIOD 12 - param set FW_P_D 0 - param set FW_P_I 0 - param set FW_P_IMAX 15 - param set FW_P_LIM_MAX 50 - param set FW_P_LIM_MIN -50 - param set FW_P_P 60 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 - param set FW_P_ROLLFF 1.1 - param set FW_R_D 0 - param set FW_R_I 5 - param set FW_R_IMAX 20 - param set FW_R_P 80 - param set FW_R_RMAX 100 - param set FW_THR_CRUISE 0.75 - param set FW_THR_MAX 1 - param set FW_THR_MIN 0 - param set FW_T_SINK_MAX 5.0 - param set FW_T_SINK_MIN 4.0 - param set FW_T_TIME_CONST 9 - param set FW_Y_ROLLFF 1.1 - param set RC_SCALE_ROLL 1.0 - param set RC_SCALE_PITCH 1.0 - - param set SYS_AUTOCONFIG 0 - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing -# -param set MAV_TYPE 1 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - - sh /etc/init.d/rc.io - # Limit to 100 Hz updates and (implicit) 50 Hz PWM - px4io limit 100 -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -# -# Load mixer and start controllers (depends on px4io) -# -if [ -f /fs/microsd/etc/mixers/FMU_FX79.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_FX79.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_FX79.mix -else - echo "Using /etc/mixers/FMU_FX79.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_FX79.mix -fi - -# -# Start common fixedwing apps -# -sh /etc/init.d/rc.fixedwing diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 352a916ac1..69e88fcd09 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -77,24 +77,24 @@ then #sh /etc/init.d/3030_io_camflyer fi -if param compare SYS_AUTOSTART 3031 31 +if param compare SYS_AUTOSTART 3031 then - #sh /etc/init.d/3031_io_phantom + sh /etc/init.d/3031_phantom fi -if param compare SYS_AUTOSTART 3032 32 +if param compare SYS_AUTOSTART 3032 then #sh /etc/init.d/3032_skywalker_x5 fi -if param compare SYS_AUTOSTART 3033 33 +if param compare SYS_AUTOSTART 3033 then - #sh /etc/init.d/3033_io_wingwing + sh /etc/init.d/3033_wingwing fi -if param compare SYS_AUTOSTART 3034 34 +if param compare SYS_AUTOSTART 3034 then - #sh /etc/init.d/3034_io_fx79 + sh /etc/init.d/3034_fx79 fi # From 3f6f26e5f6bb89056a0b07b4761fdf81f0f8491a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 11 Jan 2014 00:40:19 +0100 Subject: [PATCH 010/108] Autostart for fixed wing bug fixed --- ROMFS/px4fmu_common/init.d/rcS | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 178ed18124..c92876f269 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -348,7 +348,7 @@ then sh /etc/init.d/rc.fw_interface # Start standard fixedwing apps - sh /etc/init.d/rc.mc_apps + sh /etc/init.d/rc.fw_apps fi # From 95c99618f9f22ba08b4826a98c32f995ea3bcf17 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 11 Jan 2014 11:26:55 +0100 Subject: [PATCH 011/108] Autostart: mkblctrl mixer loading fixed --- ROMFS/px4fmu_common/init.d/rc.mc_interface | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_interface b/ROMFS/px4fmu_common/init.d/rc.mc_interface index 7fbcbd2821..401ff775f9 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_interface +++ b/ROMFS/px4fmu_common/init.d/rc.mc_interface @@ -30,7 +30,13 @@ fi echo "[init] Frame geometry: $FRAME_GEOMETRY" set MIXER /etc/mixers/FMU_$FRAME_GEOMETRY.mix echo "[init] Loading mixer: $MIXER" -mixer load /dev/pwm_output $MIXER +if [ $OUTPUT_MODE == mkblctrl ] +then + set OUTPUT_DEV /dev/mkblctrl +else + set OUTPUT_DEV /dev/pwm_output +fi +mixer load $OUTPUT_DEV $MIXER if [ $OUTPUT_MODE == fmu_pwm -o $OUTPUT_MODE == io_pwm ] then From 41add86164e15d553a5b1d2d2f9d55d964ca4ebe Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 11 Jan 2014 11:34:18 +0100 Subject: [PATCH 012/108] Autostart: standalone FMUv1 setup fixed --- ROMFS/px4fmu_common/init.d/rcS | 1 - 1 file changed, 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index c92876f269..f8796bb085 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -247,7 +247,6 @@ then if fmu mode_pwm then echo "[init] PX4FMU PWM output started" - sh /etc/init.d/rc.io else echo "[init] PX4FMU PWM output start error" tone_alarm MNGGG From 4dd5c13b98f57e490274d7b3fb6d297e18e79853 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 11 Jan 2014 17:52:29 +0100 Subject: [PATCH 013/108] Autostart: fixes --- ROMFS/px4fmu_common/init.d/rc.mc_interface | 2 +- ROMFS/px4fmu_common/init.d/rcS | 240 +++++++++++++-------- 2 files changed, 147 insertions(+), 95 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_interface b/ROMFS/px4fmu_common/init.d/rc.mc_interface index 401ff775f9..12be3a931d 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_interface +++ b/ROMFS/px4fmu_common/init.d/rc.mc_interface @@ -38,7 +38,7 @@ else fi mixer load $OUTPUT_DEV $MIXER -if [ $OUTPUT_MODE == fmu_pwm -o $OUTPUT_MODE == io_pwm ] +if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == io ] then # # Set PWM output frequency diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index f8796bb085..5e13e4fa21 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -96,82 +96,20 @@ then fi fi - # - # Check if PX4IO present and update firmware if needed - # - if [ -f /etc/extras/px4io-v2_default.bin ] - then - set IO_FILE /etc/extras/px4io-v2_default.bin - else - set IO_FILE /etc/extras/px4io-v1_default.bin - fi - - set IO_PRESENT no - - if px4io checkcrc $IO_FILE - then - echo "[init] PX4IO CRC OK" - echo "PX4IO CRC OK" >> $LOG_FILE - - set IO_PRESENT yes - else - echo "[init] PX4IO CRC failure" - echo "PX4IO CRC failure" >> $LOG_FILE - tone_alarm MBABGP - if px4io forceupdate 14662 $IO_FILE - then - usleep 500000 - if px4io start - then - echo "[init] PX4IO restart OK" - echo "PX4IO restart OK" >> $LOG_FILE - tone_alarm MSPAA - - set IO_PRESENT yes - else - echo "[init] PX4IO restart failed" - echo "PX4IO restart failed" >> $LOG_FILE - if hw_ver compare PX4FMU_V2 - then - tone_alarm MNGGG - sleep 10 - reboot - fi - fi - else - echo "[init] PX4IO update failed" - echo "PX4IO update failed" >> $LOG_FILE - if hw_ver compare PX4FMU_V2 - then - tone_alarm MNGGG - fi - fi - fi - # # Set default values # set HIL no set VEHICLE_TYPE none set FRAME_GEOMETRY none + set USE_IO yes + set OUTPUT_MODE none set PWM_RATE none set PWM_DISARMED none set PWM_MIN none set PWM_MAX none - - # - # Set default output - # - if [ $IO_PRESENT == yes ] - then - # If PX4IO present, use it as primary PWM output by default - set OUTPUT_MODE io_pwm - else - # Else use PX4FMU PWM output - set OUTPUT_MODE fmu_pwm - fi - - set EXIT_ON_END no + set MKBLCTRL_MODE none + set FMU_MODE pwm # # Set DO_AUTOCONFIG flag to use it in AUTOSTART scripts @@ -182,7 +120,7 @@ then else set DO_AUTOCONFIG no fi - + # # Set parameters and env variables for selected AUTOSTART # @@ -204,6 +142,82 @@ then echo "[init] Config file not found: $CONFIG_FILE" fi + # + # If autoconfig parameter was set, reset it and save parameters + # + if [ $DO_AUTOCONFIG == yes ] + then + param set SYS_AUTOCONFIG 0 + param save + fi + + set IO_PRESENT no + + if [ $USE_IO == yes ] + then + # + # Check if PX4IO present and update firmware if needed + # + if [ -f /etc/extras/px4io-v2_default.bin ] + then + set IO_FILE /etc/extras/px4io-v2_default.bin + else + set IO_FILE /etc/extras/px4io-v1_default.bin + fi + + if px4io checkcrc $IO_FILE + then + echo "[init] PX4IO CRC OK" + echo "PX4IO CRC OK" >> $LOG_FILE + + set IO_PRESENT yes + else + echo "[init] PX4IO CRC failure" + echo "PX4IO CRC failure" >> $LOG_FILE + tone_alarm MBABGP + if px4io forceupdate 14662 $IO_FILE + then + usleep 500000 + if px4io checkcrc $IO_FILE + then + echo "[init] PX4IO CRC OK after updating" + echo "PX4IO CRC OK after updating" >> $LOG_FILE + tone_alarm MSPAA + + set IO_PRESENT yes + else + echo "[init] PX4IO update failed" + echo "PX4IO update failed" >> $LOG_FILE + fi + else + echo "[init] PX4IO update failed" + echo "PX4IO update failed" >> $LOG_FILE + fi + fi + + if [ $IO_PRESENT == no ] + then + echo "[init] ERROR: PX4IO not found, set vehicle type to NONE" + tone_alarm MNGGG + set VEHICLE_TYPE none + fi + fi + + # + # Set default output if not set + # + if [ $OUTPUT_MODE == none ] + then + if [ $IO_PRESENT == yes ] + then + # If PX4IO present, use it as primary PWM output by default + set OUTPUT_MODE io + else + # Else use PX4FMU PWM output + set OUTPUT_MODE fmu + fi + fi + if [ $HIL == yes ] then set OUTPUT_MODE hil @@ -212,15 +226,6 @@ then nshterm /dev/ttyACM0 & fi - # - # If autoconfig parameter was set, reset it and save parameters - # - if [ $DO_AUTOCONFIG == yes ] - then - param set SYS_AUTOCONFIG 0 - param save - fi - # # Start the Commander (needs to be this early for in-air-restarts) # @@ -229,7 +234,9 @@ then # # Start primary output # - if [ $OUTPUT_MODE == io_pwm ] + set TTYS1_BUSY no + + if [ $OUTPUT_MODE == io ] then echo "[init] Use PX4IO PWM as primary output" if px4io start @@ -241,21 +248,43 @@ then tone_alarm MNGGG fi fi - if [ $OUTPUT_MODE == fmu_pwm ] + if [ $OUTPUT_MODE == fmu ] then - echo "[init] Use PX4FMU PWM as primary output" - if fmu mode_pwm + echo "[init] Use FMU PWM as primary output" + if fmu mode_$FMU_MODE then - echo "[init] PX4FMU PWM output started" + echo "[init] FMU mode_$FMU_MODE started" else - echo "[init] PX4FMU PWM output start error" + echo "[init] FMU mode_$FMU_MODE start error" tone_alarm MNGGG fi + + if hw_ver compare PX4FMU_V1 + then + if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ] + then + set TTYS1_BUSY yes + fi + if [ $FMU_MODE == pwm_gpio ] + then + set TTYS1_BUSY yes + fi + fi fi if [ $OUTPUT_MODE == mkblctrl ] then echo "[init] Use MKBLCTRL as primary output" - if mkblctrl + set MKBLCTRL_ARG "" + if [ $MKBLCTRL_MODE == x ] + then + set MKBLCTRL_ARG "-mkmode x" + fi + if [ $MKBLCTRL_MODE == + ] + then + set MKBLCTRL_ARG "-mkmode +" + fi + + if mkblctrl $MKBLCTRL_ARG then echo "[init] MKBLCTRL started" else @@ -277,32 +306,55 @@ then fi # - # Start PX4IO as secondary output if needed + # Start IO or FMU for RC PPM input if needed # - if [ $IO_PRESENT == yes -a $OUTPUT_MODE != io_pwm ] + if [ $IO_PRESENT == yes ] then - echo "[init] Use PX4IO PWM as secondary output" - if px4io start + if [ $OUTPUT_MODE != io ] then - echo "[init] PX4IO started" - sh /etc/init.d/rc.io - else - echo "[init] PX4IO start error" - tone_alarm MNGGG + if px4io start + then + echo "[init] PX4IO started" + sh /etc/init.d/rc.io + else + echo "[init] PX4IO start error" + tone_alarm MNGGG + fi + fi + else + if [ $OUTPUT_MODE != fmu ] + then + if fmu mode_$FMU_MODE + then + echo "[init] FMU mode_$FMU_MODE started" + else + echo "[init] FMU mode_$FMU_MODE start error" + tone_alarm MNGGG + fi + + if hw_ver compare PX4FMU_V1 + then + if [ $FMU_MODE == pwm -o $FMU_MODE == gpio -o $FMU_MODE == pwm_gpio ] + then + set TTYS1_BUSY yes + fi + fi fi fi # # MAVLink # + set EXIT_ON_END no + if [ $HIL == yes ] then mavlink start -b 230400 -d /dev/ttyACM0 usleep 5000 else - if [ $OUTPUT_MODE == fmu_pwm -a hw_ver compare PX4FMU_V1 ] + if [ $TTYS1_BUSY == yes ] then - # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as PWM output + # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else mavlink start -d /dev/ttyS0 usleep 5000 From 3339edeae6bdb4119179eeacc34ab91c0b8aced1 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 11 Jan 2014 18:45:56 +0100 Subject: [PATCH 014/108] Autostart: tones cleanup --- ROMFS/px4fmu_common/init.d/rc.fw_interface | 10 +++++++-- ROMFS/px4fmu_common/init.d/rc.mc_interface | 11 ++++++++-- ROMFS/px4fmu_common/init.d/rcS | 24 ++++++++++++---------- 3 files changed, 30 insertions(+), 15 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_interface b/ROMFS/px4fmu_common/init.d/rc.fw_interface index c864e7c618..69f3ed7f7d 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_interface +++ b/ROMFS/px4fmu_common/init.d/rc.fw_interface @@ -14,5 +14,11 @@ param set MAV_TYPE 1 # echo "[init] Frame geometry: $FRAME_GEOMETRY" set MIXER /etc/mixers/FMU_$FRAME_GEOMETRY.mix -echo "[init] Loading mixer: $MIXER" -mixer load /dev/pwm_output $MIXER + +if mixer load /dev/pwm_output $MIXER +then + echo "[init] Mixer loaded: $MIXER" +else + echo "[init] Error loading mixer: $MIXER" + tone_alarm $TUNE_OUT_ERROR +fi diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_interface b/ROMFS/px4fmu_common/init.d/rc.mc_interface index 12be3a931d..6e4e4ed314 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_interface +++ b/ROMFS/px4fmu_common/init.d/rc.mc_interface @@ -29,14 +29,21 @@ fi # echo "[init] Frame geometry: $FRAME_GEOMETRY" set MIXER /etc/mixers/FMU_$FRAME_GEOMETRY.mix -echo "[init] Loading mixer: $MIXER" + if [ $OUTPUT_MODE == mkblctrl ] then set OUTPUT_DEV /dev/mkblctrl else set OUTPUT_DEV /dev/pwm_output fi -mixer load $OUTPUT_DEV $MIXER + +if mixer load $OUTPUT_DEV $MIXER +then + echo "[init] Mixer loaded: $MIXER" +else + echo "[init] Error loading mixer: $MIXER" + tone_alarm $TUNE_OUT_ERROR +fi if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == io ] then diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 5e13e4fa21..a809f2870e 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -13,6 +13,7 @@ set RC_FILE /fs/microsd/etc/rc.txt set CONFIG_FILE /fs/microsd/etc/config.txt set EXTRAS_FILE /fs/microsd/etc/extras.txt +set TUNE_OUT_ERROR ML<> $LOG_FILE - tone_alarm MBABGP + + tone_alarm MLL32CP8MB + if px4io forceupdate 14662 $IO_FILE then usleep 500000 @@ -182,7 +185,7 @@ then then echo "[init] PX4IO CRC OK after updating" echo "PX4IO CRC OK after updating" >> $LOG_FILE - tone_alarm MSPAA + tone_alarm MLL8CDE set IO_PRESENT yes else @@ -197,9 +200,8 @@ then if [ $IO_PRESENT == no ] then - echo "[init] ERROR: PX4IO not found, set vehicle type to NONE" - tone_alarm MNGGG - set VEHICLE_TYPE none + echo "[init] ERROR: PX4IO not found" + tone_alarm $TUNE_OUT_ERROR fi fi @@ -245,7 +247,7 @@ then sh /etc/init.d/rc.io else echo "[init] PX4IO start error" - tone_alarm MNGGG + tone_alarm $TUNE_OUT_ERROR fi fi if [ $OUTPUT_MODE == fmu ] @@ -256,7 +258,7 @@ then echo "[init] FMU mode_$FMU_MODE started" else echo "[init] FMU mode_$FMU_MODE start error" - tone_alarm MNGGG + tone_alarm $TUNE_OUT_ERROR fi if hw_ver compare PX4FMU_V1 @@ -289,7 +291,7 @@ then echo "[init] MKBLCTRL started" else echo "[init] MKBLCTRL start error" - tone_alarm MNGGG + tone_alarm $TUNE_OUT_ERROR fi fi @@ -301,7 +303,7 @@ then echo "[init] HIL output started" else echo "[init] HIL output error" - tone_alarm MNGGG + tone_alarm $TUNE_OUT_ERROR fi fi @@ -318,7 +320,7 @@ then sh /etc/init.d/rc.io else echo "[init] PX4IO start error" - tone_alarm MNGGG + tone_alarm $TUNE_OUT_ERROR fi fi else @@ -329,7 +331,7 @@ then echo "[init] FMU mode_$FMU_MODE started" else echo "[init] FMU mode_$FMU_MODE start error" - tone_alarm MNGGG + tone_alarm $TUNE_OUT_ERROR fi if hw_ver compare PX4FMU_V1 From d079074e801a6cb590f91053e2e6d96d5113c5c5 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 12 Jan 2014 18:49:43 +0100 Subject: [PATCH 015/108] update the skywalker x5 script --- ROMFS/px4fmu_common/init.d/3032_skywalker_x5 | 58 +++----------------- 1 file changed, 7 insertions(+), 51 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 index 41e0416548..bda05aeb1c 100644 --- a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 +++ b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 @@ -1,58 +1,14 @@ #!nsh - -echo "[init] PX4FMU v1, v2 with or without IO on Skywalker X5" - # -# Load default params for this platform +# Skywalker X5 Flying Wing # -if param compare SYS_AUTOCONFIG 1 +# Maintainers: Thomas Gubler , Julian Oes +# + +if [ $DO_AUTOCONFIG == yes ] then - # Set all params here, then disable autoconfig # TODO - - param set SYS_AUTOCONFIG 0 - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing -# -param set MAV_TYPE 1 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - - sh /etc/init.d/rc.io - # Limit to 100 Hz updates and (implicit) 50 Hz PWM - px4io limit 100 -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes fi -# -# Load mixer and start controllers (depends on px4io) -# -if [ -f /fs/microsd/etc/mixers/FMU_Q.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix -else - echo "Using /etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix -fi - -# -# Start common fixedwing apps -# -sh /etc/init.d/rc.fixedwing +set VEHICLE_TYPE fw +set FRAME_GEOMETRY Q From 449ed058c3554d2317e25395a2f5ebdb21816294 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 12 Jan 2014 18:55:41 +0100 Subject: [PATCH 016/108] rc.fw_interface: use mixer file from sd if it exists --- ROMFS/px4fmu_common/init.d/rc.fw_interface | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_interface b/ROMFS/px4fmu_common/init.d/rc.fw_interface index 69f3ed7f7d..1bf7a0e030 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_interface +++ b/ROMFS/px4fmu_common/init.d/rc.fw_interface @@ -13,7 +13,17 @@ param set MAV_TYPE 1 # Load mixer # echo "[init] Frame geometry: $FRAME_GEOMETRY" -set MIXER /etc/mixers/FMU_$FRAME_GEOMETRY.mix +set MIXERSD /fs/microsd/etc/mixers/FMU_$FRAME_GEOMETRY.mix + +#Use the mixer file from the sd-card if it exists +if [ -f $MIXERSD ] +then + set MIXER MIXERSD +else + set MIXER /etc/mixers/FMU_$FRAME_GEOMETRY.mix +fi + + if mixer load /dev/pwm_output $MIXER then From 1008d0c38320544579eb449d54c053e14568585e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 13 Jan 2014 08:03:38 +0100 Subject: [PATCH 017/108] fix small error in rc.fw_interface --- ROMFS/px4fmu_common/init.d/rc.fw_interface | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_interface b/ROMFS/px4fmu_common/init.d/rc.fw_interface index 1bf7a0e030..133b65218a 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_interface +++ b/ROMFS/px4fmu_common/init.d/rc.fw_interface @@ -18,7 +18,7 @@ set MIXERSD /fs/microsd/etc/mixers/FMU_$FRAME_GEOMETRY.mix #Use the mixer file from the sd-card if it exists if [ -f $MIXERSD ] then - set MIXER MIXERSD + set MIXER $MIXERSD else set MIXER /etc/mixers/FMU_$FRAME_GEOMETRY.mix fi From d1b2186806e0b9e32808a04f6c85d26a703c596e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 14 Jan 2014 21:59:48 +0100 Subject: [PATCH 018/108] Autostart 3DR Iris updated --- ROMFS/px4fmu_common/init.d/10016_3dr_iris | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris index b8fc5e606f..6740e2d94c 100644 --- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris @@ -38,9 +38,16 @@ then param set MPC_Z_VEL_I 0.1 param set MPC_Z_VEL_MAX 2 param set MPC_Z_VEL_P 0.20 + + param set BAT_V_SCALING 0.00989 + param set BAT_C_SCALING 0.0124 fi set VEHICLE_TYPE mc set FRAME_GEOMETRY quad_w set PWM_RATE 400 + +set PWM_DISARMED 900 +set PWM_MIN 1000 +set PWM_MAX 2000 From a8d362de13b23a2523dc69d582c68fe672ac236d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 15 Jan 2014 00:02:57 +0100 Subject: [PATCH 019/108] Autostart: use MIXER instead of FRAME_GEOMETRY --- .../px4fmu_common/init.d/10015_tbs_discovery | 3 +- ROMFS/px4fmu_common/init.d/10016_3dr_iris | 4 +- ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil | 2 +- ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil | 2 +- ROMFS/px4fmu_common/init.d/2100_mpx_easystar | 2 +- ROMFS/px4fmu_common/init.d/3031_phantom | 2 +- ROMFS/px4fmu_common/init.d/3032_skywalker_x5 | 2 +- ROMFS/px4fmu_common/init.d/3033_wingwing | 2 +- ROMFS/px4fmu_common/init.d/3034_fx79 | 2 +- ROMFS/px4fmu_common/init.d/4010_dji_f330 | 4 +- ROMFS/px4fmu_common/init.d/4011_dji_f450 | 4 +- ROMFS/px4fmu_common/init.d/4012_hk_x550 | 3 +- ROMFS/px4fmu_common/init.d/rc.fw_interface | 34 ----- ROMFS/px4fmu_common/init.d/rc.interface | 72 ++++++++++ ROMFS/px4fmu_common/init.d/rc.io | 10 +- ROMFS/px4fmu_common/init.d/rc.mc_interface | 77 ---------- ROMFS/px4fmu_common/init.d/rc.sensors | 21 +-- ROMFS/px4fmu_common/init.d/rcS | 131 ++++++++++++------ 18 files changed, 195 insertions(+), 182 deletions(-) delete mode 100644 ROMFS/px4fmu_common/init.d/rc.fw_interface create mode 100644 ROMFS/px4fmu_common/init.d/rc.interface delete mode 100644 ROMFS/px4fmu_common/init.d/rc.mc_interface diff --git a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery index 53140caff3..63798bb3c0 100644 --- a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery @@ -25,6 +25,7 @@ then fi set VEHICLE_TYPE mc -set FRAME_GEOMETRY quad_w +set MIXER FMU_quad_w +set PWM_OUTPUTS 1234 set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris index 6740e2d94c..67c24fab37 100644 --- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris @@ -44,10 +44,10 @@ then fi set VEHICLE_TYPE mc -set FRAME_GEOMETRY quad_w +set MIXER FMU_quad_w +set PWM_OUTPUTS 1234 set PWM_RATE 400 - set PWM_DISARMED 900 set PWM_MIN 1000 set PWM_MAX 2000 diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil index c5b92d7d4d..8c0797d7cb 100644 --- a/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil +++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil @@ -43,4 +43,4 @@ fi set HIL yes set VEHICLE_TYPE mc -set FRAME_GEOMETRY quad_x +set MIXER FMU_quad_x diff --git a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil index 5ec70043a0..bce3015fc7 100644 --- a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil +++ b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil @@ -43,4 +43,4 @@ fi set HIL yes set VEHICLE_TYPE mc -set FRAME_GEOMETRY quad_+ +set MIXER FMU_quad_+ diff --git a/ROMFS/px4fmu_common/init.d/2100_mpx_easystar b/ROMFS/px4fmu_common/init.d/2100_mpx_easystar index 43f911a781..0e5bf60d60 100644 --- a/ROMFS/px4fmu_common/init.d/2100_mpx_easystar +++ b/ROMFS/px4fmu_common/init.d/2100_mpx_easystar @@ -36,4 +36,4 @@ then fi set VEHICLE_TYPE fw -set FRAME_GEOMETRY RET +set MIXER FMU_RET diff --git a/ROMFS/px4fmu_common/init.d/3031_phantom b/ROMFS/px4fmu_common/init.d/3031_phantom index 29af48ec65..4ebbe9c61e 100644 --- a/ROMFS/px4fmu_common/init.d/3031_phantom +++ b/ROMFS/px4fmu_common/init.d/3031_phantom @@ -41,4 +41,4 @@ then fi set VEHICLE_TYPE fw -set FRAME_GEOMETRY Q +set MIXER FMU_Q diff --git a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 index bda05aeb1c..03f2822375 100644 --- a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 +++ b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 @@ -11,4 +11,4 @@ then fi set VEHICLE_TYPE fw -set FRAME_GEOMETRY Q +set MIXER FMU_Q diff --git a/ROMFS/px4fmu_common/init.d/3033_wingwing b/ROMFS/px4fmu_common/init.d/3033_wingwing index e0340a8d9a..e537632784 100644 --- a/ROMFS/px4fmu_common/init.d/3033_wingwing +++ b/ROMFS/px4fmu_common/init.d/3033_wingwing @@ -40,4 +40,4 @@ then fi set VEHICLE_TYPE fw -set FRAME_GEOMETRY Q +set MIXER FMU_Q diff --git a/ROMFS/px4fmu_common/init.d/3034_fx79 b/ROMFS/px4fmu_common/init.d/3034_fx79 index c4dab7ba66..8d179d1fd4 100644 --- a/ROMFS/px4fmu_common/init.d/3034_fx79 +++ b/ROMFS/px4fmu_common/init.d/3034_fx79 @@ -40,4 +40,4 @@ then fi set VEHICLE_TYPE fw -set FRAME_GEOMETRY Q +set MIXER FMU_FX79 diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330 index 94afe91ae9..ab1db94d0a 100644 --- a/ROMFS/px4fmu_common/init.d/4010_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330 @@ -41,10 +41,10 @@ then fi set VEHICLE_TYPE mc -set FRAME_GEOMETRY quad_x +set MIXER FMU_quad_x +set PWM_OUTPUTS 1234 set PWM_RATE 400 - # DJI ESC range set PWM_DISARMED 900 set PWM_MIN 1200 diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450 index 21b3088d32..299771c1d9 100644 --- a/ROMFS/px4fmu_common/init.d/4011_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450 @@ -27,10 +27,10 @@ then fi set VEHICLE_TYPE mc -set FRAME_GEOMETRY quad_x +set MIXER FMU_quad_x +set PWM_OUTPUTS 1234 set PWM_RATE 400 - # DJI ESC range set PWM_DISARMED 900 set PWM_MIN 1200 diff --git a/ROMFS/px4fmu_common/init.d/4012_hk_x550 b/ROMFS/px4fmu_common/init.d/4012_hk_x550 index 27f73471d9..7e020cf592 100644 --- a/ROMFS/px4fmu_common/init.d/4012_hk_x550 +++ b/ROMFS/px4fmu_common/init.d/4012_hk_x550 @@ -27,6 +27,7 @@ then fi set VEHICLE_TYPE mc -set FRAME_GEOMETRY quad_x +set MIXER FMU_quad_x +set PWM_OUTPUTS 1234 set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_interface b/ROMFS/px4fmu_common/init.d/rc.fw_interface deleted file mode 100644 index 133b65218a..0000000000 --- a/ROMFS/px4fmu_common/init.d/rc.fw_interface +++ /dev/null @@ -1,34 +0,0 @@ -#!nsh -# -# Script to configure fixedwing control interface -# - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing -# -param set MAV_TYPE 1 - -# -# Load mixer -# -echo "[init] Frame geometry: $FRAME_GEOMETRY" -set MIXERSD /fs/microsd/etc/mixers/FMU_$FRAME_GEOMETRY.mix - -#Use the mixer file from the sd-card if it exists -if [ -f $MIXERSD ] -then - set MIXER $MIXERSD -else - set MIXER /etc/mixers/FMU_$FRAME_GEOMETRY.mix -fi - - - -if mixer load /dev/pwm_output $MIXER -then - echo "[init] Mixer loaded: $MIXER" -else - echo "[init] Error loading mixer: $MIXER" - tone_alarm $TUNE_OUT_ERROR -fi diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface new file mode 100644 index 0000000000..928d3aeebc --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -0,0 +1,72 @@ +#!nsh +# +# Script to configure control interface +# + +if [ $MIXER != none ] +then + # + # Load mixer + # + set MIXERSD /fs/microsd/etc/mixers/$MIXER.mix + + #Use the mixer file from the SD-card if it exists + if [ -f $MIXERSD ] + then + set MIXER_FILE $MIXERSD + else + set MIXER_FILE /etc/mixers/$MIXER.mix + fi + + if [ $OUTPUT_MODE == mkblctrl ] + then + set OUTPUT_DEV /dev/mkblctrl + else + set OUTPUT_DEV /dev/pwm_output + fi + + if mixer load $OUTPUT_DEV $MIXER_FILE + then + echo "[init] Mixer loaded: $MIXER_FILE" + else + echo "[init] Error loading mixer: $MIXER_FILE" + tone_alarm $TUNE_OUT_ERROR + fi +else + echo "[init] Mixer not defined + tone_alarm $TUNE_OUT_ERROR +fi + +if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == io ] +then + if [ $PWM_OUTPUTS != none ] + then + # + # Set PWM output frequency + # + if [ $PWM_RATE != none ] + then + echo "[init] Set PWM rate: $PWM_RATE" + pwm rate -c $PWM_OUTPUTS -r $PWM_RATE + fi + + # + # Set disarmed, min and max PWM values + # + if [ $PWM_DISARMED != none ] + then + echo "[init] Set PWM disarmed: $PWM_DISARMED" + pwm disarmed -c $PWM_OUTPUTS -p $PWM_DISARMED + fi + if [ $PWM_MIN != none ] + then + echo "[init] Set PWM min: $PWM_MIN" + pwm min -c $PWM_OUTPUTS -p $PWM_MIN + fi + if [ $PWM_MAX != none ] + then + echo "[init] Set PWM max: $PWM_MAX" + pwm max -c $PWM_OUTPUTS -p $PWM_MAX + fi + fi +fi diff --git a/ROMFS/px4fmu_common/init.d/rc.io b/ROMFS/px4fmu_common/init.d/rc.io index 5a010cc9b0..c9d964f8e2 100644 --- a/ROMFS/px4fmu_common/init.d/rc.io +++ b/ROMFS/px4fmu_common/init.d/rc.io @@ -9,11 +9,13 @@ px4io recovery # -# Adjust px4io topic limiting +# Adjust PX4IO update rate limit # +set PX4IO_LIMIT 400 if hw_ver compare PX4FMU_V1 then - px4io limit 200 -else - px4io limit 400 + set PX4IO_LIMIT 200 fi + +echo "[init] Set PX4IO update rate limit: $PX4IO_LIMIT Hz" +px4io limit $PX4IO_LIMIT diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_interface b/ROMFS/px4fmu_common/init.d/rc.mc_interface deleted file mode 100644 index 6e4e4ed314..0000000000 --- a/ROMFS/px4fmu_common/init.d/rc.mc_interface +++ /dev/null @@ -1,77 +0,0 @@ -#!nsh -# -# Script to configure multicopter control interface -# - -if [ $FRAME_GEOMETRY == quad_x -o $FRAME_GEOMETRY == quad_+ ] -then - set OUTPUTS 1234 - param set MAV_TYPE 2 -fi -if [ $FRAME_GEOMETRY == quad_w -o $FRAME_GEOMETRY == quad_v ] -then - set OUTPUTS 1234 - param set MAV_TYPE 2 -fi -if [ $FRAME_GEOMETRY == hex_x -o $FRAME_GEOMETRY == hex_+ ] -then - set OUTPUTS 123456 - param set MAV_TYPE 13 -fi -if [ $FRAME_GEOMETRY == octo_x -o $FRAME_GEOMETRY == octo_+ ] -then - set OUTPUTS 12345678 - param set MAV_TYPE 14 -fi - -# -# Load mixer -# -echo "[init] Frame geometry: $FRAME_GEOMETRY" -set MIXER /etc/mixers/FMU_$FRAME_GEOMETRY.mix - -if [ $OUTPUT_MODE == mkblctrl ] -then - set OUTPUT_DEV /dev/mkblctrl -else - set OUTPUT_DEV /dev/pwm_output -fi - -if mixer load $OUTPUT_DEV $MIXER -then - echo "[init] Mixer loaded: $MIXER" -else - echo "[init] Error loading mixer: $MIXER" - tone_alarm $TUNE_OUT_ERROR -fi - -if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == io ] -then - # - # Set PWM output frequency - # - if [ $PWM_RATE != none ] - then - echo "[init] Set PWM rate: $PWM_RATE" - pwm rate -c $OUTPUTS -r $PWM_RATE - fi - - # - # Set disarmed, min and max PWM values - # - if [ $PWM_DISARMED != none ] - then - echo "[init] Set PWM disarmed: $PWM_DISARMED" - pwm disarmed -c $OUTPUTS -p $PWM_DISARMED - fi - if [ $PWM_MIN != none ] - then - echo "[init] Set PWM min: $PWM_MIN" - pwm min -c $OUTPUTS -p $PWM_MIN - fi - if [ $PWM_MAX != none ] - then - echo "[init] Set PWM max: $PWM_MAX" - pwm max -c $OUTPUTS -p $PWM_MAX - fi -fi diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index a2517135f3..badbf92c3c 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -10,39 +10,42 @@ ms5611 start adc start -# mag might be external +# Mag might be external if hmc5883 start then - echo "using HMC5883" + echo "[init] Using HMC5883" fi if mpu6000 start then - echo "using MPU6000" + echo "[init] Using MPU6000" fi if l3gd20 start then - echo "using L3GD20(H)" + echo "[init] Using L3GD20(H)" fi -if lsm303d start +if hw_ver compare PX4FMU_V2 then - echo "using LSM303D" + if lsm303d start + then + echo "[init] Using LSM303D" + fi fi # Start airspeed sensors if meas_airspeed start then - echo "using MEAS airspeed sensor" + echo "[init] Using MEAS airspeed sensor" else if ets_airspeed start then - echo "using ETS airspeed sensor (bus 3)" + echo "[init] Using ETS airspeed sensor (bus 3)" else if ets_airspeed start -b 1 then - echo "Using ETS airspeed sensor (bus 1)" + echo "[init] Using ETS airspeed sensor (bus 1)" fi fi fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 441d99ecf7..92121ac176 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -14,6 +14,7 @@ set CONFIG_FILE /fs/microsd/etc/config.txt set EXTRAS_FILE /fs/microsd/etc/extras.txt set TUNE_OUT_ERROR ML<> $LOG_FILE tone_alarm MLL32CP8MB @@ -193,17 +193,17 @@ then usleep 500000 if px4io checkcrc $IO_FILE then - echo "[init] PX4IO CRC OK after updating" + echo "[init] PX4IO CRC OK, update successful" echo "PX4IO CRC OK after updating" >> $LOG_FILE tone_alarm MLL8CDE set IO_PRESENT yes else - echo "[init] PX4IO update failed" + echo "[init] ERROR: PX4IO update failed" echo "PX4IO update failed" >> $LOG_FILE fi else - echo "[init] PX4IO update failed" + echo "[init] ERROR: PX4IO update failed" echo "PX4IO update failed" >> $LOG_FILE fi fi @@ -220,16 +220,27 @@ then # if [ $OUTPUT_MODE == none ] then - if [ $IO_PRESENT == yes ] + if [ $USE_IO == yes ] then - # If PX4IO present, use it as primary PWM output by default set OUTPUT_MODE io else - # Else use PX4FMU PWM output set OUTPUT_MODE fmu fi fi + if [ $OUTPUT_MODE == io -a $IO_PRESENT != yes ] + then + # Need IO for output but it not present, disable output + set OUTPUT_MODE none + echo "[init] ERROR: PX4IO not found, disabling output" + + # Avoid using ttyS0 for MAVLink on FMUv1 + if hw_ver compare PX4FMU_V1 + then + set FMU_MODE serial + fi + fi + if [ $HIL == yes ] then set OUTPUT_MODE hil @@ -256,7 +267,7 @@ then echo "[init] PX4IO started" sh /etc/init.d/rc.io else - echo "[init] PX4IO start error" + echo "[init] ERROR: PX4IO start failed" tone_alarm $TUNE_OUT_ERROR fi fi @@ -267,7 +278,7 @@ then then echo "[init] FMU mode_$FMU_MODE started" else - echo "[init] FMU mode_$FMU_MODE start error" + echo "[init] ERROR: FMU mode_$FMU_MODE start failed" tone_alarm $TUNE_OUT_ERROR fi @@ -300,7 +311,7 @@ then then echo "[init] MKBLCTRL started" else - echo "[init] MKBLCTRL start error" + echo "[init] ERROR: MKBLCTRL start failed" tone_alarm $TUNE_OUT_ERROR fi @@ -312,7 +323,7 @@ then then echo "[init] HIL output started" else - echo "[init] HIL output error" + echo "[init] ERROR: HIL output start failed" tone_alarm $TUNE_OUT_ERROR fi fi @@ -329,7 +340,7 @@ then echo "[init] PX4IO started" sh /etc/init.d/rc.io else - echo "[init] PX4IO start error" + echo "[init] ERROR: PX4IO start failed" tone_alarm $TUNE_OUT_ERROR fi fi @@ -340,13 +351,17 @@ then then echo "[init] FMU mode_$FMU_MODE started" else - echo "[init] FMU mode_$FMU_MODE start error" + echo "[init] ERROR: FMU mode_$FMU_MODE start failed" tone_alarm $TUNE_OUT_ERROR fi if hw_ver compare PX4FMU_V1 then - if [ $FMU_MODE == pwm -o $FMU_MODE == gpio -o $FMU_MODE == pwm_gpio ] + if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ] + then + set TTYS1_BUSY yes + fi + if [ $FMU_MODE == pwm_gpio ] then set TTYS1_BUSY yes fi @@ -401,14 +416,22 @@ then then echo "[init] Vehicle type: FIXED WING" - if [ $FRAME_GEOMETRY == none ] + if [ $MIXER == none ] then - # Set default frame geometry for fixed wing - set FRAME_GEOMETRY AERT + # Set default mixer for fixed wing if not defined + set MIXER FMU_AERT fi + if [ $MAV_TYPE == none ] + then + # Use MAV_TYPE = 1 (fixed wing) if not defined + set MAV_TYPE 1 + fi + + param set MAV_TYPE $MAV_TYPE + # Load mixer and configure outputs - sh /etc/init.d/rc.fw_interface + sh /etc/init.d/rc.interface # Start standard fixedwing apps sh /etc/init.d/rc.fw_apps @@ -420,15 +443,37 @@ then if [ $VEHICLE_TYPE == mc ] then echo "[init] Vehicle type: MULTICOPTER" - - if [ $FRAME_GEOMETRY == none ] + + if [ $MIXER == none ] then - # Set default frame geometry for multicopter - set FRAME_GEOMETRY quad_x + # Set default mixer for multicopter if not defined + set MIXER quad_x fi + + if [ $MAV_TYPE == none ] + then + # Use MAV_TYPE = 2 (quadcopter) if not defined + set MAV_TYPE 2 + + # Use mixer to detect vehicle type + if [ $MIXER == FMU_hex_x -o $MIXER == FMU_hex_+ ] + then + param set MAV_TYPE 13 + fi + if [ $MIXER == FMU_octo_x -o $MIXER == FMU_octo_+ ] + then + param set MAV_TYPE 14 + fi + if [ $MIXER == FMU_octo_cox ] + then + param set MAV_TYPE 14 + fi + fi + + param set MAV_TYPE $MAV_TYPE # Load mixer and configure outputs - sh /etc/init.d/rc.mc_interface + sh /etc/init.d/rc.interface # Start standard multicopter apps sh /etc/init.d/rc.mc_apps @@ -440,9 +485,9 @@ then if [ $VEHICLE_TYPE == none ] then echo "[init] Vehicle type: GENERIC" - - attitude_estimator_ekf start - position_estimator_inav start + + # Load mixer and configure outputs + sh /etc/init.d/rc.interface fi # Start any custom addons From a0bb6674da5c0ca9c23f1db91bc9506c75242398 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 16 Jan 2014 19:03:14 +0100 Subject: [PATCH 020/108] Fix FMUs B/E led pin config --- src/drivers/boards/px4fmu-v2/board_config.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index 264d911f32..7cfca7656e 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -75,7 +75,7 @@ __BEGIN_DECLS /* PX4FMU GPIOs ***********************************************************************************/ /* LEDs */ -#define GPIO_LED1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN12) +#define GPIO_LED1 (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN12) /* External interrupts */ #define GPIO_EXTI_GYRO_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN0) From 2600c96e92de4ce8123b20210176456ec7e5332d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 16 Jan 2014 20:13:17 +0100 Subject: [PATCH 021/108] Configuring PX4IOv1 led pins --- src/drivers/boards/px4io-v1/board_config.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/drivers/boards/px4io-v1/board_config.h b/src/drivers/boards/px4io-v1/board_config.h index c3f39addfe..1be4877bad 100644 --- a/src/drivers/boards/px4io-v1/board_config.h +++ b/src/drivers/boards/px4io-v1/board_config.h @@ -58,11 +58,11 @@ /* PX4IO GPIOs **********************************************************************/ /* LEDs */ -#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ +#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|\ GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN14) -#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ +#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|\ GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN15) -#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ +#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|\ GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10) /* Safety switch button *************************************************************/ From 71b11d54e0cc441dabed878db270881a6308a7c7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 16 Jan 2014 20:13:35 +0100 Subject: [PATCH 022/108] Configuring PX4IOv2 led pins --- src/drivers/boards/px4io-v2/board_config.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/drivers/boards/px4io-v2/board_config.h b/src/drivers/boards/px4io-v2/board_config.h index 8da5552110..8152ea4d4b 100644 --- a/src/drivers/boards/px4io-v2/board_config.h +++ b/src/drivers/boards/px4io-v2/board_config.h @@ -74,9 +74,9 @@ /* LEDS **********************************************************************/ -#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14) -#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15) -#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13) +#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14) +#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15) +#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13) /* Safety switch button *******************************************************/ From e691bab71a69216273fdc253695ef849671af9a7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 16 Jan 2014 22:46:55 +0100 Subject: [PATCH 023/108] Cleaned up test output to be more readable --- src/systemcmds/tests/test_mtd.c | 35 +++++++++++++++++++++++++++------ 1 file changed, 29 insertions(+), 6 deletions(-) diff --git a/src/systemcmds/tests/test_mtd.c b/src/systemcmds/tests/test_mtd.c index 8a626b65cc..bac9efbdb3 100644 --- a/src/systemcmds/tests/test_mtd.c +++ b/src/systemcmds/tests/test_mtd.c @@ -90,6 +90,16 @@ int check_user_abort(int fd) { return 1; } +void print_fail() +{ + printf("<[T]: MTD: FAIL>\n"); +} + +void print_success() +{ + printf("<[T]: MTD: OK>\n"); +} + int test_mtd(int argc, char *argv[]) { @@ -99,6 +109,7 @@ test_mtd(int argc, char *argv[]) struct stat buffer; if (stat(PARAM_FILE_NAME, &buffer)) { warnx("file %s not found, aborting MTD test", PARAM_FILE_NAME); + print_fail(); return 1; } @@ -123,9 +134,17 @@ test_mtd(int argc, char *argv[]) uint8_t read_buf[chunk_sizes[c]] __attribute__((aligned(64))); hrt_abstime start, end; - int fd = open(PARAM_FILE_NAME, O_WRONLY); + int fd = open(PARAM_FILE_NAME, O_RDONLY); + int rret = read(fd, read_buf, chunk_sizes[c]); + close(fd); - warnx("testing unaligned writes - please wait.."); + fd = open(PARAM_FILE_NAME, O_WRONLY); + + printf("printing 2 percent of the first chunk:\n"); + for (int i = 0; i < sizeof(read_buf) / 50; i++) { + printf("%02X", read_buf[i]); + } + printf("\n"); iterations = file_size / chunk_sizes[c]; @@ -133,9 +152,9 @@ test_mtd(int argc, char *argv[]) for (unsigned i = 0; i < iterations; i++) { int wret = write(fd, write_buf, chunk_sizes[c]); - if (wret != chunk_sizes[c]) { + if (wret != (int)chunk_sizes[c]) { warn("WRITE ERROR!"); - + print_fail(); return 1; } @@ -156,6 +175,7 @@ test_mtd(int argc, char *argv[]) if (rret != chunk_sizes[c]) { warnx("READ ERROR!"); + print_fail(); return 1; } @@ -165,6 +185,7 @@ test_mtd(int argc, char *argv[]) for (int j = 0; j < chunk_sizes[c]; j++) { if (read_buf[j] != write_buf[j]) { warnx("COMPARISON ERROR: byte %d", j); + print_fail(); compare_ok = false; break; } @@ -172,6 +193,7 @@ test_mtd(int argc, char *argv[]) if (!compare_ok) { warnx("ABORTING FURTHER COMPARISON DUE TO ERROR"); + print_fail(); return 1; } @@ -183,7 +205,6 @@ test_mtd(int argc, char *argv[]) close(fd); - printf("RESULT: OK! No readback errors.\n\n"); } /* fill the file with 0xFF to make it look new again */ @@ -193,14 +214,16 @@ test_mtd(int argc, char *argv[]) for (int i = 0; i < file_size / sizeof(ffbuf); i++) { int ret = write(fd, ffbuf, sizeof(ffbuf)); - if (ret) { + if (ret != sizeof(ffbuf)) { warnx("ERROR! Could not fill file with 0xFF"); close(fd); + print_fail(); return 1; } } (void)close(fd); + print_success(); return 0; } From 0f30db08c0d44e753005b2a40fef8900ed5dba33 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 17 Jan 2014 15:44:03 +0100 Subject: [PATCH 024/108] Small documentation correction --- src/systemcmds/tests/tests.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h index 223edc14a9..82de05dffb 100644 --- a/src/systemcmds/tests/tests.h +++ b/src/systemcmds/tests/tests.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2014 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 From c94eb3198aa37e57643fb3373d14ecef46807935 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 17 Jan 2014 23:15:23 +0100 Subject: [PATCH 025/108] rcS: use mtd on FMUv1 too --- ROMFS/px4fmu_common/init.d/rcS | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 92121ac176..32c7991efb 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -75,12 +75,9 @@ then # Load parameters # set PARAM_FILE /fs/microsd/params - if hw_ver compare PX4FMU_V2 + if mtd start then - if mtd start - then - set PARAM_FILE /fs/mtd_params - fi + set PARAM_FILE /fs/mtd_params fi param select $PARAM_FILE From 5db68264c7b1240811d28d04149e4a49891ab423 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 18 Jan 2014 00:07:25 +0100 Subject: [PATCH 026/108] rcS: HIL fixed --- ROMFS/px4fmu_common/init.d/rcS | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 32c7991efb..6325bb94f6 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -241,6 +241,7 @@ then if [ $HIL == yes ] then set OUTPUT_MODE hil + set FMU_MODE serial else # Try to get an USB console if not in HIL mode nshterm /dev/ttyACM0 & @@ -373,6 +374,7 @@ then if [ $HIL == yes ] then + sleep 1 mavlink start -b 230400 -d /dev/ttyACM0 usleep 5000 else From 4d7e99fd6c47cb94d63a118d5557eefbe6df8f2e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 18 Jan 2014 01:09:58 +0100 Subject: [PATCH 027/108] Writing RSSI field not only in servo rail topic --- src/drivers/px4io/px4io.cpp | 20 +++++++++++++------- 1 file changed, 13 insertions(+), 7 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 0ca35d2f29..7cc7d3b6d1 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -270,7 +270,8 @@ private: orb_advert_t _to_servorail; ///< servorail status orb_advert_t _to_safety; ///< status of safety - actuator_outputs_s _outputs; /// 0) { - orb_publish(ORB_ID(servorail_status), _to_servorail, &servorail_status); + orb_publish(ORB_ID(servorail_status), _to_servorail, &_servorail_status); } else { - _to_servorail = orb_advertise(ORB_ID(servorail_status), &servorail_status); + _to_servorail = orb_advertise(ORB_ID(servorail_status), &_servorail_status); } } @@ -1450,6 +1451,11 @@ PX4IO::io_publish_raw_rc() rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN; } + /* set RSSI */ + + // XXX the correct scaling needs to be validated here + rc_val.rssi = _servorail_status.rssi_v / 3.3f; + /* lazily advertise on first publication */ if (_to_input_rc == 0) { _to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_val); From dd9df7b1b0974a9838d3e21842a0d90f3eff54d9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 18 Jan 2014 01:11:36 +0100 Subject: [PATCH 028/108] RSSI field init --- src/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 7cc7d3b6d1..dede5976da 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1454,7 +1454,7 @@ PX4IO::io_publish_raw_rc() /* set RSSI */ // XXX the correct scaling needs to be validated here - rc_val.rssi = _servorail_status.rssi_v / 3.3f; + rc_val.rssi = (_servorail_status.rssi_v / 3.3f) * UINT8_MAX; /* lazily advertise on first publication */ if (_to_input_rc == 0) { From d174998b45349348ffe41150aa1d22d7d943b790 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 18 Jan 2014 12:00:17 +0100 Subject: [PATCH 029/108] RSSI and concurrent S.Bus output handling --- src/drivers/px4io/px4io.cpp | 6 ++++-- src/modules/px4iofirmware/controls.c | 9 +++++++++ src/modules/px4iofirmware/px4io.c | 5 +++++ src/modules/px4iofirmware/px4io.h | 1 + 4 files changed, 19 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index dede5976da..8e080c043b 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1453,8 +1453,10 @@ PX4IO::io_publish_raw_rc() /* set RSSI */ - // XXX the correct scaling needs to be validated here - rc_val.rssi = (_servorail_status.rssi_v / 3.3f) * UINT8_MAX; + if (rc_val.input_source != RC_INPUT_SOURCE_PX4IO_SBUS) { + // XXX the correct scaling needs to be validated here + rc_val.rssi = (_servorail_status.rssi_v / 3.3f) * UINT8_MAX; + } /* lazily advertise on first publication */ if (_to_input_rc == 0) { diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 541eed0e1c..5859f768bf 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -114,9 +114,18 @@ controls_tick() { perf_begin(c_gather_sbus); bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &rssi, PX4IO_RC_INPUT_CHANNELS); + + bool sbus_status = (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS); + if (sbus_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS; } + + /* switch S.Bus output pin as needed */ + if (sbus_status != (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS)) { + ENABLE_SBUS_OUT((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS)); + } + perf_end(c_gather_sbus); /* diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index 0b8c4a6a8b..150af35df1 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -196,6 +196,11 @@ user_start(int argc, char *argv[]) POWER_SERVO(true); #endif + /* turn off S.Bus out (if supported) */ +#ifdef ENABLE_SBUS_OUT + ENABLE_SBUS_OUT(false); +#endif + /* start the safety switch handler */ safety_init(); diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index a0daa97ea0..18c7468f69 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -160,6 +160,7 @@ extern pwm_limit_t pwm_limit; # define PX4IO_RELAY_CHANNELS 0 # define POWER_SPEKTRUM(_s) stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_s)) +# define ENABLE_SBUS_OUT(_s) stm32_gpiowrite(GPIO_SBUS_OENABLE, (_s)) # define VDD_SERVO_FAULT (!stm32_gpioread(GPIO_SERVO_FAULT_DETECT)) From bb8cf0894f01e9889a261d4cd62798048c0cf7f5 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 18 Jan 2014 16:47:23 +0100 Subject: [PATCH 030/108] autostart: HIL and bad PX4IO fixes --- ROMFS/px4fmu_common/init.d/rcS | 165 +++++++++++++++++---------------- 1 file changed, 86 insertions(+), 79 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 6325bb94f6..a9c5c59ea9 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -241,7 +241,10 @@ then if [ $HIL == yes ] then set OUTPUT_MODE hil - set FMU_MODE serial + if hw_ver compare PX4FMU_V1 + then + set FMU_MODE serial + fi else # Try to get an USB console if not in HIL mode nshterm /dev/ttyACM0 & @@ -257,82 +260,12 @@ then # set TTYS1_BUSY no - if [ $OUTPUT_MODE == io ] + # If OUTPUT_MODE == none then something is wrong with setup and we shouldn't try to enable output + if [ $OUTPUT_MODE != none ] then - echo "[init] Use PX4IO PWM as primary output" - if px4io start - then - echo "[init] PX4IO started" - sh /etc/init.d/rc.io - else - echo "[init] ERROR: PX4IO start failed" - tone_alarm $TUNE_OUT_ERROR - fi - fi - if [ $OUTPUT_MODE == fmu ] - then - echo "[init] Use FMU PWM as primary output" - if fmu mode_$FMU_MODE - then - echo "[init] FMU mode_$FMU_MODE started" - else - echo "[init] ERROR: FMU mode_$FMU_MODE start failed" - tone_alarm $TUNE_OUT_ERROR - fi - - if hw_ver compare PX4FMU_V1 - then - if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ] - then - set TTYS1_BUSY yes - fi - if [ $FMU_MODE == pwm_gpio ] - then - set TTYS1_BUSY yes - fi - fi - fi - if [ $OUTPUT_MODE == mkblctrl ] - then - echo "[init] Use MKBLCTRL as primary output" - set MKBLCTRL_ARG "" - if [ $MKBLCTRL_MODE == x ] - then - set MKBLCTRL_ARG "-mkmode x" - fi - if [ $MKBLCTRL_MODE == + ] - then - set MKBLCTRL_ARG "-mkmode +" - fi - - if mkblctrl $MKBLCTRL_ARG - then - echo "[init] MKBLCTRL started" - else - echo "[init] ERROR: MKBLCTRL start failed" - tone_alarm $TUNE_OUT_ERROR - fi - - fi - if [ $OUTPUT_MODE == hil ] - then - echo "[init] Use HIL as primary output" - if hil mode_pwm - then - echo "[init] HIL output started" - else - echo "[init] ERROR: HIL output start failed" - tone_alarm $TUNE_OUT_ERROR - fi - fi - - # - # Start IO or FMU for RC PPM input if needed - # - if [ $IO_PRESENT == yes ] - then - if [ $OUTPUT_MODE != io ] + if [ $OUTPUT_MODE == io ] then + echo "[init] Use PX4IO PWM as primary output" if px4io start then echo "[init] PX4IO started" @@ -342,9 +275,9 @@ then tone_alarm $TUNE_OUT_ERROR fi fi - else - if [ $OUTPUT_MODE != fmu ] + if [ $OUTPUT_MODE == fmu ] then + echo "[init] Use FMU PWM as primary output" if fmu mode_$FMU_MODE then echo "[init] FMU mode_$FMU_MODE started" @@ -352,7 +285,7 @@ then echo "[init] ERROR: FMU mode_$FMU_MODE start failed" tone_alarm $TUNE_OUT_ERROR fi - + if hw_ver compare PX4FMU_V1 then if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ] @@ -365,8 +298,82 @@ then fi fi fi + if [ $OUTPUT_MODE == mkblctrl ] + then + echo "[init] Use MKBLCTRL as primary output" + set MKBLCTRL_ARG "" + if [ $MKBLCTRL_MODE == x ] + then + set MKBLCTRL_ARG "-mkmode x" + fi + if [ $MKBLCTRL_MODE == + ] + then + set MKBLCTRL_ARG "-mkmode +" + fi + + if mkblctrl $MKBLCTRL_ARG + then + echo "[init] MKBLCTRL started" + else + echo "[init] ERROR: MKBLCTRL start failed" + tone_alarm $TUNE_OUT_ERROR + fi + + fi + if [ $OUTPUT_MODE == hil ] + then + echo "[init] Use HIL as primary output" + if hil mode_pwm + then + echo "[init] HIL output started" + else + echo "[init] ERROR: HIL output start failed" + tone_alarm $TUNE_OUT_ERROR + fi + fi + + # + # Start IO or FMU for RC PPM input if needed + # + if [ $IO_PRESENT == yes ] + then + if [ $OUTPUT_MODE != io ] + then + if px4io start + then + echo "[init] PX4IO started" + sh /etc/init.d/rc.io + else + echo "[init] ERROR: PX4IO start failed" + tone_alarm $TUNE_OUT_ERROR + fi + fi + else + if [ $OUTPUT_MODE != fmu ] + then + if fmu mode_$FMU_MODE + then + echo "[init] FMU mode_$FMU_MODE started" + else + echo "[init] ERROR: FMU mode_$FMU_MODE start failed" + tone_alarm $TUNE_OUT_ERROR + fi + + if hw_ver compare PX4FMU_V1 + then + if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ] + then + set TTYS1_BUSY yes + fi + if [ $FMU_MODE == pwm_gpio ] + then + set TTYS1_BUSY yes + fi + fi + fi + fi fi - + # # MAVLink # From a9e288dfdd803cf43136a1c413f5fd86d9bd2794 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 18 Jan 2014 20:08:08 +0100 Subject: [PATCH 031/108] rc: bug fixed --- ROMFS/px4fmu_common/init.d/rc.interface | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index 928d3aeebc..d25f01dde0 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -33,7 +33,7 @@ then tone_alarm $TUNE_OUT_ERROR fi else - echo "[init] Mixer not defined + echo "[init] Mixer not defined" tone_alarm $TUNE_OUT_ERROR fi From d96e63960c9f951dac02f122ac1e067f7dd3316f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 19 Jan 2014 12:42:43 +0100 Subject: [PATCH 032/108] enable new autostart scripts for Rascal HIL and X5 --- .../init.d/1004_rc_fw_Rascal110.hil | 68 +++---------------- ROMFS/px4fmu_common/init.d/rc.autostart | 4 +- 2 files changed, 10 insertions(+), 62 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil index 344d784224..75a00a675c 100644 --- a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil +++ b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil @@ -1,14 +1,13 @@ #!nsh # -# USB HIL start +# HIL Rascal 110 (Flightgear) +# +# Maintainers: Thomas Gubler # -echo "[HIL] HILStar starting.." +echo "HIL Rascal 110 starting.." -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 +if [ $DO_AUTOCONFIG == yes ] then # Set all params here, then disable autoconfig @@ -40,59 +39,8 @@ then param save fi -# Allow USB some time to come up -sleep 1 -# Tell MAVLink that this link is "fast" -mavlink start -b 230400 -d /dev/ttyACM0 +set HIL yes -# Create a fake HIL /dev/pwm_output interface -hil mode_pwm - -# -# 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 we got an IO -# -if px4io start -then - echo "IO started" -else - fmu mode_serial - echo "FMU started" -fi - -# -# Start the sensors (depends on orb, px4io) -# -sh /etc/init.d/rc.sensors - -# -# Start the attitude estimator (depends on orb) -# -att_pos_estimator_ekf start - -# -# Load mixer and start controllers (depends on px4io) -# -set MODE autostart -mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix -if [ -f /fs/microsd/etc/mixers/FMU_AERT.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_AERT.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_AERT.mix -else - echo "Using /etc/mixers/FMU_AERT.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix -fi - - -fw_pos_control_l1 start -fw_att_control start - -echo "[HIL] setup done, running" +set VEHICLE_TYPE fw +set MIXER FMU_AERT diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 69e88fcd09..325520dd07 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -43,7 +43,7 @@ fi if param compare SYS_AUTOSTART 1004 then - #sh /etc/init.d/1004_rc_fw_Rascal110.hil + sh /etc/init.d/1004_rc_fw_Rascal110.hil fi # @@ -84,7 +84,7 @@ fi if param compare SYS_AUTOSTART 3032 then - #sh /etc/init.d/3032_skywalker_x5 + sh /etc/init.d/3032_skywalker_x5 fi if param compare SYS_AUTOSTART 3033 From 40a0ac5736c35eeecb7e73050e5c685bf3195361 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 19 Jan 2014 20:59:15 +0100 Subject: [PATCH 033/108] sdlog2: use GPS time for naming log dirs and files, some fixes --- src/modules/sdlog2/sdlog2.c | 232 ++++++++++++++++++++++-------------- 1 file changed, 145 insertions(+), 87 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 06b1eddaa1..e9d809834d 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -108,7 +108,7 @@ static bool main_thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int deamon_task; /**< Handle of deamon task / thread */ static bool logwriter_should_exit = false; /**< Logwriter thread exit flag */ -static const int MAX_NO_LOGFOLDER = 999; /**< Maximum number of log folders */ +static const int MAX_NO_LOGFOLDER = 999; /**< Maximum number of log dirs */ static const int MAX_NO_LOGFILE = 999; /**< Maximum number of log files */ static const int LOG_BUFFER_SIZE_DEFAULT = 8192; static const int MAX_WRITE_CHUNK = 512; @@ -122,14 +122,17 @@ struct logbuffer_s lb; static pthread_mutex_t logbuffer_mutex; static pthread_cond_t logbuffer_cond; -static char folder_path[64]; +static char log_dir[32]; /* statistics counters */ -static unsigned long log_bytes_written = 0; static uint64_t start_time = 0; +static unsigned long log_bytes_written = 0; static unsigned long log_msgs_written = 0; static unsigned long log_msgs_skipped = 0; +/* GPS time, used for log files naming */ +static uint64_t gps_time = 0; + /* current state of logging */ static bool logging_enabled = false; /* enable logging on start (-e option) */ @@ -138,11 +141,14 @@ static bool log_on_start = false; static bool log_when_armed = false; /* delay = 1 / rate (rate defined by -r option) */ static useconds_t sleep_delay = 0; +/* use date/time for naming directories and files (-t option) */ +static bool log_name_timestamp = false; /* helper flag to track system state changes */ static bool flag_system_armed = false; static pthread_t logwriter_pthread = 0; +static pthread_attr_t logwriter_attr; /** * Log buffer writing thread. Open and close file here. @@ -203,9 +209,9 @@ static void handle_command(struct vehicle_command_s *cmd); static void handle_status(struct vehicle_status_s *cmd); /** - * Create folder for current logging session. Store folder name in 'log_folder'. + * Create dir for current logging session. Store dir name in 'log_dir'. */ -static int create_logfolder(void); +static int create_logdir(void); /** * Select first free log file name and open it. @@ -218,11 +224,12 @@ sdlog2_usage(const char *reason) if (reason) fprintf(stderr, "%s\n", reason); - errx(1, "usage: sdlog2 {start|stop|status} [-r ] [-b ] -e -a\n" + errx(1, "usage: sdlog2 {start|stop|status} [-r ] [-b ] -e -a -t\n" "\t-r\tLog rate in Hz, 0 means unlimited rate\n" "\t-b\tLog buffer size in KiB, default is 8\n" "\t-e\tEnable logging by default (if not, can be started by command)\n" - "\t-a\tLog only when armed (can be still overriden by command)\n"); + "\t-a\tLog only when armed (can be still overriden by command)\n" + "\t-t\tUse date/time for naming log directories and files\n"); } /** @@ -280,82 +287,101 @@ int sdlog2_main(int argc, char *argv[]) exit(1); } -int create_logfolder() +int create_log_dir() { - /* make folder on sdcard */ - uint16_t folder_number = 1; // start with folder sess001 + /* create dir on sdcard if needed */ + uint16_t dir_number = 1; // start with dir sess001 int mkdir_ret; - /* look for the next folder that does not exist */ - while (folder_number <= MAX_NO_LOGFOLDER) { - /* set up folder path: e.g. /fs/microsd/sess001 */ - sprintf(folder_path, "%s/sess%03u", mountpoint, folder_number); - mkdir_ret = mkdir(folder_path, S_IRWXU | S_IRWXG | S_IRWXO); - /* the result is -1 if the folder exists */ + if (log_name_timestamp && gps_time != 0) { + /* use GPS date for log dir naming: e.g. /fs/microsd/2014-01-19 */ + time_t gps_time_sec = gps_time / 1000000; + struct tm t; + gmtime_r(&gps_time_sec, &t); + int n = snprintf(log_dir, sizeof(log_dir), "%s/", mountpoint); + strftime(log_dir + n, sizeof(log_dir) - n, "%Y-%m-%d", &t); + mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO); + if (mkdir_ret != 0 && errno != EEXIST) { + warn("failed creating new dir"); + return -1; + } + } else { + /* look for the next dir that does not exist */ + while (dir_number <= MAX_NO_LOGFOLDER) { + /* format log dir: e.g. /fs/microsd/sess001 */ + sprintf(log_dir, "%s/sess%03u", mountpoint, dir_number); + mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO); - if (mkdir_ret == 0) { - /* folder does not exist, success */ + if (mkdir_ret != 0) { + if (errno == EEXIST) { + /* dir exists already */ + dir_number++; + continue; + } else { + warn("failed creating new dir"); + return -1; + } + } + /* dir does not exist, success */ break; + } - } else if (mkdir_ret == -1) { - /* folder exists already */ - folder_number++; - continue; - - } else { - warn("failed creating new folder"); + if (dir_number >= MAX_NO_LOGFOLDER) { + /* we should not end up here, either we have more than MAX_NO_LOGFOLDER on the SD card, or another problem */ + warnx("all %d possible dirs exist already.", MAX_NO_LOGFOLDER); return -1; } } - if (folder_number >= MAX_NO_LOGFOLDER) { - /* we should not end up here, either we have more than MAX_NO_LOGFOLDER on the SD card, or another problem */ - warnx("all %d possible folders exist already.", MAX_NO_LOGFOLDER); - return -1; - } - + /* print logging path, important to find log file later */ + warnx("logging to dir: %s", log_dir); + mavlink_log_info(mavlink_fd, "[sdlog2] logging to dir: %s", log_dir); return 0; } int open_logfile() { - /* make folder on sdcard */ - uint16_t file_number = 1; // start with file log001 - /* string to hold the path to the log */ - char path_buf[64] = ""; + char log_file[64] = ""; - int fd = 0; + if (log_name_timestamp && gps_time != 0) { + /* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ + int n = snprintf(log_file, sizeof(log_file), "%s/", log_dir); + time_t gps_time_sec = gps_time / 1000000; + struct tm t; + gmtime_r(&gps_time_sec, &t); + n += strftime(log_file + n, sizeof(log_file) - n, "%H_%M_%S.bin", &t); + } else { + uint16_t file_number = 1; // start with file log001 + /* look for the next file that does not exist */ + while (file_number <= MAX_NO_LOGFILE) { + /* format log file path: e.g. /fs/microsd/sess001/log001.bin */ + snprintf(log_file, sizeof(log_file), "%s/log%03u.bin", log_dir, file_number); - /* look for the next file that does not exist */ - while (file_number <= MAX_NO_LOGFILE) { - /* set up file path: e.g. /fs/microsd/sess001/log001.bin */ - sprintf(path_buf, "%s/log%03u.bin", folder_path, file_number); - - if (file_exist(path_buf)) { + if (!file_exist(log_file)) { + break; + } file_number++; - continue; } - fd = open(path_buf, O_CREAT | O_WRONLY | O_DSYNC); - - if (fd == 0) { - warn("opening %s failed", path_buf); + if (file_number > MAX_NO_LOGFILE) { + /* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */ + warnx("all %d possible files exist already.", MAX_NO_LOGFILE); + return -1; } - - warnx("logging to: %s.", path_buf); - mavlink_log_info(mavlink_fd, "[sdlog2] log: %s", path_buf); - - return fd; } - if (file_number > MAX_NO_LOGFILE) { - /* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */ - warnx("all %d possible files exist already.", MAX_NO_LOGFILE); - return -1; + int fd = open(log_file, O_CREAT | O_WRONLY | O_DSYNC); + + if (fd < 0) { + warn("failed opening %s", log_file); + mavlink_log_info(mavlink_fd, "[sdlog2] failed opening %s", log_file); + } else { + warnx("logging to: %s", log_file); + mavlink_log_info(mavlink_fd, "[sdlog2] log: %s", log_file); } - return 0; + return fd; } static void *logwriter_thread(void *arg) @@ -363,10 +389,13 @@ static void *logwriter_thread(void *arg) /* set name */ prctl(PR_SET_NAME, "sdlog2_writer", 0); - struct logbuffer_s *logbuf = (struct logbuffer_s *)arg; - int log_fd = open_logfile(); + if (log_fd < 0) + return; + + struct logbuffer_s *logbuf = (struct logbuffer_s *)arg; + /* write log messages formats, version and parameters */ log_bytes_written += write_formats(log_fd); log_bytes_written += write_version(log_fd); @@ -443,7 +472,7 @@ static void *logwriter_thread(void *arg) fsync(log_fd); close(log_fd); - return OK; + return; } void sdlog2_start_log() @@ -451,6 +480,22 @@ void sdlog2_start_log() warnx("start logging."); mavlink_log_info(mavlink_fd, "[sdlog2] start logging"); + /* create log dir if needed */ + if (create_log_dir() != 0) { + mavlink_log_critical(mavlink_fd, "[sdlog2] error creating log dir"); + errx(1, "error creating log dir"); + } + + const char *converter_in = "/etc/logging/conv.zip"; + char *converter_out = malloc(120); + sprintf(converter_out, "%s/conv.zip", log_dir); + + if (file_copy(converter_in, converter_out)) { + warnx("unable to copy conversion scripts"); + } + + free(converter_out); + /* initialize statistics counter */ log_bytes_written = 0; start_time = hrt_absolute_time(); @@ -458,25 +503,23 @@ void sdlog2_start_log() log_msgs_skipped = 0; /* initialize log buffer emptying thread */ - pthread_attr_t receiveloop_attr; - pthread_attr_init(&receiveloop_attr); + pthread_attr_init(&logwriter_attr); struct sched_param param; /* low priority, as this is expensive disk I/O */ param.sched_priority = SCHED_PRIORITY_DEFAULT - 40; - (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); + (void)pthread_attr_setschedparam(&logwriter_attr, ¶m); - pthread_attr_setstacksize(&receiveloop_attr, 2048); + pthread_attr_setstacksize(&logwriter_attr, 2048); logwriter_should_exit = false; /* start log buffer emptying thread */ - if (0 != pthread_create(&logwriter_pthread, &receiveloop_attr, logwriter_thread, &lb)) { + if (0 != pthread_create(&logwriter_pthread, &logwriter_attr, logwriter_thread, &lb)) { errx(1, "error creating logwriter thread"); } logging_enabled = true; - // XXX we have to destroy the attr at some point } void sdlog2_stop_log() @@ -501,6 +544,7 @@ void sdlog2_stop_log() } logwriter_pthread = 0; + pthread_attr_destroy(&logwriter_attr); sdlog2_status(); } @@ -594,12 +638,19 @@ int sdlog2_thread_main(int argc, char *argv[]) /* log buffer size */ int log_buffer_size = LOG_BUFFER_SIZE_DEFAULT; + logging_enabled = false; + log_on_start = false; + log_when_armed = false; + log_name_timestamp = false; + + flag_system_armed = false; + /* work around some stupidity in task_create's argv handling */ argc -= 2; argv += 2; int ch; - while ((ch = getopt(argc, argv, "r:b:ea")) != EOF) { + while ((ch = getopt(argc, argv, "r:b:eat")) != EOF) { switch (ch) { case 'r': { unsigned long r = strtoul(optarg, NULL, 10); @@ -632,6 +683,10 @@ int sdlog2_thread_main(int argc, char *argv[]) log_when_armed = true; break; + case 't': + log_name_timestamp = true; + break; + case '?': if (optopt == 'c') { warnx("Option -%c requires an argument.", optopt); @@ -649,27 +704,12 @@ int sdlog2_thread_main(int argc, char *argv[]) } } + gps_time = 0; + if (!file_exist(mountpoint)) { errx(1, "logging mount point %s not present, exiting.", mountpoint); } - if (create_logfolder()) { - errx(1, "unable to create logging folder, exiting."); - } - - const char *converter_in = "/etc/logging/conv.zip"; - char *converter_out = malloc(120); - sprintf(converter_out, "%s/conv.zip", folder_path); - - if (file_copy(converter_in, converter_out)) { - errx(1, "unable to copy conversion scripts, exiting."); - } - - free(converter_out); - - /* only print logging path, important to find log file later */ - warnx("logging to directory: %s", folder_path); - /* initialize log buffer with specified size */ warnx("log buffer size: %i bytes.", log_buffer_size); @@ -919,15 +959,22 @@ int sdlog2_thread_main(int argc, char *argv[]) uint16_t differential_pressure_counter = 0; /* enable logging on start if needed */ - if (log_on_start) + if (log_on_start) { + /* check GPS topic to get GPS time */ + if (log_name_timestamp) { + if (OK == orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos)) { + gps_time = buf.gps_pos.time_gps_usec; + } + } sdlog2_start_log(); + } while (!main_thread_should_exit) { /* decide use usleep() or blocking poll() */ bool use_sleep = sleep_delay > 0 && logging_enabled; /* poll all topics if logging enabled or only management (first 2) if not */ - int poll_ret = poll(fds, logging_enabled ? fdsc_count : 2, use_sleep ? 0 : poll_timeout); + int poll_ret = poll(fds, logging_enabled ? fdsc_count : 3, use_sleep ? 0 : poll_timeout); /* handle the poll result */ if (poll_ret < 0) { @@ -960,6 +1007,17 @@ int sdlog2_thread_main(int argc, char *argv[]) handled_topics++; } + /* --- GPS POSITION - LOG MANAGEMENT --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); + + if (log_name_timestamp) { + gps_time = buf.gps_pos.time_gps_usec; + } + + handled_topics++; + } + if (!logging_enabled || !check_data || handled_topics >= poll_ret) { continue; } @@ -988,7 +1046,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- GPS POSITION --- */ if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); + // Don't orb_copy, it's already done few lines above log_msg.msg_type = LOG_GPS_MSG; log_msg.body.log_GPS.gps_time = buf.gps_pos.time_gps_usec; log_msg.body.log_GPS.fix_type = buf.gps_pos.fix_type; From 5e3c365cd4809def0c5b21136a1b5741a98ae35e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 19 Jan 2014 22:56:24 +0100 Subject: [PATCH 034/108] rc: use sdlog2 -t option sdlog2: move all logs and conv.zip to "log" dir, messages cleanup --- ROMFS/px4fmu_common/init.d/rc.logging | 4 +- src/modules/sdlog2/sdlog2.c | 129 +++++++++++++------------- 2 files changed, 68 insertions(+), 65 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging index dc4be8055a..6c02bf2270 100644 --- a/ROMFS/px4fmu_common/init.d/rc.logging +++ b/ROMFS/px4fmu_common/init.d/rc.logging @@ -7,8 +7,8 @@ if [ -d /fs/microsd ] then if [ $BOARD == fmuv1 ] then - sdlog2 start -r 50 -a -b 16 + sdlog2 start -r 50 -a -b 16 -t else - sdlog2 start -r 200 -a -b 16 + sdlog2 start -r 200 -a -b 16 -t fi fi diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index e9d809834d..656575573a 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -114,7 +114,7 @@ static const int LOG_BUFFER_SIZE_DEFAULT = 8192; static const int MAX_WRITE_CHUNK = 512; static const int MIN_BYTES_TO_WRITE = 512; -static const char *mountpoint = "/fs/microsd"; +static const char *log_root = "/fs/microsd/log"; static int mavlink_fd = -1; struct logbuffer_s lb; @@ -211,12 +211,12 @@ static void handle_status(struct vehicle_status_s *cmd); /** * Create dir for current logging session. Store dir name in 'log_dir'. */ -static int create_logdir(void); +static int create_log_dir(void); /** * Select first free log file name and open it. */ -static int open_logfile(void); +static int open_log_file(void); static void sdlog2_usage(const char *reason) @@ -298,67 +298,69 @@ int create_log_dir() time_t gps_time_sec = gps_time / 1000000; struct tm t; gmtime_r(&gps_time_sec, &t); - int n = snprintf(log_dir, sizeof(log_dir), "%s/", mountpoint); + int n = snprintf(log_dir, sizeof(log_dir), "%s/", log_root); strftime(log_dir + n, sizeof(log_dir) - n, "%Y-%m-%d", &t); mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO); - if (mkdir_ret != 0 && errno != EEXIST) { - warn("failed creating new dir"); + if (mkdir_ret == OK) { + warnx("log dir created: %s", log_dir); + } else if (errno != EEXIST) { + warn("failed creating new dir: %s", log_dir); return -1; } } else { /* look for the next dir that does not exist */ while (dir_number <= MAX_NO_LOGFOLDER) { /* format log dir: e.g. /fs/microsd/sess001 */ - sprintf(log_dir, "%s/sess%03u", mountpoint, dir_number); + sprintf(log_dir, "%s/sess%03u", log_root, dir_number); mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO); - if (mkdir_ret != 0) { - if (errno == EEXIST) { - /* dir exists already */ - dir_number++; - continue; - } else { - warn("failed creating new dir"); - return -1; - } + if (mkdir_ret == 0) { + warnx("log dir created: %s", log_dir); + break; + } else if (errno != EEXIST) { + warn("failed creating new dir: %s", log_dir); + return -1; } - /* dir does not exist, success */ - break; + /* dir exists already */ + dir_number++; + continue; } if (dir_number >= MAX_NO_LOGFOLDER) { /* we should not end up here, either we have more than MAX_NO_LOGFOLDER on the SD card, or another problem */ - warnx("all %d possible dirs exist already.", MAX_NO_LOGFOLDER); + warnx("all %d possible dirs exist already", MAX_NO_LOGFOLDER); return -1; } } /* print logging path, important to find log file later */ - warnx("logging to dir: %s", log_dir); - mavlink_log_info(mavlink_fd, "[sdlog2] logging to dir: %s", log_dir); + warnx("log dir: %s", log_dir); + mavlink_log_info(mavlink_fd, "[sdlog2] log dir: %s", log_dir); return 0; } -int open_logfile() +int open_log_file() { /* string to hold the path to the log */ - char log_file[64] = ""; + char log_file_name[16] = ""; + char log_file_path[48] = ""; if (log_name_timestamp && gps_time != 0) { /* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ - int n = snprintf(log_file, sizeof(log_file), "%s/", log_dir); time_t gps_time_sec = gps_time / 1000000; struct tm t; gmtime_r(&gps_time_sec, &t); - n += strftime(log_file + n, sizeof(log_file) - n, "%H_%M_%S.bin", &t); + strftime(log_file_name, sizeof(log_file_name), "%H_%M_%S.bin", &t); + snprintf(log_file_path, sizeof(log_file_path), "%s/%s", log_dir, log_file_name); } else { uint16_t file_number = 1; // start with file log001 /* look for the next file that does not exist */ while (file_number <= MAX_NO_LOGFILE) { /* format log file path: e.g. /fs/microsd/sess001/log001.bin */ - snprintf(log_file, sizeof(log_file), "%s/log%03u.bin", log_dir, file_number); + snprintf(log_file_name, sizeof(log_file_name), "log%03u.bin", file_number); + snprintf(log_file_path, sizeof(log_file_path), "%s/%s", log_dir, log_file_name); - if (!file_exist(log_file)) { + if (!file_exist(log_file_path)) { break; } file_number++; @@ -366,19 +368,19 @@ int open_logfile() if (file_number > MAX_NO_LOGFILE) { /* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */ - warnx("all %d possible files exist already.", MAX_NO_LOGFILE); + warnx("all %d possible files exist already", MAX_NO_LOGFILE); return -1; } } - int fd = open(log_file, O_CREAT | O_WRONLY | O_DSYNC); + int fd = open(log_file_path, O_CREAT | O_WRONLY | O_DSYNC); if (fd < 0) { - warn("failed opening %s", log_file); - mavlink_log_info(mavlink_fd, "[sdlog2] failed opening %s", log_file); + warn("failed opening log: %s", log_file_name); + mavlink_log_info(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name); } else { - warnx("logging to: %s", log_file); - mavlink_log_info(mavlink_fd, "[sdlog2] log: %s", log_file); + warnx("log file: %s", log_file_name); + mavlink_log_info(mavlink_fd, "[sdlog2] log file: %s", log_file_name); } return fd; @@ -389,7 +391,7 @@ static void *logwriter_thread(void *arg) /* set name */ prctl(PR_SET_NAME, "sdlog2_writer", 0); - int log_fd = open_logfile(); + int log_fd = open_log_file(); if (log_fd < 0) return; @@ -477,7 +479,7 @@ static void *logwriter_thread(void *arg) void sdlog2_start_log() { - warnx("start logging."); + warnx("start logging"); mavlink_log_info(mavlink_fd, "[sdlog2] start logging"); /* create log dir if needed */ @@ -486,16 +488,6 @@ void sdlog2_start_log() errx(1, "error creating log dir"); } - const char *converter_in = "/etc/logging/conv.zip"; - char *converter_out = malloc(120); - sprintf(converter_out, "%s/conv.zip", log_dir); - - if (file_copy(converter_in, converter_out)) { - warnx("unable to copy conversion scripts"); - } - - free(converter_out); - /* initialize statistics counter */ log_bytes_written = 0; start_time = hrt_absolute_time(); @@ -524,7 +516,7 @@ void sdlog2_start_log() void sdlog2_stop_log() { - warnx("stop logging."); + warnx("stop logging"); mavlink_log_info(mavlink_fd, "[sdlog2] stop logging"); logging_enabled = false; @@ -632,7 +624,7 @@ int sdlog2_thread_main(int argc, char *argv[]) mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); if (mavlink_fd < 0) { - warnx("failed to open MAVLink log stream, start mavlink app first."); + warnx("failed to open MAVLink log stream, start mavlink app first"); } /* log buffer size */ @@ -689,32 +681,43 @@ int sdlog2_thread_main(int argc, char *argv[]) case '?': if (optopt == 'c') { - warnx("Option -%c requires an argument.", optopt); + warnx("option -%c requires an argument", optopt); } else if (isprint(optopt)) { - warnx("Unknown option `-%c'.", optopt); + warnx("unknown option `-%c'", optopt); } else { - warnx("Unknown option character `\\x%x'.", optopt); + warnx("unknown option character `\\x%x'", optopt); } default: sdlog2_usage("unrecognized flag"); - errx(1, "exiting."); + errx(1, "exiting"); } } gps_time = 0; - if (!file_exist(mountpoint)) { - errx(1, "logging mount point %s not present, exiting.", mountpoint); + /* create log root dir */ + int mkdir_ret = mkdir(log_root, S_IRWXU | S_IRWXG | S_IRWXO); + if (mkdir_ret != 0 && errno != EEXIST) { + err("failed creating log root dir: %s", log_root); } + /* copy conversion scripts */ + const char *converter_in = "/etc/logging/conv.zip"; + char *converter_out = malloc(64); + snprintf(converter_out, 64, "%s/conv.zip", log_root); + if (file_copy(converter_in, converter_out) != OK) { + warn("unable to copy conversion scripts"); + } + free(converter_out); + /* initialize log buffer with specified size */ - warnx("log buffer size: %i bytes.", log_buffer_size); + warnx("log buffer size: %i bytes", log_buffer_size); if (OK != logbuffer_init(&lb, log_buffer_size)) { - errx(1, "can't allocate log buffer, exiting."); + errx(1, "can't allocate log buffer, exiting"); } struct vehicle_status_s buf_status; @@ -935,7 +938,7 @@ int sdlog2_thread_main(int argc, char *argv[]) * differs from the number of messages in the above list. */ if (fdsc_count > fdsc) { - warn("WARNING: Not enough space for poll fds allocated. Check %s:%d.", __FILE__, __LINE__); + warn("WARNING: Not enough space for poll fds allocated. Check %s:%d", __FILE__, __LINE__); fdsc_count = fdsc; } @@ -978,7 +981,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* handle the poll result */ if (poll_ret < 0) { - warnx("ERROR: poll error, stop logging."); + warnx("ERROR: poll error, stop logging"); main_thread_should_exit = true; } else if (poll_ret > 0) { @@ -1337,7 +1340,7 @@ int sdlog2_thread_main(int argc, char *argv[]) free(lb.data); - warnx("exiting."); + warnx("exiting"); thread_running = false; @@ -1350,8 +1353,8 @@ void sdlog2_status() float mebibytes = kibibytes / 1024.0f; float seconds = ((float)(hrt_absolute_time() - start_time)) / 1000000.0f; - warnx("wrote %lu msgs, %4.2f MiB (average %5.3f KiB/s), skipped %lu msgs.", log_msgs_written, (double)mebibytes, (double)(kibibytes / seconds), log_msgs_skipped); - mavlink_log_info(mavlink_fd, "[sdlog2] wrote %lu msgs, skipped %lu msgs.", log_msgs_written, log_msgs_skipped); + warnx("wrote %lu msgs, %4.2f MiB (average %5.3f KiB/s), skipped %lu msgs", log_msgs_written, (double)mebibytes, (double)(kibibytes / seconds), log_msgs_skipped); + mavlink_log_info(mavlink_fd, "[sdlog2] wrote %lu msgs, skipped %lu msgs", log_msgs_written, log_msgs_skipped); } /** @@ -1370,7 +1373,7 @@ int file_copy(const char *file_old, const char *file_new) int ret = 0; if (source == NULL) { - warnx("failed opening input file to copy."); + warnx("failed opening input file to copy"); return 1; } @@ -1378,7 +1381,7 @@ int file_copy(const char *file_old, const char *file_new) if (target == NULL) { fclose(source); - warnx("failed to open output file to copy."); + warnx("failed to open output file to copy"); return 1; } @@ -1389,7 +1392,7 @@ int file_copy(const char *file_old, const char *file_new) ret = fwrite(buf, 1, nread, target); if (ret <= 0) { - warnx("error writing file."); + warnx("error writing file"); ret = 1; break; } From 47c226988ccf0e90bda9fe7c106bcdaf8b2e67fd Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 19 Jan 2014 22:56:57 +0100 Subject: [PATCH 035/108] sdlog2: code style fixed --- src/modules/sdlog2/sdlog2.c | 17 +++++++++++++++-- 1 file changed, 15 insertions(+), 2 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 656575573a..2319dbb3b5 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -301,12 +301,15 @@ int create_log_dir() int n = snprintf(log_dir, sizeof(log_dir), "%s/", log_root); strftime(log_dir + n, sizeof(log_dir) - n, "%Y-%m-%d", &t); mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO); + if (mkdir_ret == OK) { warnx("log dir created: %s", log_dir); + } else if (errno != EEXIST) { warn("failed creating new dir: %s", log_dir); return -1; } + } else { /* look for the next dir that does not exist */ while (dir_number <= MAX_NO_LOGFOLDER) { @@ -317,10 +320,12 @@ int create_log_dir() if (mkdir_ret == 0) { warnx("log dir created: %s", log_dir); break; + } else if (errno != EEXIST) { warn("failed creating new dir: %s", log_dir); return -1; } + /* dir exists already */ dir_number++; continue; @@ -352,8 +357,10 @@ int open_log_file() gmtime_r(&gps_time_sec, &t); strftime(log_file_name, sizeof(log_file_name), "%H_%M_%S.bin", &t); snprintf(log_file_path, sizeof(log_file_path), "%s/%s", log_dir, log_file_name); + } else { uint16_t file_number = 1; // start with file log001 + /* look for the next file that does not exist */ while (file_number <= MAX_NO_LOGFILE) { /* format log file path: e.g. /fs/microsd/sess001/log001.bin */ @@ -363,6 +370,7 @@ int open_log_file() if (!file_exist(log_file_path)) { break; } + file_number++; } @@ -378,6 +386,7 @@ int open_log_file() if (fd < 0) { warn("failed opening log: %s", log_file_name); mavlink_log_info(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name); + } else { warnx("log file: %s", log_file_name); mavlink_log_info(mavlink_fd, "[sdlog2] log file: %s", log_file_name); @@ -605,8 +614,8 @@ int write_parameters(int fd) } case PARAM_TYPE_FLOAT: - param_get(param, &value); - break; + param_get(param, &value); + break; default: break; @@ -700,6 +709,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* create log root dir */ int mkdir_ret = mkdir(log_root, S_IRWXU | S_IRWXG | S_IRWXO); + if (mkdir_ret != 0 && errno != EEXIST) { err("failed creating log root dir: %s", log_root); } @@ -708,9 +718,11 @@ int sdlog2_thread_main(int argc, char *argv[]) const char *converter_in = "/etc/logging/conv.zip"; char *converter_out = malloc(64); snprintf(converter_out, 64, "%s/conv.zip", log_root); + if (file_copy(converter_in, converter_out) != OK) { warn("unable to copy conversion scripts"); } + free(converter_out); /* initialize log buffer with specified size */ @@ -969,6 +981,7 @@ int sdlog2_thread_main(int argc, char *argv[]) gps_time = buf.gps_pos.time_gps_usec; } } + sdlog2_start_log(); } From 1e6d83fc9ea1c33b6a76623cc86a7889dc4f6c49 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 20 Jan 2014 13:10:37 +0100 Subject: [PATCH 036/108] Hotfix for IO battery voltage --- src/drivers/px4io/px4io.cpp | 1 + src/modules/sensors/sensors.cpp | 4 ++-- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index dede5976da..c8f77a6116 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1302,6 +1302,7 @@ PX4IO::io_handle_battery(uint16_t vbatt, uint16_t ibatt) /* voltage is scaled to mV */ battery_status.voltage_v = vbatt / 1000.0f; + battery_status.voltage_filtered_v = vbatt / 1000.0f; /* ibatt contains the raw ADC count, as 12 bit ADC diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index ff6c5882ec..9903a90a0e 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1548,8 +1548,8 @@ Sensors::task_main() raw.adc_voltage_v[3] = 0.0f; memset(&_battery_status, 0, sizeof(_battery_status)); - _battery_status.voltage_v = 0.0f; - _battery_status.voltage_filtered_v = 0.0f; + _battery_status.voltage_v = -1.0f; + _battery_status.voltage_filtered_v = -1.0f; _battery_status.current_a = -1.0f; _battery_status.discharged_mah = -1.0f; From 5bc61c3c57164a7d2d118d3f7399a2a24e7199cd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 22 Jan 2014 08:30:48 +0100 Subject: [PATCH 037/108] S.BUS output disable cleanup --- src/drivers/boards/px4io-v2/board_config.h | 2 +- src/drivers/boards/px4io-v2/px4iov2_init.c | 2 -- src/modules/px4iofirmware/px4io.h | 2 +- 3 files changed, 2 insertions(+), 4 deletions(-) diff --git a/src/drivers/boards/px4io-v2/board_config.h b/src/drivers/boards/px4io-v2/board_config.h index 8152ea4d4b..ef9bb5cadd 100644 --- a/src/drivers/boards/px4io-v2/board_config.h +++ b/src/drivers/boards/px4io-v2/board_config.h @@ -114,7 +114,7 @@ /* XXX these should be UART pins */ #define GPIO_SBUS_INPUT (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN11) #define GPIO_SBUS_OUTPUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10) -#define GPIO_SBUS_OENABLE (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN4) +#define GPIO_SBUS_OENABLE (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN4) /* * High-resolution timer diff --git a/src/drivers/boards/px4io-v2/px4iov2_init.c b/src/drivers/boards/px4io-v2/px4iov2_init.c index ccd01edf56..9f8c0eeb27 100644 --- a/src/drivers/boards/px4io-v2/px4iov2_init.c +++ b/src/drivers/boards/px4io-v2/px4iov2_init.c @@ -124,8 +124,6 @@ __EXPORT void stm32_boardinitialize(void) stm32_configgpio(GPIO_ADC_VSERVO); stm32_configgpio(GPIO_SBUS_INPUT); /* xxx alternate function */ - - stm32_gpiowrite(GPIO_SBUS_OUTPUT, false); stm32_configgpio(GPIO_SBUS_OUTPUT); /* sbus output enable is active low - disable it by default */ diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 18c7468f69..d7f3e9adb5 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -160,7 +160,7 @@ extern pwm_limit_t pwm_limit; # define PX4IO_RELAY_CHANNELS 0 # define POWER_SPEKTRUM(_s) stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_s)) -# define ENABLE_SBUS_OUT(_s) stm32_gpiowrite(GPIO_SBUS_OENABLE, (_s)) +# define ENABLE_SBUS_OUT(_s) stm32_gpiowrite(GPIO_SBUS_OENABLE, !(_s)) # define VDD_SERVO_FAULT (!stm32_gpioread(GPIO_SERVO_FAULT_DETECT)) From a794ee6c04550e1fd15b2c8bdf7835f6f0086251 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 22 Jan 2014 17:07:25 +0100 Subject: [PATCH 038/108] Hotfix for CMSIS exclude --- Documentation/Doxyfile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Documentation/Doxyfile b/Documentation/Doxyfile index 2417028114..b674fbc485 100644 --- a/Documentation/Doxyfile +++ b/Documentation/Doxyfile @@ -599,7 +599,7 @@ RECURSIVE = YES # excluded from the INPUT source files. This way you can easily exclude a # subdirectory from a directory tree whose root is specified with the INPUT tag. -EXCLUDE = ../src/modules/mathlib/CMSIS \ +EXCLUDE = ../src/lib/mathlib/CMSIS \ ../src/modules/attitude_estimator_ekf/codegen # The EXCLUDE_SYMLINKS tag can be used select whether or not files or From 6c23e2f159b226fec394cca506e55b9b1d77365e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 22 Jan 2014 17:20:55 +0100 Subject: [PATCH 039/108] Added Doxygen main page --- src/mainpage.dox | 9 +++++++++ 1 file changed, 9 insertions(+) create mode 100644 src/mainpage.dox diff --git a/src/mainpage.dox b/src/mainpage.dox new file mode 100644 index 0000000000..7ca410341e --- /dev/null +++ b/src/mainpage.dox @@ -0,0 +1,9 @@ +/** +\mainpage PX4 Autopilot Flight Control Stack and Middleware + +This software repository offers a middleware for micro aerial vehicles capable of running efficiently on a 168 MHz Cortex M4F processor and a state of the art flight control stack supporting multicopter and fixed wing aircraft. It can be easily used for experimental air (and ground) vehicles as well, as the application on a spherical blimp shows. + +http://pixhawk.org + + +*/ \ No newline at end of file From 1c40ce968a668a8d80535ab6799518da68fe0ac2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 23 Jan 2014 08:01:55 +0100 Subject: [PATCH 040/108] RC config params set to more useful default values - needs more testing --- src/modules/sensors/sensor_params.c | 63 ++++++++++++++++++++++++++++- 1 file changed, 61 insertions(+), 2 deletions(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index bbc84ef938..30659fd3a0 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -381,14 +381,73 @@ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.00459340659f); #endif PARAM_DEFINE_FLOAT(BAT_C_SCALING, 0.0124); /* scaling for 3DR power brick */ +/** + * Roll control channel mapping. + * + * The channel index (starting from 1 for channel 1) indicates + * which channel should be used for reading roll inputs from. + * A value of zero indicates the switch is not assigned. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ PARAM_DEFINE_INT32(RC_MAP_ROLL, 1); + +/** + * Pitch control channel mapping. + * + * The channel index (starting from 1 for channel 1) indicates + * which channel should be used for reading pitch inputs from. + * A value of zero indicates the switch is not assigned. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ PARAM_DEFINE_INT32(RC_MAP_PITCH, 2); + +/** + * Throttle control channel mapping. + * + * The channel index (starting from 1 for channel 1) indicates + * which channel should be used for reading throttle inputs from. + * A value of zero indicates the switch is not assigned. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3); + +/** + * Yaw control channel mapping. + * + * The channel index (starting from 1 for channel 1) indicates + * which channel should be used for reading yaw inputs from. + * A value of zero indicates the switch is not assigned. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ PARAM_DEFINE_INT32(RC_MAP_YAW, 4); -PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5); +/** + * Mode switch channel mapping. + * + * This is the main flight mode selector. + * The channel index (starting from 1 for channel 1) indicates + * which channel should be used for deciding about the main mode. + * A value of zero indicates the switch is not assigned. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ +PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0); PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); -PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 6); +PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0); PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0); //PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); From ac37172b5280e661395035c17d2dfde91672b4b5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 23 Jan 2014 08:17:04 +0100 Subject: [PATCH 041/108] Hotfix: Prevent failures in boot handling due to missing microSD card logfile - we are not depending on the microSD any more completely --- ROMFS/px4fmu_common/init.d/rcS | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index a9c5c59ea9..a2e562d6e4 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -8,7 +8,6 @@ # set MODE autostart -set LOG_FILE /fs/microsd/bootlog.txt set RC_FILE /fs/microsd/etc/rc.txt set CONFIG_FILE /fs/microsd/etc/config.txt set EXTRAS_FILE /fs/microsd/etc/extras.txt @@ -21,10 +20,12 @@ set TUNE_OUT_ERROR ML< Date: Thu, 23 Jan 2014 08:25:37 +0100 Subject: [PATCH 042/108] Enable the PX4IO self check and debug interfaces. No reason to disable them, since they are runtime-configured (and needed, for the case of memory) --- src/modules/px4iofirmware/px4io.c | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index 150af35df1..e9838d38ca 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2014 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 @@ -228,7 +228,6 @@ user_start(int argc, char *argv[]) /* initialize PWM limit lib */ pwm_limit_init(&pwm_limit); -#if 0 /* not enough memory, lock down */ if (minfo.mxordblk < 500) { lowsyslog("ERR: not enough MEM"); @@ -245,7 +244,6 @@ user_start(int argc, char *argv[]) phase = !phase; usleep(300000); } -#endif /* * Run everything in a tight loop. @@ -275,7 +273,6 @@ user_start(int argc, char *argv[]) check_reboot(); -#if 0 /* check for debug activity */ show_debug_messages(); @@ -292,7 +289,6 @@ user_start(int argc, char *argv[]) (unsigned)minfo.mxordblk); last_debug_time = hrt_absolute_time(); } -#endif } } From f504863feef167c1ad493bf58c44420265892193 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 23 Jan 2014 08:25:56 +0100 Subject: [PATCH 043/108] Make startup less chatty --- ROMFS/px4fmu_common/init.d/rcS | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index a2e562d6e4..cbbdeeee8f 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -181,7 +181,7 @@ then set IO_PRESENT yes else - echo "[init] PX4IO CRC failure, trying to update" + echo "[init] Trying to update" echo "PX4IO CRC failure" >> $LOG_FILE tone_alarm MLL32CP8MB From c3e4e4ee68f1f31d3ae281b0afb281fc7c58bc27 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 23 Jan 2014 08:26:53 +0100 Subject: [PATCH 044/108] Build fix, replaced usleep with up_udelay in memory lockdown state --- src/modules/px4iofirmware/px4io.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index e9838d38ca..d8ebb43e91 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -242,7 +242,7 @@ user_start(int argc, char *argv[]) } phase = !phase; - usleep(300000); + up_udelay(300000); } /* From a0db455334c0928b953ab088e868e72fa3fc08f7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 23 Jan 2014 08:43:55 +0100 Subject: [PATCH 045/108] Cleanup wording in rcS --- ROMFS/px4fmu_common/init.d/rcS | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index cbbdeeee8f..edf2c77fba 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -182,7 +182,7 @@ then set IO_PRESENT yes else echo "[init] Trying to update" - echo "PX4IO CRC failure" >> $LOG_FILE + echo "PX4IO Trying to update" >> $LOG_FILE tone_alarm MLL32CP8MB From 8833f81b48aa738125b42a08aca05e3131cb8f8f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 23 Jan 2014 08:45:29 +0100 Subject: [PATCH 046/108] Do not make PX4IO start mandatory for forceupdate --- src/drivers/px4io/px4io.cpp | 37 ++++++++++++++++++++++++++++--------- 1 file changed, 28 insertions(+), 9 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 27ee1fb420..4844372b84 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2367,8 +2367,10 @@ start(int argc, char *argv[]) /* create the driver - it will set g_dev */ (void)new PX4IO(interface); - if (g_dev == nullptr) + if (g_dev == nullptr) { + delete interface; errx(1, "driver alloc failed"); + } if (OK != g_dev->init()) { delete g_dev; @@ -2769,18 +2771,35 @@ px4io_main(int argc, char *argv[]) } if (g_dev == nullptr) { warnx("px4io is not started, still attempting upgrade"); - } else { - uint16_t arg = atol(argv[2]); - int ret = g_dev->ioctl(nullptr, PX4IO_REBOOT_BOOTLOADER, arg); - if (ret != OK) { - printf("reboot failed - %d\n", ret); - exit(1); + + /* allocate the interface */ + device::Device *interface = get_interface(); + + /* create the driver - it will set g_dev */ + (void)new PX4IO(interface); + + if (g_dev == nullptr) { + delete interface; + errx(1, "driver alloc failed"); } - // tear down the px4io instance - delete g_dev; + if (OK != g_dev->init()) { + delete g_dev; + g_dev = nullptr; + errx(1, "driver init failed"); + } } + uint16_t arg = atol(argv[2]); + int ret = g_dev->ioctl(nullptr, PX4IO_REBOOT_BOOTLOADER, arg); + if (ret != OK) { + printf("reboot failed - %d\n", ret); + exit(1); + } + + // tear down the px4io instance + delete g_dev; + // upload the specified firmware const char *fn[2]; fn[0] = argv[3]; From 1ac8501d95f9d01bd2efc5b75373260dcc8d4530 Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 22 Jan 2014 23:51:22 -0800 Subject: [PATCH 047/108] Clear the screen more properly. --- src/drivers/px4io/px4io.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 4844372b84..b85638788a 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2646,17 +2646,17 @@ monitor(void) read(0, &c, 1); if (cancels-- == 0) { - printf("\033[H"); /* move cursor home and clear screen */ + printf("\033[2J\033[H"); /* move cursor home and clear screen */ exit(0); } } if (g_dev != nullptr) { - printf("\033[H"); /* move cursor home and clear screen */ + printf("\033[2J\033[H"); /* move cursor home and clear screen */ (void)g_dev->print_status(); (void)g_dev->print_debug(); - printf("[ Use 'px4io debug ' for more output. Hit three times to exit monitor mode ]\n"); + printf("\n\n\n[ Use 'px4io debug ' for more output. Hit three times to exit monitor mode ]\n"); } else { errx(1, "driver not loaded, exiting"); From 0994412ccae65349b144e0f29781889b18cd23ca Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 22 Jan 2014 23:51:53 -0800 Subject: [PATCH 048/108] Fix the initialisation and operation of the PX4IO ADC - now RSSI and VSERVO voltages should be read correctly. --- src/modules/px4iofirmware/adc.c | 53 ++++++++++++++----------------- src/modules/px4iofirmware/px4io.c | 3 ++ 2 files changed, 26 insertions(+), 30 deletions(-) diff --git a/src/modules/px4iofirmware/adc.c b/src/modules/px4iofirmware/adc.c index 81566eb2a2..2f5908ac5e 100644 --- a/src/modules/px4iofirmware/adc.c +++ b/src/modules/px4iofirmware/adc.c @@ -83,6 +83,14 @@ adc_init(void) { adc_perf = perf_alloc(PC_ELAPSED, "adc"); + /* put the ADC into power-down mode */ + rCR2 &= ~ADC_CR2_ADON; + up_udelay(10); + + /* bring the ADC out of power-down mode */ + rCR2 |= ADC_CR2_ADON; + up_udelay(10); + /* do calibration if supported */ #ifdef ADC_CR2_CAL rCR2 |= ADC_CR2_RSTCAL; @@ -96,41 +104,25 @@ adc_init(void) if (rCR2 & ADC_CR2_CAL) return -1; - #endif - /* arbitrarily configure all channels for 55 cycle sample time */ - rSMPR1 = 0b00000011011011011011011011011011; + /* + * Configure sampling time. + * + * For electrical protection reasons, we want to be able to have + * 10K in series with ADC inputs that leave the board. At 12MHz this + * means we need 28.5 cycles of sampling time (per table 43 in the + * datasheet). + */ + rSMPR1 = 0b00000000011011011011011011011011; rSMPR2 = 0b00011011011011011011011011011011; - /* XXX for F2/4, might want to select 12-bit mode? */ - rCR1 = 0; - - /* enable the temperature sensor / Vrefint channel if supported*/ - rCR2 = -#ifdef ADC_CR2_TSVREFE - /* enable the temperature sensor in CR2 */ - ADC_CR2_TSVREFE | -#endif - 0; - -#ifdef ADC_CCR_TSVREFE - /* enable temperature sensor in CCR */ - rCCR = ADC_CCR_TSVREFE; -#endif + rCR2 |= ADC_CR2_TSVREFE; /* enable the temperature sensor / Vrefint channel */ /* configure for a single-channel sequence */ rSQR1 = 0; rSQR2 = 0; - rSQR3 = 0; /* will be updated with the channel each tick */ - - /* power-cycle the ADC and turn it on */ - rCR2 &= ~ADC_CR2_ADON; - up_udelay(10); - rCR2 |= ADC_CR2_ADON; - up_udelay(10); - rCR2 |= ADC_CR2_ADON; - up_udelay(10); + rSQR3 = 0; /* will be updated with the channel at conversion time */ return 0; } @@ -141,11 +133,12 @@ adc_init(void) uint16_t adc_measure(unsigned channel) { + perf_begin(adc_perf); /* clear any previous EOC */ - if (rSR & ADC_SR_EOC) - rSR &= ~ADC_SR_EOC; + rSR = 0; + (void)rDR; /* run a single conversion right now - should take about 60 cycles (a few microseconds) max */ rSQR3 = channel; @@ -158,7 +151,6 @@ adc_measure(unsigned channel) /* never spin forever - this will give a bogus result though */ if (hrt_elapsed_time(&now) > 100) { - debug("adc timeout"); perf_end(adc_perf); return 0xffff; } @@ -166,6 +158,7 @@ adc_measure(unsigned channel) /* read the result and clear EOC */ uint16_t result = rDR; + rSR = 0; perf_end(adc_perf); return result; diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index d8ebb43e91..d03b648094 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -210,6 +210,9 @@ user_start(int argc, char *argv[]) /* initialise the control inputs */ controls_init(); + /* set up the ADC */ + adc_init(); + /* start the FMU interface */ interface_init(); From d77a15e94fd024633661eb92f72455d737a0aa84 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 23 Jan 2014 09:16:40 +0100 Subject: [PATCH 049/108] Last small fixes to IO driver to support updates with and without switch pressed and with and without px4io start call before the forceupdate call --- src/drivers/px4io/px4io.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 4844372b84..879e19973a 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2784,9 +2784,7 @@ px4io_main(int argc, char *argv[]) } if (OK != g_dev->init()) { - delete g_dev; - g_dev = nullptr; - errx(1, "driver init failed"); + warnx("driver init failed, still trying.."); } } From 2aa76f1a3c4eb99074b38d287e0f18a98973671d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 23 Jan 2014 09:17:46 +0100 Subject: [PATCH 050/108] Fixes to memory check handling, split out switch handling to allow separate initialization --- src/modules/px4iofirmware/px4io.c | 41 ++++++++++++++++++++++-------- src/modules/px4iofirmware/px4io.h | 1 + src/modules/px4iofirmware/safety.c | 8 ++++-- 3 files changed, 37 insertions(+), 13 deletions(-) diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index d8ebb43e91..dba35752e2 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -228,23 +228,42 @@ user_start(int argc, char *argv[]) /* initialize PWM limit lib */ pwm_limit_init(&pwm_limit); - /* not enough memory, lock down */ - if (minfo.mxordblk < 500) { + /* + * P O L I C E L I G H T S + * + * Not enough memory, lock down. + * + * We might need to allocate mixers later, and this will + * ensure that a developer doing a change will notice + * that he just burned the remaining RAM with static + * allocations. We don't want him to be able to + * get past that point. This needs to be clearly + * documented in the dev guide. + * + */ + if (minfo.mxordblk < 600) { + lowsyslog("ERR: not enough MEM"); bool phase = false; - if (phase) { - LED_AMBER(true); - LED_BLUE(false); - } else { - LED_AMBER(false); - LED_BLUE(true); - } + while (true) { - phase = !phase; - up_udelay(300000); + if (phase) { + LED_AMBER(true); + LED_BLUE(false); + } else { + LED_AMBER(false); + LED_BLUE(true); + } + up_udelay(250000); + + phase = !phase; + } } + /* Start the failsafe led init */ + failsafe_led_init(); + /* * Run everything in a tight loop. */ diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index d7f3e9adb5..bffbc0ce2a 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -184,6 +184,7 @@ extern void mixer_handle_text(const void *buffer, size_t length); * Safety switch/LED. */ extern void safety_init(void); +extern void failsafe_led_init(void); /** * FMU communications diff --git a/src/modules/px4iofirmware/safety.c b/src/modules/px4iofirmware/safety.c index cdb54a80ad..2ce479ffd3 100644 --- a/src/modules/px4iofirmware/safety.c +++ b/src/modules/px4iofirmware/safety.c @@ -84,7 +84,11 @@ safety_init(void) { /* arrange for the button handler to be called at 10Hz */ hrt_call_every(&arming_call, 1000, 100000, safety_check_button, NULL); +} +void +failsafe_led_init(void) +{ /* arrange for the failsafe blinker to be called at 8Hz */ hrt_call_every(&failsafe_call, 1000, 125000, failsafe_blink, NULL); } @@ -165,8 +169,8 @@ failsafe_blink(void *arg) /* indicate that a serious initialisation error occured */ if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)) { LED_AMBER(true); - return; - } + return; + } static bool failsafe = false; From 9e72e726442198feb6b4d3a725c8195ef55ffe55 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 23 Jan 2014 09:29:59 +0100 Subject: [PATCH 051/108] Make SBUS switching conditional to be friendly to IO v1 --- src/modules/px4iofirmware/controls.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 5859f768bf..5e2c92bf46 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -123,7 +123,9 @@ controls_tick() { /* switch S.Bus output pin as needed */ if (sbus_status != (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS)) { + #ifdef ENABLE_SBUS_OUT ENABLE_SBUS_OUT((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS)); + #endif } perf_end(c_gather_sbus); From 6a40acdbdc6abe57fb202a02e6ab6fa8c90698a9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 23 Jan 2014 09:58:22 +0100 Subject: [PATCH 052/108] Fixed PPM warning to be only printed with PPM inputs enabled --- src/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 08ff3792fe..a54bb39646 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1810,7 +1810,7 @@ PX4IO::print_status() printf("\n"); - if (raw_inputs > 0) { + if ((flags & PX4IO_P_STATUS_FLAGS_RC_PPM)) { int frame_len = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_RC_DATA); printf("RC data (PPM frame len) %u us\n", frame_len); From 1afe7f2c50016fbb39cb532252be171ba03ad357 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 23 Jan 2014 18:39:32 +0100 Subject: [PATCH 053/108] Added tune on IO upgrade error --- ROMFS/px4fmu_common/init.d/rcS | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index edf2c77fba..6f4e1f3b56 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -199,10 +199,12 @@ then else echo "[init] ERROR: PX4IO update failed" echo "PX4IO update failed" >> $LOG_FILE + tone_alarm $TUNE_OUT_ERROR fi else echo "[init] ERROR: PX4IO update failed" echo "PX4IO update failed" >> $LOG_FILE + tone_alarm $TUNE_OUT_ERROR fi fi From dda50c62bfd26463718f50d2f9c1cdbecc7de4ac Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 22 Jan 2014 16:33:35 +1100 Subject: [PATCH 054/108] hmc5883: much faster calibration code with bug fixes this fixes two bugs in "hmc5883 calibrate" and also makes it much faster, so it can be run on every boot. It now uses the correct 2.5Ga range when calibrating, and fixes the expected values for X/Y/Z axes The basic calibration approach is similar to the APM2 driver, waiting for 10 good samples after discarding some initial samples. That allows the calibration to run fast enough that it can be done on every boot without causing too much boot delay. --- src/drivers/hmc5883/hmc5883.cpp | 160 +++++++++++++++++--------------- 1 file changed, 87 insertions(+), 73 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index d3b99ae66e..9b9c11af29 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -849,42 +849,24 @@ HMC5883::collect() /* scale values for output */ - /* - * 1) Scale raw value to SI units using scaling from datasheet. - * 2) Subtract static offset (in SI units) - * 3) Scale the statically calibrated values with a linear - * dynamically obtained factor - * - * Note: the static sensor offset is the number the sensor outputs - * at a nominally 'zero' input. Therefore the offset has to - * be subtracted. - * - * Example: A gyro outputs a value of 74 at zero angular rate - * the offset is 74 from the origin and subtracting - * 74 from all measurements centers them around zero. - */ - #ifdef PX4_I2C_BUS_ONBOARD if (_bus == PX4_I2C_BUS_ONBOARD) { - /* to align the sensor axes with the board, x and y need to be flipped */ - new_report.x = ((report.y * _range_scale) - _scale.x_offset) * _scale.x_scale; - /* flip axes and negate value for y */ - new_report.y = ((-report.x * _range_scale) - _scale.y_offset) * _scale.y_scale; - /* z remains z */ - new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; - } else { -#endif - /* the standard external mag by 3DR has x pointing to the right, y pointing backwards, and z down, - * therefore switch x and y and invert y */ - new_report.x = ((-report.y * _range_scale) - _scale.x_offset) * _scale.x_scale; - /* flip axes and negate value for y */ - new_report.y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale; - /* z remains z */ - new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; -#ifdef PX4_I2C_BUS_ONBOARD - } + // convert onboard so it matches offboard for the + // scaling below + report.y = -report.y; + report.x = -report.x; + } #endif + /* the standard external mag by 3DR has x pointing to the + * right, y pointing backwards, and z down, therefore switch x + * and y and invert y */ + new_report.x = ((-report.y * _range_scale) - _scale.x_offset) * _scale.x_scale; + /* flip axes and negate value for y */ + new_report.y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale; + /* z remains z */ + new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; + if (_mag_topic != -1) { /* publish it */ orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report); @@ -910,6 +892,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) struct mag_report report; ssize_t sz; int ret = 1; + uint8_t good_count = 0; // XXX do something smarter here int fd = (int)enable; @@ -932,32 +915,17 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) 1.0f, }; - float avg_excited[3] = {0.0f, 0.0f, 0.0f}; - unsigned i; + float sum_excited[3] = {0.0f, 0.0f, 0.0f}; + + /* expected axis scaling. The datasheet says that 766 will + * be places in the X and Y axes and 713 in the Z + * axis. Experiments show that in fact 766 is placed in X, + * and 713 in Y and Z. This is relative to a base of 660 + * LSM/Ga, giving 1.16 and 1.08 */ + float expected_cal[3] = { 1.16f, 1.08f, 1.08f }; warnx("starting mag scale calibration"); - /* do a simple demand read */ - sz = read(filp, (char *)&report, sizeof(report)); - - if (sz != sizeof(report)) { - warn("immediate read failed"); - ret = 1; - goto out; - } - - warnx("current measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); - warnx("time: %lld", report.timestamp); - warnx("sampling 500 samples for scaling offset"); - - /* set the queue depth to 10 */ - /* don't do this for now, it can lead to a crash in start() respectively work_queue() */ -// if (OK != ioctl(filp, SENSORIOCSQUEUEDEPTH, 10)) { -// warn("failed to set queue depth"); -// ret = 1; -// goto out; -// } - /* start the sensor polling at 50 Hz */ if (OK != ioctl(filp, SENSORIOCSPOLLRATE, 50)) { warn("failed to set 2Hz poll rate"); @@ -965,8 +933,9 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) goto out; } - /* Set to 2.5 Gauss */ - if (OK != ioctl(filp, MAGIOCSRANGE, 2)) { + /* Set to 2.5 Gauss. We ask for 3 to get the right part of + * the chained if statement above. */ + if (OK != ioctl(filp, MAGIOCSRANGE, 3)) { warnx("failed to set 2.5 Ga range"); ret = 1; goto out; @@ -990,8 +959,8 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) goto out; } - /* read the sensor 10x and report each value */ - for (i = 0; i < 500; i++) { + // discard 10 samples to let the sensor settle + for (uint8_t i = 0; i < 10; i++) { struct pollfd fds; /* wait for data to be ready */ @@ -1009,32 +978,69 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) if (sz != sizeof(report)) { warn("periodic read failed"); + ret = -EIO; goto out; + } + } - } else { - avg_excited[0] += report.x; - avg_excited[1] += report.y; - avg_excited[2] += report.z; + /* read the sensor up to 50x, stopping when we have 10 good values */ + for (uint8_t i = 0; i < 50 && good_count < 10; i++) { + struct pollfd fds; + + /* wait for data to be ready */ + fds.fd = fd; + fds.events = POLLIN; + ret = ::poll(&fds, 1, 2000); + + if (ret != 1) { + warn("timed out waiting for sensor data"); + goto out; + } + + /* now go get it */ + sz = ::read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + warn("periodic read failed"); + ret = -EIO; + goto out; + } + float cal[3] = {fabsf(expected_cal[0] / report.x), + fabsf(expected_cal[1] / report.y), + fabsf(expected_cal[2] / report.z)}; + + if (cal[0] > 0.7f && cal[0] < 1.35f && + cal[1] > 0.7f && cal[1] < 1.35f && + cal[2] > 0.7f && cal[2] < 1.35f) { + good_count++; + sum_excited[0] += cal[0]; + sum_excited[1] += cal[1]; + sum_excited[2] += cal[2]; } //warnx("periodic read %u", i); //warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); + //warnx("cal: %.6f %.6f %.6f", (double)cal[0], (double)cal[1], (double)cal[2]); } - avg_excited[0] /= i; - avg_excited[1] /= i; - avg_excited[2] /= i; + if (good_count < 5) { + warn("failed calibration"); + ret = -EIO; + goto out; + } - warnx("done. Performed %u reads", i); - warnx("measurement avg: %.6f %.6f %.6f", (double)avg_excited[0], (double)avg_excited[1], (double)avg_excited[2]); +#if 0 + warnx("measurement avg: %.6f %.6f %.6f", + (double)sum_excited[0]/good_count, + (double)sum_excited[1]/good_count, + (double)sum_excited[2]/good_count); +#endif float scaling[3]; - /* calculate axis scaling */ - scaling[0] = fabsf(1.16f / avg_excited[0]); - /* second axis inverted */ - scaling[1] = fabsf(1.16f / -avg_excited[1]); - scaling[2] = fabsf(1.08f / avg_excited[2]); + scaling[0] = sum_excited[0] / good_count; + scaling[1] = sum_excited[1] / good_count; + scaling[2] = sum_excited[2] / good_count; warnx("axes scaling: %.6f %.6f %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]); @@ -1165,6 +1171,8 @@ int HMC5883::set_excitement(unsigned enable) conf_reg &= ~0x03; } + // ::printf("set_excitement enable=%d regA=0x%x\n", (int)enable, (unsigned)conf_reg); + ret = write_reg(ADDR_CONF_A, conf_reg); if (OK != ret) @@ -1173,6 +1181,8 @@ int HMC5883::set_excitement(unsigned enable) uint8_t conf_reg_ret; read_reg(ADDR_CONF_A, conf_reg_ret); + //print_info(); + return !(conf_reg == conf_reg_ret); } @@ -1211,6 +1221,10 @@ HMC5883::print_info() perf_print_counter(_comms_errors); perf_print_counter(_buffer_overflows); printf("poll interval: %u ticks\n", _measure_ticks); + printf("offsets (%.2f %.2f %.2f)\n", (double)_scale.x_offset, (double)_scale.y_offset, (double)_scale.z_offset); + printf("scaling (%.2f %.2f %.2f) 1/range_scale %.2f range_ga %.2f\n", + (double)_scale.x_scale, (double)_scale.y_scale, (double)_scale.z_scale, + (double)1.0/_range_scale, (double)_range_ga); _reports->print_info("report queue"); } From 4524fe3e4888d569f855d1e7a82c8d5116636a0a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 20 Jan 2014 16:20:43 +1100 Subject: [PATCH 055/108] px4fmu: added PWM_SERVO_SET_COUNT API this allows the balance between PWM channels and GPIOs to be changed after the main flight code has started, which makes it possible to change the balance with a parameter in APM --- src/drivers/drv_pwm_output.h | 4 ++++ src/drivers/px4fmu/fmu.cpp | 35 ++++++++++++++++++++++++++++++++++- 2 files changed, 38 insertions(+), 1 deletion(-) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 51f916f37b..88da94b1ed 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -189,6 +189,10 @@ ORB_DECLARE(output_pwm); /** get the maximum PWM value the output will send */ #define PWM_SERVO_GET_MAX_PWM _IOC(_PWM_SERVO_BASE, 19) +/** set the number of servos in (unsigned)arg - allows change of + * split between servos and GPIO */ +#define PWM_SERVO_SET_COUNT _IOC(_PWM_SERVO_BASE, 20) + /** set a single servo to a specific value */ #define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 4b1b0ed0bb..c067d363b7 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1006,6 +1006,40 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; + case PWM_SERVO_SET_COUNT: { + /* change the number of outputs that are enabled for + * PWM. This is used to change the split between GPIO + * and PWM under control of the flight config + * parameters. Note that this does not allow for + * changing a set of pins to be used for serial on + * FMUv1 + */ + switch (arg) { + case 0: + set_mode(MODE_NONE); + break; + + case 2: + set_mode(MODE_2PWM); + break; + + case 4: + set_mode(MODE_4PWM); + break; + +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) + case 6: + set_mode(MODE_6PWM); + break; +#endif + + default: + ret = -EINVAL; + break; + } + break; + } + case MIXERIOCRESET: if (_mixers != nullptr) { delete _mixers; @@ -1443,7 +1477,6 @@ void sensor_reset(int ms) { int fd; - int ret; fd = open(PX4FMU_DEVICE_PATH, O_RDWR); From 0c116e8de5c5c958b9463f147576f3e0377c4c00 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 23 Jan 2014 22:10:03 +0100 Subject: [PATCH 056/108] Implemented S.Bus 2 decoding support --- src/modules/px4iofirmware/sbus.c | 27 +++++++++++++++++++++++++-- 1 file changed, 25 insertions(+), 2 deletions(-) diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 11ccd7356a..4efa72dc1b 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2014 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 @@ -218,7 +218,30 @@ static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_values) { /* check frame boundary markers to avoid out-of-sync cases */ - if ((frame[0] != 0x0f) || (frame[24] != 0x00)) { + if ((frame[0] != 0x0f)) { + sbus_frame_drops++; + return false; + } + + switch (frame[24]) { + case 0x00: + /* this is S.BUS 1 */ + break; + case 0x03: + /* S.BUS 2 SLOT0: RX battery and external voltage */ + break; + case 0x83: + /* S.BUS 2 SLOT1 */ + break; + case 0x43: + case 0xC3: + case 0x23: + case 0xA3: + case 0x63: + case 0xE3: + break; + default: + /* we expect one of these, else abort */ sbus_frame_drops++; return false; } From 8bdbce5fe2893353bf9582294c28ab2831f96a9d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 23 Jan 2014 22:27:04 +0100 Subject: [PATCH 057/108] We do not know all secret S.BUS2 codes yet --- src/modules/px4iofirmware/sbus.c | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 4efa72dc1b..4954477402 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -241,9 +241,8 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint case 0xE3: break; default: - /* we expect one of these, else abort */ - sbus_frame_drops++; - return false; + /* we expect one of the bits above, but there are some we don't know yet */ + break; } /* we have received something we think is a frame */ From d8c1131f1e8e61bcb15b0faa36de1bba00e9716d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 24 Jan 2014 11:06:33 +1100 Subject: [PATCH 058/108] px4io: improved reliability of forceupdate re-starting px4io this adds a 0.1s delay after update to give px4io time to boot. It removes the need for the user to reboot after an IO update --- src/drivers/px4io/px4io_uploader.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index 41f93a8ee1..d9f991210d 100644 --- a/src/drivers/px4io/px4io_uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -51,6 +51,7 @@ #include #include #include +#include #include @@ -226,6 +227,11 @@ PX4IO_Uploader::upload(const char *filenames[]) close(_fw_fd); close(_io_fd); _io_fd = -1; + + // sleep for enough time for the IO chip to boot. This makes + // forceupdate more reliably startup IO again after update + up_udelay(100*1000); + return ret; } From 65118f0c2ef6e4305259a35751c8cb92d751b671 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 24 Jan 2014 17:26:13 +0100 Subject: [PATCH 059/108] Disable debug in the airspeed sensor driver - prevents console spam if it fails (and on probing during startup) --- src/drivers/airspeed/airspeed.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index 5e45cc936c..f73a3ef01e 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -91,7 +91,7 @@ Airspeed::Airspeed(int bus, int address, unsigned conversion_interval) : _comms_errors(perf_alloc(PC_COUNT, "airspeed_comms_errors")) { // enable debug() calls - _debug_enabled = true; + _debug_enabled = false; // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); From 4f78c3e60596d1b596e5ebcf4bb4e101a5b356e7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 24 Jan 2014 17:27:28 +0100 Subject: [PATCH 060/108] Disable PX4IO debug - spams console on comms failure. Each command does report the failure separately, so we get a better feedback level without the spam. --- src/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index a54bb39646..519aa96eb7 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -505,7 +505,7 @@ PX4IO::PX4IO(device::Device *interface) : /* open MAVLink text channel */ _mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0); - _debug_enabled = true; + _debug_enabled = false; _servorail_status.rssi_v = 0; } From 2f968357a368ee59f53d75119b893487abd3883b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 24 Jan 2014 17:28:04 +0100 Subject: [PATCH 061/108] Make the protocol version more descriptive - helps to understand when / how px4io detect fails. --- src/drivers/px4io/px4io.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 519aa96eb7..6f1323fced 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -580,6 +580,12 @@ PX4IO::init() /* get some parameters */ unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION); + if (protocol == _io_reg_get_error) { + log("failed to communicate with IO"); + mavlink_log_emergency(_mavlink_fd, "[IO] failed to communicate with IO, abort."); + return -1; + } + if (protocol != PX4IO_PROTOCOL_VERSION) { log("protocol/firmware mismatch"); mavlink_log_emergency(_mavlink_fd, "[IO] protocol/firmware mismatch, abort."); From 1960f7d6c5c502860ad4f2520eae364a4abfe9e7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 24 Jan 2014 17:29:27 +0100 Subject: [PATCH 062/108] Initialize null pointers correctly, always set the pointer to null after deletes. Remove some verbosity from startup and do not try to initialise IO when we just want to reboot it into the bootloader. --- src/drivers/px4io/px4io.cpp | 39 ++++++++++++++++++++++++++----------- 1 file changed, 28 insertions(+), 11 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 6f1323fced..df847a64da 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -451,7 +451,7 @@ private: namespace { -PX4IO *g_dev; +PX4IO *g_dev = nullptr; } @@ -780,8 +780,6 @@ PX4IO::task_main() hrt_abstime poll_last = 0; hrt_abstime orb_check_last = 0; - log("starting"); - _thread_mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0); /* @@ -815,8 +813,6 @@ PX4IO::task_main() fds[0].fd = _t_actuator_controls_0; fds[0].events = POLLIN; - log("ready"); - /* lock against the ioctl handler */ lock(); @@ -1679,7 +1675,18 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries) total_len++; } - int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, total_len / 2); + int ret; + + for (int i = 0; i < 30; i++) { + /* failed, but give it a 2nd shot */ + ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, total_len / 2); + + if (ret) { + usleep(333); + } else { + break; + } + } /* print mixer chunk */ if (debuglevel > 5 || ret) { @@ -1703,7 +1710,18 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries) msg->text[0] = '\n'; msg->text[1] = '\0'; - int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, (sizeof(px4io_mixdata) + 2) / 2); + int ret; + + for (int i = 0; i < 30; i++) { + /* failed, but give it a 2nd shot */ + ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, (sizeof(px4io_mixdata) + 2) / 2); + + if (ret) { + usleep(333); + } else { + break; + } + } if (ret) return ret; @@ -2705,6 +2723,7 @@ px4io_main(int argc, char *argv[]) printf("[px4io] loaded, detaching first\n"); /* stop the driver */ delete g_dev; + g_dev = nullptr; } PX4IO_Uploader *up; @@ -2788,10 +2807,6 @@ px4io_main(int argc, char *argv[]) delete interface; errx(1, "driver alloc failed"); } - - if (OK != g_dev->init()) { - warnx("driver init failed, still trying.."); - } } uint16_t arg = atol(argv[2]); @@ -2803,6 +2818,7 @@ px4io_main(int argc, char *argv[]) // tear down the px4io instance delete g_dev; + g_dev = nullptr; // upload the specified firmware const char *fn[2]; @@ -2861,6 +2877,7 @@ px4io_main(int argc, char *argv[]) /* stop the driver */ delete g_dev; + g_dev = nullptr; exit(0); } From 73a483c2657d97619021b85759bc742f637cfff4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 24 Jan 2014 17:30:40 +0100 Subject: [PATCH 063/108] Finally fix the timing race between the IO driver, IO uploader and the on-IO firmware by making the uploader tolerant of timing offsets. --- src/drivers/px4io/px4io_uploader.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index d9f991210d..dd8abbac5a 100644 --- a/src/drivers/px4io/px4io_uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -121,8 +121,15 @@ PX4IO_Uploader::upload(const char *filenames[]) cfsetspeed(&t, 115200); tcsetattr(_io_fd, TCSANOW, &t); - /* look for the bootloader */ - ret = sync(); + /* look for the bootloader for 150 ms */ + for (int i = 0; i < 15; i++) { + ret = sync(); + if (ret == OK) { + break; + } else { + usleep(10000); + } + } if (ret != OK) { /* this is immediately fatal */ From 92a6c7d7344ae0a463e0c04c3b5bc6cf8f4ddc53 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 24 Jan 2014 17:33:04 +0100 Subject: [PATCH 064/108] Set timeouts back to short, now that we have multiple tries in the uploader. This ensures we try often enough in the 200 ms IO bootloader wait phase to hit it. --- src/drivers/px4io/uploader.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4io/uploader.h b/src/drivers/px4io/uploader.h index 22387a3e23..55f63eef96 100644 --- a/src/drivers/px4io/uploader.h +++ b/src/drivers/px4io/uploader.h @@ -91,7 +91,7 @@ private: void drain(); int send(uint8_t c); int send(uint8_t *p, unsigned count); - int get_sync(unsigned timeout = 1000); + int get_sync(unsigned timeout = 40); int sync(); int get_info(int param, uint32_t &val); int erase(); From c5cb3cfd2187c82b11bb1f12d644e77ecd807efc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 24 Jan 2014 17:34:42 +0100 Subject: [PATCH 065/108] Make the IO mixer upload report not only a global success / fail flag, but on transfer basis. Also use a crude lock to avoid updating the mixer while it runs (we have no proper mutexes on IO, and this is a pure read/write locking case with two locks, which should make the execution even with this crude approach thread-safe). --- src/modules/px4iofirmware/mixer.cpp | 30 ++++++++++++++++++++++------- 1 file changed, 23 insertions(+), 7 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index e55ef784a9..2e79f0ac65 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2014 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 @@ -71,6 +71,7 @@ extern "C" { static bool mixer_servos_armed = false; static bool should_arm = false; static bool should_always_enable_pwm = false; +static volatile bool in_mixer = false; /* selected control values and count for mixing */ enum mixer_source { @@ -95,6 +96,7 @@ static void mixer_set_failsafe(); void mixer_tick(void) { + /* check that we are receiving fresh data from the FMU */ if (hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) { @@ -199,13 +201,17 @@ mixer_tick(void) } - } else if (source != MIX_NONE) { + } else if (source != MIX_NONE && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { float outputs[PX4IO_SERVO_COUNT]; unsigned mixed; /* mix */ + + /* poor mans mutex */ + in_mixer = true; mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT); + in_mixer = false; pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); @@ -297,12 +303,17 @@ mixer_callback(uintptr_t handle, static char mixer_text[256]; /* large enough for one mixer */ static unsigned mixer_text_length = 0; -void +int mixer_handle_text(const void *buffer, size_t length) { /* do not allow a mixer change while safety off */ if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) { - return; + return 1; + } + + /* abort if we're in the mixer */ + if (in_mixer) { + return 1; } px4io_mixdata *msg = (px4io_mixdata *)buffer; @@ -310,7 +321,7 @@ mixer_handle_text(const void *buffer, size_t length) isr_debug(2, "mix txt %u", length); if (length < sizeof(px4io_mixdata)) - return; + return 0; unsigned text_length = length - sizeof(px4io_mixdata); @@ -328,13 +339,16 @@ mixer_handle_text(const void *buffer, size_t length) case F2I_MIXER_ACTION_APPEND: isr_debug(2, "append %d", length); + /* disable mixing during the update */ + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; + /* 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; + return 0; } - /* append mixer text and nul-terminate */ + /* append mixer text and nul-terminate, guard against overflow */ memcpy(&mixer_text[mixer_text_length], msg->text, text_length); mixer_text_length += text_length; mixer_text[mixer_text_length] = '\0'; @@ -369,6 +383,8 @@ mixer_handle_text(const void *buffer, size_t length) break; } + + return 0; } static void From 15f8e5acf12125eb4fb7b3d5d530b3e27c25f34c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 24 Jan 2014 17:35:29 +0100 Subject: [PATCH 066/108] =?UTF-8?q?Make=20in=20the=20comments=20explicit?= =?UTF-8?q?=20that=20we=20don=E2=80=99t=20do=20anything=20here=20under=20n?= =?UTF-8?q?ormal=20circumstances=20to=20make=20it=20less=20tempting=20to?= =?UTF-8?q?=20comment=20out=20the=20helpful=20debug=20tools=20in=20this=20?= =?UTF-8?q?section.?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/modules/px4iofirmware/px4io.c | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index e2da359394..d4c25911e3 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -295,10 +295,12 @@ user_start(int argc, char *argv[]) check_reboot(); - /* check for debug activity */ + /* check for debug activity (default: none) */ show_debug_messages(); - /* post debug state at ~1Hz */ + /* post debug state at ~1Hz - this is via an auxiliary serial port + * DEFAULTS TO OFF! + */ if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) { struct mallinfo minfo = mallinfo(); From 33688fec9c66692e88a1b328fd022adc6e906853 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 24 Jan 2014 17:36:13 +0100 Subject: [PATCH 067/108] Make the sensors app less verbose --- src/modules/sensors/sensors.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 9903a90a0e..23f20b0cb2 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -797,7 +797,6 @@ Sensors::accel_init() #endif - warnx("using system accel"); close(fd); } } @@ -837,7 +836,6 @@ Sensors::gyro_init() #endif - warnx("using system gyro"); close(fd); } } @@ -1507,9 +1505,6 @@ void Sensors::task_main() { - /* inform about start */ - warnx("Initializing.."); - /* start individual sensors */ accel_init(); gyro_init(); From bd15653b173029dfc12c3d4a73b897570e0867c0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 24 Jan 2014 17:37:01 +0100 Subject: [PATCH 068/108] Use the proper status registers for locking out from mixer updates and return the value of the mixer change. --- src/modules/px4iofirmware/registers.c | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index ad44730736..2c437d2c0c 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -382,7 +382,10 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* handle text going to the mixer parser */ case PX4IO_PAGE_MIXERLOAD: - mixer_handle_text(values, num_values * sizeof(*values)); + if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) || + (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { + return mixer_handle_text(values, num_values * sizeof(*values)); + } break; default: @@ -509,8 +512,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) case PX4IO_P_SETUP_REBOOT_BL: if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) || - (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) || - (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { + (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { // don't allow reboot while armed break; } @@ -540,8 +542,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) * do not allow a RC config change while outputs armed */ if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) || - (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) || - (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { + (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { break; } From 880342b9c1e90e0c22180d7fe1411d0988d97a49 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 24 Jan 2014 17:37:34 +0100 Subject: [PATCH 069/108] Missing header for mixer status change. --- src/modules/px4iofirmware/px4io.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index bffbc0ce2a..393e0560e8 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -178,7 +178,7 @@ extern pwm_limit_t pwm_limit; * Mixer */ extern void mixer_tick(void); -extern void mixer_handle_text(const void *buffer, size_t length); +extern int mixer_handle_text(const void *buffer, size_t length); /** * Safety switch/LED. From e07d91613b0a90fd4a9ce8c2e10d3bff8a1ebc44 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 24 Jan 2014 18:24:54 +0100 Subject: [PATCH 070/108] Remove unused field --- src/modules/px4iofirmware/safety.c | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/px4iofirmware/safety.c b/src/modules/px4iofirmware/safety.c index 2ce479ffd3..ff2e4af6ea 100644 --- a/src/modules/px4iofirmware/safety.c +++ b/src/modules/px4iofirmware/safety.c @@ -45,7 +45,6 @@ #include "px4io.h" static struct hrt_call arming_call; -static struct hrt_call heartbeat_call; static struct hrt_call failsafe_call; /* From f2f94f0f176b9a2d818a0849eae18f018d7fb5a9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 25 Jan 2014 23:03:21 +0100 Subject: [PATCH 071/108] IO driver: Variable name and comment cleanup, no binary / functionality changes. --- src/drivers/px4io/px4io.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index df847a64da..e83fa68d5e 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -444,7 +444,7 @@ private: * @param vservo vservo register * @param vrssi vrssi register */ - void io_handle_vservo(uint16_t vbatt, uint16_t ibatt); + void io_handle_vservo(uint16_t vservo, uint16_t vrssi); }; @@ -1357,7 +1357,10 @@ PX4IO::io_get_status() uint16_t regs[6]; int ret; - /* get STATUS_FLAGS, STATUS_ALARMS, STATUS_VBATT, STATUS_IBATT in that order */ + /* get + * STATUS_FLAGS, STATUS_ALARMS, STATUS_VBATT, STATUS_IBATT, + * STATUS_VSERVO, STATUS_VRSSI, STATUS_PRSSI + * in that order */ ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, ®s[0], sizeof(regs) / sizeof(regs[0])); if (ret != OK) From bafcbd99a695a3c4d478fb58e1d53940f331392f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 25 Jan 2014 23:04:16 +0100 Subject: [PATCH 072/108] Stop setting RSSI by cross-reading servo status. --- src/drivers/px4io/px4io.cpp | 7 ------- 1 file changed, 7 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index e83fa68d5e..4bdf530a79 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1457,13 +1457,6 @@ PX4IO::io_publish_raw_rc() rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN; } - /* set RSSI */ - - if (rc_val.input_source != RC_INPUT_SOURCE_PX4IO_SBUS) { - // XXX the correct scaling needs to be validated here - rc_val.rssi = (_servorail_status.rssi_v / 3.3f) * UINT8_MAX; - } - /* lazily advertise on first publication */ if (_to_input_rc == 0) { _to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_val); From aff11d6d8611d5e5b72742d2bee4132168fefa72 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 25 Jan 2014 23:06:53 +0100 Subject: [PATCH 073/108] =?UTF-8?q?IO=20firmware:=20Use=20right=20base=20r?= =?UTF-8?q?eg=20value=20-=20since=20the=20wrong=20one=20had=20the=20same?= =?UTF-8?q?=20value=20this=20hasn=E2=80=99t=20been=20an=20issue,=20but=20i?= =?UTF-8?q?t=20would=20have=20become=20one=20once=20one=20of=20them=20chan?= =?UTF-8?q?ged.?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/modules/px4iofirmware/px4io.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 393e0560e8..ee9d5b0c41 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -97,7 +97,7 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */ #define r_raw_rc_count r_page_raw_rc_input[PX4IO_P_RAW_RC_COUNT] #define r_raw_rc_values (&r_page_raw_rc_input[PX4IO_P_RAW_RC_BASE]) #define r_rc_valid r_page_rc_input[PX4IO_P_RC_VALID] -#define r_rc_values (&r_page_rc_input[PX4IO_P_RAW_RC_BASE]) +#define r_rc_values (&r_page_rc_input[PX4IO_P_RC_BASE]) #define r_setup_features r_page_setup[PX4IO_P_SETUP_FEATURES] #define r_setup_arming r_page_setup[PX4IO_P_SETUP_ARMING] From b06d199129d57eabe2b73da713c9ac4ce98a68bf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 25 Jan 2014 23:10:48 +0100 Subject: [PATCH 074/108] Fixed year in controls.c comment. --- src/modules/px4iofirmware/controls.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 5e2c92bf46..ed37cc43d4 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2014 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 From eee2508644ea1ca3b267ed89db6d0deb8fe0d3e1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Jan 2014 00:14:19 +0100 Subject: [PATCH 075/108] Add additional flags to RC topic, not used yet. --- src/drivers/drv_rc_input.h | 28 ++++++++++++++++++++++++++++ 1 file changed, 28 insertions(+) diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h index 6b87141e9d..0afe2f16f0 100644 --- a/src/drivers/drv_rc_input.h +++ b/src/drivers/drv_rc_input.h @@ -92,6 +92,34 @@ struct rc_input_values { /** receive signal strength indicator (RSSI): < 0: Undefined, 0: no signal, 255: full reception */ int32_t rssi; + /** + * explicit failsafe flag: true on TX failure or TX out of range , false otherwise. + * Only the true state is reliable, as there are some (PPM) receivers on the market going + * into failsafe without telling us explicitly. + * */ + bool rc_failsafe; + + /** + * RC receiver connection status: True,if no frame has arrived in the expected time, false otherwise. + * True usally means that the receiver has been disconnected, but can also indicate a radio link loss on "stupid" systems. + * Will remain false, if a RX with failsafe option continues to transmit frames after a link loss. + * */ + bool rc_lost; + + /** + * Number of lost RC frames. + * Note: intended purpose: observe the radio link quality if RSSI is not available + * This value must not be used to trigger any failsafe-alike funtionality. + * */ + uint16_t rc_lost_frame_count; + + /** + * Number of total RC frames. + * Note: intended purpose: observe the radio link quality if RSSI is not available + * This value must not be used to trigger any failsafe-alike funtionality. + * */ + uint16_t rc_total_frame_count; + /** Input source */ enum RC_INPUT_SOURCE input_source; From 57d38bc8cec1362308f632e74e99485f82a35501 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Jan 2014 00:17:26 +0100 Subject: [PATCH 076/108] Clean up RC related metadata, put everything into the RC data page. This ensures atomic reads, makes the reads more efficient and allows for some headroom for more RC flags. The IO driver side is updated as well, however, these flags are not published yet. --- src/drivers/px4io/px4io.cpp | 10 ++++-- src/modules/px4iofirmware/controls.c | 49 +++++++++++++++++++++------ src/modules/px4iofirmware/protocol.h | 14 +++++--- src/modules/px4iofirmware/px4io.h | 3 +- src/modules/px4iofirmware/registers.c | 8 +++-- src/modules/px4iofirmware/sbus.c | 21 +++++++----- 6 files changed, 76 insertions(+), 29 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 4bdf530a79..382acb6d08 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1763,6 +1763,7 @@ PX4IO::print_status() printf("%u bytes free\n", io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM)); uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS); + uint16_t io_status_flags = flags; printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s%s\n", flags, ((flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED) ? " OUTPUTS_ARMED" : ""), @@ -1770,8 +1771,6 @@ PX4IO::print_status() ((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""), ((flags & PX4IO_P_STATUS_FLAGS_RC_OK) ? " RC_OK" : " RC_FAIL"), ((flags & PX4IO_P_STATUS_FLAGS_RC_PPM) ? " PPM" : ""), - (((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (!(flags & PX4IO_P_STATUS_FLAGS_RC_DSM11))) ? " DSM10" : ""), - (((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_STATUS_FLAGS_RC_DSM11)) ? " DSM11" : ""), ((flags & PX4IO_P_STATUS_FLAGS_RC_SBUS) ? " SBUS" : ""), ((flags & PX4IO_P_STATUS_FLAGS_FMU_OK) ? " FMU_OK" : " FMU_FAIL"), ((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) ? " RAW_PWM_PASSTHROUGH" : ""), @@ -1824,6 +1823,11 @@ PX4IO::print_status() printf("\n"); uint16_t raw_inputs = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT); printf("%d raw R/C inputs", raw_inputs); + flags = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_FLAGS); + printf("status 0x%04x%s", flags, + (((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (!(flags & PX4IO_P_RAW_RC_FLAGS_RC_DSM11))) ? " DSM10" : ""), + (((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_RAW_RC_FLAGS_RC_DSM11)) ? " DSM11" : "") + ); for (unsigned i = 0; i < raw_inputs; i++) printf(" %u", io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + i)); @@ -1831,7 +1835,7 @@ PX4IO::print_status() printf("\n"); if ((flags & PX4IO_P_STATUS_FLAGS_RC_PPM)) { - int frame_len = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_RC_DATA); + int frame_len = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_DATA); printf("RC data (PPM frame len) %u us\n", frame_len); if ((frame_len - raw_inputs * 2000 - 3000) < 0) { diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index ed37cc43d4..1cac977d7a 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -97,28 +97,57 @@ controls_tick() { /* receive signal strenght indicator (RSSI). 0 = no connection, 255: perfect connection */ uint16_t rssi = 0; +#ifdef ADC_RSSI + unsigned counts = adc_measure(ADC_RSSI); + if (counts != 0xffff) { + /* use 1:1 scaling on 3.3V ADC input */ + unsigned mV = counts * 3300 / 4096; + + /* scale to 0..253 */ + rssi = mV / 13; + } +#endif + perf_begin(c_gather_dsm); uint16_t temp_count = r_raw_rc_count; bool dsm_updated = dsm_input(r_raw_rc_values, &temp_count); if (dsm_updated) { - r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM; + r_raw_rc_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM; r_raw_rc_count = temp_count & 0x7fff; if (temp_count & 0x8000) - r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM11; + r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11; else - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RC_DSM11; + r_raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_DSM11; - rssi = 255; } perf_end(c_gather_dsm); perf_begin(c_gather_sbus); - bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &rssi, PX4IO_RC_INPUT_CHANNELS); bool sbus_status = (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS); + bool sbus_failsafe, sbus_frame_drop; + bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, PX4IO_RC_INPUT_CHANNELS); + if (sbus_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS; + + rssi = 255; + + if (sbus_frame_drop) { + r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FRAME_DROP; + rssi = 100; + } else { + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); + } + + if (sbus_failsafe) { + r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE; + rssi = 0; + } else { + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); + } + } /* switch S.Bus output pin as needed */ @@ -136,12 +165,9 @@ controls_tick() { * disable the PPM decoder completely if we have S.bus signal. */ perf_begin(c_gather_ppm); - bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_status[PX4IO_P_STATUS_RC_DATA]); + bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_raw_rc_input[PX4IO_P_RAW_RC_DATA]); if (ppm_updated) { - /* XXX sample RSSI properly here */ - rssi = 255; - r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM; } perf_end(c_gather_ppm); @@ -150,6 +176,9 @@ controls_tick() { if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS) r_raw_rc_count = PX4IO_RC_INPUT_CHANNELS; + /* store RSSI */ + r_page_raw_rc_input[PX4IO_P_RAW_RC_NRSSI] = rssi; + /* * In some cases we may have received a frame, but input has still * been lost. @@ -247,7 +276,7 @@ controls_tick() { * This might happen if a protocol-based receiver returns an update * that contains no channels that we have mapped. */ - if (assigned_channels == 0 || rssi == 0) { + if (assigned_channels == 0 || (r_raw_rc_flags & (PX4IO_P_RAW_RC_FLAGS_FAILSAFE))) { rc_input_lost = true; } else { /* set RC OK flag */ diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index e5bef6eb3a..738dc7d6ed 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -111,7 +111,6 @@ #define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 10) /* initialisation of the IO completed without error */ #define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 11) /* failsafe is active */ #define PX4IO_P_STATUS_FLAGS_SAFETY_OFF (1 << 12) /* safety is off */ -#define PX4IO_P_STATUS_FLAGS_RC_DSM11 (1 << 13) /* DSM input is 11 bit data */ #define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */ #define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* [1] VBatt is very close to regulator dropout */ @@ -128,8 +127,6 @@ #define PX4IO_P_STATUS_VSERVO 6 /* [2] servo rail voltage in mV */ #define PX4IO_P_STATUS_VRSSI 7 /* [2] RSSI voltage */ #define PX4IO_P_STATUS_PRSSI 8 /* [2] RSSI PWM value */ -#define PX4IO_P_STATUS_NRSSI 9 /* [2] Normalized RSSI value, 0: no reception, 255: perfect reception */ -#define PX4IO_P_STATUS_RC_DATA 10 /* [1] + [2] Details about the RC source (PPM frame length, Spektrum protocol type) */ /* array of post-mix actuator outputs, -10000..10000 */ #define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */ @@ -140,7 +137,16 @@ /* array of raw RC input values, microseconds */ #define PX4IO_PAGE_RAW_RC_INPUT 4 #define PX4IO_P_RAW_RC_COUNT 0 /* number of valid channels */ -#define PX4IO_P_RAW_RC_BASE 1 /* CONFIG_RC_INPUT_COUNT channels from here */ +#define PX4IO_P_RAW_RC_FLAGS 1 /* RC detail status flags */ +#define PX4IO_P_RAW_RC_FLAGS_FRAME_DROP (1 << 0) /* single frame drop */ +#define PX4IO_P_RAW_RC_FLAGS_FAILSAFE (1 << 1) /* receiver is in failsafe mode */ +#define PX4IO_P_RAW_RC_FLAGS_RC_DSM11 (1 << 2) /* DSM decoding is 11 bit mode */ + +#define PX4IO_P_RAW_RC_NRSSI 2 /* [2] Normalized RSSI value, 0: no reception, 255: perfect reception */ +#define PX4IO_P_RAW_RC_DATA 3 /* [1] + [2] Details about the RC source (PPM frame length, Spektrum protocol type) */ +#define PX4IO_P_RAW_FRAME_COUNT 4 /* Number of total received frames (wrapping counter) */ +#define PX4IO_P_RAW_LOST_FRAME_COUNT 5 /* Number of total dropped frames (wrapping counter) */ +#define PX4IO_P_RAW_RC_BASE 6 /* CONFIG_RC_INPUT_COUNT channels from here */ /* array of scaled RC input values, -10000..10000 */ #define PX4IO_PAGE_RC_INPUT 5 diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index ee9d5b0c41..a2e1ed6cde 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -96,6 +96,7 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */ #define r_raw_rc_count r_page_raw_rc_input[PX4IO_P_RAW_RC_COUNT] #define r_raw_rc_values (&r_page_raw_rc_input[PX4IO_P_RAW_RC_BASE]) +#define r_raw_rc_flags r_page_raw_rc_input[PX4IO_P_RAW_RC_FLAGS] #define r_rc_valid r_page_rc_input[PX4IO_P_RC_VALID] #define r_rc_values (&r_page_rc_input[PX4IO_P_RC_BASE]) @@ -215,7 +216,7 @@ extern int dsm_init(const char *device); extern bool dsm_input(uint16_t *values, uint16_t *num_values); extern void dsm_bind(uint16_t cmd, int pulses); extern int sbus_init(const char *device); -extern bool sbus_input(uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels); +extern bool sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels); /** global debug level for isr_debug() */ extern volatile uint8_t debug_level; diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 2c437d2c0c..477efa0826 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -90,8 +90,6 @@ uint16_t r_page_status[] = { [PX4IO_P_STATUS_VSERVO] = 0, [PX4IO_P_STATUS_VRSSI] = 0, [PX4IO_P_STATUS_PRSSI] = 0, - [PX4IO_P_STATUS_NRSSI] = 0, - [PX4IO_P_STATUS_RC_DATA] = 0 }; /** @@ -116,6 +114,12 @@ uint16_t r_page_servos[PX4IO_SERVO_COUNT]; uint16_t r_page_raw_rc_input[] = { [PX4IO_P_RAW_RC_COUNT] = 0, + [PX4IO_P_RAW_RC_FLAGS] = 0, + [PX4IO_P_RAW_RC_NRSSI] = 0, + [PX4IO_P_RAW_RC_DATA] = 0, + [PX4IO_P_RAW_FRAME_COUNT] = 0, + [PX4IO_P_RAW_LOST_FRAME_COUNT] = 0, + [PX4IO_P_RAW_RC_DATA] = 0, [PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + PX4IO_RC_INPUT_CHANNELS)] = 0 }; diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 4954477402..32ab2a6988 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -87,7 +87,7 @@ static unsigned partial_frame_count; unsigned sbus_frame_drops; -static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels); +static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels); int sbus_init(const char *device) @@ -118,7 +118,7 @@ sbus_init(const char *device) } bool -sbus_input(uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels) +sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels) { ssize_t ret; hrt_abstime now; @@ -175,7 +175,7 @@ sbus_input(uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_ * decode it. */ partial_frame_count = 0; - return sbus_decode(now, values, num_values, rssi, max_channels); + return sbus_decode(now, values, num_values, sbus_failsafe, sbus_frame_drop, max_channels); } /* @@ -215,7 +215,7 @@ static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = { }; static bool -sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_values) +sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values) { /* check frame boundary markers to avoid out-of-sync cases */ if ((frame[0] != 0x0f)) { @@ -289,20 +289,23 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint /* decode and handle failsafe and frame-lost flags */ if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FAILSAFE_BIT)) { /* failsafe */ /* report that we failed to read anything valid off the receiver */ - *rssi = 0; + *sbus_failsafe = true; + *sbus_frame_drop = true; return false; } else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */ - /* set a special warning flag or try to calculate some kind of RSSI information - to be implemented + /* set a special warning flag * * Attention! This flag indicates a skipped frame only, not a total link loss! Handling this * condition as fail-safe greatly reduces the reliability and range of the radio link, * e.g. by prematurely issueing return-to-launch!!! */ - *rssi = 100; // XXX magic number indicating bad signal, but not a signal loss (yet) + *sbus_failsafe = false; + *sbus_frame_drop = true; + } else { + *sbus_failsafe = false; + *sbus_frame_drop = false; } - *rssi = 255; - return true; } From a737a2a4061babb3de524ac2001a659786081e4a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Jan 2014 13:22:26 +0100 Subject: [PATCH 077/108] RSSI and SBUS out config now handled as setup feature flags. --- src/modules/px4iofirmware/protocol.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 738dc7d6ed..69c12d877d 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -163,6 +163,10 @@ /* setup page */ #define PX4IO_PAGE_SETUP 50 #define PX4IO_P_SETUP_FEATURES 0 +#define PX4IO_P_SETUP_FEATURES_SBUS1_OUT (1 << 0) /* enable S.Bus v1 output */ +#define PX4IO_P_SETUP_FEATURES_SBUS2_OUT (1 << 1) /* enable S.Bus v2 output */ +#define PX4IO_P_SETUP_FEATURES_PWM_RSSI (1 << 2) /* enable PWM RSSI parsing */ +#define PX4IO_P_SETUP_FEATURES_ADC_RSSI (1 << 3) /* enable ADC RSSI parsing */ #define PX4IO_P_SETUP_ARMING 1 /* arming controls */ #define PX4IO_P_SETUP_ARMING_IO_ARM_OK (1 << 0) /* OK to arm the IO side */ From 16eb68f2e9b1de47b4f77d2ccaf72f102eb67fdf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Jan 2014 14:13:57 +0100 Subject: [PATCH 078/108] Allow the setup flags to control the S.BUS / RSSI port instead of doing some wild, likely incorrect, guesses. --- src/modules/px4iofirmware/controls.c | 21 +++++-------- src/modules/px4iofirmware/registers.c | 44 +++++++++++++++++++++++++-- 2 files changed, 49 insertions(+), 16 deletions(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 1cac977d7a..e70eaed093 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -98,13 +98,15 @@ controls_tick() { uint16_t rssi = 0; #ifdef ADC_RSSI - unsigned counts = adc_measure(ADC_RSSI); - if (counts != 0xffff) { - /* use 1:1 scaling on 3.3V ADC input */ - unsigned mV = counts * 3300 / 4096; + if (r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) { + unsigned counts = adc_measure(ADC_RSSI); + if (counts != 0xffff) { + /* use 1:1 scaling on 3.3V ADC input */ + unsigned mV = counts * 3300 / 4096; - /* scale to 0..253 */ - rssi = mV / 13; + /* scale to 0..253 */ + rssi = mV / 13; + } } #endif @@ -150,13 +152,6 @@ controls_tick() { } - /* switch S.Bus output pin as needed */ - if (sbus_status != (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS)) { - #ifdef ENABLE_SBUS_OUT - ENABLE_SBUS_OUT((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS)); - #endif - } - perf_end(c_gather_sbus); /* diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 477efa0826..2d7a1fe0d1 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -148,7 +148,12 @@ uint16_t r_page_scratch[32]; */ volatile uint16_t r_page_setup[] = { +#ifdef CONFIG_ARCH_BOARD_PX4IO_V2 + /* default to RSSI ADC functionality */ + [PX4IO_P_SETUP_FEATURES] = PX4IO_P_SETUP_FEATURES_ADC_RSSI, +#else [PX4IO_P_SETUP_FEATURES] = 0, +#endif [PX4IO_P_SETUP_ARMING] = 0, [PX4IO_P_SETUP_PWM_RATES] = 0, [PX4IO_P_SETUP_PWM_DEFAULTRATE] = 50, @@ -166,7 +171,14 @@ volatile uint16_t r_page_setup[] = [PX4IO_P_SETUP_CRC ... (PX4IO_P_SETUP_CRC+1)] = 0, }; -#define PX4IO_P_SETUP_FEATURES_VALID (0) +#ifdef CONFIG_ARCH_BOARD_PX4IO_V2 +#define PX4IO_P_SETUP_FEATURES_VALID (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | \ + PX4IO_P_SETUP_FEATURES_SBUS2_OUT | \ + PX4IO_P_SETUP_FEATURES_ADC_RSSI | \ + PX4IO_P_SETUP_FEATURES_PWM_RSSI) +#else +#define PX4IO_P_SETUP_FEATURES_VALID 0 +#endif #define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_FMU_ARMED | \ PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \ PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \ @@ -442,9 +454,35 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) case PX4IO_P_SETUP_FEATURES: value &= PX4IO_P_SETUP_FEATURES_VALID; - r_setup_features = value; - /* no implemented feature selection at this point */ + /* some of the options conflict - give S.BUS out precedence, then ADC RSSI, then PWM RSSI */ + + /* switch S.Bus output pin as needed */ + #ifdef ENABLE_SBUS_OUT + ENABLE_SBUS_OUT(value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT)); + + /* disable the conflicting options */ + if (value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT)) { + value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | PX4IO_P_SETUP_FEATURES_ADC_RSSI); + } + #endif + + /* disable the conflicting options with ADC RSSI */ + if (value & (PX4IO_P_SETUP_FEATURES_ADC_RSSI)) { + value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | + PX4IO_P_SETUP_FEATURES_SBUS1_OUT | + PX4IO_P_SETUP_FEATURES_SBUS2_OUT); + } + + /* disable the conflicting options with PWM RSSI (without effect here, but for completeness) */ + if (value & (PX4IO_P_SETUP_FEATURES_PWM_RSSI)) { + value &= ~(PX4IO_P_SETUP_FEATURES_ADC_RSSI | + PX4IO_P_SETUP_FEATURES_SBUS1_OUT | + PX4IO_P_SETUP_FEATURES_SBUS2_OUT); + } + + /* apply changes */ + r_setup_features = value; break; From 731ab465b3d7d40ffb5ce3ca3d14660c6fee1ae6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Jan 2014 14:22:54 +0100 Subject: [PATCH 079/108] Add support in the IO driver to control the S.Bus / RSSI port. --- src/drivers/drv_rc_input.h | 8 ++- src/drivers/drv_sbus.h | 58 ++++++++++++++++++++ src/drivers/px4io/px4io.cpp | 103 ++++++++++++++++++++++++++++++++++-- 3 files changed, 163 insertions(+), 6 deletions(-) create mode 100644 src/drivers/drv_sbus.h diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h index 0afe2f16f0..715df7e013 100644 --- a/src/drivers/drv_rc_input.h +++ b/src/drivers/drv_rc_input.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2014 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 @@ -135,8 +135,12 @@ ORB_DECLARE(input_rc); #define _RC_INPUT_BASE 0x2b00 /** Fetch R/C input values into (rc_input_values *)arg */ - #define RC_INPUT_GET _IOC(_RC_INPUT_BASE, 0) +/** Enable RSSI input via ADC */ +#define RC_INPUT_ENABLE_RSSI_ANALOG _IOC(_RC_INPUT_BASE, 1) + +/** Enable RSSI input via PWM signal */ +#define RC_INPUT_ENABLE_RSSI_PWM _IOC(_RC_INPUT_BASE, 2) #endif /* _DRV_RC_INPUT_H */ diff --git a/src/drivers/drv_sbus.h b/src/drivers/drv_sbus.h new file mode 100644 index 0000000000..927c904ec2 --- /dev/null +++ b/src/drivers/drv_sbus.h @@ -0,0 +1,58 @@ +/**************************************************************************** + * + * Copyright (c) 2014 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 drv_sbus.h + * + * Futaba S.BUS / S.BUS 2 compatible interface. + */ + +#ifndef _DRV_SBUS_H +#define _DRV_SBUS_H + +#include +#include + +#include "drv_orb_dev.h" + +/** + * Path for the default S.BUS device + */ +#define SBUS_DEVICE_PATH "/dev/sbus" + +#define _SBUS_BASE 0x2c00 + +/** Enable S.BUS version 1 / 2 output (0 to disable) */ +#define SBUS_SET_PROTO_VERSION _IOC(_SBUS_BASE, 0) + +#endif /* _DRV_SBUS_H */ diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 382acb6d08..0fb7b7d24f 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -61,6 +61,7 @@ #include #include #include +#include #include #include #include @@ -1824,9 +1825,11 @@ PX4IO::print_status() uint16_t raw_inputs = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT); printf("%d raw R/C inputs", raw_inputs); flags = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_FLAGS); - printf("status 0x%04x%s", flags, + printf("status 0x%04x%s%s%s%s", flags, (((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (!(flags & PX4IO_P_RAW_RC_FLAGS_RC_DSM11))) ? " DSM10" : ""), - (((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_RAW_RC_FLAGS_RC_DSM11)) ? " DSM11" : "") + (((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_RAW_RC_FLAGS_RC_DSM11)) ? " DSM11" : ""), + ((flags & PX4IO_P_RAW_RC_FLAGS_FRAME_DROP) ? " FRAME_DROP" : ""), + ((flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) ? " FAILSAFE" : "") ); for (unsigned i = 0; i < raw_inputs; i++) @@ -1861,7 +1864,13 @@ PX4IO::print_status() printf("\n"); /* setup and state */ - printf("features 0x%04x\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES)); + uint16_t features = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES); + printf("features 0x%04x%s%s\n", features, + ((features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) ? " S.BUS1_OUT" : ""), + ((features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) ? " S.BUS2_OUT" : ""), + ((features & PX4IO_P_SETUP_FEATURES_PWM_RSSI) ? " RSSI_PWM" : ""), + ((features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) ? " RSSI_ADC" : "") + ); uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING); printf("arming 0x%04x%s%s%s%s%s%s\n", arming, @@ -2283,6 +2292,38 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) break; + case RC_INPUT_ENABLE_RSSI_ANALOG: + + if (arg) { + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_ADC_RSSI); + } else { + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, PX4IO_P_SETUP_FEATURES_ADC_RSSI, 0); + } + + break; + + case RC_INPUT_ENABLE_RSSI_PWM: + + if (arg) { + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_PWM_RSSI); + } else { + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, PX4IO_P_SETUP_FEATURES_PWM_RSSI, 0); + } + + break; + + case SBUS_SET_PROTO_VERSION: + + if (arg == 1) { + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_SBUS1_OUT); + } else if (arg == 2) { + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_SBUS2_OUT); + } else { + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT), 0); + } + + break; + default: /* not a recognized value */ ret = -ENOTTY; @@ -2932,6 +2973,60 @@ px4io_main(int argc, char *argv[]) if (!strcmp(argv[1], "bind")) bind(argc, argv); + if (!strcmp(argv[1], "sbus1_out")) { + /* we can cheat and call the driver directly, as it + * doesn't reference filp in ioctl() + */ + int ret = g_dev->ioctl(nullptr, SBUS_SET_PROTO_VERSION, 1); + + if (ret != 0) { + errx(ret, "S.BUS v1 failed"); + } + + exit(0); + } + + if (!strcmp(argv[1], "sbus2_out")) { + /* we can cheat and call the driver directly, as it + * doesn't reference filp in ioctl() + */ + int ret = g_dev->ioctl(nullptr, SBUS_SET_PROTO_VERSION, 2); + + if (ret != 0) { + errx(ret, "S.BUS v2 failed"); + } + + exit(0); + } + + if (!strcmp(argv[1], "rssi_analog")) { + /* we can cheat and call the driver directly, as it + * doesn't reference filp in ioctl() + */ + int ret = g_dev->ioctl(nullptr, RC_INPUT_ENABLE_RSSI_ANALOG, 1); + + if (ret != 0) { + errx(ret, "RSSI analog failed"); + } + + exit(0); + } + + if (!strcmp(argv[1], "rssi_pwm")) { + /* we can cheat and call the driver directly, as it + * doesn't reference filp in ioctl() + */ + int ret = g_dev->ioctl(nullptr, RC_INPUT_ENABLE_RSSI_PWM, 1); + + if (ret != 0) { + errx(ret, "RSSI PWM failed"); + } + + exit(0); + } + out: - errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'bind', 'checkcrc', 'forceupdate' or 'update'"); + errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug ',\n" + "'recovery', 'limit ', 'current', 'bind', 'checkcrc',\n" + "'forceupdate', 'update', 'sbus1_out', 'sbus2_out', 'rssi_analog' or 'rssi_pwm'"); } From 9cdbbab855d463bffb39d8dd55888fc1e0423818 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Jan 2014 14:52:46 +0100 Subject: [PATCH 080/108] Differentiate between publication and signal receive timestamp, correctly set the rc_lost flag in the frame. Ready for prime-time testing. --- src/drivers/drv_rc_input.h | 18 +++++++++++++++-- src/drivers/px4fmu/fmu.cpp | 12 ++++++++++-- src/drivers/px4io/px4io.cpp | 31 ++++++++++++++++++++++++------ src/modules/mavlink/orb_listener.c | 2 +- src/modules/sensors/sensors.cpp | 9 ++++++--- src/systemcmds/tests/test_rc.c | 2 +- 6 files changed, 59 insertions(+), 15 deletions(-) diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h index 715df7e013..20763e2657 100644 --- a/src/drivers/drv_rc_input.h +++ b/src/drivers/drv_rc_input.h @@ -62,6 +62,11 @@ */ #define RC_INPUT_MAX_CHANNELS 18 +/** + * Maximum RSSI value + */ +#define RC_INPUT_RSSI_MAX 255 + /** * Input signal type, value is a control position from zero to 100 * percent. @@ -83,8 +88,11 @@ enum RC_INPUT_SOURCE { * on the board involved. */ struct rc_input_values { - /** decoding time */ - uint64_t timestamp; + /** publication time */ + uint64_t timestamp_publication; + + /** last valid reception time */ + uint64_t timestamp_last_signal; /** number of channels actually being seen */ uint32_t channel_count; @@ -120,6 +128,12 @@ struct rc_input_values { * */ uint16_t rc_total_frame_count; + /** + * Length of a single PPM frame. + * Zero for non-PPM systems + */ + uint16_t rc_ppm_frame_length; + /** Input source */ enum RC_INPUT_SOURCE input_source; diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index c067d363b7..0fbd849243 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -626,7 +626,7 @@ PX4FMU::task_main() #ifdef HRT_PPM_CHANNEL // see if we have new PPM input data - if (ppm_last_valid_decode != rc_in.timestamp) { + if (ppm_last_valid_decode != rc_in.timestamp_last_signal) { // we have a new PPM frame. Publish it. rc_in.channel_count = ppm_decoded_channels; @@ -638,7 +638,15 @@ PX4FMU::task_main() rc_in.values[i] = ppm_buffer[i]; } - rc_in.timestamp = ppm_last_valid_decode; + rc_in.timestamp_publication = ppm_last_valid_decode; + rc_in.timestamp_last_signal = ppm_last_valid_decode; + + rc_in.rc_ppm_frame_length = ppm_frame_length; + rc_in.rssi = RC_INPUT_RSSI_MAX; + rc_in.rc_failsafe = false; + rc_in.rc_lost = false; + rc_in.rc_lost_frame_count = 0; + rc_in.rc_total_frame_count = 0; /* lazily advertise on first publication */ if (to_input_rc == 0) { diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 0fb7b7d24f..5601230a42 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -239,6 +239,7 @@ private: unsigned _update_interval; ///< Subscription interval limiting send rate bool _rc_handling_disabled; ///< If set, IO does not evaluate, but only forward the RC values unsigned _rc_chan_count; ///< Internal copy of the last seen number of RC channels + uint64_t _rc_last_valid; ///< last valid timestamp volatile int _task; ///< worker task id volatile bool _task_should_exit; ///< worker terminate flag @@ -468,6 +469,7 @@ PX4IO::PX4IO(device::Device *interface) : _update_interval(0), _rc_handling_disabled(false), _rc_chan_count(0), + _rc_last_valid(0), _task(-1), _task_should_exit(false), _mavlink_fd(-1), @@ -1398,7 +1400,8 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) * * This should be the common case (9 channel R/C control being a reasonable upper bound). */ - input_rc.timestamp = hrt_absolute_time(); + input_rc.timestamp_publication = hrt_absolute_time(); + ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT, ®s[0], prolog + 9); if (ret != OK) @@ -1408,13 +1411,25 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) * Get the channel count any any extra channels. This is no more expensive than reading the * channel count once. */ - channel_count = regs[0]; + channel_count = regs[PX4IO_P_RAW_RC_COUNT]; if (channel_count != _rc_chan_count) perf_count(_perf_chan_count); _rc_chan_count = channel_count; + input_rc.rc_ppm_frame_length = regs[PX4IO_P_RAW_RC_DATA]; + input_rc.rssi = regs[PX4IO_P_RAW_RC_NRSSI]; + input_rc.rc_failsafe = (regs[PX4IO_P_RAW_RC_FLAGS] & PX4IO_P_RAW_RC_FLAGS_FAILSAFE); + input_rc.rc_lost_frame_count = regs[PX4IO_P_RAW_LOST_FRAME_COUNT]; + input_rc.rc_total_frame_count = regs[PX4IO_P_RAW_FRAME_COUNT]; + + /* rc_lost has to be set before the call to this function */ + if (!input_rc.rc_lost && !input_rc.rc_failsafe) + _rc_last_valid = input_rc.timestamp_publication; + + input_rc.timestamp_last_signal = _rc_last_valid; + if (channel_count > 9) { ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + 9, ®s[prolog + 9], channel_count - 9); @@ -1431,13 +1446,12 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) int PX4IO::io_publish_raw_rc() { - /* if no raw RC, just don't publish */ - if (!(_status & PX4IO_P_STATUS_FLAGS_RC_OK)) - return OK; /* fetch values from IO */ rc_input_values rc_val; - rc_val.timestamp = hrt_absolute_time(); + + /* set the RC status flag ORDER MATTERS! */ + rc_val.rc_lost = !(_status & PX4IO_P_STATUS_FLAGS_RC_OK); int ret = io_get_raw_rc_input(rc_val); @@ -1456,6 +1470,11 @@ PX4IO::io_publish_raw_rc() } else { rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN; + + /* we do not know the RC input, only publish if RC OK flag is set */ + /* if no raw RC, just don't publish */ + if (!(_status & PX4IO_P_STATUS_FLAGS_RC_OK)) + return OK; } /* lazily advertise on first publication */ diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 92b1b45be7..41c7544058 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -355,7 +355,7 @@ l_input_rc(const struct listener *l) for (unsigned i = 0; (i * port_width) < (rc_raw.channel_count + port_width); i++) { /* Channels are sent in MAVLink main loop at a fixed interval */ mavlink_msg_rc_channels_raw_send(chan, - rc_raw.timestamp / 1000, + rc_raw.timestamp_publication / 1000, i, (rc_raw.channel_count > (i * port_width) + 0) ? rc_raw.values[(i * port_width) + 0] : UINT16_MAX, (rc_raw.channel_count > (i * port_width) + 1) ? rc_raw.values[(i * port_width) + 1] : UINT16_MAX, diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 23f20b0cb2..df6cbb7b2f 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1276,6 +1276,9 @@ Sensors::rc_poll() orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); + if (rc_input.rc_lost) + return; + struct manual_control_setpoint_s manual_control; struct actuator_controls_s actuator_group_3; @@ -1320,7 +1323,7 @@ Sensors::rc_poll() channel_limit = _rc_max_chan_count; /* we are accepting this message */ - _rc_last_valid = rc_input.timestamp; + _rc_last_valid = rc_input.timestamp_last_signal; /* Read out values from raw message */ for (unsigned int i = 0; i < channel_limit; i++) { @@ -1369,9 +1372,9 @@ Sensors::rc_poll() } _rc.chan_count = rc_input.channel_count; - _rc.timestamp = rc_input.timestamp; + _rc.timestamp = rc_input.timestamp_last_signal; - manual_control.timestamp = rc_input.timestamp; + manual_control.timestamp = rc_input.timestamp_last_signal; /* roll input - rolling right is stick-wise and rotation-wise positive */ manual_control.roll = limit_minus_one_to_one(_rc.chan[_rc.function[ROLL]].scaled); diff --git a/src/systemcmds/tests/test_rc.c b/src/systemcmds/tests/test_rc.c index 6a602ecfc9..57c0e7f4c7 100644 --- a/src/systemcmds/tests/test_rc.c +++ b/src/systemcmds/tests/test_rc.c @@ -121,7 +121,7 @@ int test_rc(int argc, char *argv[]) return ERROR; } - if (hrt_absolute_time() - rc_input.timestamp > 100000) { + if (hrt_absolute_time() - rc_input.timestamp_last_signal > 100000) { warnx("TIMEOUT, less than 10 Hz updates"); (void)close(_rc_sub); return ERROR; From 2dc3cf5e43154a516505d768885e734a5ab25e14 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Jan 2014 14:58:21 +0100 Subject: [PATCH 081/108] Remove unneeded header and commented out dead code from MEAS airspeed driver --- src/drivers/meas_airspeed/meas_airspeed.cpp | 22 +++++++-------------- 1 file changed, 7 insertions(+), 15 deletions(-) diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index a95c4576b0..9251cff7b2 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -77,7 +77,6 @@ #include #include #include -#include #include #include @@ -178,24 +177,17 @@ MEASAirspeed::collect() return ret; } - //uint16_t diff_pres_pa = (val[1]) | ((val[0] & ~(0xC0)) << 8); - uint16_t temp = (val[3] & 0xE0) << 8 | val[2]; - - // XXX leaving this in until new calculation method has been cross-checked - //diff_pres_pa = abs(diff_pres_pa - (16384 / 2.0f)); - //diff_pres_pa -= _diff_pres_offset; int16_t dp_raw = 0, dT_raw = 0; dp_raw = (val[0] << 8) + val[1]; - dp_raw = 0x3FFF & dp_raw; //mask the used bits + /* mask the used bits */ + dp_raw = 0x3FFF & dp_raw; dT_raw = (val[2] << 8) + val[3]; dT_raw = (0xFFE0 & dT_raw) >> 5; float temperature = ((200 * dT_raw) / 2047) - 50; - // XXX we may want to smooth out the readings to remove noise. - - // Calculate differential pressure. As its centered around 8000 - // and can go positive or negative, enforce absolute value -// uint16_t diff_press_pa = abs(dp_raw - (16384 / 2.0f)); + /* calculate differential pressure. As its centered around 8000 + * and can go positive or negative, enforce absolute value + */ const float P_min = -1.0f; const float P_max = 1.0f; float diff_press_pa = fabsf( ( ((float)dp_raw - 0.1f*16383.0f) * (P_max-P_min)/(0.8f*16383.0f) + P_min) * 6894.8f) - _diff_pres_offset; @@ -204,7 +196,7 @@ MEASAirspeed::collect() struct differential_pressure_s report; - // Track maximum differential pressure measured (so we can work out top speed). + /* track maximum differential pressure measured (so we can work out top speed). */ if (diff_press_pa > _max_differential_pressure_pa) { _max_differential_pressure_pa = diff_press_pa; } @@ -392,7 +384,7 @@ test() err(1, "immediate read failed"); warnx("single read"); - warnx("diff pressure: %d pa", report.differential_pressure_pa); + warnx("diff pressure: %d pa", (double)report.differential_pressure_pa); /* start the sensor polling at 2Hz */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) From b67f7b2c7cb4d176ff8b3c5b7ee5e839845eef94 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Jan 2014 15:12:12 +0100 Subject: [PATCH 082/108] Fix printing of IO status --- src/drivers/px4io/px4io.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 5601230a42..e24236de73 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1784,7 +1784,7 @@ PX4IO::print_status() io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM)); uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS); uint16_t io_status_flags = flags; - printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s%s\n", + printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s\n", flags, ((flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED) ? " OUTPUTS_ARMED" : ""), ((flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ? " SAFETY_OFF" : " SAFETY_SAFE"), @@ -1843,6 +1843,12 @@ PX4IO::print_status() printf("\n"); uint16_t raw_inputs = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT); printf("%d raw R/C inputs", raw_inputs); + + for (unsigned i = 0; i < raw_inputs; i++) + printf(" %u", io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + i)); + + printf("\n"); + flags = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_FLAGS); printf("status 0x%04x%s%s%s%s", flags, (((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (!(flags & PX4IO_P_RAW_RC_FLAGS_RC_DSM11))) ? " DSM10" : ""), @@ -1851,11 +1857,6 @@ PX4IO::print_status() ((flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) ? " FAILSAFE" : "") ); - for (unsigned i = 0; i < raw_inputs; i++) - printf(" %u", io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + i)); - - printf("\n"); - if ((flags & PX4IO_P_STATUS_FLAGS_RC_PPM)) { int frame_len = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_DATA); printf("RC data (PPM frame len) %u us\n", frame_len); @@ -1884,7 +1885,7 @@ PX4IO::print_status() /* setup and state */ uint16_t features = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES); - printf("features 0x%04x%s%s\n", features, + printf("features 0x%04x%s%s%s%s\n", features, ((features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) ? " S.BUS1_OUT" : ""), ((features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) ? " S.BUS2_OUT" : ""), ((features & PX4IO_P_SETUP_FEATURES_PWM_RSSI) ? " RSSI_PWM" : ""), From ac32116f00a07d61b2873dd98a2a4ed28515d522 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Jan 2014 15:17:01 +0100 Subject: [PATCH 083/108] Fix docs header --- src/modules/px4iofirmware/px4io.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index a2e1ed6cde..39272104d6 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2014 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 From cc68d11353e7fd7f0e775b8c11d99bd21982d2a3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Jan 2014 15:26:19 +0100 Subject: [PATCH 084/108] Add flag to indicate proper RC mapping --- src/modules/px4iofirmware/protocol.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 69c12d877d..e3f499435a 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -141,6 +141,7 @@ #define PX4IO_P_RAW_RC_FLAGS_FRAME_DROP (1 << 0) /* single frame drop */ #define PX4IO_P_RAW_RC_FLAGS_FAILSAFE (1 << 1) /* receiver is in failsafe mode */ #define PX4IO_P_RAW_RC_FLAGS_RC_DSM11 (1 << 2) /* DSM decoding is 11 bit mode */ +#define PX4IO_P_RAW_RC_FLAGS_MAPPING_OK (1 << 3) /* Channel mapping is ok */ #define PX4IO_P_RAW_RC_NRSSI 2 /* [2] Normalized RSSI value, 0: no reception, 255: perfect reception */ #define PX4IO_P_RAW_RC_DATA 3 /* [1] + [2] Details about the RC source (PPM frame length, Spektrum protocol type) */ From d1c934233f42e32a59117cbec317c1116d39be82 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Jan 2014 15:30:16 +0100 Subject: [PATCH 085/108] Fix S.BUS decoder to return value even if its just failsafe --- src/modules/px4iofirmware/sbus.c | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 32ab2a6988..f6ec542eb0 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -291,7 +291,6 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool /* report that we failed to read anything valid off the receiver */ *sbus_failsafe = true; *sbus_frame_drop = true; - return false; } else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */ /* set a special warning flag From 2a30c574ce569c876dd2b95919a0d86c7c1b1023 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Jan 2014 15:50:19 +0100 Subject: [PATCH 086/108] IO status printing improvements / fixes --- src/drivers/px4io/px4io.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index e24236de73..9595518ec9 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1850,14 +1850,15 @@ PX4IO::print_status() printf("\n"); flags = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_FLAGS); - printf("status 0x%04x%s%s%s%s", flags, + printf("R/C flags: 0x%04x%s%s%s%s%s\n", flags, (((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (!(flags & PX4IO_P_RAW_RC_FLAGS_RC_DSM11))) ? " DSM10" : ""), (((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_RAW_RC_FLAGS_RC_DSM11)) ? " DSM11" : ""), ((flags & PX4IO_P_RAW_RC_FLAGS_FRAME_DROP) ? " FRAME_DROP" : ""), - ((flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) ? " FAILSAFE" : "") + ((flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) ? " FAILSAFE" : ""), + ((flags & PX4IO_P_RAW_RC_FLAGS_MAPPING_OK) ? " MAPPING_OK" : "") ); - if ((flags & PX4IO_P_STATUS_FLAGS_RC_PPM)) { + if ((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_PPM)) { int frame_len = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_DATA); printf("RC data (PPM frame len) %u us\n", frame_len); @@ -2717,7 +2718,7 @@ monitor(void) /* clear screen */ printf("\033[2J"); - unsigned cancels = 3; + unsigned cancels = 2; for (;;) { pollfd fds[1]; From 00a3270dc696e09ad1e8f7b0eec579b92b6c0e2e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Jan 2014 15:52:51 +0100 Subject: [PATCH 087/108] Differentiate between failsafe having kicked in (which stops the normal output mixing based on RC outputs and prevents unwanted control commands due to failsafe) and a true loss of the receiver, where we stop outputting RC channel readings downstream on FMU. --- src/modules/px4iofirmware/controls.c | 167 +++++++++++++++------------ src/modules/px4iofirmware/px4io.h | 3 +- 2 files changed, 96 insertions(+), 74 deletions(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index e70eaed093..b3a999f223 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -59,6 +59,11 @@ static perf_counter_t c_gather_ppm; void controls_init(void) { + /* no channels */ + r_raw_rc_count = 0; + rc_channels_timestamp_received = 0; + rc_channels_timestamp_valid = 0; + /* DSM input (USART1) */ dsm_init("/dev/ttyS0"); @@ -121,6 +126,9 @@ controls_tick() { else r_raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_DSM11; + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); + } perf_end(c_gather_dsm); @@ -164,6 +172,8 @@ controls_tick() { if (ppm_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM; + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); } perf_end(c_gather_ppm); @@ -185,97 +195,100 @@ controls_tick() { */ if (dsm_updated || sbus_updated || ppm_updated) { - /* update RC-received timestamp */ - system_state.rc_channels_timestamp = hrt_absolute_time(); - /* record a bitmask of channels assigned */ unsigned assigned_channels = 0; - /* map raw inputs to mapped inputs */ - /* XXX mapping should be atomic relative to protocol */ - for (unsigned i = 0; i < r_raw_rc_count; i++) { + /* update RC-received timestamp */ + system_state.rc_channels_timestamp_received = hrt_absolute_time(); - /* map the input channel */ - uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE]; + /* do not command anything in failsafe, kick in the RC loss counter */ + if (!(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) { - if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { + /* update RC-received timestamp */ + system_state.rc_channels_timestamp_valid = system_state.rc_channels_timestamp_received; - uint16_t raw = r_raw_rc_values[i]; + /* map raw inputs to mapped inputs */ + /* XXX mapping should be atomic relative to protocol */ + for (unsigned i = 0; i < r_raw_rc_count; i++) { - int16_t scaled; + /* map the input channel */ + uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE]; - /* - * 1) Constrain to min/max values, as later processing depends on bounds. - */ - if (raw < conf[PX4IO_P_RC_CONFIG_MIN]) - raw = conf[PX4IO_P_RC_CONFIG_MIN]; - if (raw > conf[PX4IO_P_RC_CONFIG_MAX]) - raw = conf[PX4IO_P_RC_CONFIG_MAX]; + if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { - /* - * 2) Scale around the mid point differently for lower and upper range. - * - * This is necessary as they don't share the same endpoints and slope. - * - * First normalize to 0..1 range with correct sign (below or above center), - * then scale to 20000 range (if center is an actual center, -10000..10000, - * if parameters only support half range, scale to 10000 range, e.g. if - * center == min 0..10000, if center == max -10000..0). - * - * As the min and max bounds were enforced in step 1), division by zero - * cannot occur, as for the case of center == min or center == max the if - * statement is mutually exclusive with the arithmetic NaN case. - * - * DO NOT REMOVE OR ALTER STEP 1! - */ - if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) { - scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])); + uint16_t raw = r_raw_rc_values[i]; - } else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) { - scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN])); + int16_t scaled; - } else { - /* in the configured dead zone, output zero */ - scaled = 0; - } + /* + * 1) Constrain to min/max values, as later processing depends on bounds. + */ + if (raw < conf[PX4IO_P_RC_CONFIG_MIN]) + raw = conf[PX4IO_P_RC_CONFIG_MIN]; + if (raw > conf[PX4IO_P_RC_CONFIG_MAX]) + raw = conf[PX4IO_P_RC_CONFIG_MAX]; - /* invert channel if requested */ - if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) - scaled = -scaled; + /* + * 2) Scale around the mid point differently for lower and upper range. + * + * This is necessary as they don't share the same endpoints and slope. + * + * First normalize to 0..1 range with correct sign (below or above center), + * then scale to 20000 range (if center is an actual center, -10000..10000, + * if parameters only support half range, scale to 10000 range, e.g. if + * center == min 0..10000, if center == max -10000..0). + * + * As the min and max bounds were enforced in step 1), division by zero + * cannot occur, as for the case of center == min or center == max the if + * statement is mutually exclusive with the arithmetic NaN case. + * + * DO NOT REMOVE OR ALTER STEP 1! + */ + if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) { + scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])); - /* and update the scaled/mapped version */ - unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]; - if (mapped < PX4IO_CONTROL_CHANNELS) { + } else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) { + scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN])); - /* invert channel if pitch - pulling the lever down means pitching up by convention */ - if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */ + } else { + /* in the configured dead zone, output zero */ + scaled = 0; + } + + /* invert channel if requested */ + if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) scaled = -scaled; - r_rc_values[mapped] = SIGNED_TO_REG(scaled); - assigned_channels |= (1 << mapped); + /* and update the scaled/mapped version */ + unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]; + if (mapped < PX4IO_CONTROL_CHANNELS) { + /* invert channel if pitch - pulling the lever down means pitching up by convention */ + if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */ + scaled = -scaled; + + r_rc_values[mapped] = SIGNED_TO_REG(scaled); + assigned_channels |= (1 << mapped); + + } } } - } - /* set un-assigned controls to zero */ - for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) { - if (!(assigned_channels & (1 << i))) - r_rc_values[i] = 0; - } + /* set un-assigned controls to zero */ + for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) { + if (!(assigned_channels & (1 << i))) + r_rc_values[i] = 0; + } - /* - * If we got an update with zero channels, treat it as - * a loss of input. - * - * This might happen if a protocol-based receiver returns an update - * that contains no channels that we have mapped. - */ - if (assigned_channels == 0 || (r_raw_rc_flags & (PX4IO_P_RAW_RC_FLAGS_FAILSAFE))) { - rc_input_lost = true; - } else { - /* set RC OK flag */ + /* set RC OK flag, as we got an update */ r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK; + + /* if we have enough channels (5) to control the vehicle, the mapping is ok */ + if (assigned_channels > 4) { + r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK; + } else { + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK); + } } /* @@ -288,7 +301,7 @@ controls_tick() { * If we haven't seen any new control data in 200ms, assume we * have lost input. */ - if (hrt_elapsed_time(&system_state.rc_channels_timestamp) > 200000) { + if (hrt_elapsed_time(&system_state.rc_channels_timestamp_received) > 200000) { rc_input_lost = true; /* clear the input-kind flags here */ @@ -296,24 +309,32 @@ controls_tick() { PX4IO_P_STATUS_FLAGS_RC_PPM | PX4IO_P_STATUS_FLAGS_RC_DSM | PX4IO_P_STATUS_FLAGS_RC_SBUS); + } /* * Handle losing RC input */ - if (rc_input_lost) { + /* this kicks in if the receiver is gone or the system went to failsafe */ + if (rc_input_lost || (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) { /* Clear the RC input status flag, clear manual override flag */ r_status_flags &= ~( PX4IO_P_STATUS_FLAGS_OVERRIDE | PX4IO_P_STATUS_FLAGS_RC_OK); + /* Mark all channels as invalid, as we just lost the RX */ + r_rc_valid = 0; + /* Set the RC_LOST alarm */ r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST; + } - /* Mark the arrays as empty */ + /* this kicks in if the receiver is completely gone */ + if (rc_input_lost) { + + /* Set channel count to zero */ r_raw_rc_count = 0; - r_rc_valid = 0; } /* diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 39272104d6..bb224f3880 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -116,7 +116,8 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */ */ struct sys_state_s { - volatile uint64_t rc_channels_timestamp; + volatile uint64_t rc_channels_timestamp_received; + volatile uint64_t rc_channels_timestamp_valid; /** * Last FMU receive time, in microseconds since system boot From 0393b2aa129050307896b87c657ee0ed8e449891 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Jan 2014 16:01:39 +0100 Subject: [PATCH 088/108] Build fix for IO control input parsing. --- src/modules/px4iofirmware/controls.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index b3a999f223..941500f0d9 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -61,8 +61,8 @@ controls_init(void) { /* no channels */ r_raw_rc_count = 0; - rc_channels_timestamp_received = 0; - rc_channels_timestamp_valid = 0; + system_state.rc_channels_timestamp_received = 0; + system_state.rc_channels_timestamp_valid = 0; /* DSM input (USART1) */ dsm_init("/dev/ttyS0"); From ba90dc87f6fcb6198e36265239774d35a8efe9cd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 27 Jan 2014 16:43:44 +0100 Subject: [PATCH 089/108] HOTFIX: Re-enable legacy config support, uncomment commented out configs. Needs more work. --- ROMFS/px4fmu_common/init.d/rc.autostart | 40 ++++++++++++------------- 1 file changed, 20 insertions(+), 20 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 325520dd07..75cac3e50c 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -33,7 +33,7 @@ fi if param compare SYS_AUTOSTART 1002 then - #sh /etc/init.d/1002_rc_fw_state.hil + sh /etc/init.d/1002_rc_fw_state.hil fi if param compare SYS_AUTOSTART 1003 @@ -52,47 +52,47 @@ fi if param compare SYS_AUTOSTART 2100 100 then - #sh /etc/init.d/2100_mpx_easystar - #set MODE custom + sh /etc/init.d/2100_mpx_easystar + set MODE custom fi if param compare SYS_AUTOSTART 2101 101 then - #sh /etc/init.d/2101_hk_bixler - #set MODE custom + sh /etc/init.d/2101_hk_bixler + set MODE custom fi if param compare SYS_AUTOSTART 2102 102 then - #sh /etc/init.d/2102_3dr_skywalker - #set MODE custom + sh /etc/init.d/2102_3dr_skywalker + set MODE custom fi # # Flying wing # -if param compare SYS_AUTOSTART 3030 +if param compare SYS_AUTOSTART 3030 30 then - #sh /etc/init.d/3030_io_camflyer + sh /etc/init.d/3030_io_camflyer fi -if param compare SYS_AUTOSTART 3031 +if param compare SYS_AUTOSTART 3031 31 then sh /etc/init.d/3031_phantom fi -if param compare SYS_AUTOSTART 3032 +if param compare SYS_AUTOSTART 3032 32 then sh /etc/init.d/3032_skywalker_x5 fi -if param compare SYS_AUTOSTART 3033 +if param compare SYS_AUTOSTART 3033 33 then sh /etc/init.d/3033_wingwing fi -if param compare SYS_AUTOSTART 3034 +if param compare SYS_AUTOSTART 3034 34 then sh /etc/init.d/3034_fx79 fi @@ -101,27 +101,27 @@ fi # Quad X # -if param compare SYS_AUTOSTART 4008 +if param compare SYS_AUTOSTART 4008 8 then #sh /etc/init.d/4008_ardrone fi -if param compare SYS_AUTOSTART 4009 +if param compare SYS_AUTOSTART 4009 9 then #sh /etc/init.d/4009_ardrone_flow fi -if param compare SYS_AUTOSTART 4010 +if param compare SYS_AUTOSTART 4010 10 then sh /etc/init.d/4010_dji_f330 fi -if param compare SYS_AUTOSTART 4011 +if param compare SYS_AUTOSTART 4011 11 then sh /etc/init.d/4011_dji_f450 fi -if param compare SYS_AUTOSTART 4012 +if param compare SYS_AUTOSTART 4012 12 then sh /etc/init.d/4012_hk_x550 fi @@ -130,12 +130,12 @@ fi # Wide arm / H frame # -if param compare SYS_AUTOSTART 10015 +if param compare SYS_AUTOSTART 10015 15 then sh /etc/init.d/10015_tbs_discovery fi -if param compare SYS_AUTOSTART 10016 +if param compare SYS_AUTOSTART 10016 16 then sh /etc/init.d/10016_3dr_iris fi From 068cc190e20b8901b8e4128f3c21200724f0cd4f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 27 Jan 2014 17:14:18 +0100 Subject: [PATCH 090/108] Updated / added all system types that have been available before --- .../init.d/1000_rc_fw_easystar.hil | 58 ++----------- .../px4fmu_common/init.d/1002_rc_fw_state.hil | 56 +++---------- .../init.d/1004_rc_fw_Rascal110.hil | 1 - ROMFS/px4fmu_common/init.d/12001_octo_cox_pwm | 37 ++++++++ ROMFS/px4fmu_common/init.d/2101_hk_bixler | 48 +---------- ROMFS/px4fmu_common/init.d/2102_3dr_skywalker | 48 +---------- ROMFS/px4fmu_common/init.d/3030_io_camflyer | 84 ++++++++----------- ROMFS/px4fmu_common/init.d/3032_skywalker_x5 | 33 +++++++- ROMFS/px4fmu_common/init.d/5001_quad_+_pwm | 37 ++++++++ ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm | 37 ++++++++ ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm | 37 ++++++++ ROMFS/px4fmu_common/init.d/8001_octo_x_pwm | 37 ++++++++ ROMFS/px4fmu_common/init.d/9001_octo_+_pwm | 37 ++++++++ 13 files changed, 312 insertions(+), 238 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/12001_octo_cox_pwm create mode 100644 ROMFS/px4fmu_common/init.d/5001_quad_+_pwm create mode 100644 ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm create mode 100644 ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm create mode 100644 ROMFS/px4fmu_common/init.d/8001_octo_x_pwm create mode 100644 ROMFS/px4fmu_common/init.d/9001_octo_+_pwm diff --git a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil index 40a13b5d17..ebe8a1a1ea 100644 --- a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil +++ b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil @@ -1,14 +1,13 @@ #!nsh # -# USB HIL start +# HILStar / X-Plane +# +# Maintainers: Thomas Gubler # -echo "[HIL] HILStar starting.." +echo "HIL Rascal 110 starting.." -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 +if [ $DO_AUTOCONFIG == yes ] then # Set all params here, then disable autoconfig @@ -40,48 +39,7 @@ then param save fi -# Allow USB some time to come up -sleep 1 -# Tell MAVLink that this link is "fast" -mavlink start -b 230400 -d /dev/ttyACM0 - -# Create a fake HIL /dev/pwm_output interface -hil mode_pwm - -# -# 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 we got an IO -# -if px4io start -then - echo "IO started" -else - fmu mode_serial - echo "FMU started" -fi - -# -# Start the sensors (depends on orb, px4io) -# -sh /etc/init.d/rc.sensors - -# -# Start the attitude estimator (depends on orb) -# -att_pos_estimator_ekf start - -# -# Load mixer and start controllers (depends on px4io) -# -mixer load /dev/pwm_output /etc/mixers/FMU_AET.mix -fw_pos_control_l1 start -fw_att_control start - -echo "[HIL] setup done, running" +set HIL yes +set VEHICLE_TYPE fw +set MIXER FMU_AERT diff --git a/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil b/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil index 7b9f41bf6d..46da24d35f 100644 --- a/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil +++ b/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil @@ -1,14 +1,13 @@ #!nsh # -# USB HIL start +# HIL Rascal 110 (Flightgear) +# +# Maintainers: Thomas Gubler # -echo "[HIL] HILStar starting in state-HIL mode.." +echo "HIL Rascal 110 starting.." -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 +if [ $DO_AUTOCONFIG == yes ] then # Set all params here, then disable autoconfig @@ -32,48 +31,15 @@ then param set FW_T_SINK_MAX 5.0 param set FW_T_SINK_MIN 4.0 param set FW_Y_ROLLFF 1.1 + param set FW_L1_PERIOD 16 + param set RC_SCALE_ROLL 1.0 + param set RC_SCALE_PITCH 1.0 param set SYS_AUTOCONFIG 0 param save fi -# Allow USB some time to come up -sleep 1 -# Tell MAVLink that this link is "fast" -mavlink start -b 230400 -d /dev/ttyACM0 - -# Create a fake HIL /dev/pwm_output interface -hil mode_pwm - -# -# 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 we got an IO -# -if px4io start -then - echo "IO started" -else - fmu mode_serial - echo "FMU started" -fi - -# -# Start the sensors (depends on orb, px4io) -# -sh /etc/init.d/rc.sensors - -# -# Load mixer and start controllers (depends on px4io) -# -mixer load /dev/pwm_output /etc/mixers/FMU_AET.mix -fw_pos_control_l1 start -fw_att_control start - -echo "[HIL] setup done, running" +set HIL yes +set VEHICLE_TYPE fw +set MIXER FMU_AERT diff --git a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil index 75a00a675c..46da24d35f 100644 --- a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil +++ b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil @@ -43,4 +43,3 @@ set HIL yes set VEHICLE_TYPE fw set MIXER FMU_AERT - diff --git a/ROMFS/px4fmu_common/init.d/12001_octo_cox_pwm b/ROMFS/px4fmu_common/init.d/12001_octo_cox_pwm new file mode 100644 index 0000000000..5f3cec4e0a --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/12001_octo_cox_pwm @@ -0,0 +1,37 @@ +#!nsh +# +# Generic 10” Octo coaxial geometry +# +# Maintainers: Lorenz Meier +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set MC_ATT_P 7.0 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 + param set MC_YAWPOS_P 2.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + # TODO add default MPC parameters +fi + +set VEHICLE_TYPE mc +set MIXER FMU_octo_cox + +set PWM_OUTPUTS 1234 +set PWM_RATE 400 +# DJI ESC range +set PWM_DISARMED 900 +set PWM_MIN 1200 +set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/2101_hk_bixler b/ROMFS/px4fmu_common/init.d/2101_hk_bixler index 995d3ba074..1ed923b19c 100644 --- a/ROMFS/px4fmu_common/init.d/2101_hk_bixler +++ b/ROMFS/px4fmu_common/init.d/2101_hk_bixler @@ -1,11 +1,11 @@ #!nsh -echo "[init] PX4FMU v1, v2 with or without IO on HK Bixler" +echo "[init] PX4FMU v1, v2 with or without IO on 3DR SkyWalker" # # Load default params for this platform # -if param compare SYS_AUTOCONFIG 1 +if [ $DO_AUTOCONFIG == yes ] then # Set all params here, then disable autoconfig param set FW_P_D 0 @@ -35,46 +35,6 @@ then param set SYS_AUTOCONFIG 0 param save fi - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing -# -param set MAV_TYPE 1 -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - - sh /etc/init.d/rc.io - # Limit to 100 Hz updates and (implicit) 50 Hz PWM - px4io limit 100 -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -# -# Load mixer and start controllers (depends on px4io) -# -if [ -f /fs/microsd/etc/mixers/FMU_AERT.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_AERT.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_AERT.mix -else - echo "Using /etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix -fi - -# -# Start common fixedwing apps -# -sh /etc/init.d/rc.fixedwing +set VEHICLE_TYPE fw +set MIXER FMU_AERT \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker b/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker index a6d2ace962..1ed923b19c 100644 --- a/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker +++ b/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker @@ -5,7 +5,7 @@ echo "[init] PX4FMU v1, v2 with or without IO on 3DR SkyWalker" # # Load default params for this platform # -if param compare SYS_AUTOCONFIG 1 +if [ $DO_AUTOCONFIG == yes ] then # Set all params here, then disable autoconfig param set FW_P_D 0 @@ -35,48 +35,6 @@ then param set SYS_AUTOCONFIG 0 param save fi - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing -# -param set MAV_TYPE 1 -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - - sh /etc/init.d/rc.io - # Limit to 100 Hz updates and (implicit) 50 Hz PWM - px4io limit 100 -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -pwm disarmed -c 3 -p 1056 - -# -# Load mixer and start controllers (depends on px4io) -# -if [ -f /fs/microsd/etc/mixers/FMU_AERT.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_AERT.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_AETR.mix -else - echo "Using /etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_AETR.mix -fi - -# -# Start common fixedwing apps -# -sh /etc/init.d/rc.fixedwing +set VEHICLE_TYPE fw +set MIXER FMU_AERT \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/3030_io_camflyer b/ROMFS/px4fmu_common/init.d/3030_io_camflyer index 65f01c9743..cbcc6189b6 100644 --- a/ROMFS/px4fmu_common/init.d/3030_io_camflyer +++ b/ROMFS/px4fmu_common/init.d/3030_io_camflyer @@ -2,57 +2,39 @@ echo "[init] PX4FMU v1, v2 with or without IO on Camflyer" -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 +if [ $DO_AUTOCONFIG == yes ] then - # Set all params here, then disable autoconfig - # TODO - - param set SYS_AUTOCONFIG 0 - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing -# -param set MAV_TYPE 1 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - - sh /etc/init.d/rc.io - # Limit to 100 Hz updates and (implicit) 50 Hz PWM - px4io limit 100 -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes + # + # Default parameters for this platform + # + param set FW_AIRSPD_MIN 7 + param set FW_AIRSPD_TRIM 9 + param set FW_AIRSPD_MAX 14 + param set FW_L1_PERIOD 10 + param set FW_P_D 0 + param set FW_P_I 0 + param set FW_P_IMAX 20 + param set FW_P_LIM_MAX 30 + param set FW_P_LIM_MIN -20 + param set FW_P_P 30 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 2 + param set FW_R_D 0 + param set FW_R_I 5 + param set FW_R_IMAX 20 + param set FW_R_P 60 + param set FW_R_RMAX 60 + param set FW_THR_CRUISE 0.65 + param set FW_THR_MAX 0.7 + param set FW_THR_MIN 0 + param set FW_T_SINK_MAX 5 + param set FW_T_SINK_MIN 2 + param set FW_T_TIME_CONST 9 + param set FW_Y_ROLLFF 2.0 + param set RC_SCALE_ROLL 1.0 + param set RC_SCALE_PITCH 1.0 fi -# -# Load mixer and start controllers (depends on px4io) -# -if [ -f /fs/microsd/etc/mixers/FMU_Q.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix -else - echo "Using /etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix -fi - -# -# Start common fixedwing apps -# -sh /etc/init.d/rc.fixedwing +set VEHICLE_TYPE fw +set MIXER FMU_Q diff --git a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 index 03f2822375..143310af95 100644 --- a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 +++ b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 @@ -7,8 +7,37 @@ if [ $DO_AUTOCONFIG == yes ] then - # TODO + # + # Default parameters for this platform + # + param set FW_AIRSPD_MIN 7 + param set FW_AIRSPD_TRIM 9 + param set FW_AIRSPD_MAX 14 + param set FW_L1_PERIOD 10 + param set FW_P_D 0 + param set FW_P_I 0 + param set FW_P_IMAX 20 + param set FW_P_LIM_MAX 30 + param set FW_P_LIM_MIN -20 + param set FW_P_P 30 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 2 + param set FW_R_D 0 + param set FW_R_I 5 + param set FW_R_IMAX 20 + param set FW_R_P 60 + param set FW_R_RMAX 60 + param set FW_THR_CRUISE 0.65 + param set FW_THR_MAX 0.7 + param set FW_THR_MIN 0 + param set FW_T_SINK_MAX 5 + param set FW_T_SINK_MIN 2 + param set FW_T_TIME_CONST 9 + param set FW_Y_ROLLFF 2.0 + param set RC_SCALE_ROLL 1.0 + param set RC_SCALE_PITCH 1.0 fi set VEHICLE_TYPE fw -set MIXER FMU_Q +set MIXER FMU_X5 diff --git a/ROMFS/px4fmu_common/init.d/5001_quad_+_pwm b/ROMFS/px4fmu_common/init.d/5001_quad_+_pwm new file mode 100644 index 0000000000..2e5f6ca4f5 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/5001_quad_+_pwm @@ -0,0 +1,37 @@ +#!nsh +# +# Generic 10” Quad + geometry +# +# Maintainers: Lorenz Meier +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set MC_ATT_P 7.0 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 + param set MC_YAWPOS_P 2.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + # TODO add default MPC parameters +fi + +set VEHICLE_TYPE mc +set MIXER FMU_quad_+ + +set PWM_OUTPUTS 1234 +set PWM_RATE 400 +# DJI ESC range +set PWM_DISARMED 900 +set PWM_MIN 1200 +set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm b/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm new file mode 100644 index 0000000000..ddec8f36ef --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm @@ -0,0 +1,37 @@ +#!nsh +# +# Generic 10” Hexa X geometry +# +# Maintainers: Lorenz Meier +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set MC_ATT_P 7.0 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 + param set MC_YAWPOS_P 2.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + # TODO add default MPC parameters +fi + +set VEHICLE_TYPE mc +set MIXER FMU_hexa_x + +set PWM_OUTPUTS 1234 +set PWM_RATE 400 +# DJI ESC range +set PWM_DISARMED 900 +set PWM_MIN 1200 +set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm b/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm new file mode 100644 index 0000000000..106e0fb54c --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm @@ -0,0 +1,37 @@ +#!nsh +# +# Generic 10” Hexa + geometry +# +# Maintainers: Lorenz Meier +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set MC_ATT_P 7.0 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 + param set MC_YAWPOS_P 2.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + # TODO add default MPC parameters +fi + +set VEHICLE_TYPE mc +set MIXER FMU_hexa_+ + +set PWM_OUTPUTS 1234 +set PWM_RATE 400 +# DJI ESC range +set PWM_DISARMED 900 +set PWM_MIN 1200 +set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm b/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm new file mode 100644 index 0000000000..f0eea339b8 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm @@ -0,0 +1,37 @@ +#!nsh +# +# Generic 10” Octo X geometry +# +# Maintainers: Lorenz Meier +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set MC_ATT_P 7.0 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 + param set MC_YAWPOS_P 2.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + # TODO add default MPC parameters +fi + +set VEHICLE_TYPE mc +set MIXER FMU_octo_x + +set PWM_OUTPUTS 1234 +set PWM_RATE 400 +# DJI ESC range +set PWM_DISARMED 900 +set PWM_MIN 1200 +set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm b/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm new file mode 100644 index 0000000000..992a7aeba6 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm @@ -0,0 +1,37 @@ +#!nsh +# +# Generic 10” Octo + geometry +# +# Maintainers: Lorenz Meier +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set MC_ATT_P 7.0 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 + param set MC_YAWPOS_P 2.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + # TODO add default MPC parameters +fi + +set VEHICLE_TYPE mc +set MIXER FMU_octo_+ + +set PWM_OUTPUTS 1234 +set PWM_RATE 400 +# DJI ESC range +set PWM_DISARMED 900 +set PWM_MIN 1200 +set PWM_MAX 1900 From 4c4cd41b72471ce28ccebc4b24b592d6159f8626 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 27 Jan 2014 17:14:31 +0100 Subject: [PATCH 091/108] Registered all new system types --- ROMFS/px4fmu_common/init.d/rc.autostart | 54 +++++++++++++++++++++++++ 1 file changed, 54 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 75cac3e50c..34da2dfef2 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -126,6 +126,51 @@ then sh /etc/init.d/4012_hk_x550 fi +# +# Quad + +# + +if param compare SYS_AUTOSTART 5001 +then + sh /etc/init.d/5001_quad_+_pwm +fi + +# +# Hexa X +# + +if param compare SYS_AUTOSTART 6001 +then + sh /etc/init.d/6001_hexa_x_pwm +fi + +# +# Hexa + +# + +if param compare SYS_AUTOSTART 7001 +then + sh /etc/init.d/7001_hexa_+_pwm +fi + +# +# Octo X +# + +if param compare SYS_AUTOSTART 8001 +then + sh /etc/init.d/8001_octo_x_pwm +fi + +# +# Octo + +# + +if param compare SYS_AUTOSTART 9001 +then + sh /etc/init.d/9001_octo_+_pwm +fi + # # Wide arm / H frame # @@ -139,3 +184,12 @@ if param compare SYS_AUTOSTART 10016 16 then sh /etc/init.d/10016_3dr_iris fi + +# +# Octo Coaxial +# + +if param compare SYS_AUTOSTART 12001 +then + sh /etc/init.d/12001_octo_cox_pwm +fi From 7e855b5a23c9053dc8a59fcf33670e1cb93dd34b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 27 Jan 2014 17:20:24 +0100 Subject: [PATCH 092/108] Deleted test - should not be in mainline --- ROMFS/px4fmu_common/init.d/cmp_test | 9 --------- 1 file changed, 9 deletions(-) delete mode 100644 ROMFS/px4fmu_common/init.d/cmp_test diff --git a/ROMFS/px4fmu_common/init.d/cmp_test b/ROMFS/px4fmu_common/init.d/cmp_test deleted file mode 100644 index f86f4f85bb..0000000000 --- a/ROMFS/px4fmu_common/init.d/cmp_test +++ /dev/null @@ -1,9 +0,0 @@ -#!nsh - -cp /etc/extras/px4io-v2_default.bin /fs/microsd/px4io.loaded -if cmp /etc/extras/px4io-v2_default.bin /fs/microsd/px4io.loaded -then - echo "CMP returned true" -else - echo "CMP returned false" -fi \ No newline at end of file From dfaa5a0c7c81f432d64cfffdaaf3d683c01f10b4 Mon Sep 17 00:00:00 2001 From: Darryl Taylor Date: Tue, 28 Jan 2014 09:57:47 +0800 Subject: [PATCH 093/108] Reduced stack from 2048 to 1024. Top reports stack usage at 812 under flight conditions. --- src/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index df847a64da..dbe97a1780 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -756,7 +756,7 @@ PX4IO::init() } /* start the IO interface task */ - _task = task_create("px4io", SCHED_PRIORITY_ACTUATOR_OUTPUTS, 2048, (main_t)&PX4IO::task_main_trampoline, nullptr); + _task = task_create("px4io", SCHED_PRIORITY_ACTUATOR_OUTPUTS, 1024, (main_t)&PX4IO::task_main_trampoline, nullptr); if (_task < 0) { debug("task start failed: %d", errno); From d1fb7651876236432dc66c5331c60258ff962352 Mon Sep 17 00:00:00 2001 From: Darryl Taylor Date: Tue, 28 Jan 2014 10:30:16 +0800 Subject: [PATCH 094/108] Reduced low priority thread stack size to 1728. Top indicates the high-water-mark is at 1380 during accelerometer calibration. Safety margin ~25% --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index add7312de7..6f15e07027 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -648,7 +648,7 @@ int commander_thread_main(int argc, char *argv[]) pthread_attr_t commander_low_prio_attr; pthread_attr_init(&commander_low_prio_attr); - pthread_attr_setstacksize(&commander_low_prio_attr, 2992); + pthread_attr_setstacksize(&commander_low_prio_attr, 1728); struct sched_param param; (void)pthread_attr_getschedparam(&commander_low_prio_attr, ¶m); From 0488d5b41c5c91658494242f778c7b9de99bcca4 Mon Sep 17 00:00:00 2001 From: Darryl Taylor Date: Tue, 28 Jan 2014 10:43:33 +0800 Subject: [PATCH 095/108] Reduced commander main task stack size to 2088. The high-water-mark measured at 1668 after calibration and flight. 25% safety margin, but commander is fairly complex. There are surely untested code paths here, but each is relatively shallow. --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 6f15e07027..33589940c3 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -237,7 +237,7 @@ int commander_main(int argc, char *argv[]) daemon_task = task_spawn_cmd("commander", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, - 3000, + 2088, commander_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); From 19c3525f58b4123c38592644252226219ddef07a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 28 Jan 2014 11:11:34 +0100 Subject: [PATCH 096/108] Hotfix: Fixed telemetry transmission of RC channels - we always sent one set too much - by lieron --- src/modules/mavlink/orb_listener.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 92b1b45be7..192c8fdbab 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -352,7 +352,7 @@ l_input_rc(const struct listener *l) const unsigned port_width = 8; - for (unsigned i = 0; (i * port_width) < (rc_raw.channel_count + port_width); i++) { + for (unsigned i = 0; (i * port_width) < rc_raw.channel_count; i++) { /* Channels are sent in MAVLink main loop at a fixed interval */ mavlink_msg_rc_channels_raw_send(chan, rc_raw.timestamp / 1000, From 338b753a3c56813c5820e54209b4949e04a12ad9 Mon Sep 17 00:00:00 2001 From: Darryl Taylor Date: Wed, 29 Jan 2014 17:00:07 +0800 Subject: [PATCH 097/108] Reduced stack size to 1200. Max stack space used was 956. --- src/modules/mavlink/mavlink.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 20853379de..ebf01a2f47 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -773,7 +773,7 @@ int mavlink_main(int argc, char *argv[]) mavlink_task = task_spawn_cmd("mavlink", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 2048, + 1200, mavlink_thread_main, (const char **)argv); From 58a1f19d79da8a621454f11055534610921accd6 Mon Sep 17 00:00:00 2001 From: Darryl Taylor Date: Wed, 29 Jan 2014 17:09:05 +0800 Subject: [PATCH 098/108] Stack size reduced to 1816. Max stack reported by top was 1448 under HIL. --- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 7b6fad6589..9fc7b748ad 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -871,7 +871,7 @@ receive_start(int uart) param.sched_priority = SCHED_PRIORITY_MAX - 40; (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); - pthread_attr_setstacksize(&receiveloop_attr, 3000); + pthread_attr_setstacksize(&receiveloop_attr, 1816); pthread_t thread; pthread_create(&thread, &receiveloop_attr, receive_thread, &uart); From b0f65bb708e2728d562dbef8db04a846170d0ca6 Mon Sep 17 00:00:00 2001 From: Darryl Taylor Date: Wed, 29 Jan 2014 17:12:16 +0800 Subject: [PATCH 099/108] Stack size reduced to 1648. Max stack usage reported top at 1316. --- src/modules/mavlink/orb_listener.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 92b1b45be7..ed04882b90 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -838,7 +838,7 @@ uorb_receive_start(void) pthread_attr_init(&uorb_attr); /* Set stack size, needs less than 2k */ - pthread_attr_setstacksize(&uorb_attr, 2048); + pthread_attr_setstacksize(&uorb_attr, 1648); pthread_t thread; pthread_create(&thread, &uorb_attr, uorb_receive_thread, NULL); From 70b1037c2ebd38e06a3d12dca7bad295da02e16c Mon Sep 17 00:00:00 2001 From: Darryl Taylor Date: Wed, 29 Jan 2014 17:35:34 +0800 Subject: [PATCH 100/108] Stack size reduced to 2408. Max stack usage reported by top in EASY mode at 1924. --- src/modules/multirotor_pos_control/multirotor_pos_control.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 3d23d0c096..a89c7eace0 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -126,7 +126,7 @@ int multirotor_pos_control_main(int argc, char *argv[]) deamon_task = task_spawn_cmd("multirotor_pos_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 60, - 4096, + 2408, multirotor_pos_control_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(0); From 0cc311b872c688f2242a4745cd3a6de4933c9e62 Mon Sep 17 00:00:00 2001 From: Darryl Taylor Date: Wed, 29 Jan 2014 17:39:35 +0800 Subject: [PATCH 101/108] Reduced stack size to 2568. Max stack usage reported as 2052 with MARG + GPS with 3d lock - no px4flow, but should be more than enough buffer to accomodate its addition. --- .../position_estimator_inav/position_estimator_inav_main.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 3084b6d928..eb5a23b697 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -128,7 +128,7 @@ int position_estimator_inav_main(int argc, char *argv[]) thread_should_exit = false; position_estimator_inav_task = task_spawn_cmd("position_estimator_inav", - SCHED_RR, SCHED_PRIORITY_MAX - 5, 4096, + SCHED_RR, SCHED_PRIORITY_MAX - 5, 2568, position_estimator_inav_thread_main, (argv) ? (const char **) &argv[2] : (const char **) NULL); exit(0); From 1d70a65f40895b0c9e1ee5c65970efa1eae841e8 Mon Sep 17 00:00:00 2001 From: Darryl Taylor Date: Wed, 29 Jan 2014 17:52:22 +0800 Subject: [PATCH 102/108] Stack size reduced to 1280. Max stack size reported as 1020 with UBX GPS with 3d lock. MTK not tested. --- src/drivers/gps/gps.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 6b72d560fa..f2faf711ba 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -209,7 +209,7 @@ GPS::init() goto out; /* start the GPS driver worker task */ - _task = task_create("gps", SCHED_PRIORITY_SLOW_DRIVER, 2048, (main_t)&GPS::task_main_trampoline, nullptr); + _task = task_create("gps", SCHED_PRIORITY_SLOW_DRIVER, 1280, (main_t)&GPS::task_main_trampoline, nullptr); if (_task < 0) { warnx("task start failed: %d", errno); From 6b709e21d3aa5e88458b4f3319a39b4abbadc085 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 29 Jan 2014 21:26:28 +1100 Subject: [PATCH 103/108] FMUv2: set SPI GPIO pins on bus 1 to 50MHz this solves the ms5611 issue related to temperature. We now need to test if it is OK with all sensors --- nuttx-configs/px4fmu-v2/include/board.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/nuttx-configs/px4fmu-v2/include/board.h b/nuttx-configs/px4fmu-v2/include/board.h index 507df70a23..c6c771491d 100755 --- a/nuttx-configs/px4fmu-v2/include/board.h +++ b/nuttx-configs/px4fmu-v2/include/board.h @@ -260,9 +260,9 @@ * * There are sensors on SPI1, and SPI2 is connected to the FRAM. */ -#define GPIO_SPI1_MISO GPIO_SPI1_MISO_1 -#define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_1 -#define GPIO_SPI1_SCK GPIO_SPI1_SCK_1 +#define GPIO_SPI1_MISO (GPIO_SPI1_MISO_1|GPIO_SPEED_50MHz) +#define GPIO_SPI1_MOSI (GPIO_SPI1_MOSI_1|GPIO_SPEED_50MHz) +#define GPIO_SPI1_SCK (GPIO_SPI1_SCK_1|GPIO_SPEED_50MHz) #define GPIO_SPI2_MISO GPIO_SPI2_MISO_1 #define GPIO_SPI2_MOSI GPIO_SPI2_MOSI_1 From e7e3e132adcbc755cd6cc2b97f6c8c827a9140bf Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 29 Jan 2014 21:30:15 +1100 Subject: [PATCH 104/108] FMUv2: push SPI2 GPIO speed up to 50MHz as well --- nuttx-configs/px4fmu-v2/include/board.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/nuttx-configs/px4fmu-v2/include/board.h b/nuttx-configs/px4fmu-v2/include/board.h index c6c771491d..e56b14ba41 100755 --- a/nuttx-configs/px4fmu-v2/include/board.h +++ b/nuttx-configs/px4fmu-v2/include/board.h @@ -264,9 +264,9 @@ #define GPIO_SPI1_MOSI (GPIO_SPI1_MOSI_1|GPIO_SPEED_50MHz) #define GPIO_SPI1_SCK (GPIO_SPI1_SCK_1|GPIO_SPEED_50MHz) -#define GPIO_SPI2_MISO GPIO_SPI2_MISO_1 -#define GPIO_SPI2_MOSI GPIO_SPI2_MOSI_1 -#define GPIO_SPI2_SCK GPIO_SPI2_SCK_2 +#define GPIO_SPI2_MISO (GPIO_SPI2_MISO_1|GPIO_SPEED_50MHz) +#define GPIO_SPI2_MOSI (GPIO_SPI2_MOSI_1|GPIO_SPEED_50MHz) +#define GPIO_SPI2_SCK (GPIO_SPI2_SCK_2|GPIO_SPEED_50MHz) /************************************************************************************ * Public Data From 08a6057ef8c4aa796751c5ac07ab8efa7529b150 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 29 Jan 2014 13:23:06 +0100 Subject: [PATCH 105/108] Increase SPI GPIO speed for FMUv1 analog to v2 --- nuttx-configs/px4fmu-v1/include/board.h | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/nuttx-configs/px4fmu-v1/include/board.h b/nuttx-configs/px4fmu-v1/include/board.h index 27ace4b7db..ff1c634244 100644 --- a/nuttx-configs/px4fmu-v1/include/board.h +++ b/nuttx-configs/px4fmu-v1/include/board.h @@ -246,14 +246,14 @@ * * There are sensors on SPI1, and SPI3 is connected to the microSD slot. */ -#define GPIO_SPI1_MISO GPIO_SPI1_MISO_1 -#define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_1 -#define GPIO_SPI1_SCK GPIO_SPI1_SCK_1 +#define GPIO_SPI1_MISO (GPIO_SPI1_MISO_1|GPIO_SPEED_50MHz) +#define GPIO_SPI1_MOSI (GPIO_SPI1_MOSI_1|GPIO_SPEED_50MHz) +#define GPIO_SPI1_SCK (GPIO_SPI1_SCK_1|GPIO_SPEED_50MHz) -#define GPIO_SPI3_MISO GPIO_SPI3_MISO_2 -#define GPIO_SPI3_MOSI GPIO_SPI3_MOSI_1 -#define GPIO_SPI3_SCK GPIO_SPI3_SCK_2 -#define GPIO_SPI3_NSS GPIO_SPI3_NSS_2 +#define GPIO_SPI3_MISO (GPIO_SPI3_MISO_2|GPIO_SPEED_50MHz) +#define GPIO_SPI3_MOSI (GPIO_SPI3_MOSI_1|GPIO_SPEED_50MHz) +#define GPIO_SPI3_SCK (GPIO_SPI3_SCK_2|GPIO_SPEED_50MHz) +#define GPIO_SPI3_NSS (GPIO_SPI3_NSS_2|GPIO_SPEED_50MHz) /* SPI DMA configuration for SPI3 (microSD) */ #define DMACHAN_SPI3_RX DMAMAP_SPI3_RX_1 From d40382425f3b79bd662949e7259479e568e8698e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 29 Jan 2014 14:51:57 +0100 Subject: [PATCH 106/108] Triggering param documentation generation --- Tools/px4params/.gitignore | 3 ++- Tools/px4params/dokuwikiout.py | 29 +++++++++++++++++++++++++++-- Tools/px4params/xmlrpc.sh | 5 +++++ 3 files changed, 34 insertions(+), 3 deletions(-) create mode 100644 Tools/px4params/xmlrpc.sh diff --git a/Tools/px4params/.gitignore b/Tools/px4params/.gitignore index 73cf39575d..5d0378b4a2 100644 --- a/Tools/px4params/.gitignore +++ b/Tools/px4params/.gitignore @@ -1,2 +1,3 @@ parameters.wiki -parameters.xml \ No newline at end of file +parameters.xml +cookies.txt \ No newline at end of file diff --git a/Tools/px4params/dokuwikiout.py b/Tools/px4params/dokuwikiout.py index 4d40a6201d..c4fadbd354 100644 --- a/Tools/px4params/dokuwikiout.py +++ b/Tools/px4params/dokuwikiout.py @@ -1,10 +1,24 @@ import output +from xml.sax.saxutils import escape class DokuWikiOutput(output.Output): def Generate(self, groups): - result = "" + pre_text = """ + + wiki.putPage + + + + :dev:parameters:reference + + + + + """ + result = "====== Parameter Reference ======\nThis list is auto-generated every few minutes and contains the most recent parameter names and default values." for group in groups: result += "==== %s ====\n\n" % group.GetName() + result += "|< 100% 20% 20% 10% 10% 10% 30%>|\n" result += "^ Name ^ Description ^ Min ^ Max ^ Default ^ Comment ^\n" for param in group.GetParams(): code = param.GetFieldValue("code") @@ -34,4 +48,15 @@ class DokuWikiOutput(output.Output): result += "|" result += "|\n" result += "\n" - return result + post_text = """ + + + + + sum + Updated parameters automagically from code. + + + + """ + return pre_text + escape(result) + post_text diff --git a/Tools/px4params/xmlrpc.sh b/Tools/px4params/xmlrpc.sh new file mode 100644 index 0000000000..36c52ff714 --- /dev/null +++ b/Tools/px4params/xmlrpc.sh @@ -0,0 +1,5 @@ +python px_process_params.py + +rm cookies.txt +curl --cookie cookies.txt --cookie-jar cookies.txt --user-agent Mozilla/4.0 --data "u=$XMLRPCUSER&p=$XMLRPCPASS" https://pixhawk.org/start?do=login +curl -k --cookie cookies.txt -H "Content-Type: application/xml" -X POST --data-binary @parameters.wiki "https://pixhawk.org/lib/exe/xmlrpc.php" From 60859607ff240f1aaad05b306ba86b84d15be2ac Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 29 Jan 2014 15:11:50 +0100 Subject: [PATCH 107/108] Better output --- Tools/px4params/dokuwikiout.py | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/Tools/px4params/dokuwikiout.py b/Tools/px4params/dokuwikiout.py index c4fadbd354..42da19e8bb 100644 --- a/Tools/px4params/dokuwikiout.py +++ b/Tools/px4params/dokuwikiout.py @@ -27,26 +27,26 @@ class DokuWikiOutput(output.Output): result += "| %s | %s " % (code, name) min_val = param.GetFieldValue("min") if min_val is not None: - result += "| %s " % min_val + result += " | %s " % min_val else: - result += "|" + result += " | " max_val = param.GetFieldValue("max") if max_val is not None: - result += "| %s " % max_val + result += " | %s " % max_val else: - result += "|" + result += " | " def_val = param.GetFieldValue("default") if def_val is not None: result += "| %s " % def_val else: - result += "|" + result += " | " long_desc = param.GetFieldValue("long_desc") if long_desc is not None: long_desc = long_desc.replace("\n", "") result += "| %s " % long_desc else: - result += "|" - result += "|\n" + result += " | " + result += " |\n" result += "\n" post_text = """ From 2b17909f0c4131f6a728697f5de1eb13a1337234 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 29 Jan 2014 19:18:44 +0100 Subject: [PATCH 108/108] Changed param location --- Tools/px4params/dokuwikiout.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/px4params/dokuwikiout.py b/Tools/px4params/dokuwikiout.py index 42da19e8bb..c5cf65ea60 100644 --- a/Tools/px4params/dokuwikiout.py +++ b/Tools/px4params/dokuwikiout.py @@ -9,7 +9,7 @@ class DokuWikiOutput(output.Output): - :dev:parameters:reference + :firmware:parameters