diff options
Diffstat (limited to 'src/drivers/px4io')
-rw-r--r-- | src/drivers/px4io/px4io.cpp | 354 | ||||
-rw-r--r-- | src/drivers/px4io/px4io_uploader.cpp | 5 |
2 files changed, 262 insertions, 97 deletions
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index ef6ca04e9..4e9daf910 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -54,6 +54,7 @@ #include <unistd.h> #include <fcntl.h> #include <math.h> +#include <crc32.h> #include <arch/board/board.h> @@ -72,7 +73,6 @@ #include <systemlib/param/param.h> #include <uORB/topics/actuator_controls.h> -#include <uORB/topics/actuator_controls_effective.h> #include <uORB/topics/actuator_outputs.h> #include <uORB/topics/actuator_armed.h> #include <uORB/topics/safety.h> @@ -95,6 +95,8 @@ extern device::Device *PX4IO_serial_interface() weak_function; #define PX4IO_SET_DEBUG _IOC(0xff00, 0) #define PX4IO_INAIR_RESTART_ENABLE _IOC(0xff00, 1) +#define PX4IO_REBOOT_BOOTLOADER _IOC(0xff00, 2) +#define PX4IO_CHECK_CRC _IOC(0xff00, 3) #define UPDATE_INTERVAL_MIN 2 // 2 ms -> 500 Hz #define ORB_CHECK_INTERVAL 200000 // 200 ms -> 5 Hz @@ -226,30 +228,36 @@ private: device::Device *_interface; // XXX - unsigned _hardware; ///< Hardware revision - unsigned _max_actuators; ///<Maximum # of actuators supported by PX4IO - unsigned _max_controls; ///<Maximum # of controls supported by PX4IO - unsigned _max_rc_input; ///<Maximum receiver channels supported by PX4IO - unsigned _max_relays; ///<Maximum relays supported by PX4IO - unsigned _max_transfer; ///<Maximum number of I2C transfers supported by PX4IO + unsigned _hardware; ///< Hardware revision + unsigned _max_actuators; ///< Maximum # of actuators supported by PX4IO + unsigned _max_controls; ///< Maximum # of controls supported by PX4IO + unsigned _max_rc_input; ///< Maximum receiver channels supported by PX4IO + unsigned _max_relays; ///< Maximum relays supported by PX4IO + unsigned _max_transfer; ///< Maximum number of I2C transfers supported by PX4IO 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 - volatile int _task; ///<worker task id - volatile bool _task_should_exit; ///<worker terminate flag + volatile int _task; ///< worker task id + volatile bool _task_should_exit; ///< worker terminate flag - int _mavlink_fd; ///<mavlink file descriptor. This is opened by class instantiation and Doesn't appear to be usable in main thread. - int _thread_mavlink_fd; ///<mavlink file descriptor for thread. + int _mavlink_fd; ///< mavlink file descriptor. This is opened by class instantiation and Doesn't appear to be usable in main thread. + int _thread_mavlink_fd; ///< mavlink file descriptor for thread. - perf_counter_t _perf_update; ///<local performance counter + perf_counter_t _perf_update; ///<local performance counter for status updates + perf_counter_t _perf_write; ///<local performance counter for PWM control writes + perf_counter_t _perf_chan_count; ///<local performance counter for channel number changes /* cached IO state */ - uint16_t _status; ///<Various IO status flags - uint16_t _alarms; ///<Various IO alarms + uint16_t _status; ///< Various IO status flags + uint16_t _alarms; ///< Various IO alarms /* subscribed topics */ - int _t_actuators; ///< actuator controls topic + int _t_actuator_controls_0; ///< actuator controls group 0 topic + int _t_actuator_controls_1; ///< actuator controls group 1 topic + int _t_actuator_controls_2; ///< actuator controls group 2 topic + int _t_actuator_controls_3; ///< actuator controls group 3 topic int _t_actuator_armed; ///< system armed control topic int _t_vehicle_control_mode;///< vehicle control mode topic int _t_param; ///< parameter update topic @@ -257,24 +265,22 @@ private: /* advertised topics */ orb_advert_t _to_input_rc; ///< rc inputs from io - orb_advert_t _to_actuators_effective; ///< effective actuator controls topic orb_advert_t _to_outputs; ///< mixed servo outputs topic orb_advert_t _to_battery; ///< battery status / voltage orb_advert_t _to_servorail; ///< servorail status orb_advert_t _to_safety; ///< status of safety actuator_outputs_s _outputs; ///<mixed outputs - actuator_controls_effective_s _controls_effective; ///<effective controls - bool _primary_pwm_device; ///<true if we are the default PWM output + bool _primary_pwm_device; ///< true if we are the default PWM output - float _battery_amp_per_volt; ///<current sensor amps/volt - float _battery_amp_bias; ///<current sensor bias - float _battery_mamphour_total;///<amp hours consumed so far - uint64_t _battery_last_timestamp;///<last amp hour calculation timestamp + float _battery_amp_per_volt; ///< current sensor amps/volt + float _battery_amp_bias; ///< current sensor bias + float _battery_mamphour_total;///< amp hours consumed so far + uint64_t _battery_last_timestamp;///< last amp hour calculation timestamp #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 - bool _dsm_vcc_ctl; ///<true if relay 1 controls DSM satellite RX power + bool _dsm_vcc_ctl; ///< true if relay 1 controls DSM satellite RX power #endif /** @@ -288,9 +294,14 @@ private: void task_main(); /** - * Send controls to IO + * Send controls for one group to IO */ - int io_set_control_state(); + int io_set_control_state(unsigned group); + + /** + * Send all controls to IO + */ + int io_set_control_groups(); /** * Update IO's arming-related state @@ -328,11 +339,6 @@ private: int io_publish_raw_rc(); /** - * Fetch and publish the mixed control values. - */ - int io_publish_mixed_controls(); - - /** * Fetch and publish the PWM servo outputs. */ int io_publish_pwm_outputs(); @@ -459,20 +465,25 @@ PX4IO::PX4IO(device::Device *interface) : _max_transfer(16), /* sensible default */ _update_interval(0), _rc_handling_disabled(false), + _rc_chan_count(0), _task(-1), _task_should_exit(false), _mavlink_fd(-1), _thread_mavlink_fd(-1), - _perf_update(perf_alloc(PC_ELAPSED, "px4io update")), + _perf_update(perf_alloc(PC_ELAPSED, "io update")), + _perf_write(perf_alloc(PC_ELAPSED, "io write")), + _perf_chan_count(perf_alloc(PC_COUNT, "io rc #")), _status(0), _alarms(0), - _t_actuators(-1), + _t_actuator_controls_0(-1), + _t_actuator_controls_1(-1), + _t_actuator_controls_2(-1), + _t_actuator_controls_3(-1), _t_actuator_armed(-1), _t_vehicle_control_mode(-1), _t_param(-1), _t_vehicle_command(-1), _to_input_rc(0), - _to_actuators_effective(0), _to_outputs(0), _to_battery(0), _to_servorail(0), @@ -769,15 +780,20 @@ PX4IO::task_main() * Subscribe to the appropriate PWM output topic based on whether we are the * primary PWM output or not. */ - _t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : - ORB_ID(actuator_controls_1)); - orb_set_interval(_t_actuators, 20); /* default to 50Hz */ + _t_actuator_controls_0 = orb_subscribe(ORB_ID(actuator_controls_0)); + orb_set_interval(_t_actuator_controls_0, 20); /* default to 50Hz */ + _t_actuator_controls_1 = orb_subscribe(ORB_ID(actuator_controls_1)); + orb_set_interval(_t_actuator_controls_1, 33); /* default to 30Hz */ + _t_actuator_controls_2 = orb_subscribe(ORB_ID(actuator_controls_2)); + orb_set_interval(_t_actuator_controls_2, 33); /* default to 30Hz */ + _t_actuator_controls_3 = orb_subscribe(ORB_ID(actuator_controls_3)); + orb_set_interval(_t_actuator_controls_3, 33); /* default to 30Hz */ _t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed)); _t_vehicle_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode)); _t_param = orb_subscribe(ORB_ID(parameter_update)); _t_vehicle_command = orb_subscribe(ORB_ID(vehicle_command)); - if ((_t_actuators < 0) || + if ((_t_actuator_controls_0 < 0) || (_t_actuator_armed < 0) || (_t_vehicle_control_mode < 0) || (_t_param < 0) || @@ -788,7 +804,7 @@ PX4IO::task_main() /* poll descriptor */ pollfd fds[1]; - fds[0].fd = _t_actuators; + fds[0].fd = _t_actuator_controls_0; fds[0].events = POLLIN; log("ready"); @@ -807,7 +823,11 @@ PX4IO::task_main() if (_update_interval > 100) _update_interval = 100; - orb_set_interval(_t_actuators, _update_interval); + orb_set_interval(_t_actuator_controls_0, _update_interval); + /* + * NOT changing the rate of groups 1-3 here, because only attitude + * really needs to run fast. + */ _update_interval = 0; } @@ -827,7 +847,10 @@ PX4IO::task_main() /* if we have new control data from the ORB, handle it */ if (fds[0].revents & POLLIN) { - io_set_control_state(); + + /* we're not nice to the lower-priority control groups and only check them + when the primary group updated (which is now). */ + io_set_control_groups(); } if (now >= poll_last + IO_POLL_INTERVAL) { @@ -840,8 +863,7 @@ PX4IO::task_main() /* get raw R/C input from IO */ io_publish_raw_rc(); - /* fetch mixed servo controls and PWM outputs from IO */ - io_publish_mixed_controls(); + /* fetch PWM outputs from IO */ io_publish_pwm_outputs(); } @@ -871,7 +893,7 @@ PX4IO::task_main() orb_copy(ORB_ID(vehicle_command), _t_vehicle_command, &cmd); // Check for a DSM pairing command - if ((cmd.command == VEHICLE_CMD_START_RX_PAIR) && (cmd.param1 == 0.0f)) { + if (((int)cmd.command == VEHICLE_CMD_START_RX_PAIR) && ((int)cmd.param1 == 0)) { dsm_bind_ioctl((int)cmd.param2); } } @@ -901,7 +923,23 @@ PX4IO::task_main() /* re-upload RC input config as it may have changed */ io_set_rc_config(); + + /* re-set the battery scaling */ + int32_t voltage_scaling_val; + param_t voltage_scaling_param; + + /* set battery voltage scaling */ + param_get(voltage_scaling_param = param_find("BAT_V_SCALE_IO"), &voltage_scaling_val); + + /* send scaling voltage to IO */ + uint16_t scaling = voltage_scaling_val; + int pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE, &scaling, 1); + + if (pret != OK) { + log("voltage scaling upload failed"); + } } + } perf_end(_perf_update); @@ -922,20 +960,74 @@ out: } int -PX4IO::io_set_control_state() +PX4IO::io_set_control_groups() +{ + bool attitude_ok = io_set_control_state(0); + + /* send auxiliary control groups */ + (void)io_set_control_state(1); + (void)io_set_control_state(2); + (void)io_set_control_state(3); + + return attitude_ok; +} + +int +PX4IO::io_set_control_state(unsigned group) { actuator_controls_s controls; ///< actuator outputs uint16_t regs[_max_actuators]; /* get controls */ - orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : - ORB_ID(actuator_controls_1), _t_actuators, &controls); + bool changed; + + switch (group) { + case 0: + { + orb_check(_t_actuator_controls_0, &changed); + + if (changed) { + orb_copy(ORB_ID(actuator_controls_0), _t_actuator_controls_0, &controls); + } + } + break; + case 1: + { + orb_check(_t_actuator_controls_1, &changed); + + if (changed) { + orb_copy(ORB_ID(actuator_controls_1), _t_actuator_controls_1, &controls); + } + } + break; + case 2: + { + orb_check(_t_actuator_controls_2, &changed); + + if (changed) { + orb_copy(ORB_ID(actuator_controls_2), _t_actuator_controls_2, &controls); + } + } + break; + case 3: + { + orb_check(_t_actuator_controls_3, &changed); + + if (changed) { + orb_copy(ORB_ID(actuator_controls_3), _t_actuator_controls_3, &controls); + } + } + break; + } + + if (!changed) + return -1; for (unsigned i = 0; i < _max_controls; i++) regs[i] = FLOAT_TO_REG(controls.control[i]); /* copy values to registers in IO */ - return io_reg_set(PX4IO_PAGE_CONTROLS, 0, regs, _max_controls); + return io_reg_set(PX4IO_PAGE_CONTROLS, group * PX4IO_PROTOCOL_MAX_CONTROL_COUNT, regs, _max_controls); } @@ -1031,7 +1123,12 @@ PX4IO::io_set_rc_config() if ((ichan >= 0) && (ichan < (int)_max_rc_input)) input_map[ichan - 1] = 3; - ichan = 4; + param_get(param_find("RC_MAP_MODE_SW"), &ichan); + + if ((ichan >= 0) && (ichan < (int)_max_rc_input)) + input_map[ichan - 1] = 4; + + ichan = 5; for (unsigned i = 0; i < _max_rc_input; i++) if (input_map[i] == -1) @@ -1309,6 +1406,11 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) */ channel_count = regs[0]; + if (channel_count != _rc_chan_count) + perf_count(_perf_chan_count); + + _rc_chan_count = channel_count; + 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); @@ -1364,50 +1466,6 @@ PX4IO::io_publish_raw_rc() } int -PX4IO::io_publish_mixed_controls() -{ - /* if no FMU comms(!) just don't publish */ - if (!(_status & PX4IO_P_STATUS_FLAGS_FMU_OK)) - return OK; - - /* if not taking raw PPM from us, must be mixing */ - if (_status & PX4IO_P_STATUS_FLAGS_RAW_PWM) - return OK; - - /* data we are going to fetch */ - actuator_controls_effective_s controls_effective; - controls_effective.timestamp = hrt_absolute_time(); - - /* get actuator controls from IO */ - uint16_t act[_max_actuators]; - int ret = io_reg_get(PX4IO_PAGE_ACTUATORS, 0, act, _max_actuators); - - if (ret != OK) - return ret; - - /* convert from register format to float */ - for (unsigned i = 0; i < _max_actuators; i++) - controls_effective.control_effective[i] = REG_TO_FLOAT(act[i]); - - /* laxily advertise on first publication */ - if (_to_actuators_effective == 0) { - _to_actuators_effective = - orb_advertise((_primary_pwm_device ? - ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : - ORB_ID(actuator_controls_effective_1)), - &controls_effective); - - } else { - orb_publish((_primary_pwm_device ? - ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : - ORB_ID(actuator_controls_effective_1)), - _to_actuators_effective, &controls_effective); - } - - return OK; -} - -int PX4IO::io_publish_pwm_outputs() { /* if no FMU comms(!) just don't publish */ @@ -1659,11 +1717,13 @@ void PX4IO::print_status() { /* basic configuration */ - printf("protocol %u hardware %u bootloader %u buffer %uB\n", + printf("protocol %u hardware %u bootloader %u buffer %uB crc 0x%04x%04x\n", io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION), io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_HARDWARE_VERSION), io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_BOOTLOADER_VERSION), - io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER)); + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_CRC), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_CRC+1)); printf("%u controls %u actuators %u R/C inputs %u analog inputs %u relays\n", io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT), io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT), @@ -2146,6 +2206,29 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG, arg); break; + case PX4IO_REBOOT_BOOTLOADER: + if (system_status() & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) + return -EINVAL; + + /* reboot into bootloader - arg must be PX4IO_REBOOT_BL_MAGIC */ + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_REBOOT_BL, arg); + // we don't expect a reply from this operation + ret = OK; + break; + + case PX4IO_CHECK_CRC: { + /* check IO firmware CRC against passed value */ + uint32_t io_crc = 0; + ret = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_CRC, (uint16_t *)&io_crc, 2); + if (ret != OK) + return ret; + if (io_crc != arg) { + debug("crc mismatch 0x%08x 0x%08x", (unsigned)io_crc, arg); + return -EINVAL; + } + break; + } + case PX4IO_INAIR_RESTART_ENABLE: /* set/clear the 'in-air restart' bit */ @@ -2176,7 +2259,10 @@ PX4IO::write(file * /*filp*/, const char *buffer, size_t len) count = _max_actuators; if (count > 0) { + + perf_begin(_perf_write); int ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, (uint16_t *)buffer, count); + perf_end(_perf_write); if (ret != OK) return ret; @@ -2255,7 +2341,7 @@ void start(int argc, char *argv[]) { if (g_dev != nullptr) - errx(1, "already loaded"); + errx(0, "already loaded"); /* allocate the interface */ device::Device *interface = get_interface(); @@ -2588,6 +2674,39 @@ px4io_main(int argc, char *argv[]) if_test((argc > 2) ? strtol(argv[2], NULL, 0) : 0); } + if (!strcmp(argv[1], "forceupdate")) { + /* + force update of the IO firmware without requiring + the user to hold the safety switch down + */ + if (argc <= 3) { + warnx("usage: px4io forceupdate MAGIC filename"); + exit(1); + } + 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); + } + + // tear down the px4io instance + delete g_dev; + } + + // upload the specified firmware + const char *fn[2]; + fn[0] = argv[3]; + fn[1] = nullptr; + PX4IO_Uploader *up = new PX4IO_Uploader; + up->upload(&fn[0]); + delete up; + exit(0); + } + /* commands below here require a started driver */ if (g_dev == nullptr) @@ -2673,6 +2792,49 @@ px4io_main(int argc, char *argv[]) exit(0); } + if (!strcmp(argv[1], "checkcrc")) { + /* + check IO CRC against CRC of a file + */ + if (argc <= 2) { + printf("usage: px4io checkcrc filename\n"); + exit(1); + } + if (g_dev == nullptr) { + printf("px4io is not started\n"); + exit(1); + } + int fd = open(argv[2], O_RDONLY); + if (fd == -1) { + printf("open of %s failed - %d\n", argv[2], errno); + exit(1); + } + const uint32_t app_size_max = 0xf000; + uint32_t fw_crc = 0; + uint32_t nbytes = 0; + while (true) { + uint8_t buf[16]; + int n = read(fd, buf, sizeof(buf)); + if (n <= 0) break; + fw_crc = crc32part(buf, n, fw_crc); + nbytes += n; + } + close(fd); + while (nbytes < app_size_max) { + uint8_t b = 0xff; + fw_crc = crc32part(&b, 1, fw_crc); + nbytes++; + } + + int ret = g_dev->ioctl(nullptr, PX4IO_CHECK_CRC, fw_crc); + if (ret != OK) { + printf("check CRC failed - %d\n", ret); + exit(1); + } + printf("CRCs match\n"); + exit(0); + } + if (!strcmp(argv[1], "rx_dsm") || !strcmp(argv[1], "rx_dsm_10bit") || !strcmp(argv[1], "rx_dsm_11bit") || @@ -2690,5 +2852,5 @@ px4io_main(int argc, char *argv[]) bind(argc, argv); out: - errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'bind' or 'update'"); + errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'bind', 'checkcrc', 'forceupdate' or 'update'"); } diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index d01dedb0d..41f93a8ee 100644 --- a/src/drivers/px4io/px4io_uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -274,7 +274,10 @@ PX4IO_Uploader::drain() int ret; do { - ret = recv(c, 1000); + // the small recv timeout here is to allow for fast + // drain when rebooting the io board for a forced + // update of the fw without using the safety switch + ret = recv(c, 40); #ifdef UDEBUG if (ret == OK) { |