diff options
author | px4dev <px4@purgatory.org> | 2012-10-22 21:39:55 -0700 |
---|---|---|
committer | px4dev <px4@purgatory.org> | 2012-10-22 21:39:55 -0700 |
commit | 0afeeb5dd8049ed371aa136482de393cb3bca24c (patch) | |
tree | b28a0a72ede70463a4bb4f8b90831a77beda5c45 /apps/px4 | |
parent | 3f240a70baac2435cbd543e305ea0c57cb65e7a4 (diff) | |
download | px4-firmware-0afeeb5dd8049ed371aa136482de393cb3bca24c.tar.gz px4-firmware-0afeeb5dd8049ed371aa136482de393cb3bca24c.tar.bz2 px4-firmware-0afeeb5dd8049ed371aa136482de393cb3bca24c.zip |
Move the PX4IO driver into the drivers folder.
Diffstat (limited to 'apps/px4')
-rw-r--r-- | apps/px4/px4io/driver/.context | 0 | ||||
-rw-r--r-- | apps/px4/px4io/driver/Makefile | 42 | ||||
-rw-r--r-- | apps/px4/px4io/driver/px4io.cpp | 608 | ||||
-rw-r--r-- | apps/px4/px4io/driver/uploader.cpp | 387 | ||||
-rw-r--r-- | apps/px4/px4io/driver/uploader.h | 99 |
5 files changed, 0 insertions, 1136 deletions
diff --git a/apps/px4/px4io/driver/.context b/apps/px4/px4io/driver/.context deleted file mode 100644 index e69de29bb..000000000 --- a/apps/px4/px4io/driver/.context +++ /dev/null diff --git a/apps/px4/px4io/driver/Makefile b/apps/px4/px4io/driver/Makefile deleted file mode 100644 index cbd942546..000000000 --- a/apps/px4/px4io/driver/Makefile +++ /dev/null @@ -1,42 +0,0 @@ -############################################################################ -# -# Copyright (C) 2012 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. -# -############################################################################ - -# -# Interface driver for the PX4IO board. -# - -APPNAME = px4io -PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 2048 - -include $(APPDIR)/mk/app.mk diff --git a/apps/px4/px4io/driver/px4io.cpp b/apps/px4/px4io/driver/px4io.cpp deleted file mode 100644 index 66db1c360..000000000 --- a/apps/px4/px4io/driver/px4io.cpp +++ /dev/null @@ -1,608 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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 serial (or possibly some other interface at a later - * point). - - */ - -#include <nuttx/config.h> - -#include <sys/types.h> -#include <stdint.h> -#include <stdbool.h> -#include <assert.h> -#include <debug.h> -#include <time.h> -#include <queue.h> -#include <errno.h> -#include <string.h> -#include <stdio.h> -#include <stdlib.h> -#include <unistd.h> -#include <fcntl.h> - -#include <arch/board/board.h> - -#include <drivers/device/device.h> -#include <drivers/drv_rc_input.h> -#include <drivers/drv_pwm_output.h> -#include <systemlib/mixer/mixer.h> -#include <drivers/drv_mixer.h> - -#include <systemlib/perf_counter.h> -#include <systemlib/hx_stream.h> -#include <systemlib/err.h> -#include <systemlib/systemlib.h> - -#include <uORB/topics/actuator_controls.h> -#include <uORB/topics/actuator_outputs.h> -#include <uORB/topics/rc_channels.h> - -#include "px4io/protocol.h" -#include "uploader.h" - - -class PX4IO : public device::CDev -{ -public: - PX4IO(); - ~PX4IO(); - - virtual int init(); - - virtual int ioctl(file *filp, int cmd, unsigned long arg); - -private: - static const unsigned _max_actuators = PX4IO_OUTPUT_CHANNELS; - - int _serial_fd; ///< serial interface to PX4IO - hx_stream_t _io_stream; ///< HX protocol stream - - int _task; ///< worker task - volatile bool _task_should_exit; - - int _t_actuators; ///< actuator output topic - actuator_controls_s _controls; ///< actuator outputs - - int _t_armed; ///< system armed control topic - actuator_armed_s _armed; ///< system armed state - - orb_advert_t _t_outputs; ///< mixed outputs topic - actuator_outputs_s _outputs; ///< mixed outputs - - MixerGroup *_mixers; ///< loaded mixers - - bool _primary_pwm_device; ///< true if we are the default PWM output - - 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 - - /** - * Trampoline to the worker task - */ - static void task_main_trampoline(int argc, char *argv[]); - - /** - * worker task - */ - void task_main(); - - /** - * Handle receiving bytes from PX4IO - */ - void io_recv(); - - /** - * HX protocol callback trampoline. - */ - static void rx_callback_trampoline(void *arg, const void *buffer, size_t bytes_received); - - /** - * Callback invoked when we receive a whole packet from PX4IO - */ - void rx_callback(const uint8_t *buffer, size_t bytes_received); - - /** - * Send an update packet to PX4IO - */ - void io_send(); - - /** - * Mixer control callback; invoked to fetch a control from a specific - * group/index during mixing. - */ - static int control_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &input); -}; - - -namespace -{ - -PX4IO *g_dev; - -} - -PX4IO::PX4IO() : - CDev("px4io", "/dev/px4io"), - _serial_fd(-1), - _io_stream(nullptr), - _task(-1), - _task_should_exit(false), - _t_actuators(-1), - _t_armed(-1), - _t_outputs(-1), - _mixers(nullptr), - _primary_pwm_device(false), - _switch_armed(false), - _send_needed(false) -{ - /* we need this potentially before it could be set in task_main */ - g_dev = this; - - _debug_enabled = true; -} - -PX4IO::~PX4IO() -{ - if (_task != -1) { - /* tell the task we want it to go away */ - _task_should_exit = true; - - /* spin waiting for the thread to stop */ - unsigned i = 10; - do { - /* wait 50ms - it should wake every 100ms or so worst-case */ - usleep(50000); - - /* if we have given up, kill it */ - if (--i == 0) { - task_delete(_task); - break; - } - - } while (_task != -1); - } - - /* clean up the alternate device node */ - if (_primary_pwm_device) - unregister_driver(PWM_OUTPUT_DEVICE_PATH); - - /* kill the HX stream */ - if (_io_stream != nullptr) - hx_stream_free(_io_stream); - - g_dev = nullptr; -} - -int -PX4IO::init() -{ - int ret; - - ASSERT(_task == -1); - - /* do regular cdev init */ - ret = CDev::init(); - if (ret != OK) - 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_DEFAULT, 4096, (main_t)&PX4IO::task_main_trampoline, nullptr); - if (_task < 0) { - debug("task start failed: %d", errno); - return -errno; - } - - return OK; -} - -void -PX4IO::task_main_trampoline(int argc, char *argv[]) -{ - g_dev->task_main(); -} - -void -PX4IO::task_main() -{ - log("starting"); - - /* open the serial port */ - _serial_fd = ::open("/dev/ttyS2", O_RDWR); - if (_serial_fd < 0) { - debug("failed to open serial port for IO: %d", errno); - _task = -1; - _exit(errno); - } - - /* protocol stream */ - _io_stream = hx_stream_init(_serial_fd, &PX4IO::rx_callback_trampoline, this); - - perf_counter_t pc_tx_frames = perf_alloc(PC_COUNT, "PX4IO frames transmitted"); - perf_counter_t pc_rx_frames = perf_alloc(PC_COUNT, "PX4IO frames received"); - perf_counter_t pc_rx_errors = perf_alloc(PC_COUNT, "PX4IO receive errors"); - hx_stream_set_counters(_io_stream, pc_tx_frames, pc_rx_frames, pc_rx_errors); - - /* XXX send a who-are-you request */ - - /* XXX verify firmware/protocol version */ - - /* - * 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)); - /* convert the update rate in hz to milliseconds, rounding down if necessary */ - //int update_rate_in_ms = int(1000 / _update_rate); - orb_set_interval(_t_actuators, 20); /* XXX 50Hz hardcoded for now */ - - _t_armed = orb_subscribe(ORB_ID(actuator_armed)); - orb_set_interval(_t_armed, 200); /* 5Hz update rate */ - - /* advertise the mixed control outputs */ - _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), - &_outputs); - - /* poll descriptor */ - pollfd fds[3]; - fds[0].fd = _serial_fd; - fds[0].events = POLLIN; - fds[1].fd = _t_actuators; - fds[1].events = POLLIN; - fds[2].fd = _t_armed; - fds[2].events = POLLIN; - - log("ready"); - - /* loop handling received serial bytes */ - while (!_task_should_exit) { - - /* sleep waiting for data, but no more than 100ms */ - int ret = ::poll(&fds[0], sizeof(fds) / sizeof(fds[0]), 1000); - - /* this would be bad... */ - if (ret < 0) { - log("poll error %d", errno); - usleep(1000000); - 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 data from the ORB, go handle it */ - if (fds[1].revents & POLLIN) { - - /* get controls */ - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls); - - /* mix */ - if (_mixers != nullptr) { - /* XXX is this the right count? */ - _mixers->mix(&_outputs.output[0], _max_actuators); - - /* convert to PWM values */ - for (unsigned i = 0; i < _max_actuators; i++) - _outputs.output[i] = 1500 + (600 * _outputs.output[i]); - - /* and flag for update */ - _send_needed = true; - } - } - if (fds[2].revents & POLLIN) { - - orb_copy(ORB_ID(actuator_armed), _t_armed, &_controls); - _send_needed = true; - } - } - - /* send an update to IO if required */ - if (_send_needed) { - _send_needed = false; - io_send(); - } - } - - /* tell the dtor that we are exiting */ - _task = -1; - _exit(0); -} - -int -PX4IO::control_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &input) -{ - const actuator_controls_s *controls = (actuator_controls_s *)handle; - - input = controls->control[control_index]; - return 0; -} - -void -PX4IO::io_recv() -{ - uint8_t buf[32]; - int count; - - /* - * 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)); - - /* pass received bytes to the packet decoder */ - for (int i = 0; i < count; i++) - hx_stream_rx(_io_stream, buf[i]); -} - -void -PX4IO::rx_callback_trampoline(void *arg, const void *buffer, size_t bytes_received) -{ - g_dev->rx_callback((const uint8_t *)buffer, bytes_received); -} - -void -PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received) -{ - const px4io_report *rep = (const px4io_report *)buffer; - - /* sanity-check the received frame size */ - if (bytes_received != sizeof(px4io_report)) - return; - - /* XXX handle R/C inputs here ... needs code sharing/library */ - - /* remember the latched arming switch state */ - _switch_armed = rep->armed; - - unlock(); -} - -void -PX4IO::io_send() -{ - px4io_command cmd; - - cmd.f2i_magic = F2I_MAGIC; - - /* set outputs */ - for (unsigned i = 0; i < _max_actuators; i++) - cmd.servo_command[i] = _outputs.output[i]; - - /* publish as we send */ - orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &_outputs); - - // XXX relays - - cmd.arm_ok = _armed.armed; - - hx_stream_send(_io_stream, &cmd, sizeof(cmd)); -} - -int -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; - break; - - case PWM_SERVO_DISARM: - /* fake a disarmed transition */ - _armed.armed = true; - _send_needed = true; - 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; - } else { - ret = -EINVAL; - } - break; - - 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)]; - break; - - case MIXERIOCGETOUTPUTCOUNT: - *(unsigned *)arg = _max_actuators; - break; - - case MIXERIOCRESET: - if (_mixers != nullptr) { - delete _mixers; - _mixers = nullptr; - } - break; - - case MIXERIOCADDSIMPLE: { - mixer_simple_s *mixinfo = (mixer_simple_s *)arg; - - /* build the new mixer from the supplied argument */ - SimpleMixer *mixer = new SimpleMixer(control_callback, - (uintptr_t)&_controls, mixinfo); - - /* validate the new mixer */ - if (mixer->check()) { - delete mixer; - ret = -EINVAL; - - } else { - /* if we don't have a group yet, allocate one */ - if (_mixers == nullptr) - _mixers = new MixerGroup(control_callback, - (uintptr_t)&_controls); - - /* add the new mixer to the group */ - _mixers->add_mixer(mixer); - } - - } - break; - - case MIXERIOCADDMULTIROTOR: - /* XXX not yet supported */ - ret = -ENOTTY; - break; - - case MIXERIOCLOADFILE: { - MixerGroup *newmixers; - const char *path = (const char *)arg; - - /* allocate a new mixer group and load it from the file */ - newmixers = new MixerGroup(control_callback, (uintptr_t)&_controls); - if (newmixers->load_from_file(path) != 0) { - delete newmixers; - ret = -EINVAL; - } - - /* swap the new mixers in for the old */ - if (_mixers != nullptr) { - delete _mixers; - } - _mixers = newmixers; - - } - break; - - default: - /* not a recognised value */ - ret = -ENOTTY; - } - unlock(); - - return ret; -} - -extern "C" __EXPORT int px4io_main(int argc, char *argv[]); - -int -px4io_main(int argc, char *argv[]) -{ - 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"); - } - - exit(0); - } - - /* note, stop not currently implemented */ - - if (!strcmp(argv[1], "update")) { - 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; - } - - errx(1, "need a verb, only support 'start' and 'update'"); -} diff --git a/apps/px4/px4io/driver/uploader.cpp b/apps/px4/px4io/driver/uploader.cpp deleted file mode 100644 index 0fbbac839..000000000 --- a/apps/px4/px4io/driver/uploader.cpp +++ /dev/null @@ -1,387 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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 Firmware uploader for PX4IO - */ - -#include <nuttx/config.h> - -#include <sys/types.h> -#include <stdint.h> -#include <stdbool.h> -#include <assert.h> -#include <errno.h> -#include <string.h> -#include <stdio.h> -#include <stdarg.h> -#include <unistd.h> -#include <fcntl.h> -#include <poll.h> - -#include "uploader.h" - -PX4IO_Uploader::PX4IO_Uploader() : - _io_fd(-1), - _fw_fd(-1) -{ -} - -PX4IO_Uploader::~PX4IO_Uploader() -{ -} - -int -PX4IO_Uploader::upload(const char *filenames[]) -{ - int ret; - - _io_fd = open("/dev/ttyS2", O_RDWR); - if (_io_fd < 0) { - log("could not open interface"); - return -errno; - } - - /* look for the bootloader */ - ret = sync(); - if (ret != OK) { - /* this is immediately fatal */ - log("bootloader not responding"); - return -EIO; - } - - for (unsigned i = 0; filenames[i] != nullptr; i++) { - _fw_fd = open(filenames[i], O_RDONLY); - - if (_fw_fd < 0) { - log("failed to open %s", filenames[i]); - continue; - } - log("using firmware from %s", filenames[i]); - break; - } - if (_fw_fd == -1) - return -ENOENT; - - /* do the usual program thing - allow for failure */ - for (unsigned retries = 0; retries < 1; retries++) { - if (retries > 0) { - log("retrying update..."); - ret = sync(); - if (ret != OK) { - /* this is immediately fatal */ - log("bootloader not responding"); - return -EIO; - } - } - - ret = erase(); - if (ret != OK) { - log("erase failed"); - continue; - } - ret = program(); - if (ret != OK) { - log("program failed"); - continue; - } - ret = verify(); - if (ret != OK) { - log("verify failed"); - continue; - } - ret = reboot(); - if (ret != OK) { - log("reboot failed"); - return ret; - } - log("update complete"); - - ret = OK; - break; - } - - close(_fw_fd); - return ret; -} - -int -PX4IO_Uploader::recv(uint8_t &c, unsigned timeout) -{ - struct pollfd fds[1]; - - fds[0].fd = _io_fd; - fds[0].events = POLLIN; - - /* wait 100 ms for a character */ - int ret = ::poll(&fds[0], 1, timeout); - if (ret < 1) { - //log("poll timeout %d", ret); - return -ETIMEDOUT; - } - - read(_io_fd, &c, 1); - //log("recv 0x%02x", c); - return OK; -} - -int -PX4IO_Uploader::recv(uint8_t *p, unsigned count) -{ - while (count--) { - int ret = recv(*p++); - if (ret != OK) - return ret; - } - return OK; -} - -void -PX4IO_Uploader::drain() -{ - uint8_t c; - int ret; - - do { - ret = recv(c, 10); - //log("discard 0x%02x", c); - } while(ret == OK); -} - -int -PX4IO_Uploader::send(uint8_t c) -{ - //log("send 0x%02x", c); - if (write(_io_fd, &c, 1) != 1) - return -errno; - return OK; -} - -int -PX4IO_Uploader::send(uint8_t *p, unsigned count) -{ - while (count--) { - int ret = send(*p++); - if (ret != OK) - return ret; - } - return OK; -} - -int -PX4IO_Uploader::get_sync(unsigned timeout) -{ - uint8_t c[2]; - int ret; - - ret = recv(c[0], timeout); - if (ret != OK) - return ret; - ret = recv(c[1], timeout); - if (ret != OK) - return ret; - if ((c[0] != PROTO_INSYNC) || (c[1] != PROTO_OK)) { - log("bad sync 0x%02x,0x%02x", c[0], c[1]); - return -EIO; - } - return OK; -} - -int -PX4IO_Uploader::sync() -{ - drain(); - /* complete any pending program operation */ - for (unsigned i = 0; i < (PROG_MULTI_MAX + 6); i++) - send(0); - send(PROTO_GET_SYNC); - send(PROTO_EOC); - return get_sync(); -} - -int -PX4IO_Uploader::get_info(int param, uint32_t &val) -{ - int ret; - - send(PROTO_GET_DEVICE); - send(param); - send(PROTO_EOC); - - ret = recv((uint8_t *)&val, sizeof(val)); - if (ret != OK) - return ret; - return get_sync(); -} - -int -PX4IO_Uploader::erase() -{ - log("erase..."); - send(PROTO_CHIP_ERASE); - send(PROTO_EOC); - return get_sync(10000); /* allow 10s timeout */ -} - -int -PX4IO_Uploader::program() -{ - uint8_t file_buf[PROG_MULTI_MAX]; - ssize_t count; - int ret; - - log("program..."); - lseek(_fw_fd, 0, SEEK_SET); - - while (true) { - /* get more bytes to program */ - //log(" %d", (int)lseek(_fw_fd, 0, SEEK_CUR)); - count = read(_fw_fd, file_buf, sizeof(file_buf)); - if (count == 0) - return OK; - if (count < 0) - return -errno; - ASSERT((count % 4) == 0); - - send(PROTO_PROG_MULTI); - send(count); - send(&file_buf[0], count); - send(PROTO_EOC); - - ret = get_sync(1000); - if (ret != OK) - return ret; - } -} - -int -PX4IO_Uploader::verify() -{ - uint8_t file_buf[PROG_MULTI_MAX]; - ssize_t count; - int ret; - - log("verify..."); - lseek(_fw_fd, 0, SEEK_SET); - - send(PROTO_CHIP_VERIFY); - send(PROTO_EOC); - ret = get_sync(); - if (ret != OK) - return ret; - - while (true) { - /* get more bytes to verify */ - int base = (int)lseek(_fw_fd, 0, SEEK_CUR); - count = read(_fw_fd, file_buf, sizeof(file_buf)); - if (count == 0) - break; - if (count < 0) - return -errno; - ASSERT((count % 4) == 0); - - send(PROTO_READ_MULTI); - send(count); - send(PROTO_EOC); - for (ssize_t i = 0; i < count; i++) { - uint8_t c; - - ret = recv(c); - if (ret != OK) { - log("%d: got %d waiting for bytes", base + i, ret); - return ret; - } - - if (c != file_buf[i]) { - log("%d: got 0x%02x expected 0x%02x", base + i, c, file_buf[i]); - return -EINVAL; - } - } - ret = get_sync(); - if (ret != OK) { - log("timeout waiting for post-verify sync"); - return ret; - } - } - return OK; -} - -int -PX4IO_Uploader::reboot() -{ - send(PROTO_REBOOT); - send(PROTO_EOC); - - return OK; -} - -int -PX4IO_Uploader::compare(bool &identical) -{ - uint32_t file_vectors[15]; - uint32_t fw_vectors[15]; - int ret; - - lseek(_fw_fd, 0, SEEK_SET); - ret = read(_fw_fd, &file_vectors[0], sizeof(file_vectors)); - - send(PROTO_CHIP_VERIFY); - send(PROTO_EOC); - ret = get_sync(); - if (ret != OK) - return ret; - - send(PROTO_READ_MULTI); - send(sizeof(fw_vectors)); - send(PROTO_EOC); - ret = recv((uint8_t *)&fw_vectors[0], sizeof(fw_vectors)); - if (ret != OK) - return ret; - - identical = (memcmp(&file_vectors[0], &fw_vectors[0], sizeof(file_vectors))) ? true : false; - - return OK; -} - -void -PX4IO_Uploader::log(const char *fmt, ...) -{ - va_list ap; - - printf("[PX4IO] "); - va_start(ap, fmt); - vprintf(fmt, ap); - va_end(ap); - printf("\n"); - fflush(stdout); -}
\ No newline at end of file diff --git a/apps/px4/px4io/driver/uploader.h b/apps/px4/px4io/driver/uploader.h deleted file mode 100644 index 8d41992f8..000000000 --- a/apps/px4/px4io/driver/uploader.h +++ /dev/null @@ -1,99 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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 Firmware uploader for PX4IO - */ - -#ifndef _PX4IO_UPLOADER_H -#define _PX4IO_UPLOADER_H value - -#include <stdint.h> -#include <stdbool.h> - - -class PX4IO_Uploader -{ -public: - PX4IO_Uploader(); - ~PX4IO_Uploader(); - - int upload(const char *filenames[]); - -private: - enum { - - PROTO_NOP = 0x00, - PROTO_OK = 0x10, - PROTO_FAILED = 0x11, - PROTO_INSYNC = 0x12, - PROTO_EOC = 0x20, - PROTO_GET_SYNC = 0x21, - PROTO_GET_DEVICE = 0x22, - PROTO_CHIP_ERASE = 0x23, - PROTO_CHIP_VERIFY = 0x24, - PROTO_PROG_MULTI = 0x27, - PROTO_READ_MULTI = 0x28, - PROTO_REBOOT = 0x30, - - INFO_BL_REV = 1, /**< bootloader protocol revision */ - BL_REV = 2, /**< supported bootloader protocol */ - INFO_BOARD_ID = 2, /**< board type */ - INFO_BOARD_REV = 3, /**< board revision */ - INFO_FLASH_SIZE = 4, /**< max firmware size in bytes */ - - PROG_MULTI_MAX = 60, /**< protocol max is 255, must be multiple of 4 */ - READ_MULTI_MAX = 60, /**< protocol max is 255, something overflows with >= 64 */ - }; - - int _io_fd; - int _fw_fd; - - void log(const char *fmt, ...); - - int recv(uint8_t &c, unsigned timeout = 1000); - int recv(uint8_t *p, unsigned count); - void drain(); - int send(uint8_t c); - int send(uint8_t *p, unsigned count); - int get_sync(unsigned timeout = 1000); - int sync(); - int get_info(int param, uint32_t &val); - int erase(); - int program(); - int verify(); - int reboot(); - int compare(bool &identical); -}; - -#endif
\ No newline at end of file |