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)); diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index a54bb39646..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; } @@ -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; } @@ -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."); @@ -774,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); /* @@ -809,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(); @@ -1673,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) { @@ -1697,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; @@ -2699,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; @@ -2782,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]); @@ -2797,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]; @@ -2855,6 +2877,7 @@ px4io_main(int argc, char *argv[]) /* stop the driver */ delete g_dev; + g_dev = nullptr; exit(0); } 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 */ 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(); 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 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(); 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. diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index a59dbe89cf..19fc61f78b 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -380,7 +380,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: @@ -507,8 +510,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; } @@ -538,8 +540,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; } diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 5e8c5edbcf..ea864390da 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -795,7 +795,6 @@ Sensors::accel_init() #endif - warnx("using system accel"); close(fd); } } @@ -835,7 +834,6 @@ Sensors::gyro_init() #endif - warnx("using system gyro"); close(fd); } } @@ -1505,9 +1503,6 @@ void Sensors::task_main() { - /* inform about start */ - warnx("Initializing.."); - /* start individual sensors */ accel_init(); gyro_init();