diff --git a/ROMFS/Makefile b/ROMFS/Makefile index 3b024de06d..d827fa4915 100644 --- a/ROMFS/Makefile +++ b/ROMFS/Makefile @@ -23,7 +23,8 @@ ROMFS_FSSPEC := $(SRCROOT)/scripts/rcS~init.d/rcS \ $(SRCROOT)/scripts/rc.PX4IOAR~init.d/rc.PX4IOAR \ $(SRCROOT)/scripts/rc.FMU_quad_x~init.d/rc.FMU_quad_x \ $(SRCROOT)/mixers/FMU_pass.mix~mixers/FMU_pass.mix \ - $(SRCROOT)/mixers/FMU_delta.mix~mixers/FMU_delta.mix \ + $(SRCROOT)/mixers/FMU_Q.mix~mixers/FMU_Q.mix \ + $(SRCROOT)/mixers/FMU_X5.mix~mixers/FMU_X5.mix \ $(SRCROOT)/mixers/FMU_AERT.mix~mixers/FMU_AERT.mix \ $(SRCROOT)/mixers/FMU_AET.mix~mixers/FMU_AET.mix \ $(SRCROOT)/mixers/FMU_RET.mix~mixers/FMU_ERT.mix \ diff --git a/ROMFS/mixers/FMU_Q.mix b/ROMFS/mixers/FMU_Q.mix new file mode 100644 index 0000000000..a35d299fda --- /dev/null +++ b/ROMFS/mixers/FMU_Q.mix @@ -0,0 +1,52 @@ +Delta-wing mixer for PX4FMU +=========================== + +Designed for Bormatec Camflyer Q + +This file defines mixers suitable for controlling a delta wing aircraft using +PX4FMU. The configuration assumes the elevon servos are connected to PX4FMU +servo outputs 0 and 1 and the motor speed control to output 3. Output 2 is +assumed to be unused. + +Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 +(roll), 1 (pitch) and 3 (thrust). + +See the README for more information on the scaler format. + +Elevon mixers +------------- +Three scalers total (output, roll, pitch). + +On the assumption that the two elevon servos are physically reversed, the pitch +input is inverted between the two servos. + +The scaling factor for roll inputs is adjusted to implement differential travel +for the elevons. + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 0 -5000 -8000 0 -10000 10000 +S: 0 1 -8000 -8000 0 -10000 10000 + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 0 -8000 -5000 0 -10000 10000 +S: 0 1 8000 8000 0 -10000 10000 + +Output 2 +-------- +This mixer is empty. + +Z: + +Motor speed mixer +----------------- +Two scalers total (output, thrust). + +This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) +range. Inputs below zero are treated as zero. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 3 0 20000 -10000 -10000 10000 + diff --git a/ROMFS/mixers/FMU_delta.mix b/ROMFS/mixers/FMU_X5.mix similarity index 100% rename from ROMFS/mixers/FMU_delta.mix rename to ROMFS/mixers/FMU_X5.mix diff --git a/apps/drivers/px4io/uploader.cpp b/apps/drivers/px4io/uploader.cpp index 2de33f4101..abf59216a6 100644 --- a/apps/drivers/px4io/uploader.cpp +++ b/apps/drivers/px4io/uploader.cpp @@ -48,6 +48,7 @@ #include #include #include +#include #include "uploader.h" @@ -109,6 +110,8 @@ int PX4IO_Uploader::upload(const char *filenames[]) { int ret; + const char *filename = NULL; + size_t fw_size; _io_fd = open("/dev/ttyS2", O_RDWR); @@ -135,9 +138,22 @@ PX4IO_Uploader::upload(const char *filenames[]) } log("using firmware from %s", filenames[i]); + filename = filenames[i]; break; } + if (filename == NULL) { + log("no firmware found"); + return -ENOENT; + } + + struct stat st; + if (stat(filename, &st) != 0) { + log("Failed to stat %s - %d\n", filename, (int)errno); + return -errno; + } + fw_size = st.st_size; + if (_fw_fd == -1) return -ENOENT; @@ -172,7 +188,7 @@ PX4IO_Uploader::upload(const char *filenames[]) continue; } - ret = program(); + ret = program(fw_size); if (ret != OK) { log("program failed"); @@ -180,9 +196,9 @@ PX4IO_Uploader::upload(const char *filenames[]) } if (bl_rev <= 2) - ret = verify_rev2(); + ret = verify_rev2(fw_size); else if(bl_rev == 3) { - ret = verify_rev3(); + ret = verify_rev3(fw_size); } if (ret != OK) { @@ -219,7 +235,7 @@ PX4IO_Uploader::recv(uint8_t &c, unsigned timeout) int ret = ::poll(&fds[0], 1, timeout); if (ret < 1) { - //log("poll timeout %d", ret); + log("poll timeout %d", ret); return -ETIMEDOUT; } @@ -232,7 +248,7 @@ int PX4IO_Uploader::recv(uint8_t *p, unsigned count) { while (count--) { - int ret = recv(*p++); + int ret = recv(*p++, 5000); if (ret != OK) return ret; @@ -248,7 +264,7 @@ PX4IO_Uploader::drain() int ret; do { - ret = recv(c, 250); + ret = recv(c, 1000); if (ret == OK) { //log("discard 0x%02x", c); @@ -343,24 +359,55 @@ PX4IO_Uploader::erase() return get_sync(10000); /* allow 10s timeout */ } + +static int read_with_retry(int fd, void *buf, size_t n) +{ + int ret; + uint8_t retries = 0; + do { + ret = read(fd, buf, n); + } while (ret == -1 && retries++ < 100); + if (retries != 0) { + printf("read of %u bytes needed %u retries\n", + (unsigned)n, + (unsigned)retries); + } + return ret; +} + int -PX4IO_Uploader::program() +PX4IO_Uploader::program(size_t fw_size) { uint8_t file_buf[PROG_MULTI_MAX]; ssize_t count; int ret; + size_t sent = 0; - log("program..."); - lseek(_fw_fd, 0, SEEK_SET); + log("programming %u bytes...", (unsigned)fw_size); + + ret = lseek(_fw_fd, 0, SEEK_SET); - while (true) { + while (sent < fw_size) { /* get more bytes to program */ - //log(" %d", (int)lseek(_fw_fd, 0, SEEK_CUR)); - count = read(_fw_fd, file_buf, sizeof(file_buf)); + size_t n = fw_size - sent; + if (n > sizeof(file_buf)) { + n = sizeof(file_buf); + } + count = read_with_retry(_fw_fd, file_buf, n); + + if (count != (ssize_t)n) { + log("firmware read of %u bytes at %u failed -> %d errno %d", + (unsigned)n, + (unsigned)sent, + (int)count, + (int)errno); + } if (count == 0) return OK; + sent += count; + if (count < 0) return -errno; @@ -376,14 +423,16 @@ PX4IO_Uploader::program() if (ret != OK) return ret; } + return OK; } int -PX4IO_Uploader::verify_rev2() +PX4IO_Uploader::verify_rev2(size_t fw_size) { - uint8_t file_buf[PROG_MULTI_MAX]; + uint8_t file_buf[4]; ssize_t count; int ret; + size_t sent = 0; log("verify..."); lseek(_fw_fd, 0, SEEK_SET); @@ -395,14 +444,27 @@ PX4IO_Uploader::verify_rev2() if (ret != OK) return ret; - while (true) { + while (sent < fw_size) { /* get more bytes to verify */ - int base = (int)lseek(_fw_fd, 0, SEEK_CUR); - count = read(_fw_fd, file_buf, sizeof(file_buf)); + size_t n = fw_size - sent; + if (n > sizeof(file_buf)) { + n = sizeof(file_buf); + } + count = read_with_retry(_fw_fd, file_buf, n); + + if (count != (ssize_t)n) { + log("firmware read of %u bytes at %u failed -> %d errno %d", + (unsigned)n, + (unsigned)sent, + (int)count, + (int)errno); + } if (count == 0) break; + sent += count; + if (count < 0) return -errno; @@ -415,15 +477,15 @@ PX4IO_Uploader::verify_rev2() for (ssize_t i = 0; i < count; i++) { uint8_t c; - ret = recv(c); + ret = recv(c, 5000); if (ret != OK) { - log("%d: got %d waiting for bytes", base + i, ret); + log("%d: got %d waiting for bytes", sent + i, ret); return ret; } if (c != file_buf[i]) { - log("%d: got 0x%02x expected 0x%02x", base + i, c, file_buf[i]); + log("%d: got 0x%02x expected 0x%02x", sent + i, c, file_buf[i]); return -EINVAL; } } @@ -440,21 +502,21 @@ PX4IO_Uploader::verify_rev2() } int -PX4IO_Uploader::verify_rev3() +PX4IO_Uploader::verify_rev3(size_t fw_size_local) { int ret; uint8_t file_buf[4]; ssize_t count; uint32_t sum = 0; uint32_t bytes_read = 0; - uint32_t fw_size = 0; uint32_t crc = 0; + uint32_t fw_size_remote; uint8_t fill_blank = 0xff; log("verify..."); lseek(_fw_fd, 0, SEEK_SET); - ret = get_info(INFO_FLASH_SIZE, fw_size); + ret = get_info(INFO_FLASH_SIZE, fw_size_remote); send(PROTO_EOC); if (ret != OK) { @@ -463,9 +525,20 @@ PX4IO_Uploader::verify_rev3() } /* read through the firmware file again and calculate the checksum*/ - while (true) { - lseek(_fw_fd, 0, SEEK_CUR); - count = read(_fw_fd, file_buf, sizeof(file_buf)); + while (bytes_read < fw_size_local) { + size_t n = fw_size_local - bytes_read; + if (n > sizeof(file_buf)) { + n = sizeof(file_buf); + } + count = read_with_retry(_fw_fd, file_buf, n); + + if (count != (ssize_t)n) { + log("firmware read of %u bytes at %u failed -> %d errno %d", + (unsigned)n, + (unsigned)bytes_read, + (int)count, + (int)errno); + } /* set the rest to ff */ if (count == 0) { @@ -482,7 +555,7 @@ PX4IO_Uploader::verify_rev3() } /* fill the rest with 0xff */ - while (bytes_read < fw_size) { + while (bytes_read < fw_size_remote) { sum = crc32(&fill_blank, sizeof(fill_blank), sum); bytes_read += sizeof(fill_blank); } @@ -516,36 +589,6 @@ PX4IO_Uploader::reboot() 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, ...) { @@ -557,4 +600,4 @@ PX4IO_Uploader::log(const char *fmt, ...) va_end(ap); printf("\n"); fflush(stdout); -} \ No newline at end of file +} diff --git a/apps/drivers/px4io/uploader.h b/apps/drivers/px4io/uploader.h index b8a3a27948..915ee92597 100644 --- a/apps/drivers/px4io/uploader.h +++ b/apps/drivers/px4io/uploader.h @@ -85,7 +85,7 @@ private: void log(const char *fmt, ...); - int recv(uint8_t &c, unsigned timeout = 1000); + int recv(uint8_t &c, unsigned timeout); int recv(uint8_t *p, unsigned count); void drain(); int send(uint8_t c); @@ -94,11 +94,10 @@ private: int sync(); int get_info(int param, uint32_t &val); int erase(); - int program(); - int verify_rev2(); - int verify_rev3(); + int program(size_t fw_size); + int verify_rev2(size_t fw_size); + int verify_rev3(size_t fw_size); int reboot(); - int compare(bool &identical); }; -#endif \ No newline at end of file +#endif