Merged master into ekf2

2026-09-21 10:26:02 +08:00 · 2015-12-24 09:53:54 +01:00
parent b958928095 45e4def3eb
commit d3149153f6
24 changed files with 1606 additions and 168 deletions
@@ -14,6 +14,15 @@ echo program: $program
 echo model: $model
 echo build_path: $build_path

+if [ "$chroot" == "1" ]
+then
+	chroot_enabled=-c
+	sudo_enabled=sudo
+else
+	chroot_enabled=""
+	sudo_enabled=""
+fi
+
 if [ "$model" == "" ] || [ "$model" == "none" ]
 then
 	echo "empty model, setting iris as default"
@@ -48,7 +57,7 @@ if [ "$program" == "jmavsim" ] && [ "$no_sim" == "" ]
 then
 	cd Tools/jMAVSim
 	ant
-	nice -n -10 java -Djava.ext.dirs= -cp lib/*:out/production/jmavsim.jar me.drton.jmavsim.Simulator -udp 127.0.0.1:14560 &
+	java -Djava.ext.dirs= -cp lib/*:out/production/jmavsim.jar me.drton.jmavsim.Simulator -udp 127.0.0.1:14560 &
 	SIM_PID=`echo $!`
 elif [ "$program" == "gazebo" ] && [ "$no_sim" == "" ]
 then
@@ -65,7 +74,7 @@ then
 		cd Tools/sitl_gazebo/Build
 		cmake -Wno-dev ..
 		make -j4
-		nice -n -10 gzserver --verbose ../worlds/${model}.world &
+		gzserver --verbose ../worlds/${model}.world &
 		SIM_PID=`echo $!`
 		gzclient --verbose &
 		GUI_PID=`echo $!`
@@ -96,7 +105,7 @@ elif [ "$debugger" == "valgrind" ]
 then
 	valgrind ./mainapp ../../../../${rc_script}_${program}_${model}
 else
-	nice -n -10 ./mainapp ../../../../${rc_script}_${program}_${model}
+	$sudo_enabled ./mainapp $chroot_enabled ../../../../${rc_script}_${program}_${model}
 fi

 if [ "$program" == "jmavsim" ]
@@ -144,7 +144,7 @@ set(config_module_list
 	#examples/math_demo
 	# Tutorial code from
 	# https://px4.io/dev/px4_simple_app
-	#examples/px4_simple_app
+	examples/px4_simple_app

 	# Tutorial code from
 	# https://px4.io/dev/daemon
@@ -59,6 +59,7 @@ set(config_module_list
 	lib/launchdetection
 	lib/terrain_estimation
 	lib/runway_takeoff
+	examples/px4_simple_app
 	)

 set(config_extra_builtin_cmds
@@ -150,6 +150,7 @@ __BEGIN_DECLS
 #define GPIO_TIM3_CH2OUT	GPIO_TIM3_CH2OUT_3
 #define GPIO_TIM3_CH3OUT	GPIO_TIM3_CH3OUT_2
 #define GPIO_TIM3_CH4OUT	GPIO_TIM3_CH4OUT_2
+#define DIRECT_PWM_OUTPUT_CHANNELS	8

 /* High-resolution timer */
 #define HRT_TIMER		8	/* use timer 8 for the HRT */
@@ -182,6 +182,7 @@ __BEGIN_DECLS
 #define GPIO_TIM2_CH2OUT	GPIO_TIM2_CH2OUT_1
 #define GPIO_TIM2_CH3OUT	GPIO_TIM2_CH3OUT_1
 #define GPIO_TIM2_CH4OUT	GPIO_TIM2_CH4OUT_1
