Merge branch 'mathlib_new' into vector_control2

ROMFS/px4fmu_logging/init.d/rcS

@@ -0,0 +1,88 @@
+#!nsh
+#
+# PX4FMU startup script for logging purposes
+#
+
+#
+# Try to mount the microSD card.
+#
+echo "[init] looking for microSD..."
+if mount -t vfat /dev/mmcsd0 /fs/microsd
+then
+	echo "[init] card mounted at /fs/microsd"
+	# Start playing the startup tune
+	tone_alarm start
+else
+	echo "[init] no microSD card found"
+	# Play SOS
+	tone_alarm error
+fi
+
+uorb start
+
+#
+# Start sensor drivers here.
+#
+
+ms5611 start
+adc start
+
+# mag might be external
+if hmc5883 start
+then
+	echo "using HMC5883"
+fi
+
+if mpu6000 start
+then
+	echo "using MPU6000"
+fi
+
+if l3gd20 start
+then
+	echo "using L3GD20(H)"
+fi
+
+if lsm303d start
+then
+	set BOARD fmuv2
+else
+	set BOARD fmuv1
+fi
+
+# Start airspeed sensors
+if meas_airspeed start
+then
+	echo "using MEAS airspeed sensor"
+else
+	if ets_airspeed start
+	then
+		echo "using ETS airspeed sensor (bus 3)"
+	else
+		if ets_airspeed start -b 1
+		then
+			echo "Using ETS airspeed sensor (bus 1)"
+		fi
+	fi
+fi
+
+#
+# Start the sensor collection task.
+# IMPORTANT: this also loads param offsets
+# ALWAYS start this task before the
+# preflight_check.
+#
+if sensors start
+then
+	echo "SENSORS STARTED"
+fi
+
+sdlog2 start -r 250 -e -b 16
+
+if sercon
+then
+	echo "[init] USB interface connected"
+
+	# Try to get an USB console
+	nshterm /dev/ttyACM0 &
+fi

ROMFS/px4fmu_test/init.d/rcS

@@ -2,3 +2,11 @@
 #
 # PX4FMU startup script for test hackery.
 #
+
+if sercon
+then
+	echo "[init] USB interface connected"
+
+	# Try to get an USB console
+	nshterm /dev/ttyACM0 &
+fi

makefiles/config_px4fmu-v2_logging.mk

