diff options
Diffstat (limited to 'apps/px4/px4io/driver')
| -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 | 560 | ||||
| -rw-r--r-- | apps/px4/px4io/driver/uploader.cpp | 387 | ||||
| -rw-r--r-- | apps/px4/px4io/driver/uploader.h | 99 |
5 files changed, 1088 insertions, 0 deletions
diff --git a/apps/px4/px4io/driver/.context b/apps/px4/px4io/driver/.context new file mode 100644 index 000000000..e69de29bb --- /dev/null +++ b/apps/px4/px4io/driver/.context diff --git a/apps/px4/px4io/driver/Makefile b/apps/px4/px4io/driver/Makefile new file mode 100644 index 000000000..cbd942546 --- /dev/null +++ b/apps/px4/px4io/driver/Makefile @@ -0,0 +1,42 @@ +############################################################################ +# +# 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 new file mode 100644 index 000000000..c3f14e3a4 --- /dev/null +++ b/apps/px4/px4io/driver/px4io.cpp @@ -0,0 +1,560 @@ +/**************************************************************************** + * + * 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 Driver for the PX4IO board. + * + * PX4IO is connected via serial (or possibly some other interface at a later + * point). + * + * XXX current design is racy as all hell; need a locking strategy. + */ + +#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 <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/perf_counter.h> +#include <systemlib/hx_stream.h> + +#include "../protocol.h" +#include "uploader.h" + +class PX4IO; + + +namespace +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +const int ERROR = -1; + +PX4IO *g_dev; + +} + + +class PX4IO_RC : public device::CDev +{ +public: + PX4IO_RC(); + ~PX4IO_RC(); + + virtual int init(); + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + + friend class PX4IO; +protected: + void set_channels(unsigned count, const servo_position_t *data); + +private: + int _publication; + struct rc_input_values _input; +}; + +/* XXX this may conflict with the onboard PPM input */ +PX4IO_RC::PX4IO_RC() : + CDev("px4io_rc", RC_INPUT_DEVICE_PATH), + _publication(-1) +{ + for (unsigned i = 0; i < RC_INPUT_MAX_CHANNELS; i++) { + _input.values[i] = 0; + } + _input.channel_count = 0; +} + +PX4IO_RC::~PX4IO_RC() +{ + if (_publication != -1) + ::close(_publication); +} + +int +PX4IO_RC::init() +{ + int ret; + + ret = CDev::init(); + + /* advertise ourselves as the RC input controller */ + if (ret == OK) { + _publication = orb_advertise(ORB_ID(input_rc), &_input); + if (_publication < 0) + ret = -errno; + } + + return ret; +} + +ssize_t +PX4IO_RC::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned channels = buflen / sizeof(rc_input_t); + rc_input_t *pdata = (rc_input_t *)buffer; + unsigned i; + + if (channels > PX4IO_INPUT_CHANNELS) + return -EIO; + + lock(); + for (i = 0; i < channels; i++) + pdata[i] = _input.values[i]; + unlock(); + + return i * sizeof(servo_position_t); +} + +void +PX4IO_RC::set_channels(unsigned count, const servo_position_t *data) +{ + + ASSERT(count <= PX4IO_INPUT_CHANNELS); + + /* convert incoming servo position values into 0-100 range */ + lock(); + for (unsigned i = 0; i < count; i++) { + rc_input_t chn; + + if (data[i] < 1000) { + chn = 0; + } else if (data[i] > 2000) { + chn = 100; + } else { + chn = (data[i] - 1000) / 10; + } + + _input.values[i] = chn; + } + _input.channel_count = count; + unlock(); + + /* publish to anyone that might be listening */ + if (_publication != -1) + orb_publish(ORB_ID(input_rc), _publication, &_input); + +} + +class PX4IO : public device::CDev +{ +public: + PX4IO(); + ~PX4IO(); + + virtual int init(); + + virtual ssize_t write(struct file *filp, const char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + +private: + int _fd; + int _task; + PX4IO_RC *_rc; + + /** command to be sent to IO */ + struct px4io_command _next_command; + + /** RC channel input from IO */ + servo_position_t _rc_channel[PX4IO_INPUT_CHANNELS]; + int _rc_channel_count; + + volatile bool _armed; + volatile bool _task_should_exit; + + bool _send_needed; + + hx_stream_t _io_stream; + + static void task_main_trampoline(int argc, char *argv[]); + void task_main(); + void io_recv(); + + static void rx_callback_trampoline(void *arg, const void *buffer, size_t bytes_received); + void rx_callback(const uint8_t *buffer, size_t bytes_received); + + void io_send(); +}; + +PX4IO::PX4IO() : + CDev("px4io", "/dev/px4io"), + _fd(-1), + _task(-1), + _rc(new PX4IO_RC), + _rc_channel_count(0), + _armed(false), + _task_should_exit(false), + _send_needed(false), + _io_stream(nullptr) +{ + /* set up the command we will use */ + _next_command.f2i_magic = F2I_MAGIC; + + /* we need this potentially before it could be set in px4io_main */ + g_dev = this; + + _debug_enabled = true; +} + +PX4IO::~PX4IO() +{ + if (_rc != nullptr) + delete _rc; + if (_task != -1) { + /* task should wake up every 100ms or so at least */ + _task_should_exit = true; + + unsigned i = 0; + do { + /* wait 20ms */ + usleep(20000); + + /* if we have given up, kill it */ + if (++i > 10) { + task_delete(_task); + break; + } + + } while (_task != -1); + } + + g_dev = nullptr; +} + +int +PX4IO::init() +{ + int ret; + + ASSERT(_task == -1); + + /* XXX send a who-are-you request */ + + /* XXX verify firmware/protocol version */ + + /* do regular cdev init */ + ret = CDev::init(); + if (ret != OK) + return ret; + + /* start the IO interface task */ + _task = task_create("px4io", SCHED_PRIORITY_DEFAULT, 1024, (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() +{ + ASSERT(_fd == -1); + + log("ready"); + + /* open the serial port */ + _fd = ::open("/dev/ttyS2", O_RDWR | O_NONBLOCK); + if (_fd < 0) { + debug("failed to open serial port for IO: %d", errno); + _task = -1; + _exit(errno); + } + + /* protocol stream */ + _io_stream = hx_stream_init(_fd, &PX4IO::rx_callback_trampoline, this); + + perf_counter_t pc_tx_bytes = perf_alloc(PC_COUNT, "PX4IO frames transmitted"); + perf_counter_t pc_rx_bytes = 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_bytes, pc_rx_bytes, pc_rx_errors); + + /* poll descriptor(s) */ + struct pollfd fds[1]; + fds[0].fd = _fd; + fds[0].events = POLLIN; + + /* loop handling received serial bytes */ + while (!_task_should_exit) { + + /* sleep waiting for data, but no more than 100ms */ + int ret = ::poll(&fds[0], 1, 100); + + /* 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 we have new data from IO, go handle it */ + if ((ret > 0) && (fds[0].revents & POLLIN)) + io_recv(); + + /* send an update to IO if required */ + if (_send_needed) { + _send_needed = false; + io_send(); + } + } + if (_io_stream != nullptr) + hx_stream_free(_io_stream); + ::close(_fd); + + /* tell the dtor that we are exiting */ + _task = -1; + _exit(0); +} + +void +PX4IO::io_recv() +{ + uint8_t c; + + /* handle bytes from IO */ + while (::read(_fd, &c, 1) == 1) + hx_stream_rx(_io_stream, c); +} + +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 struct px4io_report *rep = (const struct px4io_report *)buffer; + + /* sanity-check the received frame size */ + if (bytes_received != sizeof(struct px4io_report)) + return; + + lock(); + + /* pass RC input data to the driver */ + if (_rc != nullptr) + _rc->set_channels(rep->channel_count, &rep->rc_channel[0]); + _armed = rep->armed; + + /* send an update frame */ + _send_needed = true; + + unlock(); + +} + +void +PX4IO::io_send() +{ + lock(); + + /* send packet to IO while we're guaranteed it won't change */ + hx_stream_send(_io_stream, &_next_command, sizeof(_next_command)); + + unlock(); +} + +ssize_t +PX4IO::write(struct file *filp, const char *buffer, size_t len) +{ + unsigned channels = len / sizeof(servo_position_t); + servo_position_t *pdata = (servo_position_t *)buffer; + unsigned i; + + if (channels > PX4IO_OUTPUT_CHANNELS) + return -EIO; + + lock(); + for (i = 0; i < channels; i++) + _next_command.servo_command[i] = pdata[i]; + unlock(); + + return i * sizeof(servo_position_t); +} + +int +PX4IO::ioctl(struct file *filep, int cmd, unsigned long arg) +{ + int ret = -ENOTTY; + + lock(); + + /* regular ioctl? */ + switch (cmd) { + case PWM_SERVO_ARM: + _next_command.arm_ok = true; + ret = 0; + break; + + case PWM_SERVO_DISARM: + _next_command.arm_ok = false; + ret = 0; + break; + + default: + /* channel set? */ + if ((cmd >= PWM_SERVO_SET(0)) && (cmd < PWM_SERVO_SET(PX4IO_OUTPUT_CHANNELS))) { + /* XXX sanity-check value? */ + _next_command.servo_command[cmd - PWM_SERVO_SET(0)] = arg; + ret = 0; + break; + } + + /* channel get? */ + if ((cmd >= PWM_SERVO_GET(0)) && (cmd < PWM_SERVO_GET(PX4IO_INPUT_CHANNELS))) { + int channel = cmd - PWM_SERVO_GET(0); + + /* currently no data for this channel */ + if (channel >= _rc_channel_count) { + ret = -ERANGE; + break; + } + + *(servo_position_t *)arg = _rc_channel[channel]; + ret = 0; + break; + } + + /* 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) { + fprintf(stderr, "PX4IO: already loaded\n"); + return -EBUSY; + } + + /* create the driver - it will set g_dev */ + (void)new PX4IO; + + if (g_dev == nullptr) { + fprintf(stderr, "PX4IO: driver alloc failed\n"); + return -ENOMEM; + } + + if (OK != g_dev->init()) { + fprintf(stderr, "PX4IO: driver init failed\n"); + delete g_dev; + return -EIO; + } + + return OK; + } + + 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: + fprintf(stderr, "PX4IO firmware file '%s' not found\n", fn); + break; + case -EEXIST: + case -EIO: + fprintf(stderr, "error updating PX4IO - check that bootloader mode is enabled\n"); + break; + case -EINVAL: + fprintf(stderr, "verify failed - retry the update\n"); + break; + case -ETIMEDOUT: + fprintf(stderr, "timed out waiting for bootloader - power-cycle and try again\n"); + default: + fprintf(stderr, "unexpected error %d\n", ret); + break; + } + return ret; + } + + + + printf("need a verb, only support 'start'\n"); + return ERROR; +} diff --git a/apps/px4/px4io/driver/uploader.cpp b/apps/px4/px4io/driver/uploader.cpp new file mode 100644 index 000000000..7d6a9e171 --- /dev/null +++ b/apps/px4/px4io/driver/uploader.cpp @@ -0,0 +1,387 @@ +/**************************************************************************** + * + * 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", 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 new file mode 100644 index 000000000..8d41992f8 --- /dev/null +++ b/apps/px4/px4io/driver/uploader.h @@ -0,0 +1,99 @@ +/**************************************************************************** + * + * 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 |