diff options
-rw-r--r-- | ROMFS/px4fmu_common/init.d/rc.sensors | 35 | ||||
-rw-r--r-- | src/drivers/device/device.h | 5 | ||||
-rw-r--r-- | src/drivers/hmc5883/hmc5883.cpp | 9 | ||||
-rw-r--r-- | src/drivers/px4io/px4io_uploader.cpp | 51 | ||||
-rw-r--r-- | src/drivers/px4io/uploader.h | 1 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_commands.cpp | 7 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_ftp.cpp | 16 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_main.cpp | 10 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_main.h | 4 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_mission.h | 4 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_orb_subscription.h | 4 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 12 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.h | 4 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_stream.h | 4 | ||||
-rw-r--r-- | src/modules/mavlink/module.mk | 2 | ||||
-rw-r--r-- | src/modules/sensors/sensor_params.c | 13 |
16 files changed, 123 insertions, 58 deletions
diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index be54ea98b..121dc89d3 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 +if mpu6000 start then - echo "[init] Using HMC5883" + echo "Internal MPU6000" fi -if mpu6000 start +if l3gd20 start then - echo "[init] Using MPU6000" + echo "Internal L3GD20(H)" fi -if l3gd20 start +# MAG selection +if param compare SENS_EXT_MAG 2 then - echo "[init] Using L3GD20(H)" + 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 813e1542b..9d684e394 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 26014c6d8..f229ecc32 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 d134c0246..bf6893a7e 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 55f63eef9..3e2142cf2 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 fccd4d9a5..b502c3c86 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 675a6870e..6a2c900af 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<kMaxSession; i++) { - _session_fds[i] = -1; - } + + // initialize session list + for (size_t i=0; i<kMaxSession; i++) { + _session_fds[i] = -1; + } // drop work entries onto the free list for (unsigned i = 0; i < kRequestQueueSize; i++) { diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index cd0581af4..76b5459a3 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -216,6 +216,7 @@ Mavlink::Mavlink() : _device_name(DEFAULT_DEVICE_NAME), _task_should_exit(false), next(nullptr), + _instance_id(0), _mavlink_fd(-1), _task_running(false), _hil_enabled(false), @@ -230,17 +231,24 @@ Mavlink::Mavlink() : _mission_pub(-1), _mission_result_sub(-1), _mode(MAVLINK_MODE_NORMAL), + _channel(MAVLINK_COMM_0), + _logbuffer{}, _total_counter(0), + _receive_thread{}, _verbose(false), _forwarding_on(false), _passing_on(false), _ftp_on(false), _uart_fd(-1), + _baudrate(57600), + _datarate(10000), _mavlink_param_queue_index(0), + mavlink_link_termination_allowed(false), _subscribe_to_stream(nullptr), _subscribe_to_stream_rate(0.0f), _flow_control_enabled(true), - _message_buffer({}), + _message_buffer{}, + _message_buffer_mutex{}, _param_initialized(false), _param_system_id(0), _param_component_id(0), diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index acfc8b90e..da989d6c5 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -372,4 +372,8 @@ private: * Main mavlink task. */ int task_main(int argc, char *argv[]); + + /* do not allow copying this class */ + Mavlink(const Mavlink&); + Mavlink operator=(const Mavlink&); }; diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h index f63c32f24..b792b9aaf 100644 --- a/src/modules/mavlink/mavlink_mission.h +++ b/src/modules/mavlink/mavlink_mission.h @@ -174,4 +174,8 @@ private: mavlink_channel_t _channel; uint8_t _comp_id; + + /* do not allow copying this class */ + MavlinkMissionManager(const MavlinkMissionManager&); + MavlinkMissionManager operator=(const MavlinkMissionManager&); }; diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h index 71efb43af..7af454df6 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.h +++ b/src/modules/mavlink/mavlink_orb_subscription.h @@ -82,6 +82,10 @@ private: const orb_id_t _topic; ///< topic metadata int _fd; ///< subscription handle bool _published; ///< topic was ever published + + /* do not allow copying this class */ + MavlinkOrbSubscription(const MavlinkOrbSubscription&); + MavlinkOrbSubscription operator=(const MavlinkOrbSubscription&); }; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 75d3b6ce4..3ee7ec34d 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -86,7 +86,9 @@ static const float mg2ms2 = CONSTANTS_ONE_G / 1000.0f; MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _mavlink(parent), - + status{}, + hil_local_pos{}, + _control_mode{}, _global_pos_pub(-1), _local_pos_pub(-1), _attitude_pub(-1), @@ -111,15 +113,13 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _manual_pub(-1), _telemetry_heartbeat_time(0), _radio_status_available(false), - _control_mode_sub(-1), + _control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))), _hil_frames(0), _old_timestamp(0), _hil_local_proj_inited(0), - _hil_local_alt0(0.0) + _hil_local_alt0(0.0f), + _hil_local_proj_ref{} { - _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - memset(&hil_local_pos, 0, sizeof(hil_local_pos)); - memset(&_control_mode, 0, sizeof(_control_mode)); // make sure the FTP server is started (void)MavlinkFTP::getServer(); diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 8d38b9973..b64a060e3 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -156,4 +156,8 @@ private: bool _hil_local_proj_inited; float _hil_local_alt0; struct map_projection_reference_s _hil_local_proj_ref; + + /* do not allow copying this class */ + MavlinkReceiver(const MavlinkReceiver&); + MavlinkReceiver operator=(const MavlinkReceiver&); }; diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index 69809a386..20e1c7c44 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/src/modules/mavlink/mavlink_stream.h @@ -77,6 +77,10 @@ protected: private: hrt_abstime _last_sent; + + /* do not allow top copying this class */ + MavlinkStream(const MavlinkStream&); + MavlinkStream& operator=(const MavlinkStream&); }; diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index d49bbb7f7..1986ae3c8 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -52,3 +52,5 @@ INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink MAXOPTIMIZATION = -Os MODULE_STACKSIZE = 1024 + +EXTRACXXFLAGS = -Weffc++ diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 38b190761..7ce6ef5ef 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -292,6 +292,19 @@ PARAM_DEFINE_FLOAT(SENS_BOARD_Z_OFF, 0.0f); */ PARAM_DEFINE_INT32(SENS_EXT_MAG_ROT, 0); +/** +* Set usage of external magnetometer +* +* * Set to 0 (default) to auto-detect (will try to get the external as primary) +* * Set to 1 to force the external magnetometer as primary +* * Set to 2 to force the internal magnetometer as primary +* +* @min 0 +* @max 2 +* @group Sensor Calibration +*/ +PARAM_DEFINE_INT32(SENS_EXT_MAG, 0); + /** * RC Channel 1 Minimum |