@@ -0,0 +1,157 @@
+#
+# Makefile for the px4fmu_default configuration
+#
+
+#
+# Use the configuration's ROMFS, copy the px4iov2 firmware into
+# the ROMFS if it's available
+#
+ROMFS_ROOT	 = $(PX4_BASE)/ROMFS/px4fmu_logging
+ROMFS_OPTIONAL_FILES = $(PX4_BASE)/Images/px4io-v2_default.bin
+
+#
+# Board support modules
+#
+MODULES		+= drivers/device
+MODULES		+= drivers/stm32
+MODULES		+= drivers/stm32/adc
+MODULES		+= drivers/stm32/tone_alarm
+MODULES		+= drivers/led
+MODULES		+= drivers/px4fmu
+MODULES		+= drivers/px4io
+MODULES		+= drivers/boards/px4fmu-v2
+MODULES		+= drivers/rgbled
+MODULES		+= drivers/mpu6000
+MODULES		+= drivers/lsm303d
+MODULES		+= drivers/l3gd20
+MODULES		+= drivers/hmc5883
+MODULES		+= drivers/ms5611
+MODULES		+= drivers/mb12xx
+MODULES		+= drivers/gps
+MODULES		+= drivers/hil
+MODULES		+= drivers/hott/hott_telemetry
+MODULES		+= drivers/hott/hott_sensors
+MODULES		+= drivers/blinkm
+MODULES		+= drivers/roboclaw
+MODULES		+= drivers/airspeed
+MODULES		+= drivers/ets_airspeed
+MODULES		+= drivers/meas_airspeed
+MODULES		+= modules/sensors
+
+# Needs to be burned to the ground and re-written; for now,
+# just don't build it.
+#MODULES		+= drivers/mkblctrl
+
+#
+# System commands
+#
+MODULES		+= systemcmds/ramtron
+MODULES		+= systemcmds/bl_update
+MODULES		+= systemcmds/boardinfo
+MODULES		+= systemcmds/mixer
+MODULES		+= systemcmds/param
+MODULES		+= systemcmds/perf
+MODULES		+= systemcmds/preflight_check
+MODULES		+= systemcmds/pwm
+MODULES		+= systemcmds/esc_calib
+MODULES		+= systemcmds/reboot
+MODULES		+= systemcmds/top
+MODULES		+= systemcmds/tests
+MODULES		+= systemcmds/config
+MODULES		+= systemcmds/nshterm
+
+#
+# General system control
+#
+MODULES		+= modules/commander
+MODULES		+= modules/navigator
+MODULES		+= modules/mavlink
+MODULES		+= modules/mavlink_onboard
+
+#
+# Estimation modules (EKF/ SO3 / other filters)
+#
+MODULES		+= modules/attitude_estimator_ekf
+MODULES		+= modules/attitude_estimator_so3
+MODULES		+= modules/att_pos_estimator_ekf
+MODULES		+= modules/position_estimator_inav
+MODULES		+= examples/flow_position_estimator
+
+#
+# Vehicle Control
+#
+#MODULES		+= modules/segway # XXX Needs GCC 4.7 fix
+MODULES		+= modules/fw_pos_control_l1
+MODULES		+= modules/fw_att_control
+MODULES		+= modules/multirotor_att_control
+MODULES		+= modules/multirotor_pos_control
+
+#
+# Logging
+#
+MODULES		+= modules/sdlog2
+
+#
+# Unit tests
+#
+#MODULES 	+= modules/unit_test
+#MODULES 	+= modules/commander/commander_tests
+
+#
+# Library modules
+#
+MODULES		+= modules/systemlib
+MODULES		+= modules/systemlib/mixer
+MODULES		+= modules/controllib
+MODULES		+= modules/uORB
+
+#
+# Libraries
+#
+LIBRARIES	+= lib/mathlib/CMSIS
+MODULES		+= lib/mathlib
+MODULES		+= lib/mathlib/math/filter
+MODULES		+= lib/ecl
+MODULES		+= lib/external_lgpl
+MODULES		+= lib/geo
+MODULES		+= lib/conversion
+
+#
+# Demo apps
+#
+#MODULES		+= examples/math_demo
+# Tutorial code from
+# https://pixhawk.ethz.ch/px4/dev/hello_sky
+MODULES		+= examples/px4_simple_app
+
+# Tutorial code from
+# https://pixhawk.ethz.ch/px4/dev/daemon
+#MODULES		+= examples/px4_daemon_app
+
+# Tutorial code from
+# https://pixhawk.ethz.ch/px4/dev/debug_values
+#MODULES		+= examples/px4_mavlink_debug
+
+# Tutorial code from
+# https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control
+#MODULES			+= examples/fixedwing_control
+
+# Hardware test
+#MODULES			+= examples/hwtest
+
+#
+# Transitional support - add commands from the NuttX export archive.
+#
+# In general, these should move to modules over time.
+#
+# Each entry here is <command>.<priority>.<stacksize>.<entrypoint> but we use a helper macro
+# to make the table a bit more readable.
+#
+define _B
+	$(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4)
+endef
+
+#                  command                 priority                   stack  entrypoint
+BUILTIN_COMMANDS := \
+	$(call _B, sercon,                 ,                          2048,  sercon_main                ) \
+	$(call _B, serdis,                 ,                          2048,  serdis_main                )

src/drivers/drv_rc_input.h

@@ -60,7 +60,7 @@
 /**
  * Maximum number of R/C input channels in the system. S.Bus has up to 18 channels.
  */
-#define RC_INPUT_MAX_CHANNELS	18
+#define RC_INPUT_MAX_CHANNELS	20
 
 /**
  * Input signal type, value is a control position from zero to 100
@@ -89,7 +89,7 @@ struct rc_input_values {
 	/** number of channels actually being seen */
 	uint32_t		channel_count;
 
-	/** receive signal strength indicator (RSSI): < 0: Undefined, 0: no signal, 1000: full reception */
+	/** receive signal strength indicator (RSSI): < 0: Undefined, 0: no signal, 255: full reception */
 	int32_t			rssi;
 
 	/** Input source */

src/drivers/px4io/px4io.cpp

@@ -237,6 +237,7 @@ private:
 
 	unsigned 		_update_interval;	///< Subscription interval limiting send rate
 	bool			_rc_handling_disabled;	///< If set, IO does not evaluate, but only forward the RC values
+	unsigned		_rc_chan_count;		///< Internal copy of the last seen number of RC channels
 
 	volatile int		_task;			///<worker task id
 	volatile bool		_task_should_exit;	///<worker terminate flag
@@ -244,7 +245,9 @@ private:
 	int			_mavlink_fd;		///<mavlink file descriptor. This is opened by class instantiation and Doesn't appear to be usable in main thread.
 	int			_thread_mavlink_fd;	///<mavlink file descriptor for thread.
 
