/**************************************************************************** * * Copyright (C) 2012,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 px4io.cpp * Driver for the PX4IO board. * * PX4IO is connected via I2C. */ #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include "uploader.h" #include class PX4IO : public device::I2C { public: PX4IO(); virtual ~PX4IO(); virtual int init(); virtual int ioctl(file *filp, int cmd, unsigned long arg); virtual ssize_t write(file *filp, const char *buffer, size_t len); void print_status(); private: // XXX unsigned _max_actuators; unsigned _max_controls; unsigned _max_rc_input; unsigned _max_relays; unsigned _max_transfer; unsigned _update_interval; ///< subscription interval limiting send rate volatile int _task; ///< worker task volatile bool _task_should_exit; int _mavlink_fd; perf_counter_t _perf_update; /* cached IO state */ uint16_t _status; uint16_t _alarms; /* subscribed topics */ int _t_actuators; ///< actuator controls topic int _t_armed; ///< system armed control topic int _t_vstatus; ///< system / vehicle status int _t_param; ///< parameter update topic /* 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 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 /** * Trampoline to the worker task */ static void task_main_trampoline(int argc, char *argv[]); /** * worker task */ void task_main(); /** * Send controls to IO */ int io_set_control_state(); /** * Update IO's arming-related state */ int io_set_arming_state(); /** * Push RC channel configuration to IO. */ int io_set_rc_config(); /** * Fetch status and alarms from IO * * Also publishes battery voltage/current. */ int io_get_status(); /** * Fetch RC inputs from IO. * * @param input_rc Input structure to populate. * @return OK if data was returned. */ int io_get_raw_rc_input(rc_input_values &input_rc); /** * Fetch and publish raw RC input data. */ 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(); /** * write register(s) * * @param page Register page to write to. * @param offset Register offset to start writing at. * @param values Pointer to array of values to write. * @param num_values The number of values to write. * @return Zero if all values were successfully written. */ int io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values); /** * write a register * * @param page Register page to write to. * @param offset Register offset to write to. * @param value Value to write. * @return Zero if the value was written successfully. */ int io_reg_set(uint8_t page, uint8_t offset, const uint16_t value); /** * read register(s) * * @param page Register page to read from. * @param offset Register offset to start reading from. * @param values Pointer to array where values should be stored. * @param num_values The number of values to read. * @return Zero if all values were successfully read. */ int io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values); /** * read a register * * @param page Register page to read from. * @param offset Register offset to start reading from. * @return Register value that was read, or _io_reg_get_error on error. */ uint32_t io_reg_get(uint8_t page, uint8_t offset); static const uint32_t _io_reg_get_error = 0x80000000; /** * modify a register * * @param page Register page to modify. * @param offset Register offset to modify. * @param clearbits Bits to clear in the register. * @param setbits Bits to set in the register. */ int io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t setbits); /** * Send mixer definition text to IO */ int mixer_send(const char *buf, unsigned buflen); /** * Handle a status update from IO. * * Publish IO status information if necessary. * * @param status The status register */ int io_handle_status(uint16_t status); /** * Handle an alarm update from IO. * * Publish IO alarm information if necessary. * * @param alarm The status register */ int io_handle_alarms(uint16_t alarms); }; namespace { PX4IO *g_dev; } PX4IO::PX4IO() : I2C("px4io", "/dev/px4io", PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO, 320000), _max_actuators(0), _max_controls(0), _max_rc_input(0), _max_relays(0), _max_transfer(16), /* sensible default */ _update_interval(0), _task(-1), _task_should_exit(false), _mavlink_fd(-1), _perf_update(perf_alloc(PC_ELAPSED, "px4io update")), _status(0), _alarms(0), _t_actuators(-1), _t_armed(-1), _t_vstatus(-1), _t_param(-1), _to_input_rc(0), _to_actuators_effective(0), _to_outputs(0), _to_battery(0), _primary_pwm_device(false) { /* we need this potentially before it could be set in task_main */ g_dev = this; /* open MAVLink text channel */ _mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0); _debug_enabled = true; } PX4IO::~PX4IO() { /* tell the task we want it to go away */ _task_should_exit = true; /* spin waiting for the task to stop */ for (unsigned i = 0; (i < 10) && (_task != -1); i++) { /* give it another 100ms */ usleep(100000); } /* well, kill it anyway, though this will probably crash */ if (_task != -1) task_delete(_task); g_dev = nullptr; } int PX4IO::init() { int ret; ASSERT(_task == -1); /* do regular cdev init */ ret = I2C::init(); if (ret != OK) return ret; /* * Enable a couple of retries for operations to IO. * * Register read/write operations are intentionally idempotent * so this is safe as designed. */ _retries = 2; /* get some parameters */ _max_actuators = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT); _max_controls = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT); _max_relays = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RELAY_COUNT); _max_transfer = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER) - 2; _max_rc_input = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT); if ((_max_actuators < 1) || (_max_actuators > 255) || (_max_relays < 1) || (_max_relays > 255) || (_max_transfer < 16) || (_max_transfer > 255) || (_max_rc_input < 1) || (_max_rc_input > 255)) { log("failed getting parameters from PX4IO"); mavlink_log_emergency(_mavlink_fd, "[IO] param read fail, abort."); return -1; } if (_max_rc_input > RC_INPUT_MAX_CHANNELS) _max_rc_input = RC_INPUT_MAX_CHANNELS; /* * Check for IO flight state - if FMU was flagged to be in * armed state, FMU is recovering from an in-air reset. * Read back status and request the commander to arm * in this case. */ uint16_t 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; /* * in-air restart is only tried if the IO board reports it is * already armed, and has been configured for in-air restart */ if ((reg & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) && (reg & PX4IO_P_SETUP_ARMING_ARM_OK)) { mavlink_log_emergency(_mavlink_fd, "[IO] RECOVERING FROM FMU IN-AIR RESTART"); /* WARNING: COMMANDER app/vehicle status must be initialized. * If this fails (or the app is not started), worst-case IO * remains untouched (so manual override is still available). */ int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); /* fill with initial values, clear updated flag */ vehicle_status_s status; uint64_t try_start_time = hrt_absolute_time(); bool updated = false; /* keep checking for an update, ensure we got a recent state, not something that was published a long time ago. */ do { orb_check(vstatus_sub, &updated); if (updated) { /* got data, copy and exit loop */ orb_copy(ORB_ID(vehicle_status), vstatus_sub, &status); break; } /* wait 10 ms */ usleep(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; } } while (true); /* send command to arm system via command API */ vehicle_command_s cmd; /* request arming */ cmd.param1 = 1.0f; cmd.param2 = 0; cmd.param3 = 0; cmd.param4 = 0; cmd.param5 = 0; cmd.param6 = 0; cmd.param7 = 0; cmd.command = VEHICLE_CMD_COMPONENT_ARM_DISARM; cmd.target_system = status.system_id; cmd.target_component = status.component_id; cmd.source_system = status.system_id; cmd.source_component = status.component_id; /* ask to confirm command */ cmd.confirmation = 1; /* send command once */ (void)orb_advertise(ORB_ID(vehicle_command), &cmd); /* spin here until IO's state has propagated into the system */ do { orb_check(vstatus_sub, &updated); if (updated) { orb_copy(ORB_ID(vehicle_status), vstatus_sub, &status); } /* wait 10 ms */ usleep(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; } /* keep waiting for state change for 10 s */ } while (!status.flag_system_armed); /* regular boot, no in-air restart, init IO */ } else { /* dis-arm IO before touching anything */ io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_ARM_OK | PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK, 0); /* publish RC config to IO */ ret = io_set_rc_config(); if (ret != OK) { log("failed to update RC input config"); mavlink_log_info(_mavlink_fd, "[IO] RC config upload fail"); return ret; } } /* try to claim the generic PWM output device node as well - it's OK if we fail at this */ ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this); if (ret == OK) { log("default PWM output device"); _primary_pwm_device = true; } /* start the IO interface task */ _task = task_create("px4io", SCHED_PRIORITY_ACTUATOR_OUTPUTS, 4096, (main_t)&PX4IO::task_main_trampoline, nullptr); if (_task < 0) { debug("task start failed: %d", errno); return -errno; } mavlink_log_info(_mavlink_fd, "[IO] init ok"); return OK; } void PX4IO::task_main_trampoline(int argc, char *argv[]) { g_dev->task_main(); } void PX4IO::task_main() { hrt_abstime last_poll_time = 0; log("starting"); /* * 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_armed = orb_subscribe(ORB_ID(actuator_armed)); orb_set_interval(_t_armed, 200); /* 5Hz update rate */ _t_vstatus = orb_subscribe(ORB_ID(vehicle_status)); orb_set_interval(_t_vstatus, 200); /* 5Hz update rate max. */ _t_param = orb_subscribe(ORB_ID(parameter_update)); orb_set_interval(_t_param, 500); /* 2Hz update rate max. */ /* poll descriptor */ pollfd fds[4]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; fds[1].fd = _t_armed; fds[1].events = POLLIN; fds[2].fd = _t_vstatus; fds[2].events = POLLIN; fds[3].fd = _t_param; fds[3].events = POLLIN; debug("ready"); /* lock against the ioctl handler */ lock(); /* loop talking to IO */ while (!_task_should_exit) { /* adjust update interval */ if (_update_interval != 0) { if (_update_interval < 5) _update_interval = 5; if (_update_interval > 100) _update_interval = 100; orb_set_interval(_t_actuators, _update_interval); _update_interval = 0; } /* sleep waiting for topic updates, but no more than 20ms */ unlock(); int ret = ::poll(&fds[0], sizeof(fds) / sizeof(fds[0]), 20); lock(); /* this would be bad... */ if (ret < 0) { log("poll error %d", errno); continue; } perf_begin(_perf_update); hrt_abstime now = hrt_absolute_time(); /* if we have new control data from the ORB, handle it */ if (fds[0].revents & POLLIN) io_set_control_state(); /* if we have an arming state update, handle it */ if ((fds[1].revents & POLLIN) || (fds[2].revents & POLLIN)) io_set_arming_state(); /* * If it's time for another tick of the polling status machine, * try it now. */ if ((now - last_poll_time) >= 20000) { /* * Pull status and alarms from IO. */ io_get_status(); /* * Get raw R/C input from IO. */ io_publish_raw_rc(); /* * Fetch mixed servo controls and PWM outputs from IO. * * XXX We could do this at a reduced rate in many/most cases. */ io_publish_mixed_controls(); io_publish_pwm_outputs(); /* * If parameters have changed, re-send RC mappings to IO * * XXX this may be a bit spammy */ if (fds[3].revents & POLLIN) { parameter_update_s pupdate; /* copy to reset the notification */ orb_copy(ORB_ID(parameter_update), _t_param, &pupdate); /* re-upload RC input config as it may have changed */ io_set_rc_config(); } } perf_end(_perf_update); } unlock(); debug("exiting"); /* clean up the alternate device node */ if (_primary_pwm_device) unregister_driver(PWM_OUTPUT_DEVICE_PATH); /* tell the dtor that we are exiting */ _task = -1; _exit(0); } int PX4IO::io_set_control_state() { 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); 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); } int PX4IO::io_set_arming_state() { actuator_armed_s armed; ///< system armed state vehicle_status_s vstatus; ///< overall system state orb_copy(ORB_ID(actuator_armed), _t_armed, &armed); orb_copy(ORB_ID(vehicle_status), _t_vstatus, &vstatus); uint16_t set = 0; uint16_t clear = 0; if (armed.armed) { set |= PX4IO_P_SETUP_ARMING_ARM_OK; } else { clear |= PX4IO_P_SETUP_ARMING_ARM_OK; } if (vstatus.flag_vector_flight_mode_ok) { set |= PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK; } else { clear |= PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK; } if (vstatus.flag_external_manual_override_ok) { set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK; } else { clear |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK; } return io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, clear, set); } int PX4IO::io_set_rc_config() { unsigned offset = 0; int input_map[_max_rc_input]; int32_t ichan; int ret = OK; /* * Generate the input channel -> control channel mapping table; * assign RC_MAP_ROLL/PITCH/YAW/THROTTLE to the canonical * controls. */ for (unsigned i = 0; i < _max_rc_input; i++) input_map[i] = -1; /* * NOTE: The indices for mapped channels are 1-based * for compatibility reasons with existing * autopilots / GCS'. */ param_get(param_find("RC_MAP_ROLL"), &ichan); if ((ichan >= 0) && (ichan < (int)_max_rc_input)) input_map[ichan - 1] = 0; param_get(param_find("RC_MAP_PITCH"), &ichan); if ((ichan >= 0) && (ichan < (int)_max_rc_input)) input_map[ichan - 1] = 1; param_get(param_find("RC_MAP_YAW"), &ichan); if ((ichan >= 0) && (ichan < (int)_max_rc_input)) input_map[ichan - 1] = 2; param_get(param_find("RC_MAP_THROTTLE"), &ichan); if ((ichan >= 0) && (ichan < (int)_max_rc_input)) input_map[ichan - 1] = 3; ichan = 4; for (unsigned i = 0; i < _max_rc_input; i++) if (input_map[i] == -1) input_map[i] = ichan++; /* * Iterate all possible RC inputs. */ for (unsigned i = 0; i < _max_rc_input; i++) { uint16_t regs[PX4IO_P_RC_CONFIG_STRIDE]; char pname[16]; float fval; /* * RC params are floats, but do only * contain integer values. Do not scale * or cast them, let the auto-typeconversion * do its job here. * Channels: 500 - 2500 * Inverted flag: -1 (inverted) or 1 (normal) */ sprintf(pname, "RC%d_MIN", i + 1); param_get(param_find(pname), &fval); regs[PX4IO_P_RC_CONFIG_MIN] = fval; sprintf(pname, "RC%d_TRIM", i + 1); param_get(param_find(pname), &fval); regs[PX4IO_P_RC_CONFIG_CENTER] = fval; sprintf(pname, "RC%d_MAX", i + 1); param_get(param_find(pname), &fval); regs[PX4IO_P_RC_CONFIG_MAX] = fval; sprintf(pname, "RC%d_DZ", i + 1); param_get(param_find(pname), &fval); regs[PX4IO_P_RC_CONFIG_DEADZONE] = fval; regs[PX4IO_P_RC_CONFIG_ASSIGNMENT] = input_map[i]; regs[PX4IO_P_RC_CONFIG_OPTIONS] = PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; sprintf(pname, "RC%d_REV", i + 1); param_get(param_find(pname), &fval); /* * This has been taken for the sake of compatibility * with APM's setup / mission planner: normal: 1, * inverted: -1 */ if (fval < 0) { regs[PX4IO_P_RC_CONFIG_OPTIONS] |= PX4IO_P_RC_CONFIG_OPTIONS_REVERSE; } /* send channel config to IO */ ret = io_reg_set(PX4IO_PAGE_RC_CONFIG, offset, regs, PX4IO_P_RC_CONFIG_STRIDE); if (ret != OK) { log("rc config upload failed"); break; } /* check the IO initialisation flag */ if (!(io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_INIT_OK)) { log("config for RC%d rejected by IO", i + 1); break; } offset += PX4IO_P_RC_CONFIG_STRIDE; } return ret; } int PX4IO::io_handle_status(uint16_t status) { int ret = 1; /** * WARNING: This section handles in-air resets. */ /* check for IO reset - force it back to armed if necessary */ 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 | 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; } int PX4IO::io_handle_alarms(uint16_t alarms) { /* XXX handle alarms */ /* set new alarms state */ _alarms = alarms; return 0; } int PX4IO::io_get_status() { uint16_t regs[4]; int ret; /* get STATUS_FLAGS, STATUS_ALARMS, STATUS_VBATT, STATUS_IBATT in that order */ ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, ®s[0], sizeof(regs) / sizeof(regs[0])); if (ret != OK) return ret; io_handle_status(regs[0]); io_handle_alarms(regs[1]); /* only publish if battery has a valid minimum voltage */ if (regs[2] > 3300) { battery_status_s battery_status; battery_status.timestamp = hrt_absolute_time(); /* voltage is scaled to mV */ battery_status.voltage_v = regs[2] / 1000.0f; /* current scaling should be to cA in order to avoid limiting at 65A */ battery_status.current_a = regs[3] / 100.f; /* this requires integration over time - not currently implemented */ battery_status.discharged_mah = -1.0f; /* lazily publish the battery voltage */ if (_to_battery > 0) { orb_publish(ORB_ID(battery_status), _to_battery, &battery_status); } else { _to_battery = orb_advertise(ORB_ID(battery_status), &battery_status); } } return ret; } int PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) { uint32_t channel_count; int ret = OK; /* we don't have the status bits, so input_source has to be set elsewhere */ input_rc.input_source = RC_INPUT_SOURCE_UNKNOWN; /* * XXX Because the channel count and channel data are fetched * separately, there is a risk of a race between the two * that could leave us with channel data and a count that * are out of sync. * Fixing this would require a guarantee of atomicity from * IO, and a single fetch for both count and channels. * * XXX Since IO has the input calibration info, we ought to be * able to get the pre-fixed-up controls directly. * * XXX can we do this more cheaply? If we knew we had DMA, it would * almost certainly be better to just get all the inputs... */ channel_count = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT); if (channel_count == _io_reg_get_error) return -EIO; if (channel_count > RC_INPUT_MAX_CHANNELS) channel_count = RC_INPUT_MAX_CHANNELS; input_rc.channel_count = channel_count; if (channel_count > 0) { ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE, input_rc.values, channel_count); if (ret == OK) input_rc.timestamp = hrt_absolute_time(); } return ret; } 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(); int ret = io_get_raw_rc_input(rc_val); if (ret != OK) return ret; /* sort out the source of the values */ if (_status & PX4IO_P_STATUS_FLAGS_RC_PPM) { rc_val.input_source = RC_INPUT_SOURCE_PX4IO_PPM; } else if (_status & PX4IO_P_STATUS_FLAGS_RC_DSM) { rc_val.input_source = RC_INPUT_SOURCE_PX4IO_SPEKTRUM; } else if (_status & PX4IO_P_STATUS_FLAGS_RC_SBUS) { rc_val.input_source = RC_INPUT_SOURCE_PX4IO_SBUS; } else { rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN; } /* lazily advertise on first publication */ if (_to_input_rc == 0) { _to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_val); } else { orb_publish(ORB_ID(input_rc), _to_input_rc, &rc_val); } return OK; } 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 */ if (!(_status & PX4IO_P_STATUS_FLAGS_FMU_OK)) return OK; /* data we are going to fetch */ actuator_outputs_s outputs; outputs.timestamp = hrt_absolute_time(); /* get servo values from IO */ uint16_t ctl[_max_actuators]; int ret = io_reg_get(PX4IO_PAGE_SERVOS, 0, ctl, _max_actuators); if (ret != OK) return ret; /* convert from register format to float */ for (unsigned i = 0; i < _max_actuators; i++) outputs.output[i] = ctl[i]; outputs.noutputs = _max_actuators; /* lazily advertise on first publication */ if (_to_outputs == 0) { _to_outputs = orb_advertise((_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1)), &outputs); } else { orb_publish((_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1)), _to_outputs, &outputs); } return OK; } int PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) { /* range check the transfer */ if (num_values > ((_max_transfer) / sizeof(*values))) { debug("io_reg_set: too many registers (%u, max %u)", num_values, _max_transfer / 2); return -EINVAL; } /* set up the transfer */ uint8_t addr[2] = { page, offset }; i2c_msg_s msgv[2]; msgv[0].flags = 0; msgv[0].buffer = addr; msgv[0].length = 2; msgv[1].flags = I2C_M_NORESTART; msgv[1].buffer = (uint8_t *)values; msgv[1].length = num_values * sizeof(*values); /* perform the transfer */ int ret = transfer(msgv, 2); if (ret != OK) debug("io_reg_set: error %d", ret); return ret; } int PX4IO::io_reg_set(uint8_t page, uint8_t offset, uint16_t value) { return io_reg_set(page, offset, &value, 1); } int PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values) { /* set up the transfer */ uint8_t addr[2] = { page, offset }; i2c_msg_s msgv[2]; msgv[0].flags = 0; msgv[0].buffer = addr; msgv[0].length = 2; msgv[1].flags = I2C_M_READ; msgv[1].buffer = (uint8_t *)values; msgv[1].length = num_values * sizeof(*values); /* perform the transfer */ int ret = transfer(msgv, 2); if (ret != OK) debug("io_reg_get: data error %d", ret); return ret; } uint32_t PX4IO::io_reg_get(uint8_t page, uint8_t offset) { uint16_t value; if (io_reg_get(page, offset, &value, 1)) return _io_reg_get_error; return value; } int PX4IO::io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t setbits) { int ret; uint16_t value; ret = io_reg_get(page, offset, &value, 1); if (ret) return ret; value &= ~clearbits; value |= setbits; return io_reg_set(page, offset, value); } int PX4IO::mixer_send(const char *buf, unsigned buflen) { uint8_t frame[_max_transfer]; px4io_mixdata *msg = (px4io_mixdata *)&frame[0]; unsigned max_len = _max_transfer - sizeof(px4io_mixdata); msg->f2i_mixer_magic = F2I_MIXER_MAGIC; msg->action = F2I_MIXER_ACTION_RESET; do { unsigned count = buflen; if (count > max_len) count = max_len; if (count > 0) { memcpy(&msg->text[0], buf, count); buf += count; buflen -= count; } /* * We have to send an even number of bytes. This * will only happen on the very last transfer of a * mixer, and we are guaranteed that there will be * space left to round up as _max_transfer will be * even. */ unsigned total_len = sizeof(px4io_mixdata) + count; if (total_len % 1) { msg->text[count] = '\0'; total_len++; } int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, total_len / 2); if (ret) { log("mixer send error %d", ret); return ret; } msg->action = F2I_MIXER_ACTION_APPEND; } while (buflen > 0); /* check for the mixer-OK flag */ if (io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_MIXER_OK) { debug("mixer upload OK"); mavlink_log_info(_mavlink_fd, "[IO] mixer upload ok"); return 0; } else { debug("mixer rejected by IO"); mavlink_log_info(_mavlink_fd, "[IO] mixer upload fail"); } /* load must have failed for some reason */ return -EINVAL; } void PX4IO::print_status() { /* basic configuration */ printf("protocol %u software %u bootloader %u buffer %uB\n", io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION), io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_SOFTWARE_VERSION), io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_BOOTLOADER_VERSION), io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER)); 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), io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT), io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ADC_INPUT_COUNT), io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RELAY_COUNT)); /* 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\n", flags, ((flags & PX4IO_P_STATUS_FLAGS_ARMED) ? " ARMED" : ""), ((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) ? " DSM" : ""), ((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_PPM" : ""), ((flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ? " MIXER_OK" : " MIXER_FAIL"), ((flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC) ? " ARM_SYNC" : " ARM_NO_SYNC"), ((flags & PX4IO_P_STATUS_FLAGS_INIT_OK) ? " INIT_OK" : " INIT_FAIL")); uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS); printf("alarms 0x%04x%s%s%s%s%s%s\n", alarms, ((alarms & PX4IO_P_STATUS_ALARMS_VBATT_LOW) ? " VBATT_LOW" : ""), ((alarms & PX4IO_P_STATUS_ALARMS_TEMPERATURE) ? " TEMPERATURE" : ""), ((alarms & PX4IO_P_STATUS_ALARMS_SERVO_CURRENT) ? " SERVO_CURRENT" : ""), ((alarms & PX4IO_P_STATUS_ALARMS_ACC_CURRENT) ? " ACC_CURRENT" : ""), ((alarms & PX4IO_P_STATUS_ALARMS_FMU_LOST) ? " FMU_LOST" : ""), ((alarms & PX4IO_P_STATUS_ALARMS_RC_LOST) ? " RC_LOST" : "")); printf("vbatt %u ibatt %u\n", io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VBATT), io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT)); printf("actuators"); for (unsigned i = 0; i < _max_actuators; i++) printf(" %u", io_reg_get(PX4IO_PAGE_ACTUATORS, i)); printf("\n"); printf("servos"); for (unsigned i = 0; i < _max_actuators; i++) printf(" %u", io_reg_get(PX4IO_PAGE_SERVOS, i)); printf("\n"); uint16_t raw_inputs = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT); printf("%d raw R/C inputs", raw_inputs); for (unsigned i = 0; i < raw_inputs; i++) printf(" %u", io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + i)); printf("\n"); uint16_t mapped_inputs = io_reg_get(PX4IO_PAGE_RC_INPUT, PX4IO_P_RC_VALID); printf("mapped R/C inputs 0x%04x", mapped_inputs); for (unsigned i = 0; i < _max_rc_input; i++) { if (mapped_inputs & (1 << i)) printf(" %u:%d", i, REG_TO_SIGNED(io_reg_get(PX4IO_PAGE_RC_INPUT, PX4IO_P_RC_BASE + i))); } printf("\n"); uint16_t adc_inputs = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ADC_INPUT_COUNT); printf("ADC inputs"); for (unsigned i = 0; i < adc_inputs; i++) printf(" %u", io_reg_get(PX4IO_PAGE_RAW_ADC_INPUT, i)); printf("\n"); /* setup and state */ printf("features 0x%04x\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES)); uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING); printf("arming 0x%04x%s%s%s%s\n", arming, ((arming & PX4IO_P_SETUP_ARMING_ARM_OK) ? " ARM_OK" : ""), ((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""), ((arming & PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK) ? " VECTOR_FLIGHT_OK" : ""), ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : "")); printf("rates 0x%04x lowrate %u highrate %u relays 0x%04x\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES), io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_LOWRATE), io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_HIGHRATE), io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS)); printf("vbatt scale %u ibatt scale %u ibatt bias %u\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE), io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_IBATT_SCALE), io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_IBATT_BIAS)); printf("debuglevel %u\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG)); printf("controls"); for (unsigned i = 0; i < _max_controls; i++) printf(" %u", io_reg_get(PX4IO_PAGE_CONTROLS, i)); printf("\n"); for (unsigned i = 0; i < _max_rc_input; i++) { unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i; uint16_t options = io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_OPTIONS); printf("input %u min %u center %u max %u deadzone %u assigned %u options 0x%04x%s%s\n", i, io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MIN), io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_CENTER), io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MAX), io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_DEADZONE), io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_ASSIGNMENT), options, ((options & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) ? " ENABLED" : ""), ((options & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) ? " REVERSED" : "")); } printf("failsafe"); for (unsigned i = 0; i < _max_actuators; i++) printf(" %u", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i)); printf("\n"); } int PX4IO::ioctl(file *filep, int cmd, unsigned long arg) { int ret = OK; /* regular ioctl? */ switch (cmd) { case PWM_SERVO_ARM: /* set the 'armed' bit */ ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_ARM_OK); break; case PWM_SERVO_DISARM: /* clear the 'armed' bit */ ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_ARM_OK, 0); break; case PWM_SERVO_INAIR_RESTART_ENABLE: /* set the 'in-air restart' bit */ ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK); break; case PWM_SERVO_INAIR_RESTART_DISABLE: /* unset the 'in-air restart' bit */ ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK, 0); break; case PWM_SERVO_SET_UPDATE_RATE: /* set the requested rate */ if ((arg >= 50) && (arg <= 400)) { ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_HIGHRATE, arg); } else { ret = -EINVAL; } break; case PWM_SERVO_GET_COUNT: *(unsigned *)arg = _max_actuators; break; case PWM_SERVO_SET_DEBUG: /* set the debug level */ ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG, arg); break; case PWM_SERVO_SET(0) ... PWM_SERVO_SET(PWM_OUTPUT_MAX_CHANNELS): { unsigned channel = cmd - PWM_SERVO_SET(0); if ((channel >= _max_actuators) || (arg < 900) || (arg > 2100)) { ret = -EINVAL; } else { /* send a direct PWM value */ ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, channel, arg); } break; } case PWM_SERVO_GET(0) ... PWM_SERVO_GET(PWM_OUTPUT_MAX_CHANNELS): { unsigned channel = cmd - PWM_SERVO_GET(0); if (channel >= _max_actuators) { ret = -EINVAL; } else { /* fetch a current PWM value */ uint32_t value = io_reg_get(PX4IO_PAGE_SERVOS, channel); if (value == _io_reg_get_error) { ret = -EIO; } else { *(servo_position_t *)arg = value; } } break; } case GPIO_RESET: ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0); break; case GPIO_SET: arg &= ((1 << _max_relays) - 1); ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, arg); break; case GPIO_CLEAR: arg &= ((1 << _max_relays) - 1); ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, arg, 0); break; case GPIO_GET: *(uint32_t *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS); if (*(uint32_t *)arg == _io_reg_get_error) ret = -EIO; break; case MIXERIOCGETOUTPUTCOUNT: *(unsigned *)arg = _max_actuators; break; case MIXERIOCRESET: ret = 0; /* load always resets */ break; case MIXERIOCLOADBUF: { const char *buf = (const char *)arg; ret = mixer_send(buf, strnlen(buf, 1024)); break; } case RC_INPUT_GET: { uint16_t status; rc_input_values *rc_val = (rc_input_values *)arg; ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, &status, 1); if (ret != OK) break; /* if no R/C input, don't try to fetch anything */ if (!(status & PX4IO_P_STATUS_FLAGS_RC_OK)) { ret = -ENOTCONN; break; } /* sort out the source of the values */ if (status & PX4IO_P_STATUS_FLAGS_RC_PPM) { rc_val->input_source = RC_INPUT_SOURCE_PX4IO_PPM; } else if (status & PX4IO_P_STATUS_FLAGS_RC_DSM) { rc_val->input_source = RC_INPUT_SOURCE_PX4IO_SPEKTRUM; } else if (status & PX4IO_P_STATUS_FLAGS_RC_SBUS) { rc_val->input_source = RC_INPUT_SOURCE_PX4IO_SBUS; } else { rc_val->input_source = RC_INPUT_SOURCE_UNKNOWN; } /* read raw R/C input values */ ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE, &(rc_val->values[0]), _max_rc_input); break; } default: /* not a recognized value */ ret = -ENOTTY; } return ret; } ssize_t PX4IO::write(file *filp, const char *buffer, size_t len) { unsigned count = len / 2; if (count > _max_actuators) count = _max_actuators; if (count > 0) { int ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, (uint16_t *)buffer, count); if (ret != OK) return ret; } return count * 2; } extern "C" __EXPORT int px4io_main(int argc, char *argv[]); namespace { void test(void) { int fd; unsigned servo_count = 0; unsigned pwm_value = 1000; int direction = 1; int ret; fd = open("/dev/px4io", O_WRONLY); if (fd < 0) err(1, "failed to open device"); if (ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count)) err(1, "failed to get servo count"); if (ioctl(fd, PWM_SERVO_ARM, 0)) err(1, "failed to arm servos"); for (;;) { /* sweep all servos between 1000..2000 */ servo_position_t servos[servo_count]; for (unsigned i = 0; i < servo_count; i++) servos[i] = pwm_value; ret = write(fd, servos, sizeof(servos)); if (ret != (int)sizeof(servos)) err(1, "error writing PWM servo data, wrote %u got %d", sizeof(servos), ret); if (direction > 0) { if (pwm_value < 2000) { pwm_value++; } else { direction = -1; } } else { if (pwm_value > 1000) { pwm_value--; } else { direction = 1; } } /* readback servo values */ for (unsigned i = 0; i < servo_count; i++) { servo_position_t value; if (ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&value)) err(1, "error reading PWM servo %d", i); if (value != servos[i]) errx(1, "servo %d readback error, got %u expected %u", i, value, servos[i]); } } } void monitor(void) { unsigned cancels = 3; printf("Hit three times to exit monitor mode\n"); for (;;) { pollfd fds[1]; fds[0].fd = 0; fds[0].events = POLLIN; poll(fds, 1, 500); if (fds[0].revents == POLLIN) { int c; read(0, &c, 1); if (cancels-- == 0) exit(0); } #warning implement this // if (g_dev != nullptr) // g_dev->dump_one = true; } } } int px4io_main(int argc, char *argv[]) { /* check for sufficient number of arguments */ if (argc < 2) goto out; if (!strcmp(argv[1], "start")) { if (g_dev != nullptr) errx(1, "already loaded"); /* create the driver - it will set g_dev */ (void)new PX4IO(); if (g_dev == nullptr) errx(1, "driver alloc failed"); if (OK != g_dev->init()) { delete g_dev; errx(1, "driver init failed"); } /* look for the optional pwm update rate for the supported modes */ if ((argc > 2) && (strcmp(argv[2], "-u") == 0 || strcmp(argv[2], "--update-rate") == 0)) { if (argc > 2 + 1) { #warning implement this } else { fprintf(stderr, "missing argument for pwm update rate (-u)\n"); return 1; } } exit(0); } if (!strcmp(argv[1], "recovery")) { if (g_dev != nullptr) { /* * Enable in-air restart support. * We can cheat and call the driver directly, as it * doesn't reference filp in ioctl() */ g_dev->ioctl(NULL, PWM_SERVO_INAIR_RESTART_ENABLE, 0); } else { errx(1, "not loaded"); } exit(0); } if (!strcmp(argv[1], "stop")) { if (g_dev != nullptr) { /* stop the driver */ delete g_dev; } else { errx(1, "not loaded"); } exit(0); } if (!strcmp(argv[1], "status")) { if (g_dev != nullptr) { printf("[px4io] loaded\n"); g_dev->print_status(); } else { printf("[px4io] not loaded\n"); } exit(0); } if (!strcmp(argv[1], "debug")) { if (argc <= 2) { printf("usage: px4io debug LEVEL\n"); exit(1); } if (g_dev == nullptr) { printf("px4io is not started\n"); exit(1); } uint8_t level = atoi(argv[2]); /* we can cheat and call the driver directly, as it * doesn't reference filp in ioctl() */ int ret = g_dev->ioctl(NULL, PWM_SERVO_SET_DEBUG, level); if (ret != 0) { printf("SET_DEBUG failed - %d\n", ret); exit(1); } printf("SET_DEBUG %u OK\n", (unsigned)level); exit(0); } /* note, stop not currently implemented */ if (!strcmp(argv[1], "update")) { if (g_dev != nullptr) { printf("[px4io] loaded, detaching first\n"); /* stop the driver */ delete g_dev; } PX4IO_Uploader *up; const char *fn[3]; /* work out what we're uploading... */ if (argc > 2) { fn[0] = argv[2]; fn[1] = nullptr; } else { fn[0] = "/fs/microsd/px4io.bin"; fn[1] = "/etc/px4io.bin"; fn[2] = nullptr; } up = new PX4IO_Uploader; int ret = up->upload(&fn[0]); delete up; switch (ret) { case OK: break; case -ENOENT: errx(1, "PX4IO firmware file not found"); case -EEXIST: case -EIO: errx(1, "error updating PX4IO - check that bootloader mode is enabled"); case -EINVAL: errx(1, "verify failed - retry the update"); case -ETIMEDOUT: errx(1, "timed out waiting for bootloader - power-cycle and try again"); default: errx(1, "unexpected error %d", ret); } return ret; } if (!strcmp(argv[1], "rx_dsm") || !strcmp(argv[1], "rx_dsm_10bit") || !strcmp(argv[1], "rx_dsm_11bit") || !strcmp(argv[1], "rx_sbus") || !strcmp(argv[1], "rx_ppm")) errx(0, "receiver type is automatically detected, option '%s' is deprecated", argv[1]); if (!strcmp(argv[1], "test")) test(); if (!strcmp(argv[1], "monitor")) monitor(); out: errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug' or 'update'"); }