Merged fmuv2_bringup

7 files changed, 46 insertions, 30 deletions

diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk
index bfccb2d38..5d3d6a73c 100644
--- a/src/modules/mavlink/module.mk
+++ b/src/modules/mavlink/module.mk
@@ -39,7 +39,6 @@ MODULE_COMMAND	 = mavlink
 SRCS		 += mavlink.c \
 			   missionlib.c \
 			   mavlink_parameters.c \
-			   mavlink_log.c \
 			   mavlink_receiver.cpp \
 			   orb_listener.c \
 			   waypoints.c
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index 38727c68c..deed25836 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -148,7 +148,8 @@ mixer_tick(void)
 
 		if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
 		     (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
-		     (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
+		     (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) &&
+		     !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED)) {
 
 		 	/* if allowed, mix from RC inputs directly */
 			source = MIX_OVERRIDE;
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index 97809d9c2..f5fa0ba75 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -74,7 +74,7 @@
 #define REG_TO_FLOAT(_reg)	((float)REG_TO_SIGNED(_reg) / 10000.0f)
 #define FLOAT_TO_REG(_float)	SIGNED_TO_REG((int16_t)((_float) * 10000.0f))
 
-#define PX4IO_PROTOCOL_VERSION		3
+#define PX4IO_PROTOCOL_VERSION		4
 
 /* static configuration page */
 #define PX4IO_PAGE_CONFIG		0
@@ -159,6 +159,7 @@
 #define PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM	(1 << 3) /* use custom failsafe values, not 0 values of mixer */
 #define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK	(1 << 4) /* OK to try in-air restart */
 #define PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE	(1 << 5) /* Output of PWM right after startup enabled to help ESCs initialize and prevent them from beeping */
+#define PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED	(1 << 6) /* Disable the IO-internal evaluation of the RC */
 
 #define PX4IO_P_SETUP_PWM_RATES			2	/* bitmask, 0 = low rate, 1 = high rate */
 #define PX4IO_P_SETUP_PWM_DEFAULTRATE		3	/* 'low' PWM frame output rate in Hz */
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 3ff9307cd..655a0c7a8 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -158,9 +158,10 @@ volatile uint16_t	r_page_setup[] =
 #define PX4IO_P_SETUP_ARMING_VALID	(PX4IO_P_SETUP_ARMING_FMU_ARMED | \
 					 PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \
 					 PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \
-					 PX4IO_P_SETUP_ARMING_IO_ARM_OK) | \
+					 PX4IO_P_SETUP_ARMING_IO_ARM_OK | \
 					 PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \
-					 PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE
+					 PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | \
+					 PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED)
 #define PX4IO_P_SETUP_RATES_VALID	((1 << PX4IO_SERVO_COUNT) - 1)
 #define PX4IO_P_SETUP_RELAYS_VALID	((1 << PX4IO_RELAY_CHANNELS) - 1)
 
@@ -432,6 +433,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			// 	r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
 			// }
 
+			if (value & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) {
+				r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK;
+			}
+
 			r_setup_arming = value;
 
 			break;
@@ -523,6 +528,9 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			value &= PX4IO_P_RC_CONFIG_OPTIONS_VALID;
 			r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK;
 
+			/* clear any existing RC disabled flag */
+			r_setup_arming &= ~(PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED);
+
 			/* set all options except the enabled option */
 			conf[index] = value & ~PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
 
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index b237bd059..ded39c465 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -912,11 +912,11 @@ Sensors::gyro_init()
 
 		#else
 
-		/* set the gyro internal sampling rate up to at leat 800Hz */
-		ioctl(fd, GYROIOCSSAMPLERATE, 800);
+		/* set the gyro internal sampling rate up to at least 760Hz */
+		ioctl(fd, GYROIOCSSAMPLERATE, 760);
 
-		/* set the driver to poll at 800Hz */
-		ioctl(fd, SENSORIOCSPOLLRATE, 800);
+		/* set the driver to poll at 760Hz */
+		ioctl(fd, SENSORIOCSPOLLRATE, 760);
 
 		#endif
 