+#define DIRECT_PWM_OUTPUT_CHANNELS	4

 /* USB OTG FS
 *
@@ -210,6 +210,7 @@ __BEGIN_DECLS
 #define GPIO_TIM1_CH4OUT	GPIO_TIM1_CH4OUT_2
 #define GPIO_TIM4_CH2OUT	GPIO_TIM4_CH2OUT_2
 #define GPIO_TIM4_CH3OUT	GPIO_TIM4_CH3OUT_2
+#define DIRECT_PWM_OUTPUT_CHANNELS	6

 /* USB OTG FS
 *
@@ -65,7 +65,7 @@ __BEGIN_DECLS
 //{0,                      GPIO_PERIPH_3V3_EN,      0},	Owned by the 8266 driver
 //{0,                      GPIO_SBUS_INV,           0},	https://github.com/PX4/Firmware/blob/master/src/modules/px4iofirmware/sbus.c
 //{GPIO_8266_GPIO0,        0,                       0},   Owned by the 8266 driver
-//{0,                      GPIO_SPEKTRUM_POWER,     0},	Owned Spektum driver input to auto pilot
+//{0,                      GPIO_SPEKTRUM_PWR_EN,     0},	Owned Spektum driver input to auto pilot
 //{0,                      GPIO_8266_PD,            0},	Owned by the 8266 driver
 //{0,                      GPIO_8266_RST,           0},	Owned by the 8266 driver

@@ -200,6 +200,7 @@ __BEGIN_DECLS
 #define GPIO_TIM1_CH4OUT	GPIO_TIM1_CH4OUT_2
 #define GPIO_TIM4_CH2OUT	GPIO_TIM4_CH2OUT_2
 #define GPIO_TIM4_CH3OUT	GPIO_TIM4_CH3OUT_2
+#define DIRECT_PWM_OUTPUT_CHANNELS	6

 /* USB OTG FS
 *
@@ -223,15 +224,23 @@ __BEGIN_DECLS

 #define GPIO_RSSI_IN 			(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN1)
 #define GPIO_LED_SAFETY			(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN3)
-#define GPIO_SAFETY_SWITCH_IN	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN4)
+#define GPIO_BTN_SAFETY			(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN4)
 #define GPIO_PERIPH_3V3_EN		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN5)
 #define GPIO_SBUS_INV			(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13)

 #define GPIO_8266_GPIO0			(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN2)
-#define GPIO_SPEKTRUM_POWER		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4)
+#define GPIO_SPEKTRUM_PWR_EN		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4)
 #define GPIO_8266_PD			(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN5)
 #define GPIO_8266_RST			(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN6)

+/* Power switch controls ******************************************************/
+
+#define POWER_SPEKTRUM(_s)		stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_s))
+#define GPIO_USART1_RX_SPEKTRUM		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN7)
+#define SPEKTRUM_RX_HIGH(_s)	stm32_gpiowrite(GPIO_USART1_RX_SPEKTRUM, (_s))
+#define SPEKTRUM_RX_AS_UART()		stm32_configgpio(GPIO_USART1_RX)
+#define SPEKTRUM_RX_AS_GPIO()		stm32_configgpio(GPIO_USART1_RX_SPEKTRUM)
+
 /****************************************************************************************************
 * Public Types
 ****************************************************************************************************/
@@ -230,7 +230,7 @@ __EXPORT int nsh_archinitialize(void)

 	stm32_configgpio(GPIO_SBUS_INV);
 	stm32_configgpio(GPIO_8266_GPIO0);
-	stm32_configgpio(GPIO_SPEKTRUM_POWER);
+	stm32_configgpio(GPIO_SPEKTRUM_PWR_EN);
 	stm32_configgpio(GPIO_8266_PD);
 	stm32_configgpio(GPIO_8266_RST);

@@ -83,6 +83,13 @@
 #define GPIO_RELAY1_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN12)
 #define GPIO_RELAY2_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN11)

+#define GPIO_SPEKTRUM_PWR_EN GPIO_RELAY1_EN
+#define POWER_SPEKTRUM(_s)		stm32_gpiowrite(GPIO_RELAY1_EN, (_s))
+
+#define SPEKTRUM_RX_HIGH(_s)	stm32_gpiowrite(GPIO_USART1_RX_SPEKTRUM, (_s))
+#define SPEKTRUM_RX_AS_UART()		stm32_configgpio(GPIO_USART1_RX)
+#define SPEKTRUM_RX_AS_GPIO()		stm32_configgpio(GPIO_USART1_RX_SPEKTRUM)
+
 /* Analog inputs ********************************************************************/

 #define GPIO_ADC_VBATT	(GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN4)
