diff --git a/ROMFS/px4fmu_common/init.d/rc.PX4IO b/ROMFS/px4fmu_common/init.d/rc.PX4IO index 1e3963b9ad..7ae4a55860 100644 --- a/ROMFS/px4fmu_common/init.d/rc.PX4IO +++ b/ROMFS/px4fmu_common/init.d/rc.PX4IO @@ -5,7 +5,7 @@ set USB no set MODE camflyer # -# Start the ORB +# Start the ORB (first app to start) # uorb start @@ -27,38 +27,65 @@ fi param set MAV_TYPE 1 # -# Start the sensors. +# Check if PX4IO Firmware should be upgraded (from Andrew Tridgell) # -sh /etc/init.d/rc.sensors +if [ -f /fs/microsd/px4io.bin ] +then + echo "PX4IO Firmware found. Checking Upgrade.." + if cmp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current + then + echo "No newer version, skipping upgrade." + else + echo "Loading /fs/microsd/px4io.bin" + if px4io update /fs/microsd/px4io.bin > /fs/microsd/px4io_update.log + then + cp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current + echo "Flashed /fs/microsd/px4io.bin OK" >> /fs/microsd/px4io_update.log + else + echo "Failed flashing /fs/microsd/px4io.bin" >> /fs/microsd/px4io_update.log + echo "Failed to upgrade PX4IO firmware - check if PX4IO is in bootloader mode" + fi + fi +fi # -# Start MAVLink +# Start MAVLink (depends on orb) # mavlink start -d /dev/ttyS1 -b 57600 usleep 5000 # -# Start the commander. +# Start the commander (depends on orb, mavlink) # commander start # -# Start GPS interface -# -gps start - -# -# Start the attitude estimator -# -kalman_demo start - -# -# Start PX4IO interface +# Start PX4IO interface (depends on orb, commander) # px4io start # -# Load mixer and start controllers +# Allow PX4IO to recover from midair restarts. +# this is very unlikely, but quite safe and robust. +px4io recovery + +# +# Start the sensors (depends on orb, px4io) +# +sh /etc/init.d/rc.sensors + +# +# Start GPS interface (depends on orb) +# +gps start + +# +# Start the attitude estimator (depends on orb) +# +kalman_demo start + +# +# Load mixer and start controllers (depends on px4io) # mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix control_demo start @@ -66,8 +93,8 @@ control_demo start # # Start logging # -sdlog start -s 10 - +#sdlog start -s 4 + # # Start system state # diff --git a/ROMFS/px4fmu_common/init.d/rc.PX4IOAR b/ROMFS/px4fmu_common/init.d/rc.PX4IOAR index 72df68e350..ab29e21c7e 100644 --- a/ROMFS/px4fmu_common/init.d/rc.PX4IOAR +++ b/ROMFS/px4fmu_common/init.d/rc.PX4IOAR @@ -2,10 +2,10 @@ # # Flight startup script for PX4FMU on PX4IOAR carrier board. # - + # Disable the USB interface set USB no - + # Disable autostarting other apps set MODE ardrone @@ -25,13 +25,18 @@ if [ -f /fs/microsd/parameters ] then param load /fs/microsd/parameters fi - + # # Force some key parameters to sane values # MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor # see https://pixhawk.ethz.ch/mavlink/ # param set MAV_TYPE 2 + +# +# Configure PX4FMU for operation with PX4IOAR +# +fmu mode_gpio_serial # # Start the sensors. @@ -54,11 +59,6 @@ commander start # attitude_estimator_ekf start -# -# Configure PX4FMU for operation with PX4IOAR -# -fmu mode_gpio_serial - # # Fire up the multi rotor attitude controller # @@ -68,17 +68,17 @@ multirotor_att_control start # Fire up the AR.Drone interface. # ardrone_interface start -d /dev/ttyS1 - -# -# Start GPS capture -# -gps start # # Start logging # sdlog start -s 10 +# +# Start GPS capture +# +gps start + # # Start system state # @@ -95,4 +95,5 @@ fi # use the same UART for telemetry # echo "[init] startup done" -exit \ No newline at end of file + +exit diff --git a/ROMFS/scripts/rc.hil b/ROMFS/px4fmu_common/init.d/rc.hil similarity index 100% rename from ROMFS/scripts/rc.hil rename to ROMFS/px4fmu_common/init.d/rc.hil diff --git a/ROMFS/scripts/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb similarity index 100% rename from ROMFS/scripts/rc.usb rename to ROMFS/px4fmu_common/init.d/rc.usb diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 89a7678797..c0a70f7ddc 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -21,9 +21,9 @@ set MODE autostart set USB autoconnect # -# Start playing the startup tune + # -tone_alarm start + # # Try to mount the microSD card. @@ -32,8 +32,12 @@ echo "[init] looking for microSD..." if mount -t vfat /dev/mmcsd0 /fs/microsd then echo "[init] card mounted at /fs/microsd" + # Start playing the startup tune + tone_alarm start else echo "[init] no microSD card found" + # Play SOS + tone_alarm 2 fi # diff --git a/ROMFS/px4fmu_common/mixers/FMU_quad_v.mix b/ROMFS/px4fmu_common/mixers/FMU_quad_v.mix new file mode 100644 index 0000000000..2a4a0f3419 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/FMU_quad_v.mix @@ -0,0 +1,7 @@ +Multirotor mixer for PX4FMU +=========================== + +This file defines a single mixer for a quadrotor in the V configuration. All controls +are mixed 100%. + +R: 4v 10000 10000 10000 0 diff --git a/ROMFS/scripts/rc.PX4IO b/ROMFS/scripts/rc.PX4IO deleted file mode 100644 index 625f23bdd8..0000000000 --- a/ROMFS/scripts/rc.PX4IO +++ /dev/null @@ -1,107 +0,0 @@ -#!nsh - -# Disable USB and autostart -set USB no -set MODE camflyer - -# -# Start the ORB (first app to start) -# -uorb start - -# -# Load microSD params -# -echo "[init] loading microSD params" -param select /fs/microsd/parameters -if [ -f /fs/microsd/parameters ] -then - param load /fs/microsd/parameters -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ -# -param set MAV_TYPE 1 - -# -# Check if PX4IO Firmware should be upgraded (from Andrew Tridgell) -# -if [ -f /fs/microsd/px4io.bin ] -then - echo "PX4IO Firmware found. Checking Upgrade.." - if cmp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current - then - echo "No newer version, skipping upgrade." - else - echo "Loading /fs/microsd/px4io.bin" - if px4io update /fs/microsd/px4io.bin > /fs/microsd/px4io_update.log - then - cp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current - echo "Flashed /fs/microsd/px4io.bin OK" >> /fs/microsd/px4io_update.log - else - echo "Failed flashing /fs/microsd/px4io.bin" >> /fs/microsd/px4io_update.log - echo "Failed to upgrade PX4IO firmware - check if PX4IO is in bootloader mode" - fi - fi -fi - -# -# Start MAVLink (depends on orb) -# -mavlink start -d /dev/ttyS1 -b 57600 -usleep 5000 - -# -# Start the commander (depends on orb, mavlink) -# -commander start - -# -# Start PX4IO interface (depends on orb, commander) -# -px4io start - -# -# Allow PX4IO to recover from midair restarts. -# this is very unlikely, but quite safe and robust. -px4io recovery - -# -# Start the sensors (depends on orb, px4io) -# -sh /etc/init.d/rc.sensors - -# -# Start GPS interface (depends on orb) -# -gps start - -# -# Start the attitude estimator (depends on orb) -# -kalman_demo start - -# -# Load mixer and start controllers (depends on px4io) -# -mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix -control_demo start - -# -# Start logging -# -#sdlog start -s 4 - -# -# Start system state -# -if blinkm start -then - echo "using BlinkM for state indication" - blinkm systemstate -else - echo "no BlinkM found, OK." -fi \ No newline at end of file diff --git a/ROMFS/scripts/rc.PX4IOAR b/ROMFS/scripts/rc.PX4IOAR deleted file mode 100644 index 6af91992e2..0000000000 --- a/ROMFS/scripts/rc.PX4IOAR +++ /dev/null @@ -1,99 +0,0 @@ -#!nsh -# -# Flight startup script for PX4FMU on PX4IOAR carrier board. -# - -# Disable the USB interface -set USB no - -# Disable autostarting other apps -set MODE ardrone - -echo "[init] doing PX4IOAR startup..." - -# -# Start the ORB -# -uorb start - -# -# Init the parameter storage -# -echo "[init] loading microSD params" -param select /fs/microsd/parameters -if [ -f /fs/microsd/parameters ] -then - param load /fs/microsd/parameters -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ -# -param set MAV_TYPE 2 - -# -# Configure PX4FMU for operation with PX4IOAR -# -fmu mode_gpio_serial - -# -# Start the sensors. -# -sh /etc/init.d/rc.sensors - -# -# Start MAVLink -# -mavlink start -d /dev/ttyS0 -b 57600 -usleep 5000 - -# -# Start the commander. -# -commander start - -# -# Start the attitude estimator -# -attitude_estimator_ekf start - -# -# Fire up the multi rotor attitude controller -# -multirotor_att_control start - -# -# Fire up the AR.Drone interface. -# -ardrone_interface start -d /dev/ttyS1 - -# -# Start logging -# -sdlog start -s 10 - -# -# Start GPS capture -# -gps start - -# -# Start system state -# -if blinkm start -then - echo "using BlinkM for state indication" - blinkm systemstate -else - echo "no BlinkM found, OK." -fi - -# -# startup is done; we don't want the shell because we -# use the same UART for telemetry -# -echo "[init] startup done" - -exit \ No newline at end of file diff --git a/ROMFS/scripts/rcS b/ROMFS/scripts/rcS deleted file mode 100755 index c0a70f7ddc..0000000000 --- a/ROMFS/scripts/rcS +++ /dev/null @@ -1,83 +0,0 @@ -#!nsh -# -# PX4FMU startup script. -# -# This script is responsible for: -# -# - mounting the microSD card (if present) -# - running the user startup script from the microSD card (if present) -# - detecting the configuration of the system and picking a suitable -# startup script to continue with -# -# Note: DO NOT add configuration-specific commands to this script; -# add them to the per-configuration scripts instead. -# - -# -# Default to auto-start mode. An init script on the microSD card -# can change this to prevent automatic startup of the flight script. -# -set MODE autostart -set USB autoconnect - -# - -# - - -# -# Try to mount the microSD card. -# -echo "[init] looking for microSD..." -if mount -t vfat /dev/mmcsd0 /fs/microsd -then - echo "[init] card mounted at /fs/microsd" - # Start playing the startup tune - tone_alarm start -else - echo "[init] no microSD card found" - # Play SOS - tone_alarm 2 -fi - -# -# Look for an init script on the microSD card. -# -# To prevent automatic startup in the current flight mode, -# the script should set MODE to some other value. -# -if [ -f /fs/microsd/etc/rc ] -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 -fi - -# -# Check for USB host -# -if [ $USB != autoconnect ] -then - echo "[init] not connecting USB" -else - if sercon - then - echo "[init] USB interface connected" - else - echo "[init] No USB connected" - fi -fi - -# if this is an APM build then there will be a rc.APM script -# from an EXTERNAL_SCRIPTS build option -if [ -f /etc/init.d/rc.APM ] -then - echo Running rc.APM - # if APM startup is successful then nsh will exit - sh /etc/init.d/rc.APM -fi diff --git a/apps/systemlib/mixer/mixer.h b/apps/systemlib/mixer/mixer.h index 71386cba77..40d37fce2b 100644 --- a/apps/systemlib/mixer/mixer.h +++ b/apps/systemlib/mixer/mixer.h @@ -418,6 +418,7 @@ public: enum Geometry { QUAD_X = 0, /**< quad in X configuration */ QUAD_PLUS, /**< quad in + configuration */ + QUAD_V, /**< quad in V configuration */ HEX_X, /**< hex in X configuration */ HEX_PLUS, /**< hex in + configuration */ OCTA_X, diff --git a/apps/systemlib/mixer/mixer_multirotor.cpp b/apps/systemlib/mixer/mixer_multirotor.cpp index 4b9cfc023a..d79811c0ff 100644 --- a/apps/systemlib/mixer/mixer_multirotor.cpp +++ b/apps/systemlib/mixer/mixer_multirotor.cpp @@ -82,6 +82,12 @@ const MultirotorMixer::Rotor _config_quad_plus[] = { { 0.000000, 1.000000, -1.00 }, { -0.000000, -1.000000, -1.00 }, }; +const MultirotorMixer::Rotor _config_quad_v[] = { + { -0.927184, 0.374607, 1.00 }, + { 0.694658, -0.719340, 1.00 }, + { 0.927184, 0.374607, -1.00 }, + { -0.694658, -0.719340, -1.00 }, +}; const MultirotorMixer::Rotor _config_hex_x[] = { { -1.000000, 0.000000, -1.00 }, { 1.000000, 0.000000, 1.00 }, @@ -121,6 +127,7 @@ const MultirotorMixer::Rotor _config_octa_plus[] = { const MultirotorMixer::Rotor *_config_index[MultirotorMixer::Geometry::MAX_GEOMETRY] = { &_config_quad_x[0], &_config_quad_plus[0], + &_config_quad_v[0], &_config_hex_x[0], &_config_hex_plus[0], &_config_octa_x[0], @@ -129,6 +136,7 @@ const MultirotorMixer::Rotor *_config_index[MultirotorMixer::Geometry::MAX_GEOME const unsigned _config_rotor_count[MultirotorMixer::Geometry::MAX_GEOMETRY] = { 4, /* quad_x */ 4, /* quad_plus */ + 4, /* quad_v */ 6, /* hex_x */ 6, /* hex_plus */ 8, /* octa_x */ @@ -184,6 +192,9 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl } else if (!strcmp(geomname, "4x")) { geometry = MultirotorMixer::QUAD_X; + } else if (!strcmp(geomname, "4v")) { + geometry = MultirotorMixer::QUAD_V; + } else if (!strcmp(geomname, "6+")) { geometry = MultirotorMixer::HEX_PLUS; diff --git a/apps/systemlib/mixer/multi_tables b/apps/systemlib/mixer/multi_tables index f17ae30ca6..19a8239a6d 100755 --- a/apps/systemlib/mixer/multi_tables +++ b/apps/systemlib/mixer/multi_tables @@ -20,6 +20,13 @@ set quad_plus { 180 CW } +set quad_v { + 68 CCW + -136 CCW + -68 CW + 136 CW +} + set hex_x { 90 CW -90 CCW @@ -60,7 +67,7 @@ set octa_plus { 90 CW } -set tables {quad_x quad_plus hex_x hex_plus octa_x octa_plus} +set tables {quad_x quad_plus quad_v hex_x hex_plus octa_x octa_plus} proc factors {a d} { puts [format "\t{ %9.6f, %9.6f, %5.2f }," [rcos [expr $a + 90]] [rcos $a] [expr -$d]]}