diff options
26 files changed, 718 insertions, 159 deletions
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/rc.IO_QUAD b/ROMFS/px4fmu_common/init.d/02_io_quad_x index 5f2de0d7e..131abf8c4 100644 --- a/ROMFS/px4fmu_common/init.d/rc.IO_QUAD +++ b/ROMFS/px4fmu_common/init.d/02_io_quad_x @@ -1,8 +1,11 @@ #!nsh +# +# Flight startup script for PX4FMU+PX4IO +# -# Disable USB and autostart +# disable USB and autostart set USB no -set MODE quad +set MODE custom # # Start the ORB (first app to start) @@ -18,6 +21,16 @@ if [ -f /fs/microsd/params ] then param load /fs/microsd/params fi + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set SYS_AUTOCONFIG 0 + param save /fs/microsd/params +fi # # Force some key parameters to sane values @@ -68,6 +81,11 @@ px4io start # Allow PX4IO to recover from midair restarts. # this is very unlikely, but quite safe and robust. px4io recovery + +# +# Disable px4io topic limiting +# +px4io limit 200 # # Start the sensors (depends on orb, px4io) @@ -87,21 +105,19 @@ attitude_estimator_ekf start # # Load mixer and start controllers (depends on px4io) # -mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix +mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix +pwm -u 400 -m 0xff multirotor_att_control start # # Start logging # -#sdlog start -s 4 +sdlog2 start -r 50 -a -b 14 # # Start system state # if blinkm start then - echo "using BlinkM for state indication" blinkm systemstate -else - echo "no BlinkM found, OK." -fi
\ No newline at end of file +fi diff --git a/ROMFS/px4fmu_common/init.d/rc.PX4IOAR b/ROMFS/px4fmu_common/init.d/08_ardrone index f55ac2ae3..f55ac2ae3 100644 --- a/ROMFS/px4fmu_common/init.d/rc.PX4IOAR +++ b/ROMFS/px4fmu_common/init.d/08_ardrone diff --git a/ROMFS/px4fmu_common/init.d/rc.PX4IOAR_PX4FLOW_example b/ROMFS/px4fmu_common/init.d/09_ardrone_flow index e7173f6e6..e7173f6e6 100644 --- a/ROMFS/px4fmu_common/init.d/rc.PX4IOAR_PX4FLOW_example +++ b/ROMFS/px4fmu_common/init.d/09_ardrone_flow diff --git a/ROMFS/px4fmu_common/init.d/10_io_f330 b/ROMFS/px4fmu_common/init.d/10_io_f330 new file mode 100644 index 000000000..4107fab4f --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/10_io_f330 @@ -0,0 +1,139 @@ +#!nsh +# +# Flight startup script for PX4FMU+PX4IO +# + +# disable USB and autostart +set USB no +set MODE custom + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set SYS_AUTOCONFIG 0 + + param set MC_ATTRATE_D 0.007 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_P 0.13 + param set MC_ATT_D 0.0 + param set MC_ATT_I 0.0 + param set MC_ATT_P 7.0 + param set MC_POS_P 0.1 + param set MC_RCLOSS_THR 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWPOS_I 0.5 + param set MC_YAWPOS_P 1.0 + param set MC_YAWRATE_D 0.0 + param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_P 0.2 + + param save /fs/microsd/params +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor +# see https://pixhawk.ethz.ch/mavlink/ +# +param set MAV_TYPE 2 + +# +# Check if PX4IO Firmware should be upgraded (from Andrew Tridgell) +# +if [ -f /fs/microsd/px4io2.bin ] +then + echo "PX4IO Firmware found. Checking Upgrade.." + if cmp /fs/microsd/px4io2.bin /fs/microsd/px4io2.cur + then + echo "No newer version, skipping upgrade." + else + echo "Loading /fs/microsd/px4io2.bin" + if px4io update /fs/microsd/px4io2.bin > /fs/microsd/px4io2.log + then + cp /fs/microsd/px4io2.bin /fs/microsd/px4io2.cur + echo "Flashed /fs/microsd/px4io2.bin OK" >> /fs/microsd/px4io2.log + else + echo "Failed flashing /fs/microsd/px4io2.bin" >> /fs/microsd/px4io2.log + echo "Failed to upgrade PX4IO2 firmware - check if PX4IO2 is in bootloader mode" + fi + fi +fi + +# +# Start MAVLink (depends on orb) +# +mavlink start +usleep 5000 + +# +# Start PX4IO interface (depends on orb, commander) +# +px4io start +pwm -u 400 -m 0xff + +# +# Allow PX4IO to recover from midair restarts. +# this is very unlikely, but quite safe and robust. +px4io recovery + +# +# Disable px4io topic limiting +# +px4io limit 200 + +# +# This sets a PWM right after startup (regardless of safety button) +# +px4io idle 900 900 900 900 + +# +# The values are for spinning motors when armed using DJI ESCs +# +px4io min 1200 1200 1200 1200 + +# +# Upper limits could be higher, this is on the safe side +# +px4io max 1800 1800 1800 1800 + +# +# Start the sensors (depends on orb, px4io) +# +sh /etc/init.d/rc.sensors + +# +# Start the commander (depends on orb, mavlink) +# +commander start + +# +# Start GPS interface (depends on orb) +# +gps start + +# +# Start the attitude estimator (depends on orb) +# +attitude_estimator_ekf start + +# +# Load mixer and start controllers (depends on px4io) +# +mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix +multirotor_att_control start + +# +# Start logging +# +sdlog2 start -r 20 -a -b 14 + +# +# Start system state +# +if blinkm start +then + blinkm systemstate +fi diff --git a/ROMFS/px4fmu_common/init.d/rc.PX4IO b/ROMFS/px4fmu_common/init.d/30_io_camflyer index 925a5703e..5090b98a4 100644 --- a/ROMFS/px4fmu_common/init.d/rc.PX4IO +++ b/ROMFS/px4fmu_common/init.d/30_io_camflyer @@ -1,8 +1,11 @@ #!nsh +# +# Flight startup script for PX4FMU+PX4IO +# -# Disable USB and autostart +# disable USB and autostart set USB no -set MODE camflyer +set MODE custom # # Start the ORB (first app to start) @@ -18,6 +21,16 @@ if [ -f /fs/microsd/params ] then param load /fs/microsd/params fi + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set SYS_AUTOCONFIG 0 + param save /fs/microsd/params +fi # # Force some key parameters to sane values @@ -68,6 +81,10 @@ px4io start # Allow PX4IO to recover from midair restarts. # this is very unlikely, but quite safe and robust. px4io recovery + +# +# Set actuator limit to 100 Hz update (50 Hz PWM) +px4io limit 100 # # Start the sensors (depends on orb, px4io) @@ -93,15 +110,12 @@ control_demo start # # Start logging # -#sdlog start -s 4 +sdlog2 start -r 50 -a -b 14 # # Start system state # if blinkm start then - echo "using BlinkM for state indication" blinkm systemstate -else - echo "no BlinkM found, OK." fi diff --git a/ROMFS/px4fmu_common/init.d/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.FMU_quad_x b/ROMFS/px4fmu_common/init.d/rc.FMU_quad_x deleted file mode 100644 index 980197d68..000000000 --- a/ROMFS/px4fmu_common/init.d/rc.FMU_quad_x +++ /dev/null @@ -1,67 +0,0 @@ -#!nsh -# -# Flight startup script for PX4FMU with PWM outputs. -# - -# Disable the USB interface -set USB no - -# Disable autostarting other apps -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 - -# -# 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 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 - -# -# Start attitude control -# -multirotor_att_control start - -echo "[init] startup done, exiting" -exit
\ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 62c7184b8..73f40c503 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -34,9 +34,10 @@ fi # ALWAYS start this task before the # preflight_check. # -sensors start - -# -# Check sensors - run AFTER 'sensors start' -# -preflight_check
\ No newline at end of file +if sensors start +then + # + # Check sensors - run AFTER 'sensors start' + # + preflight_check & +fi
\ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index 31af3991a..c89932bb5 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -5,8 +5,44 @@ echo "Starting MAVLink on this USB console" +# Stop tone alarm +tone_alarm stop + # Tell MAVLink that this link is "fast" -mavlink start -b 230400 -d /dev/console +if mavlink stop +then + echo "stopped other MAVLink instance" +fi +mavlink start -b 230400 -d /dev/ttyACM0 + +if [ $MODE == autostart ] +then + + # Start the commander + commander start + + # Start sensors + sh /etc/init.d/rc.sensors + + # Start one of the estimators + if attitude_estimator_ekf status + then + echo "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 +fi echo "MAVLink started, exiting shell.." diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 22dec87cb..b22591f3c 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -7,7 +7,6 @@ # can change this to prevent automatic startup of the flight script. # set MODE autostart -set USB autoconnect # # Try to mount the microSD card. @@ -42,31 +41,83 @@ then sh /fs/microsd/etc/rc.txt 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 + if sercon + then + echo "[init] USB interface connected" + fi + + echo "Running rc.APM" + # if APM startup is successful then nsh will exit + sh /etc/init.d/rc.APM +fi + +if [ $MODE == autostart ] +then + +# +# Start the ORB (first app to start) # -# Check for USB host +uorb start + +# +# Load microSD params # -if [ $USB != autoconnect ] +if ramtron start then - echo "[init] not connecting USB" + param select /ramtron/params + if [ -f /ramtron/params ] + then + param load /ramtron/params + fi else - if sercon + param select /fs/microsd/params + if [ -f /fs/microsd/params ] then - echo "[init] USB interface connected" - else - if [ -f /dev/ttyACM0 ] - echo "[init] NSH via USB" - then - else - echo "[init] No USB connected" - fi + param load /fs/microsd/params 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 ] +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 - echo Running rc.APM - # if APM startup is successful then nsh will exit - sh /etc/init.d/rc.APM + 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/drivers/boards/px4fmu/px4fmu_init.c b/src/drivers/boards/px4fmu/px4fmu_init.c index 212a92cfa..36af2298c 100644 --- a/src/drivers/boards/px4fmu/px4fmu_init.c +++ b/src/drivers/boards/px4fmu/px4fmu_init.c @@ -194,7 +194,7 @@ __EXPORT int nsh_archinitialize(void) /* initial LED state */ drv_led_start(); led_off(LED_AMBER); - led_on(LED_BLUE); + led_off(LED_BLUE); /* Configure SPI-based devices */ diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 59e90d86c..ac3bdc132 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1221,7 +1221,8 @@ start() int fd; if (g_dev != nullptr) - errx(1, "already started"); + /* if already started, the still command succeeded */ + errx(0, "already started"); /* create the driver, attempt expansion bus first */ g_dev = new HMC5883(PX4_I2C_BUS_EXPANSION); diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 220842536..8d9054a38 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -1063,7 +1063,8 @@ start() int fd; if (g_dev != nullptr) - errx(1, "already started"); + /* if already started, the still command succeeded */ + errx(0, "already started"); /* create the driver */ g_dev = new MPU6000(1 /* XXX magic number */, (spi_dev_e)PX4_SPIDEV_MPU); diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 59ab5936e..c13b65d5a 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -969,7 +969,8 @@ start() int fd; if (g_dev != nullptr) - errx(1, "already started"); + /* if already started, the still command succeeded */ + errx(0, "already started"); /* create the driver */ g_dev = new MS5611(MS5611_BUS); diff --git a/src/modules/att_pos_estimator_ekf/kalman_main.cpp b/src/modules/att_pos_estimator_ekf/kalman_main.cpp index 4befdc879..372b2d162 100644 --- a/src/modules/att_pos_estimator_ekf/kalman_main.cpp +++ b/src/modules/att_pos_estimator_ekf/kalman_main.cpp @@ -121,12 +121,13 @@ int att_pos_estimator_ekf_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { warnx("is running\n"); + exit(0); } else { warnx("not started\n"); + exit(1); } - exit(0); } usage("unrecognized command"); diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index d1b941ffe..19c96e9f4 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -139,10 +139,12 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { - printf("\tattitude_estimator_ekf app is running\n"); + warnx("running"); + exit(0); } else { - printf("\tattitude_estimator_ekf app not started\n"); + warnx("not started"); + exit(1); } exit(0); diff --git a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp index 2a06f1628..236052b56 100755 --- a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp +++ b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp @@ -139,10 +139,12 @@ int attitude_estimator_so3_comp_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { - printf("\tattitude_estimator_so3_comp app is running\n"); + warnx("running"); + exit(0); } else { - printf("\tattitude_estimator_so3_comp app not started\n"); + warnx("not started"); + exit(1); } exit(0); diff --git a/src/modules/commander/accelerometer_calibration.c b/src/modules/commander/accelerometer_calibration.c index 50234a45e..bc9d24956 100644 --- a/src/modules/commander/accelerometer_calibration.c +++ b/src/modules/commander/accelerometer_calibration.c @@ -283,6 +283,9 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { hrt_abstime t = t_start; hrt_abstime t_prev = t_start; hrt_abstime t_still = 0; + + unsigned poll_errcount = 0; + while (true) { /* wait blocking for new data */ int poll_ret = poll(fds, 1, 1000); @@ -327,12 +330,14 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { } } } else if (poll_ret == 0) { - /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "ERROR: poll failure"); - return -3; + poll_errcount++; } if (t > t_timeout) { - mavlink_log_info(mavlink_fd, "ERROR: timeout"); + poll_errcount++; + } + + if (poll_errcount > 1000) { + mavlink_log_info(mavlink_fd, "ERROR: failed reading accel"); return -1; } } diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 5f683c443..db8ee9067 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -413,12 +413,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 */ @@ -426,37 +426,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; @@ -748,8 +746,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; 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 <string.h> #include <stdlib.h> +#include <stdio.h> #include <stdarg.h> #include <mavlink/mavlink_log.h> +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 { diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 5e334638f..89d5cd374 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -139,14 +139,12 @@ public: private: static const unsigned _rc_max_chan_count = RC_CHANNELS_MAX; /**< maximum number of r/c channels we handle */ -#if CONFIG_HRT_PPM hrt_abstime _ppm_last_valid; /**< last time we got a valid ppm signal */ /** * Gather and publish PPM input data. */ void ppm_poll(); -#endif /* XXX should not be here - should be own driver */ int _fd_adc; /**< ADC driver handle */ @@ -393,9 +391,7 @@ Sensors *g_sensors; } Sensors::Sensors() : -#ifdef CONFIG_HRT_PPM _ppm_last_valid(0), -#endif _fd_adc(-1), _last_adc(0), @@ -1120,16 +1116,18 @@ Sensors::adc_poll(struct sensor_combined_s &raw) } } -#if CONFIG_HRT_PPM void Sensors::ppm_poll() { /* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */ - bool rc_updated; - orb_check(_rc_sub, &rc_updated); + struct pollfd fds[1]; + fds[0].fd = _rc_sub; + fds[0].events = POLLIN; + /* check non-blocking for new data */ + int poll_ret = poll(fds, 1, 0); - if (rc_updated) { + if (poll_ret > 0) { struct rc_input_values rc_input; orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); @@ -1316,7 +1314,6 @@ Sensors::ppm_poll() } } -#endif void Sensors::task_main_trampoline(int argc, char *argv[]) @@ -1429,10 +1426,8 @@ Sensors::task_main() if (_publishing) orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw); -#ifdef CONFIG_HRT_PPM /* Look for new r/c input data */ ppm_poll(); -#endif perf_end(_loop_perf); } @@ -1472,7 +1467,7 @@ int sensors_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { if (sensors::g_sensors != nullptr) - errx(1, "sensors task already running"); + errx(0, "sensors task already running"); sensors::g_sensors = new Sensors; diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index fd0289c9a..b470c1227 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -47,4 +47,5 @@ SRCS = err.c \ pid/pid.c \ geo/geo.c \ systemlib.c \ - airspeed.c + airspeed.c \ + system_params.c diff --git a/src/modules/systemlib/system_params.c b/src/modules/systemlib/system_params.c new file mode 100644 index 000000000..75be090f8 --- /dev/null +++ b/src/modules/systemlib/system_params.c @@ -0,0 +1,47 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file system_params.c + * + * System wide parameters + */ + +#include <nuttx/config.h> +#include <systemlib/param/param.h> + +// Auto-start script with index #n +PARAM_DEFINE_INT32(SYS_AUTOSTART, 0); + +// Automatically configure default values +PARAM_DEFINE_INT32(SYS_AUTOCONFIG, 0); diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index 60e61d07b..40a9297a7 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -63,6 +63,7 @@ static void do_import(const char* param_file_name); static void do_show(const char* search_string); static void do_show_print(void *arg, param_t param); static void do_set(const char* name, const char* val); +static void do_compare(const char* name, const char* val); int param_main(int argc, char *argv[]) @@ -117,9 +118,17 @@ param_main(int argc, char *argv[]) errx(1, "not enough arguments.\nTry 'param set PARAM_NAME 3'"); } } + + if (!strcmp(argv[1], "compare")) { + if (argc >= 4) { + do_compare(argv[2], argv[3]); + } else { + errx(1, "not enough arguments.\nTry 'param compare PARAM_NAME 3'"); + } + } } - errx(1, "expected a command, try 'load', 'import', 'show', 'set', 'select' or 'save'"); + errx(1, "expected a command, try 'load', 'import', 'show', 'set', 'compare', 'select' or 'save'"); } static void @@ -295,3 +304,65 @@ do_set(const char* name, const char* val) exit(0); } + +static void +do_compare(const char* name, const char* val) +{ + int32_t i; + float f; + param_t param = param_find(name); + + /* set nothing if parameter cannot be found */ + if (param == PARAM_INVALID) { + /* param not found */ + errx(1, "Error: Parameter %s not found.", name); + } + + /* + * Set parameter if type is known and conversion from string to value turns out fine + */ + + int ret = 1; + + switch (param_type(param)) { + case PARAM_TYPE_INT32: + if (!param_get(param, &i)) { + + /* convert string */ + char* end; + int j = strtol(val,&end,10); + if (i == j) { + printf(" %d: ", i); + ret = 0; + } + + } + + break; + + case PARAM_TYPE_FLOAT: + if (!param_get(param, &f)) { + + /* convert string */ + char* end; + float g = strtod(val, &end); + if (fabsf(f - g) < 1e-7f) { + printf(" %4.4f: ", (double)f); + ret = 0; + } + } + + break; + + default: + errx(1, "<unknown / unsupported type %d>\n", 0 + param_type(param)); + } + + if (ret == 0) { + printf("%c %s: equal\n", + param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'), + param_name(param)); + } + + exit(ret); +} diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c index 7752ffe67..d1dd85d47 100644 --- a/src/systemcmds/preflight_check/preflight_check.c +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -135,6 +135,7 @@ int preflight_check_main(int argc, char *argv[]) close(fd); fd = open(BARO_DEVICE_PATH, 0); + close(fd); /* ---- RC CALIBRATION ---- */ @@ -251,6 +252,11 @@ system_eval: int buzzer = open("/dev/tone_alarm", O_WRONLY); int leds = open(LED_DEVICE_PATH, 0); + if (leds < 0) { + close(buzzer); + errx(1, "failed to open leds, aborting"); + } + /* flip blue led into alternating amber */ led_off(leds, LED_BLUE); led_off(leds, LED_AMBER); |