@@ -79,7 +79,7 @@
 #define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13)
 #define GPIO_LED4 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN11)

-#define GPIO_USART1_RX_SPEKTRUM		(GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | GPIO_OUTPUT_SET | GPIO_PORTA | GPIO_PIN10)
+#define GPIO_USART1_RX_SPEKTRUM		(GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN10)

 /* Safety switch button *******************************************************/

@@ -88,6 +88,11 @@
 /* Power switch controls ******************************************************/

 #define GPIO_SPEKTRUM_PWR_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)
+#define POWER_SPEKTRUM(_s)		stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_s))
+
+#define SPEKTRUM_RX_HIGH(_s)	stm32_gpiowrite(GPIO_USART1_RX_SPEKTRUM, (_s))
+#define SPEKTRUM_RX_AS_UART()		stm32_configgpio(GPIO_USART1_RX)
+#define SPEKTRUM_RX_AS_GPIO()		stm32_configgpio(GPIO_USART1_RX_SPEKTRUM)

 #define GPIO_SERVO_FAULT_DETECT (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN15)

@@ -90,12 +90,8 @@
 # include <systemlib/ppm_decode.h>
 #endif

-/*
- * This is the analog to FMU_INPUT_DROP_LIMIT_US on the IO side
- */
-
-#define CONTROL_INPUT_DROP_LIMIT_US		2000
-#define NAN_VALUE	(0.0f/0.0f)
+#define SCHEDULE_INTERVAL	2000	/**< The schedule interval in usec (500 Hz) */
+#define NAN_VALUE	(0.0f/0.0f)		/**< NaN value for throttle lock mode */

 class PX4FMU : public device::CDev
 {
@@ -123,15 +119,7 @@ public:
 	int		set_i2c_bus_clock(unsigned bus, unsigned clock_hz);

 private:
-#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
-	static const unsigned _max_actuators = 4;
-#endif
-#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
-	static const unsigned _max_actuators = 6;
-#endif
-#if defined(CONFIG_ARCH_BOARD_AEROCORE)
-	static const unsigned _max_actuators = 8;
-#endif
+	static const unsigned _max_actuators = DIRECT_PWM_OUTPUT_CHANNELS;

 	Mode		_mode;
 	unsigned	_pwm_default_rate;
@@ -189,7 +177,7 @@ private:
 	int		set_pwm_rate(unsigned rate_map, unsigned default_rate, unsigned alt_rate);
 	int		pwm_ioctl(file *filp, int cmd, unsigned long arg);
 	void		update_pwm_rev_mask();
-	void	publish_pwm_outputs(uint16_t *values, size_t numvalues);
+	void		publish_pwm_outputs(uint16_t *values, size_t numvalues);

 	struct GPIOConfig {
 		uint32_t	input;
@@ -321,11 +309,9 @@ PX4FMU::PX4FMU() :
 	memset(_controls, 0, sizeof(_controls));
 	memset(_poll_fds, 0, sizeof(_poll_fds));

-#ifdef HRT_PPM_CHANNEL
 	// rc input, published to ORB
 	memset(&_rc_in, 0, sizeof(_rc_in));
 	_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM;
-#endif

 #ifdef GPIO_SBUS_INV
 	// this board has a GPIO to control SBUS inversion
@@ -728,8 +714,8 @@ PX4FMU::cycle()
 						//main_out_latency = hrt_absolute_time() - _controls[i].timestamp - 250;

 						/* do only correct within the current phase */
-						if (abs(main_out_latency) > CONTROL_INPUT_DROP_LIMIT_US) {
-							main_out_latency = CONTROL_INPUT_DROP_LIMIT_US;
+						if (abs(main_out_latency) > SCHEDULE_INTERVAL) {
+							main_out_latency = SCHEDULE_INTERVAL;
 						}

 						if (main_out_latency < 250) {
@@ -905,7 +891,7 @@ PX4FMU::cycle()
 	}

 	work_queue(HPWORK, &_work, (worker_t)&PX4FMU::cycle_trampoline, this,
-		   USEC2TICK(CONTROL_INPUT_DROP_LIMIT_US - main_out_latency));
+		   USEC2TICK(SCHEDULE_INTERVAL - main_out_latency));
 }

 void PX4FMU::work_stop()
@@ -1683,9 +1669,9 @@ PX4FMU::peripheral_reset(int ms)

 	stm32_gpiowrite(GPIO_PERIPH_3V3_EN, 0);

-	bool last =  stm32_gpioread(GPIO_SPEKTRUM_POWER);
+	bool last = stm32_gpioread(GPIO_SPEKTRUM_PWR_EN);
 	/* Keep Spektum on to discharge rail*/
-	stm32_gpiowrite(GPIO_SPEKTRUM_POWER, 1);
+	stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, 1);

 	/* wait for the peripheral rail to reach GND */
 	usleep(ms * 1000);
