diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index be54ea98b0..121dc89d3b 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -6,28 +6,45 @@ ms5611 start adc start -# Mag might be external -if hmc5883 start -then - echo "[init] Using HMC5883" -fi - if mpu6000 start then - echo "[init] Using MPU6000" + echo "Internal MPU6000" fi if l3gd20 start then - echo "[init] Using L3GD20(H)" + echo "Internal L3GD20(H)" +fi + +# MAG selection +if param compare SENS_EXT_MAG 2 +then + if hmc5883 -I start + then + echo "Internal HMC5883" + fi +else + # Use only external as primary + if param compare SENS_EXT_MAG 1 + then + if hmc5883 -X start + then + echo "External HMC5883" + fi + else + # auto-detect the primary, prefer external + if hmc5883 start + then + echo "Default HMC5883" + fi + fi fi if ver hwcmp PX4FMU_V2 then - # IMPORTANT: EXTERNAL BUSES SHOULD BE SCANNED FIRST if lsm303d start then - echo "[init] Using LSM303D" + echo "Internal LSM303D" fi fi diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h index 813e1542bb..9d684e3948 100644 --- a/src/drivers/device/device.h +++ b/src/drivers/device/device.h @@ -240,6 +240,7 @@ private: * @param context Pointer to the interrupted context. */ static void dev_interrupt(int irq, void *context); + }; /** @@ -469,6 +470,10 @@ private: * @return OK, or -errno on error. */ int remove_poll_waiter(struct pollfd *fds); + + /* do not allow copying this class */ + CDev(const CDev&); + CDev operator=(const CDev&); }; /** diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 26014c6d8b..f229ecc325 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1358,8 +1358,8 @@ namespace hmc5883 #endif const int ERROR = -1; -HMC5883 *g_dev_int; -HMC5883 *g_dev_ext; +HMC5883 *g_dev_int = nullptr; +HMC5883 *g_dev_ext = nullptr; void start(int bus, enum Rotation rotation); void test(int bus); @@ -1395,6 +1395,11 @@ start(int bus, enum Rotation rotation) errx(0, "already started internal"); g_dev_int = new HMC5883(PX4_I2C_BUS_ONBOARD, HMC5883L_DEVICE_PATH_INT, rotation); if (g_dev_int != nullptr && OK != g_dev_int->init()) { + + /* tear down the failing onboard instance */ + delete g_dev_int; + g_dev_int = nullptr; + if (bus == PX4_I2C_BUS_ONBOARD) { goto fail; } diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index d134c0246e..bf6893a7e5 100644 --- a/src/drivers/px4io/px4io_uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -204,12 +204,8 @@ PX4IO_Uploader::upload(const char *filenames[]) if (bl_rev <= 2) { ret = verify_rev2(fw_size); - } else if(bl_rev == 3) { - ret = verify_rev3(fw_size); } else { - /* verify rev 4 and higher still uses the same approach and - * every version *needs* to be verified. - */ + /* verify rev 3 and higher. Every version *needs* to be verified. */ ret = verify_rev3(fw_size); } @@ -276,14 +272,14 @@ PX4IO_Uploader::recv(uint8_t &c, unsigned timeout) int PX4IO_Uploader::recv(uint8_t *p, unsigned count) { + int ret; while (count--) { - int ret = recv(*p++, 5000); + ret = recv(*p++, 5000); if (ret != OK) - return ret; + break; } - - return OK; + return ret; } void @@ -314,21 +310,19 @@ PX4IO_Uploader::send(uint8_t c) #endif if (write(_io_fd, &c, 1) != 1) return -errno; - return OK; } int PX4IO_Uploader::send(uint8_t *p, unsigned count) { + int ret; while (count--) { - int ret = send(*p++); - + ret = send(*p++); if (ret != OK) - return ret; + break; } - - return OK; + return ret; } int @@ -419,12 +413,15 @@ PX4IO_Uploader::program(size_t fw_size) int ret; size_t sent = 0; - file_buf = (uint8_t *)malloc(PROG_MULTI_MAX); + file_buf = new uint8_t[PROG_MULTI_MAX]; if (!file_buf) { log("Can't allocate program buffer"); return -ENOMEM; } + ASSERT((fw_size & 3) == 0); + ASSERT((PROG_MULTI_MAX & 3) == 0); + log("programming %u bytes...", (unsigned)fw_size); ret = lseek(_fw_fd, 0, SEEK_SET); @@ -443,34 +440,26 @@ PX4IO_Uploader::program(size_t fw_size) (unsigned)sent, (int)count, (int)errno); - } - - if (count == 0) { - free(file_buf); - return OK; + ret = -errno; + break; } sent += count; - if (count < 0) - return -errno; - - ASSERT((count % 4) == 0); - send(PROTO_PROG_MULTI); send(count); - send(&file_buf[0], count); + send(file_buf, count); send(PROTO_EOC); ret = get_sync(1000); if (ret != OK) { - free(file_buf); - return ret; + break; } } - free(file_buf); - return OK; + + delete [] file_buf; + return ret; } int diff --git a/src/drivers/px4io/uploader.h b/src/drivers/px4io/uploader.h index 55f63eef96..3e2142cf28 100644 --- a/src/drivers/px4io/uploader.h +++ b/src/drivers/px4io/uploader.h @@ -75,7 +75,6 @@ private: 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 */ }; diff --git a/src/modules/mavlink/mavlink_commands.cpp b/src/modules/mavlink/mavlink_commands.cpp index fccd4d9a59..b502c3c863 100644 --- a/src/modules/mavlink/mavlink_commands.cpp +++ b/src/modules/mavlink/mavlink_commands.cpp @@ -40,9 +40,12 @@ #include "mavlink_commands.h" -MavlinkCommandsStream::MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel) : _channel(channel), _cmd_time(0) +MavlinkCommandsStream::MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel) : + _cmd_sub(mavlink->add_orb_subscription(ORB_ID(vehicle_command))), + _cmd{}, + _channel(channel), + _cmd_time(0) { - _cmd_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_command)); } void diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 675a6870e1..6a2c900afe 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -50,16 +50,20 @@ MavlinkFTP::getServer() return _server; } -MavlinkFTP::MavlinkFTP() +MavlinkFTP::MavlinkFTP() : + _session_fds{}, + _workBufs{}, + _workFree{}, + _lock{} { // initialise the request freelist dq_init(&_workFree); sem_init(&_lock, 0, 1); - - // initialize session list - for (size_t i=0; i