-	perf_counter_t		_perf_update;		///<local performance counter
+	perf_counter_t		_perf_update;		///<local performance counter for status updates
+	perf_counter_t		_perf_write;		///<local performance counter for PWM control writes
+	perf_counter_t		_perf_chan_count;	///<local performance counter for channel number changes
 
 	/* cached IO state */
 	uint16_t		_status;		///<Various IO status flags
@@ -454,11 +457,14 @@ PX4IO::PX4IO(device::Device *interface) :
 	_max_transfer(16),	/* sensible default */
 	_update_interval(0),
 	_rc_handling_disabled(false),
+	_rc_chan_count(0),
 	_task(-1),
 	_task_should_exit(false),
 	_mavlink_fd(-1),
 	_thread_mavlink_fd(-1),
-	_perf_update(perf_alloc(PC_ELAPSED, "px4io update")),
+	_perf_update(perf_alloc(PC_ELAPSED, "io update")),
+	_perf_write(perf_alloc(PC_ELAPSED, "io write")),
+	_perf_chan_count(perf_alloc(PC_COUNT, "io rc #")),
 	_status(0),
 	_alarms(0),
 	_t_actuators(-1),
@@ -894,7 +900,23 @@ PX4IO::task_main()
 
 				/* re-upload RC input config as it may have changed */
 				io_set_rc_config();
+
+				/* re-set the battery scaling */
+				int32_t voltage_scaling_val;
+				param_t voltage_scaling_param;
+
+				/* set battery voltage scaling */
+				param_get(voltage_scaling_param = param_find("BAT_V_SCALE_IO"), &voltage_scaling_val);
+
+				/* send scaling voltage to IO */
+				uint16_t scaling = voltage_scaling_val;
+				int pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE, &scaling, 1);
+
+				if (pret != OK) {
+					log("voltage scaling upload failed");
+				}
 			}
+
 		}
 
 		perf_end(_perf_update);
@@ -1024,7 +1046,12 @@ PX4IO::io_set_rc_config()
 	if ((ichan >= 0) && (ichan < (int)_max_rc_input))
 		input_map[ichan - 1] = 3;
 
-	ichan = 4;
+	param_get(param_find("RC_MAP_MODE_SW"), &ichan);
+
+	if ((ichan >= 0) && (ichan < (int)_max_rc_input))
+		input_map[ichan - 1] = 4;
+
+	ichan = 5;
 
 	for (unsigned i = 0; i < _max_rc_input; i++)
 		if (input_map[i] == -1)
@@ -1302,6 +1329,11 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
 	 */
 	channel_count = regs[0];
 
+	if (channel_count != _rc_chan_count)
+		perf_count(_perf_chan_count);
+
+	_rc_chan_count = channel_count;
+
 	if (channel_count > 9) {
 		ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + 9, &regs[prolog + 9], channel_count - 9);
 
@@ -1692,6 +1724,16 @@ PX4IO::print_status()
 		printf(" %u", io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + i));
 
 	printf("\n");
+
+	if (raw_inputs > 0) {
+		int frame_len = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_RC_DATA);
+		printf("RC data (PPM frame len) %u us\n", frame_len);
+
+		if ((frame_len - raw_inputs * 2000 - 3000) < 0) {
+			printf("WARNING  WARNING  WARNING! This RC receiver does not allow safe frame detection.\n");
+		}
+	}
+
 	uint16_t mapped_inputs = io_reg_get(PX4IO_PAGE_RC_INPUT, PX4IO_P_RC_VALID);
 	printf("mapped R/C inputs 0x%04x", mapped_inputs);
 
@@ -2150,7 +2192,10 @@ PX4IO::write(file * /*filp*/, const char *buffer, size_t len)
 		count = _max_actuators;
 
 	if (count > 0) {
+
+		perf_begin(_perf_write);
 		int ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, (uint16_t *)buffer, count);
+		perf_end(_perf_write);
 
 		if (ret != OK)
 			return ret;

src/drivers/stm32/drv_hrt.c

