diff --git a/ROMFS/scripts/rc.FMU_quad_x b/ROMFS/scripts/rc.FMU_quad_x index d9c9a8457f..8787443ea2 100644 --- a/ROMFS/scripts/rc.FMU_quad_x +++ b/ROMFS/scripts/rc.FMU_quad_x @@ -1,40 +1,67 @@ +#!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 # -# Startup for X-quad on FMU1.5/1.6 -# - -echo "[init] uORB" uorb start - -echo "[init] eeprom" -eeprom start -if [ -f /eeprom/parameters ] + +# +# Load microSD params +# +echo "[init] loading microSD params" +param select /fs/microsd/parameters +if [ -f /fs/microsd/parameters ] then - param load + param load /fs/microsd/parameters fi - -echo "[init] sensors" -#bma180 start -#l3gd20 start -mpu6000 start -hmc5883 start -ms5611 start - -sensors start - -echo "[init] mavlink" + +# +# 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 - -echo "[init] commander" + +# +# Start the sensors and test them. +# +sh /etc/init.d/rc.sensors + +# +# Start the commander. +# commander start - -echo "[init] attitude control" + +# +# Start the attitude estimator +# attitude_estimator_ekf start -multirotor_att_control 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 +exit \ No newline at end of file diff --git a/ROMFS/scripts/rc.PX4IO b/ROMFS/scripts/rc.PX4IO index 84e181a5ae..1e3963b9ad 100644 --- a/ROMFS/scripts/rc.PX4IO +++ b/ROMFS/scripts/rc.PX4IO @@ -1,73 +1,80 @@ #!nsh - + +# Disable USB and autostart set USB no - +set MODE camflyer + # -# Start the object request broker +# Start the ORB # uorb start - + # -# Init the EEPROM +# Load microSD params # -echo "[init] eeprom" -eeprom start -if [ -f /eeprom/parameters ] +echo "[init] loading microSD params" +param select /fs/microsd/parameters +if [ -f /fs/microsd/parameters ] then - param load + param load /fs/microsd/parameters fi - + # -# Enable / connect to PX4IO +# Force some key parameters to sane values +# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor +# see https://pixhawk.ethz.ch/mavlink/ # -px4io start - -# -# Load an appropriate mixer. FMU_pass.mix is a passthru mixer -# which is good for testing. See ROMFS/mixers for a full list of mixers. -# -mixer load /dev/pwm_output /etc/mixers/FMU_pass.mix - +param set MAV_TYPE 1 + # # Start the sensors. # sh /etc/init.d/rc.sensors - + # -# Start MAVLink on UART1 (dev/ttyS0) at 57600 baud (CLI is now unusable) +# Start MAVLink # -mavlink start -d /dev/ttyS0 -b 57600 +mavlink start -d /dev/ttyS1 -b 57600 usleep 5000 - + # # Start the commander. # commander start - + +# +# Start GPS interface +# +gps start + # # Start the attitude estimator # -attitude_estimator_ekf start +kalman_demo start + +# +# Start PX4IO interface +# +px4io start + +# +# Load mixer and start controllers +# +mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix +control_demo start + +# +# Start logging +# +sdlog start -s 10 # -# Start the attitude and position controller +# Start system state # -fixedwing_att_control start -fixedwing_pos_control start - -# -# Start GPS capture. Comment this out if you do not have a GPS. -# -gps start - -# -# Start logging to microSD if we can -# -sh /etc/init.d/rc.logging - -# -# startup is done; we don't want the shell because we -# use the same UART for telemetry -# -echo "[init] startup done" -exit +if blinkm start +then + echo "using BlinkM for state indication" + blinkm systemstate +else + echo "no BlinkM found, OK." +fi diff --git a/ROMFS/scripts/rc.PX4IOAR b/ROMFS/scripts/rc.PX4IOAR index 640cdf5411..72df68e350 100644 --- a/ROMFS/scripts/rc.PX4IOAR +++ b/ROMFS/scripts/rc.PX4IOAR @@ -17,7 +17,7 @@ echo "[init] doing PX4IOAR startup..." uorb start # -# Load microSD params +# Init the parameter storage # echo "[init] loading microSD params" param select /fs/microsd/parameters @@ -26,17 +26,24 @@ 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 + +# +# Start the sensors. +# +sh /etc/init.d/rc.sensors + # # 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. # @@ -62,15 +69,26 @@ multirotor_att_control start # ardrone_interface start -d /dev/ttyS1 -# -# Start logging -# -#sdlog start - # # Start GPS capture # gps start + +# +# Start logging +# +sdlog start -s 10 + +# +# 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 diff --git a/apps/commander/commander.c b/apps/commander/commander.c index f917b7275d..b187145321 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -1467,8 +1467,6 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_system_id, &(current_status.system_id)); param_get(_param_component_id, &(current_status.component_id)); - } else { - warnx("ARMED, rejecting sys type change\n"); } } diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 069028c14e..b2e0c6ee45 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -279,6 +279,8 @@ PX4IO::PX4IO() : _max_relays(0), _max_transfer(16), /* sensible default */ _update_interval(0), + _status(0), + _alarms(0), _task(-1), _task_should_exit(false), _perf_update(perf_alloc(PC_ELAPSED, "px4io update")), @@ -360,8 +362,8 @@ PX4IO::init() uint16_t reg; - /* get STATUS_FLAGS, STATUS_ALARMS, STATUS_VBATT, STATUS_IBATT in that order */ - ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, ®, sizeof(reg)); + /* get IO's last seen FMU state */ + ret = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, ®, sizeof(reg)); if (ret != OK) return ret; @@ -392,8 +394,8 @@ PX4IO::init() /* wait 10 ms */ usleep(10000); - /* abort after 10s */ - if ((hrt_absolute_time() - try_start_time)/1000 > 10000) { + /* abort after 5s */ + if ((hrt_absolute_time() - try_start_time)/1000 > 50000) { log("failed to recover from in-air restart (1), aborting IO driver init."); return 1; } @@ -432,8 +434,8 @@ PX4IO::init() /* wait 10 ms */ usleep(10000); - /* abort after 10s */ - if ((hrt_absolute_time() - try_start_time)/1000 > 10000) { + /* abort after 5s */ + if ((hrt_absolute_time() - try_start_time)/1000 > 50000) { log("failed to recover from in-air restart (2), aborting IO driver init."); return 1; } @@ -770,19 +772,28 @@ PX4IO::io_handle_status(uint16_t status) */ /* check for IO reset - force it back to armed if necessary */ - if (_status & PX4IO_P_STATUS_FLAGS_ARMED && !(status & PX4IO_P_STATUS_FLAGS_ARMED)) { + if (_status & PX4IO_P_STATUS_FLAGS_ARMED && !(status & PX4IO_P_STATUS_FLAGS_ARMED) + && !(status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) { /* set the arming flag */ - ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_ARMED); + ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_ARMED | PX4IO_P_STATUS_FLAGS_ARM_SYNC); /* set new status */ _status = status; _status &= PX4IO_P_STATUS_FLAGS_ARMED; + } else if (!(_status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) { + + /* set the sync flag */ + ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_ARM_SYNC); + /* set new status */ + _status = status; + } else { ret = 0; /* set new status */ _status = status; } + return ret; } diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index 4f1b067bd2..a957a9e79a 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -102,6 +102,7 @@ #define PX4IO_P_STATUS_FLAGS_FMU_OK (1 << 6) /* controls from FMU are valid */ #define PX4IO_P_STATUS_FLAGS_RAW_PWM (1 << 7) /* raw PWM from FMU is bypassing the mixer */ #define PX4IO_P_STATUS_FLAGS_MIXER_OK (1 << 8) /* mixer is OK */ +#define PX4IO_P_STATUS_FLAGS_ARM_SYNC (1 << 9) /* the arming state between IO and FMU is in sync */ #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) /* VBatt is very close to regulator dropout */ diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index f1231b0cf1..fec5eed236 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -84,7 +84,7 @@ static volatile uint8_t msg_next_out, msg_next_in; * output. */ #define NUM_MSG 2 -static char msg[NUM_MSG][40]; +static char msg[NUM_MSG][50]; /* add a debug message to be printed on the console @@ -183,23 +183,23 @@ int user_start(int argc, char *argv[]) /* add a performance counter for mixing */ perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix"); - /* run the mixer at 100Hz (for now...) */ + /* run the mixer at ~300Hz (for now...) */ /* XXX we should use CONFIG_IDLE_CUSTOM and take over the idle thread instead of running two additional tasks */ - uint8_t counter=0; + uint16_t counter=0; for (;;) { /* - if we are not scheduled for 100ms then reset the I2C bus + if we are not scheduled for 10ms then reset the I2C bus */ - hrt_call_after(&loop_overtime_call, 100000, (hrt_callout)loop_overtime, NULL); + hrt_call_after(&loop_overtime_call, 10000, (hrt_callout)loop_overtime, NULL); - poll(NULL, 0, 10); + poll(NULL, 0, 3); perf_begin(mixer_perf); mixer_tick(); perf_end(mixer_perf); show_debug_messages(); - if (counter++ == 200) { + if (counter++ == 800) { counter = 0; - isr_debug(1, "tick dbg=%u stat=0x%x arm=0x%x feat=0x%x rst=%u", + isr_debug(1, "d:%u stat=0x%x arm=0x%x feat=0x%x rst=%u", (unsigned)debug_level, (unsigned)r_status_flags, (unsigned)r_setup_arming, diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c index be3bebada9..40bf724821 100644 --- a/apps/px4io/registers.c +++ b/apps/px4io/registers.c @@ -270,6 +270,16 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) r_status_alarms &= ~value; break; + case PX4IO_P_STATUS_FLAGS: + /* + * Allow FMU override of arming state (to allow in-air restores), + * but only if the arming state is not in sync on the IO side. + */ + if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) { + r_status_flags = value; + } + break; + default: /* just ignore writes to other registers in this page */ break;