@@ -1694,7 +1680,7 @@ PX4FMU::peripheral_reset(int ms)
 	/* re-enable power */

 	/* switch the peripheral rail back on */
-	stm32_gpiowrite(GPIO_SPEKTRUM_POWER, last);
+	stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, last);
 	stm32_gpiowrite(GPIO_PERIPH_3V3_EN, 1);
 #endif
 }
@@ -39,6 +39,8 @@
 */

 #include <px4_config.h>
+#include <px4_tasks.h>
+#include <px4_posix.h>
 #include <unistd.h>
 #include <stdio.h>
 #include <poll.h>
@@ -52,7 +54,7 @@ __EXPORT int px4_simple_app_main(int argc, char *argv[]);

 int px4_simple_app_main(int argc, char *argv[])
 {
-	printf("Hello Sky!\n");
+	PX4_INFO("Hello Sky!");

 	/* subscribe to sensor_combined topic */
 	int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined));
@@ -64,7 +66,7 @@ int px4_simple_app_main(int argc, char *argv[])
 	orb_advert_t att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);

 	/* one could wait for multiple topics with this technique, just using one here */
-	struct pollfd fds[] = {
+	px4_pollfd_struct_t fds[] = {
 		{ .fd = sensor_sub_fd,   .events = POLLIN },
 		/* there could be more file descriptors here, in the form like:
 		 * { .fd = other_sub_fd,   .events = POLLIN },
@@ -75,19 +77,19 @@ int px4_simple_app_main(int argc, char *argv[])

 	for (int i = 0; i < 5; i++) {
 		/* wait for sensor update of 1 file descriptor for 1000 ms (1 second) */
-		int poll_ret = poll(fds, 1, 1000);
+		int poll_ret = px4_poll(fds, 1, 1000);

 		/* handle the poll result */
 		if (poll_ret == 0) {
 			/* this means none of our providers is giving us data */
-			printf("[px4_simple_app] Got no data within a second\n");
+			PX4_ERR("[px4_simple_app] Got no data within a second");

 		} else if (poll_ret < 0) {
 			/* this is seriously bad - should be an emergency */
 			if (error_counter < 10 || error_counter % 50 == 0) {
 				/* use a counter to prevent flooding (and slowing us down) */
-				printf("[px4_simple_app] ERROR return value from poll(): %d\n"
-				       , poll_ret);
+				PX4_ERR("[px4_simple_app] ERROR return value from poll(): %d"
+					, poll_ret);
 			}

 			error_counter++;
@@ -99,10 +101,10 @@ int px4_simple_app_main(int argc, char *argv[])
 				struct sensor_combined_s raw;
 				/* copy sensors raw data into local buffer */
 				orb_copy(ORB_ID(sensor_combined), sensor_sub_fd, &raw);
-				printf("[px4_simple_app] Accelerometer:\t%8.4f\t%8.4f\t%8.4f\n",
-				       (double)raw.accelerometer_m_s2[0],
-				       (double)raw.accelerometer_m_s2[1],
-				       (double)raw.accelerometer_m_s2[2]);
+				PX4_WARN("[px4_simple_app] Accelerometer:\t%8.4f\t%8.4f\t%8.4f",
+					 (double)raw.accelerometer_m_s2[0],
+					 (double)raw.accelerometer_m_s2[1],
+					 (double)raw.accelerometer_m_s2[2]);

 				/* set att and publish this information for other apps */
 				att.roll = raw.accelerometer_m_s2[0];
@@ -117,5 +119,7 @@ int px4_simple_app_main(int argc, char *argv[])
 		}
 	}

+	PX4_INFO("exiting");
+
 	return 0;
 }