@@ -282,6 +282,10 @@ static void		hrt_call_invoke(void);
  * Note that we assume that M3 means STM32F1 (since we don't really care about the F2).
  */
 # ifdef CONFIG_ARCH_CORTEXM3
+#  undef GTIM_CCER_CC1NP
+#  undef GTIM_CCER_CC2NP
+#  undef GTIM_CCER_CC3NP
+#  undef GTIM_CCER_CC4NP
 #  define GTIM_CCER_CC1NP 0
 #  define GTIM_CCER_CC2NP 0
 #  define GTIM_CCER_CC3NP 0
@@ -332,14 +336,20 @@ static void		hrt_call_invoke(void);
 /*
  * PPM decoder tuning parameters
  */
-# define PPM_MAX_PULSE_WIDTH	550		/* maximum width of a valid pulse */
+# define PPM_MAX_PULSE_WIDTH	700		/* maximum width of a valid pulse */
 # define PPM_MIN_CHANNEL_VALUE	800		/* shortest valid channel signal */
 # define PPM_MAX_CHANNEL_VALUE	2200		/* longest valid channel signal */
-# define PPM_MIN_START		2500		/* shortest valid start gap */
+# define PPM_MIN_START		2400		/* shortest valid start gap (only 2nd part of pulse) */
 
 /* decoded PPM buffer */
-#define PPM_MAX_CHANNELS	12
+#define PPM_MIN_CHANNELS	5
+#define PPM_MAX_CHANNELS	20
+
+/* Number of same-sized frames required to 'lock' */
+#define PPM_CHANNEL_LOCK	4		/* should be less than the input timeout */
+
 __EXPORT uint16_t ppm_buffer[PPM_MAX_CHANNELS];
+__EXPORT uint16_t ppm_frame_length = 0;
 __EXPORT unsigned ppm_decoded_channels = 0;
 __EXPORT uint64_t ppm_last_valid_decode = 0;
 
@@ -357,7 +367,8 @@ static uint16_t ppm_temp_buffer[PPM_MAX_CHANNELS];
 struct {
 	uint16_t	last_edge;	/* last capture time */
 	uint16_t	last_mark;	/* last significant edge */
-	unsigned	next_channel;
+	uint16_t	frame_start;	/* the frame width */
+	unsigned	next_channel;	/* next channel index */
 	enum {
 		UNSYNCH = 0,
 		ARM,
@@ -440,9 +451,8 @@ hrt_ppm_decode(uint32_t status)
 	if (status & SR_OVF_PPM)
 		goto error;
 
-	/* how long since the last edge? */
+	/* how long since the last edge? - this handles counter wrapping implicitely. */
 	width = count - ppm.last_edge;
-	ppm.last_edge = count;
 
 	ppm_edge_history[ppm_edge_next++] = width;
 
@@ -455,14 +465,39 @@ hrt_ppm_decode(uint32_t status)
 	 */
 	if (width >= PPM_MIN_START) {
 
-		/* export the last set of samples if we got something sensible */
-		if (ppm.next_channel > 4) {
-			for (i = 0; i < ppm.next_channel && i < PPM_MAX_CHANNELS; i++)
-				ppm_buffer[i] = ppm_temp_buffer[i];
+		/*
+		 * If the number of channels changes unexpectedly, we don't want
+		 * to just immediately jump on the new count as it may be a result
+		 * of noise or dropped edges.  Instead, take a few frames to settle.
+		 */
+		if (ppm.next_channel != ppm_decoded_channels) {
+			static unsigned new_channel_count;
+			static unsigned new_channel_holdoff;
 
-			ppm_decoded_channels = i;
-			ppm_last_valid_decode = hrt_absolute_time();
+			if (new_channel_count != ppm.next_channel) {
+				/* start the lock counter for the new channel count */
+				new_channel_count = ppm.next_channel;
+				new_channel_holdoff = PPM_CHANNEL_LOCK;
 
+			} else if (new_channel_holdoff > 0) {
+				/* this frame matched the last one, decrement the lock counter */
+				new_channel_holdoff--;
+
+			} else {
+				/* we have seen PPM_CHANNEL_LOCK frames with the new count, accept it */
+				ppm_decoded_channels = new_channel_count;
+				new_channel_count = 0;
+			}
+
+		} else {
+			/* frame channel count matches expected, let's use it */
+			if (ppm.next_channel > PPM_MIN_CHANNELS) {
+				for (i = 0; i < ppm.next_channel; i++)
+					ppm_buffer[i] = ppm_temp_buffer[i];
+
+				ppm_last_valid_decode = hrt_absolute_time();
+
+			}
 		}
 
 		/* reset for the next frame */
@@ -471,13 +506,14 @@ hrt_ppm_decode(uint32_t status)
 		/* next edge is the reference for the first channel */
 		ppm.phase = ARM;
 
+		ppm.last_edge = count;
 		return;
 	}
 
 	switch (ppm.phase) {
 	case UNSYNCH:
 		/* we are waiting for a start pulse - nothing useful to do here */
-		return;
+		break;
 
 	case ARM:
 
@@ -486,14 +522,23 @@ hrt_ppm_decode(uint32_t status)
 			goto error;		/* pulse was too long */
 
 		/* record the mark timing, expect an inactive edge */
-		ppm.last_mark = count;
-		ppm.phase = INACTIVE;
-		return;
+		ppm.last_mark = ppm.last_edge;
+
+		/* frame length is everything including the start gap */
+		ppm_frame_length = (uint16_t)(ppm.last_edge - ppm.frame_start);
+		ppm.frame_start = ppm.last_edge;
+		ppm.phase = ACTIVE;
+		break;
 
 	case INACTIVE:
+
+		/* we expect a short pulse */
+		if (width > PPM_MAX_PULSE_WIDTH)
+			goto error;		/* pulse was too long */
+
 		/* this edge is not interesting, but now we are ready for the next mark */
 		ppm.phase = ACTIVE;
-		return;
+		break;
 
 	case ACTIVE:
 		/* determine the interval from the last mark */
@@ -514,10 +559,13 @@ hrt_ppm_decode(uint32_t status)
 			ppm_temp_buffer[ppm.next_channel++] = interval;
 
 		ppm.phase = INACTIVE;
-		return;
+		break;
 
 	}
 
+	ppm.last_edge = count;
+	return;
+
 	/* the state machine is corrupted; reset it */
 
 error:

src/modules/px4iofirmware/controls.c

@@ -50,7 +50,7 @@
 #define RC_CHANNEL_HIGH_THRESH		5000
 #define RC_CHANNEL_LOW_THRESH		-5000
 
-static bool	ppm_input(uint16_t *values, uint16_t *num_values);
+static bool	ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len);
 
 static perf_counter_t c_gather_dsm;
 static perf_counter_t c_gather_sbus;
@@ -94,7 +94,7 @@ controls_tick() {
 	 * other.  Don't do that.
 	 */
 
-	/* receive signal strenght indicator (RSSI). 0 = no connection, 1000: perfect connection */
+	/* receive signal strenght indicator (RSSI). 0 = no connection, 255: perfect connection */
 	uint16_t rssi = 0;
 
 	perf_begin(c_gather_dsm);
@@ -108,7 +108,7 @@ controls_tick() {
 		else
 			r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RC_DSM11;
 
-		rssi = 1000;
+		rssi = 255;
 	}
 	perf_end(c_gather_dsm);
 
@@ -125,11 +125,11 @@ controls_tick() {
 	 * disable the PPM decoder completely if we have S.bus signal.
 	 */
 	perf_begin(c_gather_ppm);
-	bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count);
+	bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_status[PX4IO_P_STATUS_RC_DATA]);
 	if (ppm_updated) {
 
 		/* XXX sample RSSI properly here */
-		rssi = 1000;
+		rssi = 255;
 
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM;
 	}
@@ -319,7 +319,7 @@ controls_tick() {
 }
 
 static bool
-ppm_input(uint16_t *values, uint16_t *num_values)
+ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len)
 {
 	bool result = false;
 
@@ -343,6 +343,10 @@ ppm_input(uint16_t *values, uint16_t *num_values)
 		/* clear validity */
 		ppm_last_valid_decode = 0;
 
+		/* store PPM frame length */
+		if (num_values)
+			*frame_len = ppm_frame_length;
+
 		/* good if we got any channels */
 		result = (*num_values > 0);
 	}

src/modules/px4iofirmware/protocol.h

@@ -124,7 +124,8 @@
 #define PX4IO_P_STATUS_VSERVO			6	/* [2] servo rail voltage in mV */
 #define PX4IO_P_STATUS_VRSSI			7	/* [2] RSSI voltage */
 #define PX4IO_P_STATUS_PRSSI			8	/* [2] RSSI PWM value */
-#define PX4IO_P_STATUS_NRSSI			9	/* [2] Normalized RSSI value, 0: no reception, 1000: perfect reception */
+#define PX4IO_P_STATUS_NRSSI			9	/* [2] Normalized RSSI value, 0: no reception, 255: perfect reception */
+#define PX4IO_P_STATUS_RC_DATA			10	/* [1] + [2] Details about the RC source (PPM frame length, Spektrum protocol type) */
 
 /* array of post-mix actuator outputs, -10000..10000 */
 #define PX4IO_PAGE_ACTUATORS		2		/* 0..CONFIG_ACTUATOR_COUNT-1 */

src/modules/px4iofirmware/registers.c

@@ -89,7 +89,9 @@ uint16_t		r_page_status[] = {
 	[PX4IO_P_STATUS_IBATT]			= 0,
 	[PX4IO_P_STATUS_VSERVO]			= 0,
 	[PX4IO_P_STATUS_VRSSI]			= 0,
-	[PX4IO_P_STATUS_PRSSI]			= 0
+	[PX4IO_P_STATUS_PRSSI]			= 0,
+	[PX4IO_P_STATUS_NRSSI]			= 0,
+	[PX4IO_P_STATUS_RC_DATA]		= 0
 };
 
 /**
@@ -114,7 +116,7 @@ uint16_t		r_page_servos[PX4IO_SERVO_COUNT];
 uint16_t		r_page_raw_rc_input[] =
 {
 	[PX4IO_P_RAW_RC_COUNT]			= 0,
-	[PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + 24)] = 0 // XXX ensure we have enough space to decode beefy RX, will be replaced by patch soon
+	[PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + PX4IO_CONTROL_CHANNELS)] = 0 // XXX ensure we have enough space to decode beefy RX, will be replaced by patch soon
 };
 
 /**

src/modules/px4iofirmware/sbus.c

@@ -280,7 +280,7 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint
 		*rssi = 100; // XXX magic number indicating bad signal, but not a signal loss (yet)
 	}
 
-	*rssi = 1000;
+	*rssi = 255;
 
 	return true;
 }

src/modules/sdlog2/sdlog2.c

@@ -1208,6 +1208,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 				log_msg.msg_type = LOG_RC_MSG;
 				/* Copy only the first 8 channels of 14 */
 				memcpy(log_msg.body.log_RC.channel, buf.rc.chan, sizeof(log_msg.body.log_RC.channel));
+				log_msg.body.log_RC.channel_count = buf.rc.chan_count;
 				LOGBUFFER_WRITE_AND_COUNT(RC);
 			}
 

src/modules/sdlog2/sdlog2_messages.h

@@ -159,6 +159,7 @@ struct log_STAT_s {
 #define LOG_RC_MSG 11
 struct log_RC_s {
 	float channel[8];
+	uint8_t channel_count;
 };
 
 /* --- OUT0 - ACTUATOR_0 OUTPUT --- */
@@ -291,7 +292,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"),
 	LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
 	LOG_FORMAT(STAT, "BBBfffBB", "MainState,NavState,ArmState,BatV,BatC,BatRem,BatWarn,Landed"),
-	LOG_FORMAT(RC, "ffffffff", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7"),
+	LOG_FORMAT(RC, "ffffffffB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count"),
 	LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
 	LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"),
 	LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"),

src/modules/sensors/sensor_params.c

@@ -195,6 +195,7 @@ PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */
 #endif
 PARAM_DEFINE_INT32(RC_DSM_BIND, -1); /* -1 = Idle, 0 = Start DSM2 bind, 1 = Start DSMX bind */
 
+PARAM_DEFINE_INT32(BAT_V_SCALE_IO, 10000);
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
 PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f);
 #else

src/modules/systemlib/perf_counter.c

@@ -295,10 +295,11 @@ perf_print_counter(perf_counter_t handle)
 	case PC_ELAPSED: {
 		struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
 
-		printf("%s: %llu events, %lluus elapsed, min %lluus max %lluus\n",
+		printf("%s: %llu events, %lluus elapsed, %llu avg, min %lluus max %lluus\n",
 		       handle->name,
 		       pce->event_count,
 		       pce->time_total,
+		       pce->time_total / pce->event_count,
 		       pce->time_least,
 		       pce->time_most);
 		break;

src/modules/systemlib/ppm_decode.h

@@ -57,6 +57,7 @@ __BEGIN_DECLS
  * PPM decoder state
  */
 __EXPORT extern uint16_t	ppm_buffer[PPM_MAX_CHANNELS];	/**< decoded PPM channel values */
+__EXPORT extern uint16_t	ppm_frame_length;				/**< length of the decoded PPM frame (includes gap) */
 __EXPORT extern unsigned	ppm_decoded_channels;	/**< count of decoded channels */
 __EXPORT extern hrt_abstime	ppm_last_valid_decode;	/**< timestamp of the last valid decode */
 

src/systemcmds/tests/module.mk

@@ -27,4 +27,5 @@ SRCS			 = test_adc.c \
 			   test_file.c \
 			   tests_main.c \
 			   test_param.c \
-			   test_ppm_loopback.c
+			   test_ppm_loopback.c \
+			   test_rc.c

src/systemcmds/tests/test_rc.c

@@ -0,0 +1,146 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file test_ppm_loopback.c
+ * Tests the PWM outputs and PPM input
+ *
+ */
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+
+#include <stdio.h>
+#include <poll.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <arch/board/board.h>
+#include <drivers/drv_pwm_output.h>
+#include <drivers/drv_rc_input.h>
+#include <systemlib/err.h>
+
+#include "tests.h"
+
+#include <math.h>
+#include <float.h>
+
+int test_rc(int argc, char *argv[])
+{
+
+	int _rc_sub = orb_subscribe(ORB_ID(input_rc));
+
+	/* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */
+	struct rc_input_values	rc_input;
+	struct rc_input_values	rc_last;
+	orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);
+	usleep(100000);
+
+	/* open PPM input and expect values close to the output values */
+
+	bool rc_updated;
+	orb_check(_rc_sub, &rc_updated);
+
+	warnx("Reading PPM values - press any key to abort");
+	warnx("This test guarantees: 10 Hz update rates, no glitches (channel values), no channel count changes.");
+
+	if (rc_updated) {
+
+		/* copy initial set */
+		for (unsigned i = 0; i < rc_input.channel_count; i++) {
+			rc_last.values[i] = rc_input.values[i];
+		}
+
+		rc_last.channel_count = rc_input.channel_count;
+
+		/* poll descriptor */
+		struct pollfd fds[2];
+		fds[0].fd = _rc_sub;
+		fds[0].events = POLLIN;
+		fds[1].fd = 0;
+		fds[1].events = POLLIN;
+
+		while (true) {
+
+			int ret = poll(fds, 2, 200);
+
+			if (ret > 0) {
+
+				if (fds[0].revents & POLLIN) {
+
+					orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);
+
+					/* go and check values */
+					for (unsigned i = 0; i < rc_input.channel_count; i++) {
+						if (fabsf(rc_input.values[i] - rc_last.values[i]) > 20) {
+							warnx("comparison fail: RC: %d, expected: %d", rc_input.values[i], rc_last.values[i]);
+							(void)close(_rc_sub);
+							return ERROR;
+						}
+
+						rc_last.values[i] = rc_input.values[i];
+					}
+
+					if (rc_last.channel_count != rc_input.channel_count) {
+						warnx("channel count mismatch: last: %d, now: %d", rc_last.channel_count, rc_input.channel_count);
+						(void)close(_rc_sub);
+						return ERROR;
+					}
+
+					if (hrt_absolute_time() - rc_input.timestamp > 100000) {
+						warnx("TIMEOUT, less than 10 Hz updates");
+						(void)close(_rc_sub);
+						return ERROR;
+					} 
+
+				} else {
+					/* key pressed, bye bye */
+					return 0;
+				}
+
+			}
+		}
+
+	} else {
+		warnx("failed reading RC input data");
+		return ERROR;
+	}
+
+	warnx("PPM CONTINUITY TEST PASSED SUCCESSFULLY!");
+
+	return 0;
+}

src/systemcmds/tests/test_sensors.c

@@ -78,7 +78,8 @@ static int accel(int argc, char *argv[]);
 static int gyro(int argc, char *argv[]);
 static int mag(int argc, char *argv[]);
 static int baro(int argc, char *argv[]);
-static int mpu6k(int argc, char *argv[]);
+static int accel1(int argc, char *argv[]);
+static int gyro1(int argc, char *argv[]);
 
 /****************************************************************************
  * Private Data
@@ -93,7 +94,8 @@ struct {
 	{"gyro",	"/dev/gyro",	gyro},
 	{"mag",		"/dev/mag",	mag},
 	{"baro",	"/dev/baro",	baro},
-	{"mpu6k",	"/dev/mpu6k",	mpu6k},
+	{"accel1",	"/dev/accel1",	accel1},
+	{"gyro1",	"/dev/gyro1",	gyro1},
 	{NULL, NULL, NULL}
 };
 
@@ -137,7 +139,7 @@ accel(int argc, char *argv[])
 	}
 
 	if (fabsf(buf.x) > 30.0f || fabsf(buf.y) > 30.0f || fabsf(buf.z) > 30.0f) {
-		warnx("MPU6K acceleration values out of range!");
+		warnx("ACCEL1 acceleration values out of range!");
 		return ERROR;
 	}
 
@@ -149,20 +151,19 @@ accel(int argc, char *argv[])
 }
 
 static int
-mpu6k(int argc, char *argv[])
+accel1(int argc, char *argv[])
 {
-	printf("\tMPU6K: test start\n");
+	printf("\tACCEL1: test start\n");
 	fflush(stdout);
 
 	int		fd;
 	struct accel_report buf;
-	struct gyro_report gyro_buf;
 	int		ret;
 
-	fd = open("/dev/accel_mpu6k", O_RDONLY);
+	fd = open("/dev/accel1", O_RDONLY);
 
 	if (fd < 0) {
-		printf("\tMPU6K: open fail, run <mpu6000 start> first.\n");
+		printf("\tACCEL1: open fail, run <mpu6000 start> or <lsm303d start> first.\n");
 		return ERROR;
 	}
 
@@ -173,45 +174,21 @@ mpu6k(int argc, char *argv[])
 	ret = read(fd, &buf, sizeof(buf));
 
 	if (ret != sizeof(buf)) {
-		printf("\tMPU6K: read1 fail (%d)\n", ret);
+		printf("\tACCEL1: read1 fail (%d)\n", ret);
 		return ERROR;
 
 	} else {
-		printf("\tMPU6K accel: x:%8.4f\ty:%8.4f\tz:%8.4f m/s^2\n", (double)buf.x, (double)buf.y, (double)buf.z);
+		printf("\tACCEL1 accel: x:%8.4f\ty:%8.4f\tz:%8.4f m/s^2\n", (double)buf.x, (double)buf.y, (double)buf.z);
 	}
 
 	if (fabsf(buf.x) > 30.0f || fabsf(buf.y) > 30.0f || fabsf(buf.z) > 30.0f) {
-		warnx("MPU6K acceleration values out of range!");
+		warnx("ACCEL1 acceleration values out of range!");
 		return ERROR;
 	}
 
 	/* Let user know everything is ok */
-	printf("\tOK: MPU6K ACCEL passed all tests successfully\n");
+	printf("\tOK: ACCEL1 passed all tests successfully\n");
 
-	close(fd);
-	fd = open("/dev/gyro_mpu6k", O_RDONLY);
-
-	if (fd < 0) {
-		printf("\tMPU6K GYRO: open fail, run <l3gd20 start> or <mpu6000 start> first.\n");
-		return ERROR;
-	}
-
-	/* wait at least 5 ms, sensor should have data after that */
-	usleep(5000);
-
-	/* read data - expect samples */
-	ret = read(fd, &gyro_buf, sizeof(gyro_buf));
-
-	if (ret != sizeof(gyro_buf)) {
-		printf("\tMPU6K GYRO: read fail (%d)\n", ret);
-		return ERROR;
-
-	} else {
-		printf("\tMPU6K GYRO rates: x:%8.4f\ty:%8.4f\tz:%8.4f rad/s\n", (double)gyro_buf.x, (double)gyro_buf.y, (double)gyro_buf.z);
-	}
-
-	/* Let user know everything is ok */
-	printf("\tOK: MPU6K GYRO passed all tests successfully\n");
 	close(fd);
 
 	return OK;
@@ -255,6 +232,44 @@ gyro(int argc, char *argv[])
 	return OK;
 }
 
+static int
+gyro1(int argc, char *argv[])
+{
+	printf("\tGYRO1: test start\n");
+	fflush(stdout);
+
+	int		fd;
+	struct gyro_report buf;
+	int		ret;
+
+	fd = open("/dev/gyro1", O_RDONLY);
+
+	if (fd < 0) {
+		printf("\tGYRO1: open fail, run <l3gd20 start> or <mpu6000 start> first.\n");
+		return ERROR;
+	}
+
+	/* wait at least 5 ms, sensor should have data after that */
+	usleep(5000);
+
+	/* read data - expect samples */
+	ret = read(fd, &buf, sizeof(buf));
+
+	if (ret != sizeof(buf)) {
+		printf("\tGYRO1: read fail (%d)\n", ret);
+		return ERROR;
+
+	} else {
+		printf("\tGYRO1 rates: x:%8.4f\ty:%8.4f\tz:%8.4f rad/s\n", (double)buf.x, (double)buf.y, (double)buf.z);
+	}
+
+	/* Let user know everything is ok */
+	printf("\tOK: GYRO1 passed all tests successfully\n");
+	close(fd);
+
+	return OK;
+}
+
 static int
 mag(int argc, char *argv[])
 {

src/systemcmds/tests/tests.h

@@ -108,6 +108,7 @@ extern int	test_param(int argc, char *argv[]);
 extern int	test_bson(int argc, char *argv[]);
 extern int	test_file(int argc, char *argv[]);
 extern int	test_mixer(int argc, char *argv[]);
+extern int	test_rc(int argc, char *argv[]);
 
 __END_DECLS
 

src/systemcmds/tests/tests_main.c

@@ -105,6 +105,7 @@ const struct {
 	{"bson",		test_bson,	0},
 	{"file",		test_file,	0},
 	{"mixer",		test_mixer,	OPT_NOJIGTEST | OPT_NOALLTEST},
+	{"rc",			test_rc,	OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"help",		test_help,	OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST},
 	{NULL,			NULL, 		0}
 };