diff options
Diffstat (limited to 'apps/drivers')
| -rw-r--r-- | apps/drivers/device/i2c.cpp | 20 | ||||
| -rw-r--r-- | apps/drivers/device/i2c.h | 10 | ||||
| -rw-r--r-- | apps/drivers/drv_pwm_output.h | 3 | ||||
| -rw-r--r-- | apps/drivers/px4io/px4io.cpp | 1000 |
4 files changed, 678 insertions, 355 deletions
diff --git a/apps/drivers/device/i2c.cpp b/apps/drivers/device/i2c.cpp index 474190d83..a5b2fab7e 100644 --- a/apps/drivers/device/i2c.cpp +++ b/apps/drivers/device/i2c.cpp @@ -166,4 +166,24 @@ I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re } +int +I2C::transfer(i2c_msg_s *msgv, unsigned msgs) +{ + for (unsigned i = 0; i < msgs; i++) + msgv[i].addr = _address; + + /* + * I2C architecture means there is an unavoidable race here + * if there are any devices on the bus with a different frequency + * preference. Really, this is pointless. + */ + I2C_SETFREQUENCY(_dev, _frequency); + int ret = I2C_TRANSFER(_dev, msgv, msgs); + + if (ret != OK) + up_i2creset(_dev); + + return ret; +} + } // namespace device
\ No newline at end of file diff --git a/apps/drivers/device/i2c.h b/apps/drivers/device/i2c.h index 4d630b8a8..66c34dd7c 100644 --- a/apps/drivers/device/i2c.h +++ b/apps/drivers/device/i2c.h @@ -101,6 +101,16 @@ protected: uint8_t *recv, unsigned recv_len); /** + * Perform a multi-part I2C transaction to the device. + * + * @param msgv An I2C message vector. + * @param msgs The number of entries in the message vector. + * @return OK if the transfer was successful, -errno + * otherwise. + */ + int transfer(i2c_msg_s *msgv, unsigned msgs); + + /** * Change the bus address. * * Most often useful during probe() when the driver is testing diff --git a/apps/drivers/drv_pwm_output.h b/apps/drivers/drv_pwm_output.h index c110cd5cb..fe08d3fe5 100644 --- a/apps/drivers/drv_pwm_output.h +++ b/apps/drivers/drv_pwm_output.h @@ -77,9 +77,6 @@ typedef uint16_t servo_position_t; * device. */ struct pwm_output_values { - /** desired servo update rate in Hz */ - uint32_t update_rate; - /** desired pulse widths for each of the supported channels */ servo_position_t values[PWM_OUTPUT_MAX_CHANNELS]; }; diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 2c2c236ca..c4d7ec489 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * 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 @@ -35,8 +35,7 @@ * @file px4io.cpp * Driver for the PX4IO board. * - * PX4IO is connected via serial (or possibly some other interface at a later - * point). + * PX4IO is connected via I2C. */ #include <nuttx/config.h> @@ -53,13 +52,13 @@ #include <stdio.h> #include <stdlib.h> #include <unistd.h> -#include <termios.h> #include <fcntl.h> #include <math.h> #include <arch/board/board.h> #include <drivers/device/device.h> +#include <drivers/device/i2c.h> #include <drivers/drv_rc_input.h> #include <drivers/drv_pwm_output.h> #include <drivers/drv_gpio.h> @@ -68,7 +67,6 @@ #include <systemlib/mixer/mixer.h> #include <systemlib/perf_counter.h> -#include <systemlib/hx_stream.h> #include <systemlib/err.h> #include <systemlib/systemlib.h> #include <systemlib/scheduling_priorities.h> @@ -80,12 +78,13 @@ #include <uORB/topics/vehicle_status.h> #include <uORB/topics/rc_channels.h> #include <uORB/topics/battery_status.h> +#include <uORB/topics/parameter_update.h> #include <px4io/protocol.h> #include "uploader.h" -class PX4IO : public device::CDev +class PX4IO : public device::I2C { public: PX4IO(); @@ -94,59 +93,47 @@ public: virtual int init(); virtual int ioctl(file *filp, int cmd, unsigned long arg); - - /** - * Set the PWM via serial update rate - * @warning this directly affects CPU load - */ - int set_pwm_rate(int hz); - - bool dump_one; + virtual ssize_t write(file *filp, const char *buffer, size_t len); private: // XXX - static const unsigned _max_actuators = PX4IO_CONTROL_CHANNELS; - unsigned _update_rate; ///< serial send rate in Hz + unsigned _max_actuators; + unsigned _max_rc_input; + unsigned _max_relays; + unsigned _max_transfer; - int _serial_fd; ///< serial interface to PX4IO - hx_stream_t _io_stream; ///< HX protocol stream + unsigned _update_interval; ///< subscription interval limiting send rate volatile int _task; ///< worker task volatile bool _task_should_exit; volatile bool _connected; ///< true once we have received a valid frame - int _t_actuators; ///< actuator output topic - actuator_controls_s _controls; ///< actuator outputs + perf_counter_t _perf_update; - orb_advert_t _t_actuators_effective; ///< effective actuator controls topic - actuator_controls_effective_s _controls_effective; ///< effective controls + /* cached IO state */ + uint16_t _status; + uint16_t _alarms; + /* subscribed topics */ + int _t_actuators; ///< actuator output topic int _t_armed; ///< system armed control topic - actuator_armed_s _armed; ///< system armed state int _t_vstatus; ///< system / vehicle status - vehicle_status_s _vstatus; ///< overall system state + int _t_param; ///< parameter update topic + /* advertised topics */ orb_advert_t _to_input_rc; ///< rc inputs from io - rc_input_values _input_rc; ///< rc input values - + 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 - battery_status_s _battery_status;///< battery status data - orb_advert_t _t_outputs; ///< mixed outputs topic actuator_outputs_s _outputs; ///< mixed outputs + actuator_controls_effective_s _controls_effective; ///< effective controls const char *volatile _mix_buf; ///< mixer text buffer volatile unsigned _mix_buf_len; ///< size of the mixer text buffer bool _primary_pwm_device; ///< true if we are the default PWM output - uint32_t _relays; ///< state of the PX4IO relays, one bit per relay - - volatile bool _switch_armed; ///< PX4IO switch armed state - // XXX how should this work? - - bool _send_needed; ///< If true, we need to send a packet to IO - bool _config_needed; ///< if true, we need to set a config update to IO /** * Trampoline to the worker task @@ -159,43 +146,107 @@ private: void task_main(); /** - * Handle receiving bytes from PX4IO + * Send controls to IO */ - void io_recv(); + int io_set_control_state(); /** - * HX protocol callback trampoline. + * Update IO's arming-related state */ - static void rx_callback_trampoline(void *arg, const void *buffer, size_t bytes_received); + int io_set_arming_state(); /** - * Callback invoked when we receive a whole packet from PX4IO + * Push RC channel configuration to IO. */ - void rx_callback(const uint8_t *buffer, size_t bytes_received); + int io_set_rc_config(); /** - * Send an update packet to PX4IO + * Fetch status and alarms from IO + * + * Also publishes battery voltage/current. */ - void io_send(); + int io_get_status(); /** - * Send a config packet to PX4IO + * Fetch RC inputs from IO. + * + * @param input_rc Input structure to populate. + * @return OK if data was returned. */ - void config_send(); + int io_get_raw_rc_input(rc_input_values &input_rc); /** - * Send a buffer containing mixer text to PX4IO + * Fetch and publish raw RC input data. */ - int mixer_send(const char *buf, unsigned buflen); + 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); /** - * Mixer control callback; invoked to fetch a control from a specific - * group/index during mixing. + * Send mixer definition text to IO */ - static int control_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &input); + int mixer_send(const char *buf, unsigned buflen); + }; @@ -207,26 +258,26 @@ PX4IO *g_dev; } PX4IO::PX4IO() : - CDev("px4io", "/dev/px4io"), - dump_one(false), - _update_rate(50), - _serial_fd(-1), - _io_stream(nullptr), + I2C("px4io", "/dev/px4io", PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO, 320000), + _max_actuators(0), + _max_rc_input(0), + _max_relays(0), + _max_transfer(16), /* sensible default */ + _update_interval(0), _task(-1), _task_should_exit(false), _connected(false), + _perf_update(perf_alloc(PC_ELAPSED, "px4io update")), _t_actuators(-1), - _t_actuators_effective(-1), _t_armed(-1), _t_vstatus(-1), - _t_outputs(-1), + _to_input_rc(0), + _to_actuators_effective(0), + _to_outputs(0), + _to_battery(0), _mix_buf(nullptr), _mix_buf_len(0), - _primary_pwm_device(false), - _relays(0), - _switch_armed(false), - _send_needed(false), - _config_needed(true) + _primary_pwm_device(false) { /* we need this potentially before it could be set in task_main */ g_dev = this; @@ -260,11 +311,33 @@ PX4IO::init() ASSERT(_task == -1); /* do regular cdev init */ - ret = CDev::init(); - + ret = I2C::init(); if (ret != OK) return ret; + /* get some parameters */ + _max_actuators = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_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); + _max_rc_input = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT); + if ((_max_actuators < 1) || (_max_actuators == _io_reg_get_error) || + (_max_relays < 1) || (_max_relays == _io_reg_get_error) || + (_max_relays < 16) || (_max_relays == _io_reg_get_error) || + (_max_rc_input < 1) || (_max_rc_input == _io_reg_get_error)) { + + log("failed getting parameters from PX4IO"); + return ret; + } + if (_max_rc_input > RC_INPUT_MAX_CHANNELS) + _max_rc_input = RC_INPUT_MAX_CHANNELS; + + /* publish RC config to IO */ + ret = io_set_rc_config(); + if (ret != OK) { + log("failed to update RC input config"); + 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); @@ -287,7 +360,6 @@ PX4IO::init() debug("PX4IO connected"); break; } - usleep(100000); } @@ -300,17 +372,6 @@ PX4IO::init() return OK; } -int -PX4IO::set_pwm_rate(int hz) -{ - if (hz > 0 && hz <= 400) { - _update_rate = hz; - return OK; - } else { - return -EINVAL; - } -} - void PX4IO::task_main_trampoline(int argc, char *argv[]) { @@ -320,39 +381,9 @@ PX4IO::task_main_trampoline(int argc, char *argv[]) void PX4IO::task_main() { - log("starting"); - unsigned update_rate_in_ms; - - /* open the serial port */ - _serial_fd = ::open("/dev/ttyS2", O_RDWR); - - if (_serial_fd < 0) { - log("failed to open serial port: %d", errno); - goto out; - } - - /* 115200bps, no parity, one stop bit */ - { - struct termios t; - - tcgetattr(_serial_fd, &t); - cfsetspeed(&t, 115200); - t.c_cflag &= ~(CSTOPB | PARENB); - tcsetattr(_serial_fd, TCSANOW, &t); - } - - /* protocol stream */ - _io_stream = hx_stream_init(_serial_fd, &PX4IO::rx_callback_trampoline, this); - - if (_io_stream == nullptr) { - log("failed to allocate HX protocol stream"); - goto out; - } + hrt_abstime last_poll_time = 0; - hx_stream_set_counters(_io_stream, - perf_alloc(PC_COUNT, "PX4IO frames transmitted"), - perf_alloc(PC_COUNT, "PX4IO frames received"), - perf_alloc(PC_COUNT, "PX4IO receive errors")); + log("starting"); /* * Subscribe to the appropriate PWM output topic based on whether we are the @@ -360,9 +391,7 @@ PX4IO::task_main() */ _t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : ORB_ID(actuator_controls_1)); - /* convert the update rate in hz to milliseconds, rounding down if necessary */ - update_rate_in_ms = 1000 / _update_rate; - orb_set_interval(_t_actuators, update_rate_in_ms); + 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 */ @@ -370,33 +399,18 @@ PX4IO::task_main() _t_vstatus = orb_subscribe(ORB_ID(vehicle_status)); orb_set_interval(_t_vstatus, 200); /* 5Hz update rate max. */ - /* advertise the limited control inputs */ - memset(&_controls_effective, 0, sizeof(_controls_effective)); - _t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_1), - &_controls_effective); - - /* advertise the mixed control outputs */ - memset(&_outputs, 0, sizeof(_outputs)); - _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), - &_outputs); - - /* advertise the rc inputs */ - memset(&_input_rc, 0, sizeof(_input_rc)); - _to_input_rc = orb_advertise(ORB_ID(input_rc), &_input_rc); - - /* do not advertise the battery status until its clear that a battery is connected */ - memset(&_battery_status, 0, sizeof(_battery_status)); - _to_battery = -1; + _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 = _serial_fd; + fds[0].fd = _t_actuators; fds[0].events = POLLIN; - fds[1].fd = _t_actuators; + fds[1].fd = _t_armed; fds[1].events = POLLIN; - fds[2].fd = _t_armed; + fds[2].fd = _t_vstatus; fds[2].events = POLLIN; - fds[3].fd = _t_vstatus; + fds[3].fd = _t_param; fds[3].events = POLLIN; debug("ready"); @@ -404,10 +418,20 @@ PX4IO::task_main() /* lock against the ioctl handler */ lock(); - /* loop handling received serial bytes */ + /* loop talking to IO */ while (!_task_should_exit) { - /* sleep waiting for data, but no more than 100ms */ + /* 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 100ms */ unlock(); int ret = ::poll(&fds[0], sizeof(fds) / sizeof(fds[0]), 100); lock(); @@ -418,76 +442,61 @@ PX4IO::task_main() continue; } - /* if we timed out waiting, we should send an update */ - if (ret == 0) - _send_needed = true; - - if (ret > 0) { - /* if we have new data from IO, go handle it */ - if (fds[0].revents & POLLIN) - io_recv(); - - /* if we have new control data from the ORB, handle it */ - if (fds[1].revents & POLLIN) { - - /* get controls */ - orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : - ORB_ID(actuator_controls_1), _t_actuators, &_controls); - - /* scale controls to PWM (temporary measure) */ - for (unsigned i = 0; i < _max_actuators; i++) - _outputs.output[i] = 1500 + (600 * _controls.control[i]); - - /* and flag for update */ - _send_needed = true; - } - - /* if we have an arming state update, handle it */ - if (fds[2].revents & POLLIN) { - - orb_copy(ORB_ID(actuator_armed), _t_armed, &_armed); - _send_needed = true; - } - - if (fds[3].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_status), _t_vstatus, &_vstatus); - _send_needed = true; - } - } + /* if we have new control data from the ORB, handle it */ + if (fds[0].revents & POLLIN) + io_set_control_state(); - /* send a config packet to IO if required */ - if (_config_needed) { - _config_needed = false; - config_send(); - } + /* if we have an arming state update, handle it */ + if ((fds[1].revents & POLLIN) || (fds[2].revents & POLLIN)) + io_set_arming_state(); - /* send a mixer update if needed */ - if (_mix_buf != nullptr) { - mixer_send(_mix_buf, _mix_buf_len); + hrt_abstime now = hrt_absolute_time(); - /* clear the buffer record so the ioctl handler knows we're done */ - _mix_buf = nullptr; - _mix_buf_len = 0; - } + /* + * If this isn't time for the next tick of the polling state machine, + * go back to sleep. + */ + if ((now - last_poll_time) < 20000) + continue; - /* send an update to IO if required */ - if (_send_needed) { - _send_needed = false; - io_send(); + /* + * Pull status and alarms from IO. + */ + io_get_status(); + + /* + * Get 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(); } } unlock(); -out: debug("exiting"); - /* kill the HX stream */ - if (_io_stream != nullptr) - hx_stream_free(_io_stream); - - ::close(_serial_fd); - /* clean up the alternate device node */ if (_primary_pwm_device) unregister_driver(PWM_OUTPUT_DEVICE_PATH); @@ -498,148 +507,402 @@ out: } int -PX4IO::control_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &input) +PX4IO::io_set_control_state() { - const actuator_controls_s *controls = (actuator_controls_s *)handle; + 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); - input = controls->control[control_index]; - return 0; + for (unsigned i = 0; i < _max_actuators; 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_actuators); } -void -PX4IO::io_recv() +int +PX4IO::io_set_arming_state() { - uint8_t buf[32]; - int count; + actuator_armed_s armed; ///< system armed state + vehicle_status_s vstatus; ///< overall system state - /* - * We are here because poll says there is some data, so this - * won't block even on a blocking device. If more bytes are - * available, we'll go back to poll() again... - */ - count = ::read(_serial_fd, buf, sizeof(buf)); + orb_copy(ORB_ID(actuator_armed), _t_armed, &armed); + orb_copy(ORB_ID(vehicle_status), _t_vstatus, &vstatus); - /* pass received bytes to the packet decoder */ - for (int i = 0; i < count; i++) - hx_stream_rx(_io_stream, buf[i]); -} + uint16_t set = 0; + uint16_t clear = 0; -void -PX4IO::rx_callback_trampoline(void *arg, const void *buffer, size_t bytes_received) -{ - g_dev->rx_callback((const uint8_t *)buffer, bytes_received); + 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; + } else { + clear |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE; + } + + return io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, clear, set); } -void -PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received) +int +PX4IO::io_set_rc_config() { - const px4io_report *rep = (const px4io_report *)buffer; + unsigned offset = 0; + int input_map[_max_rc_input]; + int32_t ichan; + int ret = OK; -// lock(); + /* + * 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; - /* sanity-check the received frame size */ - if (bytes_received != sizeof(px4io_report)) { - debug("got %u expected %u", bytes_received, sizeof(px4io_report)); - goto out; - } + param_get(param_find("RC_MAP_ROLL"), &ichan); + if ((ichan >= 0) && (ichan < (int)_max_rc_input)) + input_map[ichan] = 0; - if (rep->i2f_magic != I2F_MAGIC) { - debug("bad magic"); - goto out; - } + param_get(param_find("RC_MAP_PITCH"), &ichan); + if ((ichan >= 0) && (ichan < (int)_max_rc_input)) + input_map[ichan] = 1; - _connected = true; + param_get(param_find("RC_MAP_YAW"), &ichan); + if ((ichan >= 0) && (ichan < (int)_max_rc_input)) + input_map[ichan] = 2; - /* publish raw rc channel values from IO if valid channels are present */ - if (rep->channel_count > 0) { - _input_rc.timestamp = hrt_absolute_time(); - _input_rc.channel_count = rep->channel_count; + param_get(param_find("RC_MAP_THROTTLE"), &ichan); + if ((ichan >= 0) && (ichan < (int)_max_rc_input)) + input_map[ichan] = 3; - for (int i = 0; i < rep->channel_count; i++) { - _input_rc.values[i] = rep->rc_channel[i]; - } + ichan = 4; + for (unsigned i = 0; i < _max_rc_input; i++) + if (input_map[i] == -1) + input_map[i] = ichan++; - orb_publish(ORB_ID(input_rc), _to_input_rc, &_input_rc); + /* + * 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; + + sprintf(pname, "RC%d_MIN", i + 1); + param_get(param_find(pname), &fval); + regs[PX4IO_P_RC_CONFIG_MIN] = FLOAT_TO_REG(fval); + + sprintf(pname, "RC%d_TRIM", i + 1); + param_get(param_find(pname), &fval); + regs[PX4IO_P_RC_CONFIG_CENTER] = FLOAT_TO_REG(fval); + + sprintf(pname, "RC%d_MAX", i + 1); + param_get(param_find(pname), &fval); + regs[PX4IO_P_RC_CONFIG_MAX] = FLOAT_TO_REG(fval); + + sprintf(pname, "RC%d_DZ", i + 1); + param_get(param_find(pname), &fval); + regs[PX4IO_P_RC_CONFIG_DEADZONE] = FLOAT_TO_REG(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); + 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) + break; + offset += PX4IO_P_RC_CONFIG_STRIDE; } - /* remember the latched arming switch state */ - _switch_armed = rep->armed; + return ret; +} + +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, regs, sizeof(regs) / sizeof(regs[0])); + if (ret != OK) + return ret; + + _status = regs[0]; + _alarms = regs[1]; - /* publish battery information */ + /* XXX handle status */ + + /* XXX handle alarms */ /* only publish if battery has a valid minimum voltage */ - if (rep->battery_mv > 3300) { - _battery_status.timestamp = hrt_absolute_time(); - _battery_status.voltage_v = rep->battery_mv / 1000.0f; - /* current and discharge are unknown */ - _battery_status.current_a = -1.0f; - _battery_status.discharged_mah = -1.0f; - /* announce the battery voltage if needed, just publish else */ + 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); + orb_publish(ORB_ID(battery_status), _to_battery, &battery_status); } else { - _to_battery = orb_advertise(ORB_ID(battery_status), &_battery_status); + _to_battery = orb_advertise(ORB_ID(battery_status), &battery_status); } } + return ret; +} + +int +PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) +{ + uint16_t channel_count; + int ret; + + input_rc.timestamp = hrt_absolute_time(); + + /* 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... + */ + ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT, &channel_count, 1); + if (ret != OK) + return ret; + 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); + + return ret; +} + +int +PX4IO::io_publish_raw_rc() +{ + /* if no 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; +} - _send_needed = true; +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 monitoring, dump the received info */ - if (dump_one) { - dump_one = false; + /* if not taking raw PPM from us, must be mixing */ + if (_status & PX4IO_P_STATUS_FLAGS_RAW_PPM) + return OK; - printf("IO: %s armed ", rep->armed ? "" : "not"); + /* data we are going to fetch */ + actuator_controls_effective_s controls_effective; + controls_effective.timestamp = hrt_absolute_time(); - for (unsigned i = 0; i < rep->channel_count; i++) - printf("%d: %d ", i, rep->rc_channel[i]); + /* 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; - printf("\n"); + /* 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); } -out: -// unlock(); - return; + return OK; } -void -PX4IO::io_send() +int +PX4IO::io_publish_pwm_outputs() { - px4io_command cmd; - int ret; + /* if no FMU comms(!) just don't publish */ + if (!(_status & PX4IO_P_STATUS_FLAGS_FMU_OK)) + return OK; - cmd.f2i_magic = F2I_MAGIC; + /* 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; - /* set outputs */ - for (unsigned i = 0; i < _max_actuators; i++) { - cmd.output_control[i] = _outputs.output[i]; + /* convert from register format to float */ + for (unsigned i = 0; i < _max_actuators; i++) + outputs.output[i] = REG_TO_FLOAT(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); } - /* publish as we send */ - _outputs.timestamp = hrt_absolute_time(); - /* XXX needs to be based off post-mix values from the IO side */ - orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &_outputs); - - /* update relays */ - for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) - cmd.relay_state[i] = (_relays & (1<< i)) ? true : false; - - /* armed and not locked down -> arming ok */ - cmd.arm_ok = (_armed.armed && !_armed.lockdown); - /* indicate that full autonomous position control / vector flight mode is available */ - cmd.vector_flight_mode_ok = _vstatus.flag_vector_flight_mode_ok; - /* allow manual override on IO (not allowed for multirotors or other systems with SAS) */ - cmd.manual_override_ok = _vstatus.flag_external_manual_override_ok; - /* set desired PWM output rate */ - cmd.servo_rate = _update_rate; - - ret = hx_stream_send(_io_stream, &cmd, sizeof(cmd)); + return OK; +} + +int +PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) +{ + i2c_msg_s msgv[2]; + uint8_t hdr[2]; + + hdr[0] = page; + hdr[1] = offset; + + msgv[0].flags = 0; + msgv[0].buffer = hdr; + msgv[0].length = sizeof(hdr); + + msgv[1].flags = 0; + msgv[1].buffer = (uint8_t *)(values); + msgv[1].length = num_values * sizeof(*values); + + return transfer(msgv, 2); +} + +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) +{ + i2c_msg_s msgv[2]; + uint8_t hdr[2]; + + hdr[0] = page; + hdr[1] = offset; + + msgv[0].flags = 0; + msgv[0].buffer = hdr; + msgv[0].length = sizeof(hdr); + + msgv[1].flags = I2C_M_READ; + msgv[1].buffer = (uint8_t *)values; + msgv[1].length = num_values * sizeof(*values); + + return transfer(msgv, 2); +} + +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) - debug("send error %d", ret); + return ret; + value &= ~clearbits; + value |= setbits; + + return io_reg_set(page, offset, &value, 1); } + +#if 0 void PX4IO::config_send() { @@ -694,12 +957,14 @@ PX4IO::config_send() if (ret) debug("config error %d", ret); } +#endif int PX4IO::mixer_send(const char *buf, unsigned buflen) { - uint8_t frame[HX_STREAM_MAX_FRAME]; + 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; @@ -707,8 +972,8 @@ PX4IO::mixer_send(const char *buf, unsigned buflen) do { unsigned count = buflen; - if (count > F2I_MIXER_MAX_TEXT) - count = F2I_MIXER_MAX_TEXT; + if (count > max_len) + count = max_len; if (count > 0) { memcpy(&msg->text[0], buf, count); @@ -716,7 +981,20 @@ PX4IO::mixer_send(const char *buf, unsigned buflen) buflen -= count; } - int ret = hx_stream_send(_io_stream, msg, sizeof(px4io_mixdata) + 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); @@ -727,7 +1005,12 @@ PX4IO::mixer_send(const char *buf, unsigned buflen) } while (buflen > 0); - return 0; + /* check for the mixer-OK flag */ + if (io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_MIXER_OK) + return 0; + + /* load must have failed for some reason */ + return -EINVAL; } int @@ -735,71 +1018,80 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) { int ret = OK; - lock(); - /* regular ioctl? */ switch (cmd) { case PWM_SERVO_ARM: - /* fake an armed transition */ - _armed.armed = true; - _send_needed = true; + /* 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: - /* fake a disarmed transition */ - _armed.armed = false; - _send_needed = true; + /* 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_SET_UPDATE_RATE: - // not supported yet - ret = -EINVAL; + /* 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_SET(0) ... PWM_SERVO_SET(_max_actuators - 1): - - /* fake an update to the selected 'servo' channel */ - if ((arg >= 900) && (arg <= 2100)) { - _outputs.output[cmd - PWM_SERVO_SET(0)] = arg; - _send_needed = true; + case PWM_SERVO_SET(0) ... PWM_SERVO_SET(PWM_OUTPUT_MAX_CHANNELS): { - } else { + 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): { - case PWM_SERVO_GET(0) ... PWM_SERVO_GET(_max_actuators - 1): - /* copy the current output value from the channel */ - *(servo_position_t *)arg = _outputs.output[cmd - PWM_SERVO_GET(0)]; + 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_DIRECT_PWM, channel); + if (value == _io_reg_get_error) { + ret = -EIO; + } else { + *(servo_position_t *)arg = value; + } + } break; + } case GPIO_RESET: - _relays = 0; - _send_needed = true; + 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: - /* make sure only valid bits are being set */ - if ((arg & ((1UL << PX4IO_RELAY_CHANNELS) - 1)) != arg) { - ret = EINVAL; - break; - } - if (cmd == GPIO_SET) { - _relays |= arg; - } else { - _relays &= ~arg; - } - _send_needed = true; + 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 = _relays; + *(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 = PX4IO_CONTROL_CHANNELS; + *(unsigned *)arg = _max_actuators; break; case MIXERIOCRESET: @@ -807,20 +1099,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) break; case MIXERIOCLOADBUF: - - /* set the buffer up for transfer */ - _mix_buf = (const char *)arg; - _mix_buf_len = strnlen(_mix_buf, 1024); - - /* drop the lock and wait for the thread to clear the transmit */ - unlock(); - - while (_mix_buf != nullptr) - usleep(1000); - - lock(); - - ret = 0; + ret = mixer_send((const char *)arg, strnlen(_mix_buf, 1024)); break; default: @@ -828,7 +1107,22 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) ret = -ENOTTY; } - unlock(); + return ret; +} + +ssize_t +PX4IO::write(file *filp, const char *buffer, size_t len) +{ + unsigned count = len / 2; + int ret; + + if (count > 0) { + if (count > _max_actuators) + count = _max_actuators; + ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, (uint16_t *)buffer, count); + } else { + ret = -EINVAL; + } return ret; } @@ -893,8 +1187,10 @@ monitor(void) exit(0); } - if (g_dev != nullptr) - g_dev->dump_one = true; +#warning implement this + +// if (g_dev != nullptr) +// g_dev->dump_one = true; } } @@ -922,7 +1218,7 @@ px4io_main(int argc, char *argv[]) /* look for the optional pwm update rate for the supported modes */ if (strcmp(argv[2], "-u") == 0 || strcmp(argv[2], "--update-rate") == 0) { if (argc > 2 + 1) { - g_dev->set_pwm_rate(atoi(argv[2 + 1])); +#warning implement this } else { fprintf(stderr, "missing argument for pwm update rate (-u)\n"); return 1; |