@@ -42,11 +42,35 @@
 #pragma once

 #include <stdint.h>
+#include <px4_config.h>
+#include <board_config.h>
+#include <px4_defines.h>

 __BEGIN_DECLS

+#define DSM_FRAME_SIZE		16		/**< DSM frame size in bytes */
+#define DSM_FRAME_CHANNELS	7		/**< Max supported DSM channels per frame */
+#define DSM_MAX_CHANNEL_COUNT   18  /**< Max channel count of any DSM RC */
+#define DSM_BUFFER_SIZE		(DSM_FRAME_SIZE + DSM_FRAME_SIZE / 2)
+
 __EXPORT int	dsm_init(const char *device);
-__EXPORT bool	dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes, unsigned max_values);
+__EXPORT int	dsm_config(int dsm_fd);
+__EXPORT bool	dsm_input(int dsm_fd, uint16_t *values, uint16_t *num_values, bool *dsm_11_bit, uint8_t *n_bytes,
+			  uint8_t **bytes, unsigned max_values);
+
+__EXPORT bool	dsm_parse(uint64_t now, uint8_t *frame, unsigned len, uint16_t *values,
+			  uint16_t *num_values, bool *dsm_11_bit, unsigned *frame_drops, uint16_t max_channels);
+
+#ifdef GPIO_SPEKTRUM_PWR_EN
 __EXPORT void	dsm_bind(uint16_t cmd, int pulses);
+#endif
+
+enum DSM_CMD {							/* DSM bind states */
+	DSM_CMD_BIND_POWER_DOWN = 0,
+	DSM_CMD_BIND_POWER_UP,
+	DSM_CMD_BIND_SET_RX_OUT,
+	DSM_CMD_BIND_SEND_PULSES,
+	DSM_CMD_BIND_REINIT_UART
+};

 __END_DECLS
@@ -44,6 +44,10 @@
 #include <termios.h>
 #include <string.h>

+#ifdef TIOCSSINGLEWIRE
+#include <sys/ioctl.h>
+#endif
+
 #include "sbus.h"
 #include <drivers/drv_hrt.h>

@@ -122,6 +126,21 @@ sbus_init(const char *device, bool singlewire)
 {
 	int sbus_fd = open(device, O_RDWR | O_NONBLOCK);

+	int ret = sbus_config(sbus_fd, singlewire);
+
+	if (!ret) {
+		return sbus_fd;
+
+	} else {
+		return -1;
+	}
+}
+
+int
+sbus_config(int sbus_fd, bool singlewire)
+{
+	int ret = -1;
+
 	if (sbus_fd >= 0) {
 		struct termios t;

@@ -133,8 +152,8 @@ sbus_init(const char *device, bool singlewire)

 		if (singlewire) {
 			/* only defined in configs capable of IOCTL */
-#ifdef SBUS_SERIAL_PORT
-			//ioctl(uart, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED);
+#ifdef TIOCSSINGLEWIRE
+			ioctl(sbus_fd, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED);
 #endif
 		}

@@ -143,9 +162,11 @@ sbus_init(const char *device, bool singlewire)
 		last_rx_time = hrt_absolute_time();
 		last_frame_time = last_rx_time;
 		sbus_frame_drops = 0;
+
+		ret = 0;
 	}

-	return sbus_fd;
+	return ret;
 }

 void
@@ -68,6 +68,7 @@ __EXPORT int	sbus_init(const char *device, bool singlewire);
 * provides a degree of protection. Of course, it would be better
 * if we didn't drop bytes...
 */
+__EXPORT int	sbus_config(int sbus_fd, bool singlewire);
 __EXPORT bool	sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe,
 			   bool *sbus_frame_drop,
 			   uint16_t max_channels);
@@ -303,7 +303,7 @@ Mission::advance_mission()
 	}
 }

