aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorLorenz Meier <lm@inf.ethz.ch>2014-07-16 15:00:58 +0200
committerLorenz Meier <lm@inf.ethz.ch>2014-07-16 15:00:58 +0200
commit11eeb7466d80452f18fd036cc72899a2ddbd33e9 (patch)
tree87a79ae904d3c7b4e105ff1e53bb8ce255bc42ff
parent43bc2c3ef2a867a015e7198c797d089d6252fdde (diff)
parentc6c9c49823a4c19e156f4ce70bde781890ab04f9 (diff)
downloadpx4-firmware-11eeb7466d80452f18fd036cc72899a2ddbd33e9.tar.gz
px4-firmware-11eeb7466d80452f18fd036cc72899a2ddbd33e9.tar.bz2
px4-firmware-11eeb7466d80452f18fd036cc72899a2ddbd33e9.zip
Merge branch 'ext_mag_param' into logging
-rw-r--r--ROMFS/px4fmu_common/init.d/rc.sensors35
-rw-r--r--src/drivers/device/device.h5
-rw-r--r--src/drivers/hmc5883/hmc5883.cpp9
-rw-r--r--src/drivers/px4io/px4io_uploader.cpp51
-rw-r--r--src/drivers/px4io/uploader.h1
-rw-r--r--src/modules/mavlink/mavlink_commands.cpp7
-rw-r--r--src/modules/mavlink/mavlink_ftp.cpp16
-rw-r--r--src/modules/mavlink/mavlink_main.cpp10
-rw-r--r--src/modules/mavlink/mavlink_main.h4
-rw-r--r--src/modules/mavlink/mavlink_mission.h4
-rw-r--r--src/modules/mavlink/mavlink_orb_subscription.h4
-rw-r--r--src/modules/mavlink/mavlink_receiver.cpp12
-rw-r--r--src/modules/mavlink/mavlink_receiver.h4
-rw-r--r--src/modules/mavlink/mavlink_stream.h4
-rw-r--r--src/modules/mavlink/module.mk2
-rw-r--r--src/modules/sensors/sensor_params.c13
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