@@ -929,6 +929,7 @@ void
 Sensors::mag_init()
 {
 	int	fd;
+	int	ret;
 
 	fd = open(MAG_DEVICE_PATH, 0);
 
@@ -937,13 +938,27 @@ Sensors::mag_init()
 		errx(1, "FATAL: no magnetometer found");
 	}
 
-	/* set the mag internal poll rate to at least 150Hz */
-	ioctl(fd, MAGIOCSSAMPLERATE, 150);
+	/* try different mag sampling rates */
 
-	/* set the driver to poll at 150Hz */
-	ioctl(fd, SENSORIOCSPOLLRATE, 150);
 
-	int ret = ioctl(fd, MAGIOCGEXTERNAL, 0);
+	ret = ioctl(fd, MAGIOCSSAMPLERATE, 150);
+	if (ret == OK) {
+		/* set the pollrate accordingly */
+		ioctl(fd, SENSORIOCSPOLLRATE, 150);
+	} else {
+		ret = ioctl(fd, MAGIOCSSAMPLERATE, 100);
+		/* if the slower sampling rate still fails, something is wrong */
+		if (ret == OK) {
+			/* set the driver to poll also at the slower rate */
+			ioctl(fd, SENSORIOCSPOLLRATE, 100);
+		} else {
+			errx(1, "FATAL: mag sampling rate could not be set");
+		}
+	}
+
+	
+
+	ret = ioctl(fd, MAGIOCGEXTERNAL, 0);
 	if (ret < 0)
 		errx(1, "FATAL: unknown if magnetometer is external or onboard");
 	else if (ret == 1)
diff --git a/src/modules/mavlink/mavlink_log.c b/src/modules/systemlib/mavlink_log.c
index 192195856..27608bdbf 100644
--- a/src/modules/mavlink/mavlink_log.c
+++ b/src/modules/systemlib/mavlink_log.c
@@ -46,28 +46,25 @@
 
 #include <mavlink/mavlink_log.h>
 
-static FILE* text_recorder_fd = NULL;
-
-void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size)
+__EXPORT void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size)
 {
 	lb->size  = size;
 	lb->start = 0;
 	lb->count = 0;
 	lb->elems = (struct mavlink_logmessage *)calloc(lb->size, sizeof(struct mavlink_logmessage));
-	text_recorder_fd = fopen("/fs/microsd/text_recorder.txt", "w");
 }
 
-int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb)
+__EXPORT int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb)
 {
 	return lb->count == (int)lb->size;
 }
 
-int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb)
+__EXPORT int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb)
 {
 	return lb->count == 0;
 }
 
-void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_logmessage *elem)
+__EXPORT void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_logmessage *elem)
 {
 	int end = (lb->start + lb->count) % lb->size;
 	memcpy(&(lb->elems[end]), elem, sizeof(struct mavlink_logmessage));
@@ -80,19 +77,13 @@ void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_
 	}
 }
 
-int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem)
+__EXPORT int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem)
 {
 	if (!mavlink_logbuffer_is_empty(lb)) {
 		memcpy(elem, &(lb->elems[lb->start]), sizeof(struct mavlink_logmessage));
 		lb->start = (lb->start + 1) % lb->size;
 		--lb->count;
 
-		if (text_recorder_fd) {
-			fwrite(elem->text, 1, strnlen(elem->text, 50), text_recorder_fd);
-			fputc("\n", text_recorder_fd);
-			fsync(text_recorder_fd);
-		}
-
 		return 0;
 
 	} else {
@@ -100,7 +91,7 @@ int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessa
 	}
 }
 
-void mavlink_logbuffer_vasprintf(struct mavlink_logbuffer *lb, int severity, const char *fmt, ...)
+__EXPORT void mavlink_logbuffer_vasprintf(struct mavlink_logbuffer *lb, int severity, const char *fmt, ...)
 {
 	va_list ap;
 	va_start(ap, fmt);
diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk
index b470c1227..cbf829122 100644
--- a/src/modules/systemlib/module.mk
+++ b/src/modules/systemlib/module.mk
@@ -48,4 +48,5 @@ SRCS		 = err.c \
 		   geo/geo.c \
 		   systemlib.c \
 		   airspeed.c \
-		   system_params.c
+		   system_params.c \
+		   mavlink_log.c