-int
+float
 Mission::get_absolute_altitude_for_item(struct mission_item_s &mission_item)
 {
 	if (_mission_item.altitude_is_relative) {
@@ -656,8 +656,10 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
 	for (int i = 0; i < 10; i++) {

 		if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) {
-			/* mission item index out of bounds */
-			mavlink_and_console_log_critical(_navigator->get_mavlink_fd(), "[wpm] err: index: %d, max: %d", *mission_index_ptr, (int)mission->count);
+			/* mission item index out of bounds - if they are equal, we just reached the end */
+			if (*mission_index_ptr != (int)mission->count) {
+				mavlink_and_console_log_critical(_navigator->get_mavlink_fd(), "[wpm] err: index: %d, max: %d", *mission_index_ptr, (int)mission->count);
+			}
 			return false;
 		}

@@ -133,7 +133,7 @@ private:
 	 */
 	void altitude_sp_foh_reset();

-	int get_absolute_altitude_for_item(struct mission_item_s &mission_item);
+	float get_absolute_altitude_for_item(struct mission_item_s &mission_item);

 	/**
 	 * Read current or next mission item from the dataman and watch out for DO_JUMPS
@@ -74,15 +74,15 @@ static unsigned _rssi_adc_counts = 0;
 bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool *sumd_updated)
 {
 	perf_begin(c_gather_dsm);
-	uint16_t temp_count = r_raw_rc_count;
 	uint8_t n_bytes = 0;
 	uint8_t *bytes;
-	*dsm_updated = dsm_input(r_raw_rc_values, &temp_count, &n_bytes, &bytes, PX4IO_RC_INPUT_CHANNELS);
+	bool dsm_11_bit;
+	*dsm_updated = dsm_input(_dsm_fd, r_raw_rc_values, &r_raw_rc_count, &dsm_11_bit, &n_bytes, &bytes,
+				 PX4IO_RC_INPUT_CHANNELS);

 	if (*dsm_updated) {
-		r_raw_rc_count = temp_count & 0x7fff;

-		if (temp_count & 0x8000) {
+		if (dsm_11_bit) {
 			r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11;

 		} else {
@@ -170,7 +170,6 @@ extern pwm_limit_t pwm_limit;
 #ifdef CONFIG_ARCH_BOARD_PX4IO_V2

 # define PX4IO_RELAY_CHANNELS		0
-# define POWER_SPEKTRUM(_s)		stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_s))
 # define ENABLE_SBUS_OUT(_s)		stm32_gpiowrite(GPIO_SBUS_OENABLE, !(_s))

 # define VDD_SERVO_FAULT		(!stm32_gpioread(GPIO_SERVO_FAULT_DETECT))
@@ -44,6 +44,8 @@
 #include <sstream>
 #include <vector>
 #include <signal.h>
+#include <unistd.h>
+#include <stdio.h>
 #include "apps.h"
 #include "px4_middleware.h"
 #include "DriverFramework.hpp"
@@ -157,6 +159,7 @@ static void process_line(string &line, bool exit_on_fail)
 int main(int argc, char **argv)
 {
 	bool daemon_mode = false;
+	bool chroot_on = false;
 	signal(SIGINT, _SigIntHandler);
 	signal(SIGFPE, _SigFpeHandler);

@@ -174,6 +177,9 @@ int main(int argc, char **argv)
 				usage();
 				return 0;

+			} else if (strcmp(argv[index], "-c") == 0) {
+				chroot_on = true;
+
 			} else {
 				PX4_WARN("Unknown/unhandled parameter: %s", argv[index]);
 				return 1;
@@ -203,7 +209,7 @@ int main(int argc, char **argv)

 		px4::init(argc, argv, "mainapp");

-		//if commandfile is present, process the commands from the file
+		// if commandfile is present, process the commands from the file
 		if (commands_file != nullptr) {
 			ifstream infile(commands_file);

@@ -217,6 +223,41 @@ int main(int argc, char **argv)
 			}
 		}

+		if (chroot_on) {
+			// Lock this application in the current working dir
+			// this is not an attempt to secure the environment,
+			// rather, to replicate a deployed file system.
+
+#ifdef PATH_MAX
+			const unsigned path_max_len = PATH_MAX;
+#else
+			const unsigned path_max_len = 1024;
+#endif
+
+			char pwd_path[path_max_len];
+			const char *folderpath = "/rootfs/";
+
+			if (nullptr == getcwd(pwd_path, sizeof(pwd_path))) {
+				PX4_ERR("Failed aquiring working dir, abort.");
+				exit(1);
+			}
+
+			if (nullptr == strcat(pwd_path, folderpath)) {
+				PX4_ERR("Failed completing path, abort.");
+				exit(1);
+			}
+
+			if (chroot(pwd_path)) {
+				PX4_ERR("Failed chrooting application, path: %s, error: %s.", pwd_path, strerror(errno));
+				exit(1);
+			}
+
+			if (chdir("/")) {
+				PX4_ERR("Failed changing to root dir, path: %s, error: %s.", pwd_path, strerror(errno));
+				exit(1);
+			}
+		}
+
 		if (!daemon_mode) {
 			string mystr = "";
 			string string_buffer[CMD_BUFF_SIZE];
@@ -153,6 +153,12 @@ add_executable(sbus2_test sbus2_test.cpp hrt.cpp
 target_link_libraries( sbus2_test px4_platform )
 add_gtest(sbus2_test)

+# DSM test
+add_executable(dsm_test dsm_test.cpp hrt.cpp
+                          ${PX_SRC}/lib/rc/dsm.c)
+target_link_libraries( dsm_test px4_platform )
+add_gtest(dsm_test)
+
 # st24_test
 add_executable(rc_input_test st24_test.cpp hrt.cpp ${PX_SRC}/lib/rc/st24.c sumd_test.cpp ${PX_SRC}/lib/rc/sumd.c)
 target_link_libraries(rc_input_test px4_platform)
@@ -0,0 +1,74 @@
+#include <stdio.h>
+#include <string.h>
+#include <unistd.h>
+
+#include "../../src/systemcmds/tests/tests.h"
+#include <drivers/drv_hrt.h>
+// Enable DSM parser output
+#define DSM_DEBUG
+#include <rc/dsm.h>
+#include <systemlib/err.h>
+#include <systemlib/mixer/mixer.h>
+
+#include "gtest/gtest.h"
+
+TEST(DSMTest, DSM)
+{
+	const char *filepath = "testdata/dsm_x_data.txt";
+
+	FILE *fp;
+	fp = fopen(filepath, "rt");
+
+	ASSERT_TRUE(fp);
+	warnx("loading data from: %s", filepath);
+
+	float f;
+	unsigned x;
+	int ret;
+
+	// Trash the first 20 lines
+	for (unsigned i = 0; i < 20; i++) {
+		char buf[200];
+		(void)fgets(buf, sizeof(buf), fp);
+	}
+
+	// Init the parser
+	uint8_t frame[20];
+	uint16_t rc_values[18];
+	uint16_t num_values;
+	bool dsm_11_bit;
+	unsigned dsm_frame_drops = 0;
+	uint16_t max_channels = sizeof(rc_values) / sizeof(rc_values[0]);
+
+	int rate_limiter = 0;
+	unsigned last_drop = 0;
+
+	while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) {
+
+		ASSERT_GT(ret, 0);
+
+		frame[0] = x;
+		unsigned len = 1;
+
+		// Pipe the data into the parser
+		bool result = dsm_parse(f*1e6, &frame[0], len, rc_values, &num_values,
+						&dsm_11_bit, &dsm_frame_drops, max_channels);
+
+		if (result) {
+			warnx("decoded packet with %d channels and %s encoding:", num_values, (dsm_11_bit) ? "11 bit" : "10 bit");
+
+			for (unsigned i = 0; i < num_values; i++) {
+				printf("chan #%u:\t%d\n", i, (int)rc_values[i]);
+			}
+		}
+
+		if (last_drop != (dsm_frame_drops)) {
+			warnx("frame dropped, now #%d", (dsm_frame_drops));
+			last_drop = dsm_frame_drops;
+		}
+
+		rate_limiter++;
+	}
+
+	ASSERT_EQ(ret, EOF);
+}