diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-02-09 16:04:32 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-02-09 16:04:32 +0100 |
commit | 773f70a9df376745523bf78f29d6747c8878c01d (patch) | |
tree | f63d9f6c2d4bb3bb3018b230ae9f42f89ff85c7a /src/drivers/px4io | |
parent | ac326beaaae7b38d65ad6d7d13f00dfeaa6ae520 (diff) | |
parent | f52f15c7914983ea1569e584e516d53d21cdde56 (diff) | |
download | px4-firmware-773f70a9df376745523bf78f29d6747c8878c01d.tar.gz px4-firmware-773f70a9df376745523bf78f29d6747c8878c01d.tar.bz2 px4-firmware-773f70a9df376745523bf78f29d6747c8878c01d.zip |
Merged origin/master into pubsub_cleanup
Diffstat (limited to 'src/drivers/px4io')
-rw-r--r-- | src/drivers/px4io/px4io.cpp | 350 | ||||
-rw-r--r-- | src/drivers/px4io/px4io_uploader.cpp | 17 | ||||
-rw-r--r-- | src/drivers/px4io/uploader.h | 2 |
3 files changed, 313 insertions, 56 deletions
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 0ca35d2f2..efcf4d12b 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -61,6 +61,7 @@ #include <drivers/device/device.h> #include <drivers/drv_rc_input.h> #include <drivers/drv_pwm_output.h> +#include <drivers/drv_sbus.h> #include <drivers/drv_gpio.h> #include <drivers/drv_hrt.h> #include <drivers/drv_mixer.h> @@ -238,6 +239,7 @@ private: unsigned _update_interval; ///< Subscription interval limiting send rate bool _rc_handling_disabled; ///< If set, IO does not evaluate, but only forward the RC values unsigned _rc_chan_count; ///< Internal copy of the last seen number of RC channels + uint64_t _rc_last_valid; ///< last valid timestamp volatile int _task; ///< worker task id volatile bool _task_should_exit; ///< worker terminate flag @@ -270,9 +272,11 @@ private: orb_advert_t _to_servorail; ///< servorail status orb_advert_t _to_safety; ///< status of safety - actuator_outputs_s _outputs; ///<mixed outputs + actuator_outputs_s _outputs; ///< mixed outputs + servorail_status_s _servorail_status; ///< servorail status bool _primary_pwm_device; ///< true if we are the default PWM output + bool _lockdown_override; ///< allow to override the safety lockdown float _battery_amp_per_volt; ///< current sensor amps/volt float _battery_amp_bias; ///< current sensor bias @@ -443,14 +447,14 @@ private: * @param vservo vservo register * @param vrssi vrssi register */ - void io_handle_vservo(uint16_t vbatt, uint16_t ibatt); + void io_handle_vservo(uint16_t vservo, uint16_t vrssi); }; namespace { -PX4IO *g_dev; +PX4IO *g_dev = nullptr; } @@ -466,6 +470,7 @@ PX4IO::PX4IO(device::Device *interface) : _update_interval(0), _rc_handling_disabled(false), _rc_chan_count(0), + _rc_last_valid(0), _task(-1), _task_should_exit(false), _mavlink_fd(-1), @@ -489,6 +494,7 @@ PX4IO::PX4IO(device::Device *interface) : _to_servorail(0), _to_safety(0), _primary_pwm_device(false), + _lockdown_override(false), _battery_amp_per_volt(90.0f / 5.0f), // this matches the 3DR current sensor _battery_amp_bias(0), _battery_mamphour_total(0), @@ -504,7 +510,8 @@ 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; } PX4IO::~PX4IO() @@ -578,6 +585,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."); @@ -772,8 +785,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); /* @@ -807,8 +818,6 @@ PX4IO::task_main() fds[0].fd = _t_actuator_controls_0; fds[0].events = POLLIN; - log("ready"); - /* lock against the ioctl handler */ lock(); @@ -1043,13 +1052,19 @@ PX4IO::io_set_arming_state() uint16_t set = 0; uint16_t clear = 0; - if (armed.armed && !armed.lockdown) { + if (armed.armed) { set |= PX4IO_P_SETUP_ARMING_FMU_ARMED; } else { clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED; } + if (armed.lockdown && !_lockdown_override) { + set |= PX4IO_P_SETUP_ARMING_LOCKDOWN; + } else { + clear |= PX4IO_P_SETUP_ARMING_LOCKDOWN; + } + if (armed.ready_to_arm) { set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; @@ -1300,6 +1315,7 @@ PX4IO::io_handle_battery(uint16_t vbatt, uint16_t ibatt) /* voltage is scaled to mV */ battery_status.voltage_v = vbatt / 1000.0f; + battery_status.voltage_filtered_v = vbatt / 1000.0f; /* ibatt contains the raw ADC count, as 12 bit ADC @@ -1331,19 +1347,18 @@ PX4IO::io_handle_battery(uint16_t vbatt, uint16_t ibatt) void PX4IO::io_handle_vservo(uint16_t vservo, uint16_t vrssi) { - servorail_status_s servorail_status; - servorail_status.timestamp = hrt_absolute_time(); + _servorail_status.timestamp = hrt_absolute_time(); /* voltage is scaled to mV */ - servorail_status.voltage_v = vservo * 0.001f; - servorail_status.rssi_v = vrssi * 0.001f; + _servorail_status.voltage_v = vservo * 0.001f; + _servorail_status.rssi_v = vrssi * 0.001f; /* lazily publish the servorail voltages */ if (_to_servorail > 0) { - orb_publish(ORB_ID(servorail_status), _to_servorail, &servorail_status); + orb_publish(ORB_ID(servorail_status), _to_servorail, &_servorail_status); } else { - _to_servorail = orb_advertise(ORB_ID(servorail_status), &servorail_status); + _to_servorail = orb_advertise(ORB_ID(servorail_status), &_servorail_status); } } @@ -1353,7 +1368,10 @@ PX4IO::io_get_status() uint16_t regs[6]; int ret; - /* get STATUS_FLAGS, STATUS_ALARMS, STATUS_VBATT, STATUS_IBATT in that order */ + /* get + * STATUS_FLAGS, STATUS_ALARMS, STATUS_VBATT, STATUS_IBATT, + * STATUS_VSERVO, STATUS_VRSSI, STATUS_PRSSI + * in that order */ ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, ®s[0], sizeof(regs) / sizeof(regs[0])); if (ret != OK) @@ -1390,7 +1408,8 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) * * This should be the common case (9 channel R/C control being a reasonable upper bound). */ - input_rc.timestamp = hrt_absolute_time(); + input_rc.timestamp_publication = hrt_absolute_time(); + ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT, ®s[0], prolog + 9); if (ret != OK) @@ -1400,13 +1419,25 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) * Get the channel count any any extra channels. This is no more expensive than reading the * channel count once. */ - channel_count = regs[0]; + channel_count = regs[PX4IO_P_RAW_RC_COUNT]; if (channel_count != _rc_chan_count) perf_count(_perf_chan_count); _rc_chan_count = channel_count; + input_rc.rc_ppm_frame_length = regs[PX4IO_P_RAW_RC_DATA]; + input_rc.rssi = regs[PX4IO_P_RAW_RC_NRSSI]; + input_rc.rc_failsafe = (regs[PX4IO_P_RAW_RC_FLAGS] & PX4IO_P_RAW_RC_FLAGS_FAILSAFE); + input_rc.rc_lost_frame_count = regs[PX4IO_P_RAW_LOST_FRAME_COUNT]; + input_rc.rc_total_frame_count = regs[PX4IO_P_RAW_FRAME_COUNT]; + + /* rc_lost has to be set before the call to this function */ + if (!input_rc.rc_lost && !input_rc.rc_failsafe) + _rc_last_valid = input_rc.timestamp_publication; + + input_rc.timestamp_last_signal = _rc_last_valid; + if (channel_count > 9) { ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + 9, ®s[prolog + 9], channel_count - 9); @@ -1423,13 +1454,12 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) int PX4IO::io_publish_raw_rc() { - /* if no raw RC, just don't publish */ - if (!(_status & PX4IO_P_STATUS_FLAGS_RC_OK)) - return OK; /* fetch values from IO */ rc_input_values rc_val; - rc_val.timestamp = hrt_absolute_time(); + + /* set the RC status flag ORDER MATTERS! */ + rc_val.rc_lost = !(_status & PX4IO_P_STATUS_FLAGS_RC_OK); int ret = io_get_raw_rc_input(rc_val); @@ -1448,6 +1478,11 @@ PX4IO::io_publish_raw_rc() } else { rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN; + + /* we do not know the RC input, only publish if RC OK flag is set */ + /* if no raw RC, just don't publish */ + if (!(_status & PX4IO_P_STATUS_FLAGS_RC_OK)) + return OK; } /* lazily advertise on first publication */ @@ -1664,7 +1699,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) { @@ -1688,7 +1734,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; @@ -1734,15 +1791,14 @@ PX4IO::print_status() printf("%u bytes free\n", io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM)); uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS); - printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s%s\n", + uint16_t io_status_flags = flags; + printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s\n", flags, ((flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED) ? " OUTPUTS_ARMED" : ""), ((flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ? " SAFETY_OFF" : " SAFETY_SAFE"), ((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""), ((flags & PX4IO_P_STATUS_FLAGS_RC_OK) ? " RC_OK" : " RC_FAIL"), ((flags & PX4IO_P_STATUS_FLAGS_RC_PPM) ? " PPM" : ""), - (((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (!(flags & PX4IO_P_STATUS_FLAGS_RC_DSM11))) ? " DSM10" : ""), - (((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_STATUS_FLAGS_RC_DSM11)) ? " DSM11" : ""), ((flags & PX4IO_P_STATUS_FLAGS_RC_SBUS) ? " SBUS" : ""), ((flags & PX4IO_P_STATUS_FLAGS_FMU_OK) ? " FMU_OK" : " FMU_FAIL"), ((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) ? " RAW_PWM_PASSTHROUGH" : ""), @@ -1801,8 +1857,17 @@ PX4IO::print_status() printf("\n"); - if (raw_inputs > 0) { - int frame_len = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_RC_DATA); + flags = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_FLAGS); + printf("R/C flags: 0x%04x%s%s%s%s%s\n", flags, + (((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (!(flags & PX4IO_P_RAW_RC_FLAGS_RC_DSM11))) ? " DSM10" : ""), + (((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_RAW_RC_FLAGS_RC_DSM11)) ? " DSM11" : ""), + ((flags & PX4IO_P_RAW_RC_FLAGS_FRAME_DROP) ? " FRAME_DROP" : ""), + ((flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) ? " FAILSAFE" : ""), + ((flags & PX4IO_P_RAW_RC_FLAGS_MAPPING_OK) ? " MAPPING_OK" : "") + ); + + if ((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_PPM)) { + int frame_len = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_DATA); printf("RC data (PPM frame len) %u us\n", frame_len); if ((frame_len - raw_inputs * 2000 - 3000) < 0) { @@ -1828,16 +1893,23 @@ PX4IO::print_status() printf("\n"); /* setup and state */ - printf("features 0x%04x\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES)); + uint16_t features = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES); + printf("features 0x%04x%s%s%s%s\n", features, + ((features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) ? " S.BUS1_OUT" : ""), + ((features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) ? " S.BUS2_OUT" : ""), + ((features & PX4IO_P_SETUP_FEATURES_PWM_RSSI) ? " RSSI_PWM" : ""), + ((features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) ? " RSSI_ADC" : "") + ); uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING); - printf("arming 0x%04x%s%s%s%s%s%s\n", + printf("arming 0x%04x%s%s%s%s%s%s%s\n", arming, - ((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) ? " FMU_ARMED" : " FMU_DISARMED"), - ((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) ? " IO_ARM_OK" : " IO_ARM_DENIED"), - ((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""), - ((arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ? " FAILSAFE_CUSTOM" : ""), - ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : ""), - ((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) ? " ALWAYS_PWM_ENABLE" : "")); + ((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) ? " FMU_ARMED" : " FMU_DISARMED"), + ((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) ? " IO_ARM_OK" : " IO_ARM_DENIED"), + ((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""), + ((arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ? " FAILSAFE_CUSTOM" : ""), + ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : ""), + ((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) ? " ALWAYS_PWM_ENABLE" : ""), + ((arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) ? " LOCKDOWN" : "")); #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 printf("rates 0x%04x default %u alt %u relays 0x%04x\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES), @@ -2034,6 +2106,14 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) *(unsigned *)arg = _max_actuators; break; + case PWM_SERVO_SET_DISABLE_LOCKDOWN: + _lockdown_override = arg; + break; + + case PWM_SERVO_GET_DISABLE_LOCKDOWN: + *(unsigned *)arg = _lockdown_override; + break; + case DSM_BIND_START: io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down); usleep(500000); @@ -2250,6 +2330,38 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) break; + case RC_INPUT_ENABLE_RSSI_ANALOG: + + if (arg) { + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_ADC_RSSI); + } else { + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, PX4IO_P_SETUP_FEATURES_ADC_RSSI, 0); + } + + break; + + case RC_INPUT_ENABLE_RSSI_PWM: + + if (arg) { + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_PWM_RSSI); + } else { + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, PX4IO_P_SETUP_FEATURES_PWM_RSSI, 0); + } + + break; + + case SBUS_SET_PROTO_VERSION: + + if (arg == 1) { + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_SBUS1_OUT); + } else if (arg == 2) { + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_SBUS2_OUT); + } else { + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT), 0); + } + + break; + default: /* not a recognized value */ ret = -ENOTTY; @@ -2358,8 +2470,10 @@ start(int argc, char *argv[]) /* create the driver - it will set g_dev */ (void)new PX4IO(interface); - if (g_dev == nullptr) + if (g_dev == nullptr) { + delete interface; errx(1, "driver alloc failed"); + } if (OK != g_dev->init()) { delete g_dev; @@ -2621,7 +2735,7 @@ monitor(void) /* clear screen */ printf("\033[2J"); - unsigned cancels = 3; + unsigned cancels = 2; for (;;) { pollfd fds[1]; @@ -2635,17 +2749,17 @@ monitor(void) read(0, &c, 1); if (cancels-- == 0) { - printf("\033[H"); /* move cursor home and clear screen */ + printf("\033[2J\033[H"); /* move cursor home and clear screen */ exit(0); } } if (g_dev != nullptr) { - printf("\033[H"); /* move cursor home and clear screen */ + printf("\033[2J\033[H"); /* move cursor home and clear screen */ (void)g_dev->print_status(); (void)g_dev->print_debug(); - printf("[ Use 'px4io debug <N>' for more output. Hit <enter> three times to exit monitor mode ]\n"); + printf("\n\n\n[ Use 'px4io debug <N>' for more output. Hit <enter> three times to exit monitor mode ]\n"); } else { errx(1, "driver not loaded, exiting"); @@ -2657,13 +2771,72 @@ void if_test(unsigned mode) { device::Device *interface = get_interface(); + int result; - int result = interface->ioctl(1, mode); /* XXX magic numbers */ - delete interface; + if (interface) { + result = interface->ioctl(1, mode); /* XXX magic numbers */ + delete interface; + } else { + errx(1, "interface not loaded, exiting"); + } errx(0, "test returned %d", result); } +void +lockdown(int argc, char *argv[]) +{ + if (g_dev != nullptr) { + + if (argc > 2 && !strcmp(argv[2], "disable")) { + + warnx("WARNING: ACTUATORS WILL BE LIVE IN HIL! PROCEED?"); + warnx("Press 'y' to enable, any other key to abort."); + + /* check if user wants to abort */ + char c; + + struct pollfd fds; + int ret; + hrt_abstime start = hrt_absolute_time(); + const unsigned long timeout = 5000000; + + while (hrt_elapsed_time(&start) < timeout) { + fds.fd = 0; /* stdin */ + fds.events = POLLIN; + ret = poll(&fds, 1, 0); + + if (ret > 0) { + + read(0, &c, 1); + + if (c != 'y') { + exit(0); + } else if (c == 'y') { + break; + } + } + + usleep(10000); + } + + if (hrt_elapsed_time(&start) > timeout) + errx(1, "TIMEOUT! ABORTED WITHOUT CHANGES."); + + (void)g_dev->ioctl(0, PWM_SERVO_SET_DISABLE_LOCKDOWN, 1); + + warnx("WARNING: ACTUATORS ARE NOW LIVE IN HIL!"); + } else { + (void)g_dev->ioctl(0, PWM_SERVO_SET_DISABLE_LOCKDOWN, 0); + warnx("ACTUATORS ARE NOW SAFE IN HIL."); + } + + } else { + errx(1, "driver not loaded, exiting"); + } + exit(0); +} + } /* namespace */ int @@ -2688,6 +2861,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; @@ -2760,18 +2934,30 @@ px4io_main(int argc, char *argv[]) } if (g_dev == nullptr) { warnx("px4io is not started, still attempting upgrade"); - } else { - uint16_t arg = atol(argv[2]); - int ret = g_dev->ioctl(nullptr, PX4IO_REBOOT_BOOTLOADER, arg); - if (ret != OK) { - printf("reboot failed - %d\n", ret); - exit(1); + + /* allocate the interface */ + device::Device *interface = get_interface(); + + /* create the driver - it will set g_dev */ + (void)new PX4IO(interface); + + if (g_dev == nullptr) { + delete interface; + errx(1, "driver alloc failed"); } + } - // tear down the px4io instance - delete g_dev; + uint16_t arg = atol(argv[2]); + int ret = g_dev->ioctl(nullptr, PX4IO_REBOOT_BOOTLOADER, arg); + if (ret != OK) { + printf("reboot failed - %d\n", ret); + exit(1); } + // tear down the px4io instance + delete g_dev; + g_dev = nullptr; + // upload the specified firmware const char *fn[2]; fn[0] = argv[3]; @@ -2829,6 +3015,7 @@ px4io_main(int argc, char *argv[]) /* stop the driver */ delete g_dev; + g_dev = nullptr; exit(0); } @@ -2883,6 +3070,63 @@ px4io_main(int argc, char *argv[]) if (!strcmp(argv[1], "bind")) bind(argc, argv); + if (!strcmp(argv[1], "lockdown")) + lockdown(argc, argv); + + if (!strcmp(argv[1], "sbus1_out")) { + /* we can cheat and call the driver directly, as it + * doesn't reference filp in ioctl() + */ + int ret = g_dev->ioctl(nullptr, SBUS_SET_PROTO_VERSION, 1); + + if (ret != 0) { + errx(ret, "S.BUS v1 failed"); + } + + exit(0); + } + + if (!strcmp(argv[1], "sbus2_out")) { + /* we can cheat and call the driver directly, as it + * doesn't reference filp in ioctl() + */ + int ret = g_dev->ioctl(nullptr, SBUS_SET_PROTO_VERSION, 2); + + if (ret != 0) { + errx(ret, "S.BUS v2 failed"); + } + + exit(0); + } + + if (!strcmp(argv[1], "rssi_analog")) { + /* we can cheat and call the driver directly, as it + * doesn't reference filp in ioctl() + */ + int ret = g_dev->ioctl(nullptr, RC_INPUT_ENABLE_RSSI_ANALOG, 1); + + if (ret != 0) { + errx(ret, "RSSI analog failed"); + } + + exit(0); + } + + if (!strcmp(argv[1], "rssi_pwm")) { + /* we can cheat and call the driver directly, as it + * doesn't reference filp in ioctl() + */ + int ret = g_dev->ioctl(nullptr, RC_INPUT_ENABLE_RSSI_PWM, 1); + + if (ret != 0) { + errx(ret, "RSSI PWM failed"); + } + + exit(0); + } + out: - errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'bind', 'checkcrc', 'forceupdate' or 'update'"); + errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug <level>',\n" + "'recovery', 'limit <rate>', 'current', 'bind', 'checkcrc',\n" + "'forceupdate', 'update', 'sbus1_out', 'sbus2_out', 'rssi_analog' or 'rssi_pwm'"); } diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index 41f93a8ee..dd8abbac5 100644 --- a/src/drivers/px4io/px4io_uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -51,6 +51,7 @@ #include <poll.h> #include <termios.h> #include <sys/stat.h> +#include <nuttx/arch.h> #include <crc32.h> @@ -120,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 */ @@ -226,6 +234,11 @@ PX4IO_Uploader::upload(const char *filenames[]) close(_fw_fd); close(_io_fd); _io_fd = -1; + + // sleep for enough time for the IO chip to boot. This makes + // forceupdate more reliably startup IO again after update + up_udelay(100*1000); + return ret; } diff --git a/src/drivers/px4io/uploader.h b/src/drivers/px4io/uploader.h index 22387a3e2..55f63eef9 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(); |