From 8d1abf4aa441e1c6c886e8af6ecaab2c3ca6e255 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 18 Jul 2013 13:16:34 +0200 Subject: Lunchtime HOTFIX: Bring back USB console to operational, allow single-USB connection operation via QGC --- src/modules/mavlink/mavlink.c | 47 ++++++++++++++++++++----------------------- 1 file changed, 22 insertions(+), 25 deletions(-) (limited to 'src/modules/mavlink') diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 7c1c4b175..919d01561 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -420,12 +420,12 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf case 921600: speed = B921600; break; default: - fprintf(stderr, "[mavlink] ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600\n\n", baud); + warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600\n\n", baud); return -EINVAL; } /* open uart */ - printf("[mavlink] UART is %s, baudrate is %d\n", uart_name, baud); + warnx("UART is %s, baudrate is %d\n", uart_name, baud); uart = open(uart_name, O_RDWR | O_NOCTTY); /* Try to set baud rate */ @@ -433,37 +433,35 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf int termios_state; *is_usb = false; - /* make some wild guesses including that USB serial is indicated by either /dev/ttyACM0 or /dev/console */ - if (strcmp(uart_name, "/dev/ttyACM0") != OK && strcmp(uart_name, "/dev/console") != OK) { - /* Back up the original uart configuration to restore it after exit */ - if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) { - fprintf(stderr, "[mavlink] ERROR getting baudrate / termios config for %s: %d\n", uart_name, termios_state); - close(uart); - return -1; - } + /* Back up the original uart configuration to restore it after exit */ + if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) { + warnx("ERROR get termios config %s: %d\n", uart_name, termios_state); + close(uart); + return -1; + } + + /* Fill the struct for the new configuration */ + tcgetattr(uart, &uart_config); - /* Fill the struct for the new configuration */ - tcgetattr(uart, &uart_config); + /* Clear ONLCR flag (which appends a CR for every LF) */ + uart_config.c_oflag &= ~ONLCR; - /* Clear ONLCR flag (which appends a CR for every LF) */ - uart_config.c_oflag &= ~ONLCR; + /* USB serial is indicated by /dev/ttyACM0*/ + if (strcmp(uart_name, "/dev/ttyACM0") != OK && strcmp(uart_name, "/dev/ttyACM1") != OK) { /* Set baud rate */ if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { - fprintf(stderr, "[mavlink] ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state); + warnx("ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state); close(uart); return -1; } + } - if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) { - fprintf(stderr, "[mavlink] ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name); - close(uart); - return -1; - } - - } else { - *is_usb = true; + if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) { + warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name); + close(uart); + return -1; } return uart; @@ -751,8 +749,7 @@ int mavlink_thread_main(int argc, char *argv[]) pthread_join(uorb_receive_thread, NULL); /* Reset the UART flags to original state */ - if (!usb_uart) - tcsetattr(uart, TCSANOW, &uart_config_original); + tcsetattr(uart, TCSANOW, &uart_config_original); thread_running = false; -- cgit v1.2.3 From 7bf2edc3bf9f04d52c6bd9a64d383acbc2071a00 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 18 Jul 2013 14:01:42 +0200 Subject: Script cleanup, WIP on mavlink logging --- ROMFS/px4fmu_common/init.d/01_fmu_quad_x | 107 ++++++++++++++++++++ ROMFS/px4fmu_common/init.d/02_io_quad_x | 123 +++++++++++++++++++++++ ROMFS/px4fmu_common/init.d/08_ardrone | 99 ++++++++++++++++++ ROMFS/px4fmu_common/init.d/09_ardrone_flow | 94 +++++++++++++++++ ROMFS/px4fmu_common/init.d/30_io_camflyer | 121 ++++++++++++++++++++++ ROMFS/px4fmu_common/init.d/31_io_phantom | 121 ++++++++++++++++++++++ ROMFS/px4fmu_common/init.d/rc.1_fmu_quad_x | 107 -------------------- ROMFS/px4fmu_common/init.d/rc.2_fmu_io_quad_x | 123 ----------------------- ROMFS/px4fmu_common/init.d/rc.30_fmu_io_camflyer | 121 ---------------------- ROMFS/px4fmu_common/init.d/rc.31_fmu_io_phantom | 121 ---------------------- ROMFS/px4fmu_common/init.d/rc.fmu_ardrone | 99 ------------------ ROMFS/px4fmu_common/init.d/rc.fmu_ardrone_flow | 94 ----------------- ROMFS/px4fmu_common/init.d/rcS | 63 +++++++----- src/modules/mavlink/mavlink_log.c | 11 ++ 14 files changed, 715 insertions(+), 689 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/01_fmu_quad_x create mode 100644 ROMFS/px4fmu_common/init.d/02_io_quad_x create mode 100644 ROMFS/px4fmu_common/init.d/08_ardrone create mode 100644 ROMFS/px4fmu_common/init.d/09_ardrone_flow create mode 100644 ROMFS/px4fmu_common/init.d/30_io_camflyer create mode 100644 ROMFS/px4fmu_common/init.d/31_io_phantom delete mode 100644 ROMFS/px4fmu_common/init.d/rc.1_fmu_quad_x delete mode 100644 ROMFS/px4fmu_common/init.d/rc.2_fmu_io_quad_x delete mode 100644 ROMFS/px4fmu_common/init.d/rc.30_fmu_io_camflyer delete mode 100644 ROMFS/px4fmu_common/init.d/rc.31_fmu_io_phantom delete mode 100644 ROMFS/px4fmu_common/init.d/rc.fmu_ardrone delete mode 100644 ROMFS/px4fmu_common/init.d/rc.fmu_ardrone_flow (limited to 'src/modules/mavlink') diff --git a/ROMFS/px4fmu_common/init.d/01_fmu_quad_x b/ROMFS/px4fmu_common/init.d/01_fmu_quad_x new file mode 100644 index 000000000..58a970eba --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/01_fmu_quad_x @@ -0,0 +1,107 @@ +#!nsh +# +# Flight startup script for PX4FMU with PWM outputs. +# + +# disable USB and autostart +set USB no +set MODE custom + +echo "[init] doing PX4FMU Quad startup..." + +# +# Start the ORB +# +uorb start + +# +# Load microSD params +# +echo "[init] loading microSD params" +param select /fs/microsd/params +if [ -f /fs/microsd/params ] +then + param load /fs/microsd/params +fi + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set MC_ATTRATE_P 0.14 + param set MC_ATTRATE_I 0 + param set MC_ATTRATE_D 0.006 + param set MC_ATT_P 5.5 + param set MC_ATT_I 0 + param set MC_ATT_D 0 + param set MC_YAWPOS_D 0 + param set MC_YAWPOS_I 0 + param set MC_YAWPOS_P 0.6 + param set MC_YAWRATE_D 0 + param set MC_YAWRATE_I 0 + param set MC_YAWRATE_P 0.08 + param set RC_SCALE_PITCH 1 + param set RC_SCALE_ROLL 1 + param set RC_SCALE_YAW 3 + + param set SYS_AUTOCONFIG 0 + param save /fs/microsd/params +fi + +# +# 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 + +# +# Start MAVLink +# +mavlink start -d /dev/ttyS0 -b 57600 +usleep 5000 + +# +# Start the sensors and test them. +# +sh /etc/init.d/rc.sensors + +# +# Start the commander. +# +commander start + +# +# Start GPS interface (depends on orb) +# +gps start + +# +# Start the attitude estimator +# +attitude_estimator_ekf start + +echo "[init] starting PWM output" +fmu mode_pwm +mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix +pwm -u 400 -m 0xff + +# +# Start attitude control +# +multirotor_att_control start + +# +# Start logging +# +sdlog2 start -r 50 -a -b 14 + +# +# Start system state +# +if blinkm start +then + blinkm systemstate +fi diff --git a/ROMFS/px4fmu_common/init.d/02_io_quad_x b/ROMFS/px4fmu_common/init.d/02_io_quad_x new file mode 100644 index 000000000..200f49d1b --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/02_io_quad_x @@ -0,0 +1,123 @@ +#!nsh +# +# Flight startup script for PX4FMU+PX4IO +# + +# disable USB and autostart +set USB no +set MODE custom + +# +# Start the ORB (first app to start) +# +uorb start + +# +# Load microSD params +# +echo "[init] loading microSD params" +param select /fs/microsd/params +if [ -f /fs/microsd/params ] +then + param load /fs/microsd/params +fi + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set SYS_AUTOCONFIG 0 + param save /fs/microsd/params +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor +# see https://pixhawk.ethz.ch/mavlink/ +# +param set MAV_TYPE 2 + +# +# Check if PX4IO Firmware should be upgraded (from Andrew Tridgell) +# +if [ -f /fs/microsd/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 + +# +# Disable px4io topic limiting +# +px4io limit 200 + +# +# 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) +# +mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix +pwm -u 400 -m 0xff +multirotor_att_control start + +# +# Start logging +# +sdlog2 start -r 50 -a -b 14 + +# +# Start system state +# +if blinkm start +then + blinkm systemstate +fi diff --git a/ROMFS/px4fmu_common/init.d/08_ardrone b/ROMFS/px4fmu_common/init.d/08_ardrone new file mode 100644 index 000000000..f55ac2ae3 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/08_ardrone @@ -0,0 +1,99 @@ +#!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 + +# +# Load microSD params +# +echo "[init] loading microSD params" +param select /fs/microsd/params +if [ -f /fs/microsd/params ] +then + param load /fs/microsd/params +fi + +# +# 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 diff --git a/ROMFS/px4fmu_common/init.d/09_ardrone_flow b/ROMFS/px4fmu_common/init.d/09_ardrone_flow new file mode 100644 index 000000000..e7173f6e6 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/09_ardrone_flow @@ -0,0 +1,94 @@ +#!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 + +# +# Load microSD params +# +echo "[init] loading microSD params" +param select /fs/microsd/params +if [ -f /fs/microsd/params ] +then + param load /fs/microsd/params +fi + +# +# 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 and MAVLink Onboard (Flow Sensor) +# +mavlink start -d /dev/ttyS0 -b 57600 +mavlink_onboard start -d /dev/ttyS3 -b 115200 +usleep 5000 + +# +# Start the commander. +# +commander start + +# +# Start the attitude estimator +# +attitude_estimator_ekf start + +# +# Start the position estimator +# +flow_position_estimator start + +# +# Fire up the multi rotor attitude controller +# +multirotor_att_control start + +# +# Fire up the flow position controller +# +flow_position_control start + +# +# Fire up the flow speed controller +# +flow_speed_control start + +# +# Fire up the AR.Drone interface. +# +ardrone_interface start -d /dev/ttyS1 + +# +# startup is done; we don't want the shell because we +# use the same UART for telemetry +# +echo "[init] startup done" + +exit diff --git a/ROMFS/px4fmu_common/init.d/30_io_camflyer b/ROMFS/px4fmu_common/init.d/30_io_camflyer new file mode 100644 index 000000000..5090b98a4 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/30_io_camflyer @@ -0,0 +1,121 @@ +#!nsh +# +# Flight startup script for PX4FMU+PX4IO +# + +# disable USB and autostart +set USB no +set MODE custom + +# +# Start the ORB (first app to start) +# +uorb start + +# +# Load microSD params +# +echo "[init] loading microSD params" +param select /fs/microsd/params +if [ -f /fs/microsd/params ] +then + param load /fs/microsd/params +fi + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set SYS_AUTOCONFIG 0 + param save /fs/microsd/params +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor +# see https://pixhawk.ethz.ch/mavlink/ +# +param set MAV_TYPE 1 + +# +# Check if PX4IO Firmware should be upgraded (from Andrew Tridgell) +# +if [ -f /fs/microsd/px4io.bin ] +then + echo "PX4IO Firmware found. Checking Upgrade.." + if cmp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current + then + echo "No newer version, skipping upgrade." + else + echo "Loading /fs/microsd/px4io.bin" + if px4io update /fs/microsd/px4io.bin > /fs/microsd/px4io_update.log + then + cp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current + echo "Flashed /fs/microsd/px4io.bin OK" >> /fs/microsd/px4io_update.log + else + echo "Failed flashing /fs/microsd/px4io.bin" >> /fs/microsd/px4io_update.log + echo "Failed to upgrade PX4IO firmware - check if PX4IO is in bootloader mode" + fi + fi +fi + +# +# Start MAVLink (depends on orb) +# +mavlink start -d /dev/ttyS1 -b 57600 +usleep 5000 + +# +# Start the commander (depends on orb, mavlink) +# +commander start + +# +# Start PX4IO interface (depends on orb, commander) +# +px4io start + +# +# Allow PX4IO to recover from midair restarts. +# this is very unlikely, but quite safe and robust. +px4io recovery + +# +# Set actuator limit to 100 Hz update (50 Hz PWM) +px4io limit 100 + +# +# Start the sensors (depends on orb, px4io) +# +sh /etc/init.d/rc.sensors + +# +# Start GPS interface (depends on orb) +# +gps start + +# +# Start the attitude estimator (depends on orb) +# +kalman_demo start + +# +# Load mixer and start controllers (depends on px4io) +# +mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix +control_demo start + +# +# Start logging +# +sdlog2 start -r 50 -a -b 14 + +# +# Start system state +# +if blinkm start +then + blinkm systemstate +fi diff --git a/ROMFS/px4fmu_common/init.d/31_io_phantom b/ROMFS/px4fmu_common/init.d/31_io_phantom new file mode 100644 index 000000000..5090b98a4 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/31_io_phantom @@ -0,0 +1,121 @@ +#!nsh +# +# Flight startup script for PX4FMU+PX4IO +# + +# disable USB and autostart +set USB no +set MODE custom + +# +# Start the ORB (first app to start) +# +uorb start + +# +# Load microSD params +# +echo "[init] loading microSD params" +param select /fs/microsd/params +if [ -f /fs/microsd/params ] +then + param load /fs/microsd/params +fi + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set SYS_AUTOCONFIG 0 + param save /fs/microsd/params +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor +# see https://pixhawk.ethz.ch/mavlink/ +# +param set MAV_TYPE 1 + +# +# Check if PX4IO Firmware should be upgraded (from Andrew Tridgell) +# +if [ -f /fs/microsd/px4io.bin ] +then + echo "PX4IO Firmware found. Checking Upgrade.." + if cmp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current + then + echo "No newer version, skipping upgrade." + else + echo "Loading /fs/microsd/px4io.bin" + if px4io update /fs/microsd/px4io.bin > /fs/microsd/px4io_update.log + then + cp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current + echo "Flashed /fs/microsd/px4io.bin OK" >> /fs/microsd/px4io_update.log + else + echo "Failed flashing /fs/microsd/px4io.bin" >> /fs/microsd/px4io_update.log + echo "Failed to upgrade PX4IO firmware - check if PX4IO is in bootloader mode" + fi + fi +fi + +# +# Start MAVLink (depends on orb) +# +mavlink start -d /dev/ttyS1 -b 57600 +usleep 5000 + +# +# Start the commander (depends on orb, mavlink) +# +commander start + +# +# Start PX4IO interface (depends on orb, commander) +# +px4io start + +# +# Allow PX4IO to recover from midair restarts. +# this is very unlikely, but quite safe and robust. +px4io recovery + +# +# Set actuator limit to 100 Hz update (50 Hz PWM) +px4io limit 100 + +# +# Start the sensors (depends on orb, px4io) +# +sh /etc/init.d/rc.sensors + +# +# Start GPS interface (depends on orb) +# +gps start + +# +# Start the attitude estimator (depends on orb) +# +kalman_demo start + +# +# Load mixer and start controllers (depends on px4io) +# +mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix +control_demo start + +# +# Start logging +# +sdlog2 start -r 50 -a -b 14 + +# +# Start system state +# +if blinkm start +then + blinkm systemstate +fi diff --git a/ROMFS/px4fmu_common/init.d/rc.1_fmu_quad_x b/ROMFS/px4fmu_common/init.d/rc.1_fmu_quad_x deleted file mode 100644 index 58a970eba..000000000 --- a/ROMFS/px4fmu_common/init.d/rc.1_fmu_quad_x +++ /dev/null @@ -1,107 +0,0 @@ -#!nsh -# -# Flight startup script for PX4FMU with PWM outputs. -# - -# disable USB and autostart -set USB no -set MODE custom - -echo "[init] doing PX4FMU Quad startup..." - -# -# Start the ORB -# -uorb start - -# -# Load microSD params -# -echo "[init] loading microSD params" -param select /fs/microsd/params -if [ -f /fs/microsd/params ] -then - param load /fs/microsd/params -fi - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set MC_ATTRATE_P 0.14 - param set MC_ATTRATE_I 0 - param set MC_ATTRATE_D 0.006 - param set MC_ATT_P 5.5 - param set MC_ATT_I 0 - param set MC_ATT_D 0 - param set MC_YAWPOS_D 0 - param set MC_YAWPOS_I 0 - param set MC_YAWPOS_P 0.6 - param set MC_YAWRATE_D 0 - param set MC_YAWRATE_I 0 - param set MC_YAWRATE_P 0.08 - param set RC_SCALE_PITCH 1 - param set RC_SCALE_ROLL 1 - param set RC_SCALE_YAW 3 - - param set SYS_AUTOCONFIG 0 - param save /fs/microsd/params -fi - -# -# 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 - -# -# Start MAVLink -# -mavlink start -d /dev/ttyS0 -b 57600 -usleep 5000 - -# -# Start the sensors and test them. -# -sh /etc/init.d/rc.sensors - -# -# Start the commander. -# -commander start - -# -# Start GPS interface (depends on orb) -# -gps start - -# -# Start the attitude estimator -# -attitude_estimator_ekf start - -echo "[init] starting PWM output" -fmu mode_pwm -mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix -pwm -u 400 -m 0xff - -# -# Start attitude control -# -multirotor_att_control start - -# -# Start logging -# -sdlog2 start -r 50 -a -b 14 - -# -# Start system state -# -if blinkm start -then - blinkm systemstate -fi diff --git a/ROMFS/px4fmu_common/init.d/rc.2_fmu_io_quad_x b/ROMFS/px4fmu_common/init.d/rc.2_fmu_io_quad_x deleted file mode 100644 index 200f49d1b..000000000 --- a/ROMFS/px4fmu_common/init.d/rc.2_fmu_io_quad_x +++ /dev/null @@ -1,123 +0,0 @@ -#!nsh -# -# Flight startup script for PX4FMU+PX4IO -# - -# disable USB and autostart -set USB no -set MODE custom - -# -# Start the ORB (first app to start) -# -uorb start - -# -# Load microSD params -# -echo "[init] loading microSD params" -param select /fs/microsd/params -if [ -f /fs/microsd/params ] -then - param load /fs/microsd/params -fi - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set SYS_AUTOCONFIG 0 - param save /fs/microsd/params -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ -# -param set MAV_TYPE 2 - -# -# Check if PX4IO Firmware should be upgraded (from Andrew Tridgell) -# -if [ -f /fs/microsd/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 - -# -# Disable px4io topic limiting -# -px4io limit 200 - -# -# 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) -# -mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix -pwm -u 400 -m 0xff -multirotor_att_control start - -# -# Start logging -# -sdlog2 start -r 50 -a -b 14 - -# -# Start system state -# -if blinkm start -then - blinkm systemstate -fi diff --git a/ROMFS/px4fmu_common/init.d/rc.30_fmu_io_camflyer b/ROMFS/px4fmu_common/init.d/rc.30_fmu_io_camflyer deleted file mode 100644 index 5090b98a4..000000000 --- a/ROMFS/px4fmu_common/init.d/rc.30_fmu_io_camflyer +++ /dev/null @@ -1,121 +0,0 @@ -#!nsh -# -# Flight startup script for PX4FMU+PX4IO -# - -# disable USB and autostart -set USB no -set MODE custom - -# -# Start the ORB (first app to start) -# -uorb start - -# -# Load microSD params -# -echo "[init] loading microSD params" -param select /fs/microsd/params -if [ -f /fs/microsd/params ] -then - param load /fs/microsd/params -fi - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set SYS_AUTOCONFIG 0 - param save /fs/microsd/params -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ -# -param set MAV_TYPE 1 - -# -# Check if PX4IO Firmware should be upgraded (from Andrew Tridgell) -# -if [ -f /fs/microsd/px4io.bin ] -then - echo "PX4IO Firmware found. Checking Upgrade.." - if cmp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current - then - echo "No newer version, skipping upgrade." - else - echo "Loading /fs/microsd/px4io.bin" - if px4io update /fs/microsd/px4io.bin > /fs/microsd/px4io_update.log - then - cp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current - echo "Flashed /fs/microsd/px4io.bin OK" >> /fs/microsd/px4io_update.log - else - echo "Failed flashing /fs/microsd/px4io.bin" >> /fs/microsd/px4io_update.log - echo "Failed to upgrade PX4IO firmware - check if PX4IO is in bootloader mode" - fi - fi -fi - -# -# Start MAVLink (depends on orb) -# -mavlink start -d /dev/ttyS1 -b 57600 -usleep 5000 - -# -# Start the commander (depends on orb, mavlink) -# -commander start - -# -# Start PX4IO interface (depends on orb, commander) -# -px4io start - -# -# Allow PX4IO to recover from midair restarts. -# this is very unlikely, but quite safe and robust. -px4io recovery - -# -# Set actuator limit to 100 Hz update (50 Hz PWM) -px4io limit 100 - -# -# Start the sensors (depends on orb, px4io) -# -sh /etc/init.d/rc.sensors - -# -# Start GPS interface (depends on orb) -# -gps start - -# -# Start the attitude estimator (depends on orb) -# -kalman_demo start - -# -# Load mixer and start controllers (depends on px4io) -# -mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix -control_demo start - -# -# Start logging -# -sdlog2 start -r 50 -a -b 14 - -# -# Start system state -# -if blinkm start -then - blinkm systemstate -fi diff --git a/ROMFS/px4fmu_common/init.d/rc.31_fmu_io_phantom b/ROMFS/px4fmu_common/init.d/rc.31_fmu_io_phantom deleted file mode 100644 index 5090b98a4..000000000 --- a/ROMFS/px4fmu_common/init.d/rc.31_fmu_io_phantom +++ /dev/null @@ -1,121 +0,0 @@ -#!nsh -# -# Flight startup script for PX4FMU+PX4IO -# - -# disable USB and autostart -set USB no -set MODE custom - -# -# Start the ORB (first app to start) -# -uorb start - -# -# Load microSD params -# -echo "[init] loading microSD params" -param select /fs/microsd/params -if [ -f /fs/microsd/params ] -then - param load /fs/microsd/params -fi - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set SYS_AUTOCONFIG 0 - param save /fs/microsd/params -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ -# -param set MAV_TYPE 1 - -# -# Check if PX4IO Firmware should be upgraded (from Andrew Tridgell) -# -if [ -f /fs/microsd/px4io.bin ] -then - echo "PX4IO Firmware found. Checking Upgrade.." - if cmp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current - then - echo "No newer version, skipping upgrade." - else - echo "Loading /fs/microsd/px4io.bin" - if px4io update /fs/microsd/px4io.bin > /fs/microsd/px4io_update.log - then - cp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current - echo "Flashed /fs/microsd/px4io.bin OK" >> /fs/microsd/px4io_update.log - else - echo "Failed flashing /fs/microsd/px4io.bin" >> /fs/microsd/px4io_update.log - echo "Failed to upgrade PX4IO firmware - check if PX4IO is in bootloader mode" - fi - fi -fi - -# -# Start MAVLink (depends on orb) -# -mavlink start -d /dev/ttyS1 -b 57600 -usleep 5000 - -# -# Start the commander (depends on orb, mavlink) -# -commander start - -# -# Start PX4IO interface (depends on orb, commander) -# -px4io start - -# -# Allow PX4IO to recover from midair restarts. -# this is very unlikely, but quite safe and robust. -px4io recovery - -# -# Set actuator limit to 100 Hz update (50 Hz PWM) -px4io limit 100 - -# -# Start the sensors (depends on orb, px4io) -# -sh /etc/init.d/rc.sensors - -# -# Start GPS interface (depends on orb) -# -gps start - -# -# Start the attitude estimator (depends on orb) -# -kalman_demo start - -# -# Load mixer and start controllers (depends on px4io) -# -mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix -control_demo start - -# -# Start logging -# -sdlog2 start -r 50 -a -b 14 - -# -# Start system state -# -if blinkm start -then - blinkm systemstate -fi diff --git a/ROMFS/px4fmu_common/init.d/rc.fmu_ardrone b/ROMFS/px4fmu_common/init.d/rc.fmu_ardrone deleted file mode 100644 index f55ac2ae3..000000000 --- a/ROMFS/px4fmu_common/init.d/rc.fmu_ardrone +++ /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 - -# -# Load microSD params -# -echo "[init] loading microSD params" -param select /fs/microsd/params -if [ -f /fs/microsd/params ] -then - param load /fs/microsd/params -fi - -# -# 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 diff --git a/ROMFS/px4fmu_common/init.d/rc.fmu_ardrone_flow b/ROMFS/px4fmu_common/init.d/rc.fmu_ardrone_flow deleted file mode 100644 index e7173f6e6..000000000 --- a/ROMFS/px4fmu_common/init.d/rc.fmu_ardrone_flow +++ /dev/null @@ -1,94 +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 - -# -# Load microSD params -# -echo "[init] loading microSD params" -param select /fs/microsd/params -if [ -f /fs/microsd/params ] -then - param load /fs/microsd/params -fi - -# -# 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 and MAVLink Onboard (Flow Sensor) -# -mavlink start -d /dev/ttyS0 -b 57600 -mavlink_onboard start -d /dev/ttyS3 -b 115200 -usleep 5000 - -# -# Start the commander. -# -commander start - -# -# Start the attitude estimator -# -attitude_estimator_ekf start - -# -# Start the position estimator -# -flow_position_estimator start - -# -# Fire up the multi rotor attitude controller -# -multirotor_att_control start - -# -# Fire up the flow position controller -# -flow_position_control start - -# -# Fire up the flow speed controller -# -flow_speed_control start - -# -# Fire up the AR.Drone interface. -# -ardrone_interface start -d /dev/ttyS1 - -# -# startup is done; we don't want the shell because we -# use the same UART for telemetry -# -echo "[init] startup done" - -exit diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index ee7e84050..b22591f3c 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -23,30 +23,6 @@ else tone_alarm 2 fi -# -# Check if auto-setup from one of the standard scripts is wanted -# SYS_AUTOSTART = 0 means no autostart (default) -# -if param compare SYS_AUTOSTART 1 -then - sh /etc/init.d/rc.1_fmu_quad_x -fi - -if param compare SYS_AUTOSTART 2 -then - sh /etc/init.d/rc.2_fmu_io_quad_x -fi - -if param compare SYS_AUTOSTART 30 -then - sh /etc/init.d/rc.30_fmu_io_camflyer -fi - -if param compare SYS_AUTOSTART 31 -then - sh /etc/init.d/rc.31_fmu_io_phantom -fi - # # Look for an init script on the microSD card. # @@ -106,3 +82,42 @@ else fi fi + +# +# Check if auto-setup from one of the standard scripts is wanted +# SYS_AUTOSTART = 0 means no autostart (default) +# +if param compare SYS_AUTOSTART 1 +then + sh /etc/init.d/01_fmu_quad_x +fi + +if param compare SYS_AUTOSTART 2 +then + sh /etc/init.d/02_io_quad_x +fi + +if param compare SYS_AUTOSTART 8 +then + sh /etc/init.d/08_ardrone +fi + +if param compare SYS_AUTOSTART 9 +then + sh /etc/init.d/09_ardrone_flow +fi + +if param compare SYS_AUTOSTART 10 +then + sh /etc/init.d/10_io_f330 +fi + +if param compare SYS_AUTOSTART 30 +then + sh /etc/init.d/30_io_camflyer +fi + +if param compare SYS_AUTOSTART 31 +then + sh /etc/init.d/31_io_phantom +fi diff --git a/src/modules/mavlink/mavlink_log.c b/src/modules/mavlink/mavlink_log.c index d9416a08b..192195856 100644 --- a/src/modules/mavlink/mavlink_log.c +++ b/src/modules/mavlink/mavlink_log.c @@ -41,16 +41,20 @@ #include #include +#include #include #include +static FILE* text_recorder_fd = NULL; + void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size) { lb->size = size; lb->start = 0; lb->count = 0; lb->elems = (struct mavlink_logmessage *)calloc(lb->size, sizeof(struct mavlink_logmessage)); + text_recorder_fd = fopen("/fs/microsd/text_recorder.txt", "w"); } int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb) @@ -82,6 +86,13 @@ int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessa memcpy(elem, &(lb->elems[lb->start]), sizeof(struct mavlink_logmessage)); lb->start = (lb->start + 1) % lb->size; --lb->count; + + if (text_recorder_fd) { + fwrite(elem->text, 1, strnlen(elem->text, 50), text_recorder_fd); + fputc("\n", text_recorder_fd); + fsync(text_recorder_fd); + } + return 0; } else { -- cgit v1.2.3