Merge branch 'fmuv2_bringup_new_state_machine_drton' of https://github.com/cvg/Firmware_Private into fmuv2_bringup_new_state_machine_drton

.gitignore

@@ -23,10 +23,13 @@ Firmware.sublime-workspace
 Images/*.bin
 Images/*.px4
 mavlink/include/mavlink/v0.9/
-NuttX
+/nuttx-configs/px4io-v2/src/.depend
-nuttx-configs/px4io-v2/src/.depend
+/nuttx-configs/px4io-v2/src/Make.dep
-nuttx-configs/px4io-v2/src/Make.dep
+/nuttx-configs/px4io-v2/src/libboard.a
-nuttx-configs/px4io-v2/src/libboard.a
+/nuttx-configs/px4io-v1/src/.depend
-nuttx-configs/px4io-v1/src/.depend
+/nuttx-configs/px4io-v1/src/Make.dep
-nuttx-configs/px4io-v1/src/Make.dep
+/nuttx-configs/px4io-v1/src/libboard.a
-nuttx-configs/px4io-v1/src/libboard.a
+/NuttX
+/Documentation/doxy.log
+/Documentation/html/
+/Documentation/doxygen*objdb*tmp

Documentation/Doxyfile

@@ -429,19 +429,19 @@ SORT_BY_SCOPE_NAME     = NO
 # disable (NO) the todo list. This list is created by putting \todo
 # commands in the documentation.
 
-GENERATE_TODOLIST      = YES
+GENERATE_TODOLIST      = NO
 
 # The GENERATE_TESTLIST tag can be used to enable (YES) or
 # disable (NO) the test list. This list is created by putting \test
 # commands in the documentation.
 
-GENERATE_TESTLIST      = YES
+GENERATE_TESTLIST      = NO
 
 # The GENERATE_BUGLIST tag can be used to enable (YES) or
 # disable (NO) the bug list. This list is created by putting \bug
 # commands in the documentation.
 
-GENERATE_BUGLIST       = YES
+GENERATE_BUGLIST       = NO
 
 # The GENERATE_DEPRECATEDLIST tag can be used to enable (YES) or
 # disable (NO) the deprecated list. This list is created by putting
@@ -569,7 +569,7 @@ WARN_LOGFILE           = doxy.log
 # directories like "/usr/src/myproject". Separate the files or directories
 # with spaces.
 
-INPUT                  = ../apps
+INPUT                  = ../src
 
 # This tag can be used to specify the character encoding of the source files
 # that doxygen parses. Internally doxygen uses the UTF-8 encoding, which is
@@ -599,9 +599,8 @@ RECURSIVE              = YES
 # excluded from the INPUT source files. This way you can easily exclude a
 # subdirectory from a directory tree whose root is specified with the INPUT tag.
 
-EXCLUDE                = ../dist/ \
+EXCLUDE                = ../src/modules/mathlib/CMSIS \
-						 ../docs/html/ \
+			 ../src/modules/attitude_estimator_ekf/codegen
-						 html
 
 # The EXCLUDE_SYMLINKS tag can be used select whether or not files or
 # directories that are symbolic links (a Unix filesystem feature) are excluded

ROMFS/px4fmu_common/init.d/10_io_f330

@@ -45,11 +45,21 @@ param set MAV_TYPE 2
 mavlink start
 usleep 5000
 
+#
+# Start the commander (depends on orb, mavlink)
+#
+commander start
+ 
 #
 # Start PX4IO interface (depends on orb, commander)
 #
-px4io start
+if px4io start
-pwm -u 400 -m 0xff
+then
+	pwm -u 400 -m 0xff
+else
+	# SOS
+	tone_alarm 6
+fi
  
 #
 # Allow PX4IO to recover from midair restarts.
@@ -78,11 +88,6 @@ mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
 #
 sh /etc/init.d/rc.sensors
  
-#
-# Start the commander (depends on orb, mavlink)
-#
-commander start
- 
 #
 # Start GPS interface (depends on orb)
 #
@@ -102,15 +107,7 @@ if [ $BOARD == fmuv1 ]
 then
 	px4io limit 200
 	sdlog2 start -r 50 -a -b 16
-	if blinkm start
-	then
-		blinkm systemstate
-	fi
 else
 	px4io limit 400
 	sdlog2 start -r 200 -a -b 16
-	if rgbled start
-	then
-		#rgbled systemstate
-	fi
 fi

ROMFS/px4fmu_common/init.d/rcS

@@ -81,6 +81,19 @@ else
 	fi
 fi
 
+#
+# Start system state indicator
+#
+if rgbled start
+then
+	echo "Using external RGB Led"
+else
+	if blinkm start
+	then
+		blinkm systemstate
+	fi
+fi
+
 #
 # Check if PX4IO Firmware should be upgraded (from Andrew Tridgell)
 #

makefiles/config_px4fmu-v1_default.mk

@@ -30,6 +30,7 @@ MODULES		+= drivers/hil
 MODULES		+= drivers/hott/hott_telemetry
 MODULES		+= drivers/hott/hott_sensors
 MODULES		+= drivers/blinkm
+MODULES		+= drivers/rgbled
 MODULES		+= drivers/mkblctrl
 MODULES		+= drivers/md25
 MODULES		+= drivers/airspeed

nuttx-configs/px4fmu-v1/nsh/defconfig

@@ -268,7 +268,7 @@ CONFIG_USART2_RXDMA=y
 # CONFIG_USART3_RXDMA is not set
 # CONFIG_UART4_RXDMA is not set
 # CONFIG_UART5_RS485 is not set
-CONFIG_UART5_RXDMA=y
+CONFIG_UART5_RXDMA=n
 # CONFIG_USART6_RS485 is not set
 CONFIG_USART6_RXDMA=y
 # CONFIG_USART7_RXDMA is not set

src/drivers/boards/px4fmu-v1/board_config.h

@@ -103,6 +103,7 @@ __BEGIN_DECLS
 #define PX4_I2C_BUS_ESC		1
 #define PX4_I2C_BUS_ONBOARD	2
 #define PX4_I2C_BUS_EXPANSION	3
+#define PX4_I2C_BUS_LED		3
 
 /*
  * Devices on the onboard bus.
@@ -112,6 +113,7 @@ __BEGIN_DECLS
 #define PX4_I2C_OBDEV_HMC5883	0x1e
 #define PX4_I2C_OBDEV_MS5611	0x76
 #define PX4_I2C_OBDEV_EEPROM	NOTDEFINED
+#define PX4_I2C_OBDEV_LED	0x55
 
 #define PX4_I2C_OBDEV_PX4IO_BL	0x18
 #define PX4_I2C_OBDEV_PX4IO	0x1a

src/drivers/drv_rgbled.h

@@ -98,7 +98,14 @@ typedef enum {
 	RGBLED_COLOR_GREEN,
 	RGBLED_COLOR_BLUE,
 	RGBLED_COLOR_WHITE,
-	RGBLED_COLOR_AMBER
+	RGBLED_COLOR_AMBER,
+	RGBLED_COLOR_DIM_RED,
+	RGBLED_COLOR_DIM_YELLOW,
+	RGBLED_COLOR_DIM_PURPLE,
+	RGBLED_COLOR_DIM_GREEN,
+	RGBLED_COLOR_DIM_BLUE,
+	RGBLED_COLOR_DIM_WHITE,
+	RGBLED_COLOR_DIM_AMBER
 } rgbled_color_t;
 
 /* enum passed to RGBLED_SET_MODE ioctl()*/

src/drivers/px4io/px4io.cpp

@@ -94,21 +94,62 @@ extern device::Device *PX4IO_serial_interface() weak_function;
 #define PX4IO_SET_DEBUG			_IOC(0xff00, 0)
 #define PX4IO_INAIR_RESTART_ENABLE	_IOC(0xff00, 1)
 
+/**
+ * The PX4IO class.
+ *
+ * Encapsulates PX4FMU to PX4IO communications modeled as file operations.
+ */
 class PX4IO : public device::CDev
 {
 public:
+	/**
+	 * Constructor.
+	 * 
+	 * Initialize all class variables.
+	 */
 	PX4IO(device::Device *interface);
+
+	/**
+	 * Destructor.
+	 * 
+	 * Wait for worker thread to terminate.
+	 */
 	virtual ~PX4IO();
 
+	/**
+	 * Initialize the PX4IO class.
+	 * 
+	 * Initialize the physical I2C interface to PX4IO. Retrieve relevant initial system parameters. Initialize PX4IO registers.
+	 */
 	virtual int		init();
 
+	/**
+	 * IO Control handler.
+	 * 
+	 * Handle all IOCTL calls to the PX4IO file descriptor.
+	 *
+	 * @param[in] filp file handle (not used). This function is always called directly through object reference
+	 * @param[in] cmd the IOCTL command
+	 * @param[in] the IOCTL command parameter (optional)
+	 */
 	virtual int		ioctl(file *filp, int cmd, unsigned long arg);
+
+	/**
+	 * write handler.
+	 * 
+	 * Handle writes to the PX4IO file descriptor.
+	 *
+	 * @param[in] filp file handle (not used). This function is always called directly through object reference
+	 * @param[in] buffer pointer to the data buffer to be written
+	 * @param[in] len size in bytes to be written
+	 * @return number of bytes written
+	 */
 	virtual ssize_t		write(file *filp, const char *buffer, size_t len);
 
 	/**
 	* Set the update rate for actuator outputs from FMU to IO.
 	*
-	* @param rate		The rate in Hz actuator outpus are sent to IO.
+	* @param[in] rate		The rate in Hz actuator outpus are sent to IO.
 	* 			Min 10 Hz, max 400 Hz
 	*/
 	int      		set_update_rate(int rate);
@@ -116,16 +157,16 @@ public:
 	/**
 	* Set the battery current scaling and bias
 	*
-	* @param amp_per_volt
+	* @param[in] amp_per_volt
-	* @param amp_bias
+	* @param[in] amp_bias
 	*/
 	void      		set_battery_current_scaling(float amp_per_volt, float amp_bias);
 
 	/**
 	 * Push failsafe values to IO.
 	 *
-	 * @param vals		Failsafe control inputs: in us PPM (900 for zero, 1500 for centered, 2100 for full)
+	 * @param[in] vals	Failsafe control inputs: in us PPM (900 for zero, 1500 for centered, 2100 for full)
-	 * @param len		Number of channels, could up to 8
+	 * @param[in] len	Number of channels, could up to 8
 	 */
 	int			set_failsafe_values(const uint16_t *vals, unsigned len);
 
@@ -149,11 +190,21 @@ public:
 	*/
 	void			print_status();
 
+	/**
+	 * Set the DSM VCC is controlled by relay one flag
+	 *
+	 * @param[in] enable true=DSM satellite VCC is controlled by relay1, false=DSM satellite VCC not controlled
+	 */
 	inline void		set_dsm_vcc_ctl(bool enable)
 	{
 		_dsm_vcc_ctl = enable;
 	};
 
+	/**
+	 * Get the DSM VCC is controlled by relay one flag
+	 *
+	 * @return true=DSM satellite VCC is controlled by relay1, false=DSM satellite VCC not controlled
+	 */
 	inline bool		get_dsm_vcc_ctl()
 	{
 		return _dsm_vcc_ctl;
@@ -163,60 +214,50 @@ private:
 	device::Device		*_interface;
 
 	// XXX
-	unsigned		_hardware;
+	unsigned		_hardware;			///< Hardware revision
-	unsigned		_max_actuators;
+	unsigned		_max_actuators;		///<Maximum # of actuators supported by PX4IO
-	unsigned		_max_controls;
+	unsigned		_max_controls;		///<Maximum # of controls supported by PX4IO
-	unsigned		_max_rc_input;
+	unsigned		_max_rc_input;		///<Maximum receiver channels supported by PX4IO
-	unsigned		_max_relays;
+	unsigned		_max_relays;		///<Maximum relays supported by PX4IO
-	unsigned		_max_transfer;
+	unsigned		_max_transfer;		///<Maximum number of I2C transfers supported by PX4IO
 
-	unsigned 		_update_interval; ///< subscription interval limiting send rate
+	unsigned 		_update_interval;	///<Subscription interval limiting send rate
 
-	volatile int		_task;		///< worker task
+	volatile int		_task;			///<worker task id
-	volatile bool		_task_should_exit;
+	volatile bool		_task_should_exit;	///<worker terminate flag
 
-	int			_mavlink_fd;
+	int			_mavlink_fd;		///<mavlink file descriptor
 
-	perf_counter_t		_perf_update;
+	perf_counter_t		_perf_update;		///<local performance counter
 
 	/* cached IO state */
-	uint16_t		_status;
+	uint16_t		_status;		///<Various IO status flags
-	uint16_t		_alarms;
+	uint16_t		_alarms;		///<Various IO alarms
 
 	/* subscribed topics */
 	int			_t_actuators;	///< actuator controls topic
 	int			_t_actuator_armed;	///< system armed control topic
 	int 			_t_vehicle_control_mode;	///< vehicle control mode topic
-	int			_t_param;	///< parameter update topic
+	int			_t_param;		///<parameter update topic
 
 	/* advertised topics */
-	orb_advert_t 		_to_input_rc;	///< rc inputs from io
+	orb_advert_t 		_to_input_rc;		///<rc inputs from IO topic
-	orb_advert_t 		_to_actuators_effective; ///< effective actuator controls topic
+	orb_advert_t 		_to_actuators_effective; ///<effective actuator controls topic
-	orb_advert_t		_to_outputs;	///< mixed servo outputs topic
+	orb_advert_t		_to_outputs;		///<mixed servo outputs topic
-	orb_advert_t		_to_battery;	///< battery status / voltage
+	orb_advert_t		_to_battery;		///<battery status / voltage topic
-	orb_advert_t		_to_safety;	///< status of safety
+	orb_advert_t		_to_safety;		///< status of safety
 
-	actuator_outputs_s	_outputs;	///< mixed outputs
+	actuator_outputs_s	_outputs;		///<mixed outputs
-	actuator_controls_effective_s _controls_effective; ///< effective controls
+	actuator_controls_effective_s _controls_effective; ///<effective controls
 
-	bool			_primary_pwm_device;	///< true if we are the default PWM output
+	bool			_primary_pwm_device;	///<true if we are the default PWM output
 
-	float			_battery_amp_per_volt;
+	float			_battery_amp_per_volt;	///<current sensor amps/volt
-	float			_battery_amp_bias;
+	float			_battery_amp_bias;	///<current sensor bias
-	float			_battery_mamphour_total;
+	float			_battery_mamphour_total;///<amp hours consumed so far
-	uint64_t		_battery_last_timestamp;
+	uint64_t		_battery_last_timestamp;///<last amp hour calculation timestamp
 
-	/**
+	bool			_dsm_vcc_ctl;		///<true if relay 1 controls DSM satellite RX power
-	 * Relay1 is dedicated to controlling DSM receiver power
-	 */
-
-	bool			_dsm_vcc_ctl;
-
-	/**
-	 * System armed
-	 */
-
-	bool			_system_armed;
 
 	/**
 	 * Trampoline to the worker task
@@ -388,8 +429,7 @@ PX4IO::PX4IO(device::Device *interface) :
 	_battery_amp_bias(0),
 	_battery_mamphour_total(0),
 	_battery_last_timestamp(0),
-	_dsm_vcc_ctl(false),
+	_dsm_vcc_ctl(false)
-	_system_armed(false)
 {
 	/* we need this potentially before it could be set in task_main */
 	g_dev = this;
@@ -508,7 +548,7 @@ PX4IO::init()
 			usleep(10000);
 
 			/* abort after 5s */
-			if ((hrt_absolute_time() - try_start_time)/1000 > 50000) {
+			if ((hrt_absolute_time() - try_start_time)/1000 > 3000) {
 				log("failed to recover from in-air restart (1), aborting IO driver init.");
 				return 1;
 			}
@@ -732,7 +772,7 @@ PX4IO::task_main()
 				// See if bind parameter has been set, and reset it to 0
 				param_get(dsm_bind_param = param_find("RC_DSM_BIND"), &dsm_bind_val);
 				if (dsm_bind_val) {
-					if (!_system_armed) {
+					if (!(_status & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
 						if ((dsm_bind_val == 1) || (dsm_bind_val == 2)) {
 							mavlink_log_info(mavlink_fd, "[IO] binding dsm%c rx", dsm_bind_val == 1 ? '2' : 'x');
 							ioctl(nullptr, DSM_BIND_START, dsm_bind_val == 1 ? 3 : 7);
@@ -853,8 +893,6 @@ PX4IO::io_set_arming_state()
 	uint16_t set = 0;
 	uint16_t clear = 0;
 
-	_system_armed = armed.armed;
-
 	if (armed.armed && !armed.lockdown) {
 		set |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
 	} else {
@@ -1481,8 +1519,8 @@ PX4IO::print_status()
 	uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING);
 	printf("arming 0x%04x%s%s%s%s%s%s\n",
 		arming,
-		((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)          ? " FMU_ARMED" : ""),
+		((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)          ? " FMU_ARMED" : " FMU_DISARMED"),
-		((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)	    ? " IO_ARM_OK" : ""),
+		((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)	    ? " IO_ARM_OK" : " IO_ARM_DENIED"),
 		((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""),
 		((arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)   ? " FAILSAFE_CUSTOM" : ""),
 		((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK)   ? " INAIR_RESTART_OK" : ""),
@@ -1731,7 +1769,8 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
 }
 
 ssize_t
-PX4IO::write(file *filp, const char *buffer, size_t len)
+PX4IO::write(file * /*filp*/, const char *buffer, size_t len)
+/* Make it obvious that file * isn't used here */
 {
 	unsigned count = len / 2;
 

src/drivers/rgbled/rgbled.cpp

@@ -294,6 +294,27 @@ RGBLED::set_color(rgbled_color_t color) {
 		case RGBLED_COLOR_AMBER:	// amber
 			set_rgb(255,20,0);
 			break;
+		case RGBLED_COLOR_DIM_RED:		// red
+			set_rgb(90,0,0);
+			break;
+		case RGBLED_COLOR_DIM_YELLOW:	// yellow
+			set_rgb(80,30,0);
+			break;
+		case RGBLED_COLOR_DIM_PURPLE:	// purple
+			set_rgb(45,0,45);
+			break;
+		case RGBLED_COLOR_DIM_GREEN:	// green
+			set_rgb(0,90,0);
+			break;
+		case RGBLED_COLOR_DIM_BLUE:		// blue
+			set_rgb(0,0,90);
+			break;
+		case RGBLED_COLOR_DIM_WHITE:	// white
+			set_rgb(30,30,30);
+			break;
+		case RGBLED_COLOR_DIM_AMBER:	// amber
+			set_rgb(80,20,0);
+			break;
 		default:
 			warnx("color unknown");
 			break;
@@ -435,7 +456,8 @@ rgbled_main(int argc, char *argv[])
 	int rgbledadr = ADDR; /* 7bit */
 
 	int ch;
-	while ((ch = getopt(argc, argv, "a:b:")) != EOF) {
+	/* jump over start/off/etc and look at options first */
+	while ((ch = getopt(argc-1, &argv[1], "a:b:")) != EOF) {
 		switch (ch) {
 		case 'a':
 			rgbledadr = strtol(optarg, NULL, 0);
@@ -447,9 +469,8 @@ rgbled_main(int argc, char *argv[])
 			rgbled_usage();
 		}
 	}
-	argc -= optind;
+
-	argv += optind;
+	const char *verb = argv[1];
-	const char *verb = argv[0];
 
 	int fd;
 	int ret;
@@ -528,7 +549,7 @@ rgbled_main(int argc, char *argv[])
 		if (fd == -1) {
 			errx(1, "Unable to open " RGBLED_DEVICE_PATH);
 		}
-		if (argc < 4) {
+		if (argc < 5) {
 			errx(1, "Usage: rgbled rgb <red> <green> <blue>");
 		}
 		rgbled_rgbset_t v;

src/modules/commander/commander.cpp

@@ -52,6 +52,7 @@
 #include <errno.h>
 #include <debug.h>
 #include <sys/prctl.h>
+#include <sys/stat.h>
 #include <string.h>
 #include <math.h>
 #include <poll.h>
@@ -153,6 +154,8 @@ static unsigned int failsafe_lowlevel_timeout_ms;
 static unsigned int leds_counter;
 /* To remember when last notification was sent */
 static uint64_t last_print_mode_reject_time = 0;
+/* if connected via USB */
+static bool on_usb_power = false;
 
 
 /* tasks waiting for low prio thread */
@@ -205,6 +208,8 @@ transition_result_t check_main_state_machine(struct vehicle_status_s *current_st
 
 void print_reject_mode(const char *msg);
 
+void print_status();
+
 transition_result_t check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode);
 
 /**
@@ -244,6 +249,7 @@ int commander_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "status")) {
 		if (thread_running) {
 			warnx("\tcommander is running\n");
+			print_status();
 
 		} else {
 			warnx("\tcommander not started\n");
@@ -265,6 +271,47 @@ void usage(const char *reason)
 	exit(1);
 }
 
+void print_status() {
+	warnx("usb powered: %s", (on_usb_power) ? "yes" : "no");
+
+	/* read all relevant states */
+	int state_sub = orb_subscribe(ORB_ID(vehicle_status));
+	struct vehicle_status_s state;
+	orb_copy(ORB_ID(vehicle_status), state_sub, &state);
+
+	const char* armed_str;
+
+	switch (state.arming_state) {
+		case ARMING_STATE_INIT:
+			armed_str = "INIT";
+			break;
+		case ARMING_STATE_STANDBY:
+			armed_str = "STANDBY";
+			break;
+		case ARMING_STATE_ARMED:
+			armed_str = "ARMED";
+			break;
+		case ARMING_STATE_ARMED_ERROR:
+			armed_str = "ARMED_ERROR";
+			break;
+		case ARMING_STATE_STANDBY_ERROR:
+			armed_str = "STANDBY_ERROR";
+			break;
+		case ARMING_STATE_REBOOT:
+			armed_str = "REBOOT";
+			break;
+		case ARMING_STATE_IN_AIR_RESTORE:
+			armed_str = "IN_AIR_RESTORE";
+			break;
+		default:
+			armed_str = "ERR: UNKNOWN STATE";
+			break;
+	}
+
+
+	warnx("arming: %s", armed_str);
+}
+
 void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed)
 {
 	/* result of the command */
@@ -865,9 +912,14 @@ int commander_thread_main(int argc, char *argv[])
 				status.load = 1.0f - ((float)interval_runtime / 1e6f);	//system load is time spent in non-idle
 
 			last_idle_time = system_load.tasks[0].total_runtime;
+
+			/* check if board is connected via USB */
+			struct stat statbuf;
+			//on_usb_power = (stat("/dev/ttyACM0", &statbuf) == 0);
 		}
 
-		warnx("bat remaining: %2.2f", status.battery_remaining);
+		// XXX remove later
+		//warnx("bat remaining: %2.2f", status.battery_remaining);
 
 		/* if battery voltage is getting lower, warn using buzzer, etc. */
 		if (status.condition_battery_voltage_valid && status.battery_remaining < 0.25f && !low_battery_voltage_actions_done) {
@@ -930,9 +982,9 @@ int commander_thread_main(int argc, char *argv[])
 
 		/* store current state to reason later about a state change */
 		// bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok;
-		bool global_pos_valid = status.condition_global_position_valid;
+		// bool global_pos_valid = status.condition_global_position_valid;
-		bool local_pos_valid = status.condition_local_position_valid;
+		// bool local_pos_valid = status.condition_local_position_valid;
-		bool airspeed_valid = status.condition_airspeed_valid;
+		// bool airspeed_valid = status.condition_airspeed_valid;
 
 
 		/* check for global or local position updates, set a timeout of 2s */
@@ -1259,11 +1311,15 @@ int commander_thread_main(int argc, char *argv[])
 			orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode);
 		}
 
-		/* publish vehicle status at least with 1 Hz */
+		/* publish states (armed, control mode, vehicle status) at least with 5 Hz */
-		if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) {
+		if (counter % (200000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) {
 			status.counter++;
 			status.timestamp = t;
 			orb_publish(ORB_ID(vehicle_status), status_pub, &status);
+			control_mode.timestamp = t;
+			orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode);
+			armed.timestamp = t;
+			orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
 		}
 
 		/* play arming and battery warning tunes */
@@ -1362,22 +1418,22 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang
 		if (armed->armed) {
 			/* armed, solid */
 			if (status->battery_warning == VEHICLE_BATTERY_WARNING_WARNING) {
-				pattern.color[0] = RGBLED_COLOR_AMBER;
+				pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_AMBER : RGBLED_COLOR_AMBER;
 			} else if (status->battery_warning == VEHICLE_BATTERY_WARNING_ALERT) {
-				pattern.color[0] = RGBLED_COLOR_RED;
+				pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED;
 			} else {
-				pattern.color[0] = RGBLED_COLOR_GREEN;
+				pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_GREEN :RGBLED_COLOR_GREEN;
 			}
 			pattern.duration[0] = 1000;
 
 		} else if (armed->ready_to_arm) {
 			for (i=0; i<3; i++) {
 				if (status->battery_warning == VEHICLE_BATTERY_WARNING_WARNING) {
-					pattern.color[i*2] = RGBLED_COLOR_AMBER;
+					pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_AMBER : RGBLED_COLOR_AMBER;
 				} else if (status->battery_warning == VEHICLE_BATTERY_WARNING_ALERT) {
-					pattern.color[i*2] = RGBLED_COLOR_RED;
+					pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED;
 				} else {
-					pattern.color[i*2] = RGBLED_COLOR_GREEN;
+					pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_GREEN : RGBLED_COLOR_GREEN;
 				}
 				pattern.duration[i*2] = 200;
 
@@ -1385,13 +1441,13 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang
 				pattern.duration[i*2+1] = 800;
 			}
 			if (status->condition_global_position_valid) {
-				pattern.color[i*2] = RGBLED_COLOR_BLUE;
+				pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_BLUE : RGBLED_COLOR_BLUE;
 				pattern.duration[i*2] = 1000;
 				pattern.color[i*2+1] = RGBLED_COLOR_OFF;
 				pattern.duration[i*2+1] = 800;
 			} else {
 				for (i=3; i<6; i++) {
-					pattern.color[i*2] = RGBLED_COLOR_BLUE;
+					pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_BLUE : RGBLED_COLOR_BLUE;
 					pattern.duration[i*2] = 100;
 					pattern.color[i*2+1] = RGBLED_COLOR_OFF;
 					pattern.duration[i*2+1] = 100;
@@ -1402,7 +1458,7 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang
 
 		} else {
 			for (i=0; i<3; i++) {
-				pattern.color[i*2] = RGBLED_COLOR_RED;
+				pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED;
 				pattern.duration[i*2] = 200;
 				pattern.color[i*2+1] = RGBLED_COLOR_OFF;
 				pattern.duration[i*2+1] = 200;

src/modules/commander/state_machine_helper.cpp

@@ -69,6 +69,11 @@ static bool navigation_state_changed = true;
 transition_result_t
 arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety, arming_state_t new_arming_state, struct actuator_armed_s *armed)
 {
+	/*
+	 * Perform an atomic state update
+	 */
+	irqstate_t flags = irqsave();
+
 	transition_result_t ret = TRANSITION_DENIED;
 
 	/* only check transition if the new state is actually different from the current one */
@@ -114,7 +119,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s *
 			{
 				ret = TRANSITION_CHANGED;
 				armed->armed = true;
-				armed->ready_to_arm = false;
+				armed->ready_to_arm = true;
 			}
 
 			break;
@@ -168,11 +173,15 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s *
 		if (ret == TRANSITION_CHANGED) {
 			status->arming_state = new_arming_state;
 			arming_state_changed = true;
-		} else {
-			warnx("arming transition rejected");
 		}
 	}
 
+	/* end of atomic state update */
+	irqrestore(flags);
+
+	if (ret == TRANSITION_DENIED)
+		warnx("arming transition rejected");
+
 	return ret;
 }
 

src/modules/px4iofirmware/dsm.c

@@ -48,161 +48,44 @@
 
 #include <drivers/drv_hrt.h>
 
-#define DEBUG
-
 #include "px4io.h"
 
-#define DSM_FRAME_SIZE		16
+#define DSM_FRAME_SIZE		16		/**<DSM frame size in bytes*/
-#define DSM_FRAME_CHANNELS	7
+#define DSM_FRAME_CHANNELS	7		/**<Max supported DSM channels*/
 
-static int dsm_fd = -1;
+static int dsm_fd = -1;						/**< File handle to the DSM UART */
-
+static hrt_abstime dsm_last_rx_time;		/**< Timestamp when we last received */
-static hrt_abstime last_rx_time;
+static hrt_abstime dsm_last_frame_time;		/**< Timestamp for start of last dsm frame */
-static hrt_abstime last_frame_time;
+static uint8_t dsm_frame[DSM_FRAME_SIZE];	/**< DSM dsm frame receive buffer */
-
+static unsigned dsm_partial_frame_count;	/**< Count of bytes received for current dsm frame */
-static uint8_t	frame[DSM_FRAME_SIZE];
+static unsigned dsm_channel_shift;			/**< Channel resolution, 0=unknown, 1=10 bit, 2=11 bit */
-
+static unsigned dsm_frame_drops;			/**< Count of incomplete DSM frames */
-static unsigned partial_frame_count;
-static unsigned	channel_shift;
-
-unsigned dsm_frame_drops;
-
-static bool dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value);
-static void dsm_guess_format(bool reset);
-static bool dsm_decode(hrt_abstime now, uint16_t *values, uint16_t *num_values);
-
-int
-dsm_init(const char *device)
-{
-	if (dsm_fd < 0)
-		dsm_fd = open(device, O_RDONLY | O_NONBLOCK);
-
-	if (dsm_fd >= 0) {
-		struct termios t;
-
-		/* 115200bps, no parity, one stop bit */
-		tcgetattr(dsm_fd, &t);
-		cfsetspeed(&t, 115200);
-		t.c_cflag &= ~(CSTOPB | PARENB);
-		tcsetattr(dsm_fd, TCSANOW, &t);
-
-		/* initialise the decoder */
-		partial_frame_count = 0;
-		last_rx_time = hrt_absolute_time();
-
-		/* reset the format detector */
-		dsm_guess_format(true);
-
-		debug("DSM: ready");
-
-	} else {
-		debug("DSM: open failed");
-	}
-
-	return dsm_fd;
-}
-
-void
-dsm_bind(uint16_t cmd, int pulses)
-{
-	const uint32_t usart1RxAsOutp = GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN10;
-
-	if (dsm_fd < 0)
-		return;
-
-#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
-	// XXX implement
-	#warning DSM BIND NOT IMPLEMENTED ON PX4IO V2
-#else
-	switch (cmd) {
-	case dsm_bind_power_down:
-		// power down DSM satellite
-		POWER_RELAY1(0);
-		break;
-	case dsm_bind_power_up:
-		POWER_RELAY1(1);
-		dsm_guess_format(true);
-		break;
-	case dsm_bind_set_rx_out:
-		stm32_configgpio(usart1RxAsOutp); 
-		break;
-	case dsm_bind_send_pulses:
-		for (int i = 0; i < pulses; i++) {
-			stm32_gpiowrite(usart1RxAsOutp, false);
-			up_udelay(25);
-			stm32_gpiowrite(usart1RxAsOutp, true);
-			up_udelay(25);
-		}
-		break;
-	case dsm_bind_reinit_uart:
-		// Restore USART rx pin
-		stm32_configgpio(GPIO_USART1_RX);
-		break;
-	}
-#endif
-}
-
-bool
-dsm_input(uint16_t *values, uint16_t *num_values)
-{
-	ssize_t		ret;
-	hrt_abstime	now;
-
-	/*
-	 * The DSM* protocol doesn't provide any explicit framing,
-	 * so we detect frame boundaries by the inter-frame delay.
-	 *
-	 * The minimum frame spacing is 11ms; with 16 bytes at 115200bps
-	 * frame transmission time is ~1.4ms.
-	 *
-	 * We expect to only be called when bytes arrive for processing,
-	 * and if an interval of more than 5ms passes between calls,
-	 * the first byte we read will be the first byte of a frame.
-	 *
-	 * In the case where byte(s) are dropped from a frame, this also
-	 * provides a degree of protection. Of course, it would be better
-	 * if we didn't drop bytes...
-	 */
-	now = hrt_absolute_time();
-
-	if ((now - last_rx_time) > 5000) {
-		if (partial_frame_count > 0) {
-			dsm_frame_drops++;
-			partial_frame_count = 0;
-		}
-	}
-
-	/*
-	 * Fetch bytes, but no more than we would need to complete
-	 * the current frame.
-	 */
-	ret = read(dsm_fd, &frame[partial_frame_count], DSM_FRAME_SIZE - partial_frame_count);
-
-	/* if the read failed for any reason, just give up here */
-	if (ret < 1)
-		return false;
-
-	last_rx_time = now;
-
-	/*
-	 * Add bytes to the current frame
-	 */
-	partial_frame_count += ret;
-
-	/*
-	 * If we don't have a full frame, return
-	 */
-	if (partial_frame_count < DSM_FRAME_SIZE)
-		return false;
-
-	/*
-	 * Great, it looks like we might have a frame.  Go ahead and
-	 * decode it.
-	 */
-	partial_frame_count = 0;
-	return dsm_decode(now, values, num_values);
-}
 
+/**
+ * Attempt to decode a single channel raw channel datum
+ *
+ * The DSM* protocol doesn't provide any explicit framing,
+ * so we detect dsm frame boundaries by the inter-dsm frame delay.
+ *
+ * The minimum dsm frame spacing is 11ms; with 16 bytes at 115200bps
+ * dsm frame transmission time is ~1.4ms.
+ *
+ * We expect to only be called when bytes arrive for processing,
+ * and if an interval of more than 5ms passes between calls,
+ * the first byte we read will be the first byte of a dsm frame.
+ *
+ * In the case where byte(s) are dropped from a dsm frame, this also
+ * provides a degree of protection. Of course, it would be better
+ * if we didn't drop bytes...
+ *
+ * Upon receiving a full dsm frame we attempt to decode it
+ *
+ * @param[in] raw 16 bit raw channel value from dsm frame
+ * @param[in] shift position of channel number in raw data
+ * @param[out] channel pointer to returned channel number
+ * @param[out] value pointer to returned channel value
+ * @return true=raw value successfully decoded
+ */
 static bool
 dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value)
 {
@@ -220,6 +103,11 @@ dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *va
 	return true;
 }
 
+/**
+ * Attempt to guess if receiving 10 or 11 bit channel values
+ *
+ * @param[in] reset true=reset the 10/11 bit state to unknown
+ */
 static void
 dsm_guess_format(bool reset)
 {
@@ -232,14 +120,14 @@ dsm_guess_format(bool reset)
 		cs10 = 0;
 		cs11 = 0;
 		samples = 0;
-		channel_shift = 0;
+		dsm_channel_shift = 0;
 		return;
 	}
 
-	/* scan the channels in the current frame in both 10- and 11-bit mode */
+	/* scan the channels in the current dsm_frame in both 10- and 11-bit mode */
 	for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) {
 
-		uint8_t *dp = &frame[2 + (2 * i)];
+		uint8_t *dp = &dsm_frame[2 + (2 * i)];
 		uint16_t raw = (dp[0] << 8) | dp[1];
 		unsigned channel, value;
 
@@ -250,10 +138,10 @@ dsm_guess_format(bool reset)
 		if (dsm_decode_channel(raw, 11, &channel, &value) && (channel < 31))
 			cs11 |= (1 << channel);
 
-		/* XXX if we cared, we could look for the phase bit here to decide 1 vs. 2-frame format */
+		/* XXX if we cared, we could look for the phase bit here to decide 1 vs. 2-dsm_frame format */
 	}
 
-	/* wait until we have seen plenty of frames - 2 should normally be enough */
+	/* wait until we have seen plenty of frames - 5 should normally be enough */
 	if (samples++ < 5)
 		return;
 
@@ -289,13 +177,13 @@ dsm_guess_format(bool reset)
 	}
 
 	if ((votes11 == 1) && (votes10 == 0)) {
-		channel_shift = 11;
+		dsm_channel_shift = 11;
 		debug("DSM: 11-bit format");
 		return;
 	}
 
 	if ((votes10 == 1) && (votes11 == 0)) {
-		channel_shift = 10;
+		dsm_channel_shift = 10;
 		debug("DSM: 10-bit format");
 		return;
 	}
@@ -305,27 +193,136 @@ dsm_guess_format(bool reset)
 	dsm_guess_format(true);
 }
 
+/**
+ * Initialize the DSM receive functionality
+ *
+ * Open the UART for receiving DSM frames and configure it appropriately
+ *
+ * @param[in] device Device name of DSM UART
+ */
+int
+dsm_init(const char *device)
+{
+	if (dsm_fd < 0)
+		dsm_fd = open(device, O_RDONLY | O_NONBLOCK);
+
+	if (dsm_fd >= 0) {
+
+		struct termios t;
+
+		/* 115200bps, no parity, one stop bit */
+		tcgetattr(dsm_fd, &t);
+		cfsetspeed(&t, 115200);
+		t.c_cflag &= ~(CSTOPB | PARENB);
+		tcsetattr(dsm_fd, TCSANOW, &t);
+
+		/* initialise the decoder */
+		dsm_partial_frame_count = 0;
+		dsm_last_rx_time = hrt_absolute_time();
+
+		/* reset the format detector */
+		dsm_guess_format(true);
+
+		debug("DSM: ready");
+
+	} else {
+
+		debug("DSM: open failed");
+
+	}
+
+	return dsm_fd;
+}
+
+/**
+ * Handle DSM satellite receiver bind mode handler
+ *
+ * @param[in] cmd commands - dsm_bind_power_down, dsm_bind_power_up, dsm_bind_set_rx_out, dsm_bind_send_pulses, dsm_bind_reinit_uart
+ * @param[in] pulses Number of pulses for dsm_bind_send_pulses command
+ */
+void
+dsm_bind(uint16_t cmd, int pulses)
+{
+	const uint32_t usart1RxAsOutp =
+		GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | GPIO_OUTPUT_SET | GPIO_PORTA | GPIO_PIN10;
+
+	if (dsm_fd < 0)
+		return;
+
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
+	// XXX implement
+	#warning DSM BIND NOT IMPLEMENTED ON PX4IO V2
+#else
+	switch (cmd) {
+
+	case dsm_bind_power_down:
+
+		/*power down DSM satellite*/
+		POWER_RELAY1(0);
+		break;
+
+	case dsm_bind_power_up:
+
+		/*power up DSM satellite*/
+		POWER_RELAY1(1);
+		dsm_guess_format(true);
+		break;
+
+	case dsm_bind_set_rx_out:
+
+		/*Set UART RX pin to active output mode*/
+		stm32_configgpio(usart1RxAsOutp);
+		break;
+
+	case dsm_bind_send_pulses:
+
+		/*Pulse RX pin a number of times*/
+		for (int i = 0; i < pulses; i++) {
+			stm32_gpiowrite(usart1RxAsOutp, false);
+			up_udelay(25);
+			stm32_gpiowrite(usart1RxAsOutp, true);
+			up_udelay(25);
+		}
+		break;
+
+	case dsm_bind_reinit_uart:
+
+		/*Restore USART RX pin to RS232 receive mode*/
+		stm32_configgpio(GPIO_USART1_RX);
+		break;
+
+	}
+#endif
+}
+
+/**
+ * Decode the entire dsm frame (all contained channels)
+ *
+ * @param[in] frame_time timestamp when this dsm frame was received. Used to detect RX loss in order to reset 10/11 bit guess.
+ * @param[out] values pointer to per channel array of decoded values
+ * @param[out] num_values pointer to number of raw channel values returned
+ * @return true=DSM frame successfully decoded, false=no update
+ */
 static bool
 dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
 {
-
 	/*
-		debug("DSM frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x",
+	debug("DSM dsm_frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x",
-		      frame[0], frame[1], frame[2], frame[3], frame[4], frame[5], frame[6], frame[7],
+		dsm_frame[0], dsm_frame[1], dsm_frame[2], dsm_frame[3], dsm_frame[4], dsm_frame[5], dsm_frame[6], dsm_frame[7],
-		      frame[8], frame[9], frame[10], frame[11], frame[12], frame[13], frame[14], frame[15]);
+		dsm_frame[8], dsm_frame[9], dsm_frame[10], dsm_frame[11], dsm_frame[12], dsm_frame[13], dsm_frame[14], dsm_frame[15]);
 	*/
 	/*
 	 * If we have lost signal for at least a second, reset the
 	 * format guessing heuristic.
 	 */
-	if (((frame_time - last_frame_time) > 1000000) && (channel_shift != 0))
+	if (((frame_time - dsm_last_frame_time) > 1000000) && (dsm_channel_shift != 0))
 		dsm_guess_format(true);
 
-	/* we have received something we think is a frame */
+	/* we have received something we think is a dsm_frame */
-	last_frame_time = frame_time;
+	dsm_last_frame_time = frame_time;
 
-	/* if we don't know the frame format, update the guessing state machine */
+	/* if we don't know the dsm_frame format, update the guessing state machine */
-	if (channel_shift == 0) {
+	if (dsm_channel_shift == 0) {
 		dsm_guess_format(false);
 		return false;
 	}
@@ -337,17 +334,17 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
 	 * Each channel is a 16-bit unsigned value containing either a 10-
 	 * or 11-bit channel value and a 4-bit channel number, shifted
 	 * either 10 or 11 bits. The MSB may also be set to indicate the
-	 * second frame in variants of the protocol where more than
+	 * second dsm_frame in variants of the protocol where more than
 	 * seven channels are being transmitted.
 	 */
 
 	for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) {
 
-		uint8_t *dp = &frame[2 + (2 * i)];
+		uint8_t *dp = &dsm_frame[2 + (2 * i)];
 		uint16_t raw = (dp[0] << 8) | dp[1];
 		unsigned channel, value;
 
-		if (!dsm_decode_channel(raw, channel_shift, &channel, &value))
+		if (!dsm_decode_channel(raw, dsm_channel_shift, &channel, &value))
 			continue;
 
 		/* ignore channels out of range */
@@ -359,7 +356,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
 			*num_values = channel + 1;
 
 		/* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */
-		if (channel_shift == 11)
+		if (dsm_channel_shift == 11)
 			value /= 2;
 
 		value += 998;
@@ -390,7 +387,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
 		values[channel] = value;
 	}
 
-	if (channel_shift == 11)
+	if (dsm_channel_shift == 11)
 		*num_values |= 0x8000;
 
 	/*
@@ -398,3 +395,70 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
 	 */
 	return true;
 }
+
+/**
+ * Called periodically to check for input data from the DSM UART
+ *
+ * The DSM* protocol doesn't provide any explicit framing,
+ * so we detect dsm frame boundaries by the inter-dsm frame delay.
+ * The minimum dsm frame spacing is 11ms; with 16 bytes at 115200bps
+ * dsm frame transmission time is ~1.4ms.
+ * We expect to only be called when bytes arrive for processing,
+ * and if an interval of more than 5ms passes between calls,
+ * the first byte we read will be the first byte of a dsm frame.
+ * In the case where byte(s) are dropped from a dsm frame, this also
+ * provides a degree of protection. Of course, it would be better
+ * if we didn't drop bytes...
+ * Upon receiving a full dsm frame we attempt to decode it.
+ *
+ * @param[out] values pointer to per channel array of decoded values
+ * @param[out] num_values pointer to number of raw channel values returned
+ * @return true=decoded raw channel values updated, false=no update
+ */
+bool
+dsm_input(uint16_t *values, uint16_t *num_values)
+{
+	ssize_t		ret;
+	hrt_abstime	now;
+
+	/*
+	 */
+	now = hrt_absolute_time();
+
+	if ((now - dsm_last_rx_time) > 5000) {
+		if (dsm_partial_frame_count > 0) {
+			dsm_frame_drops++;
+			dsm_partial_frame_count = 0;
+		}
+	}
+
+	/*
+	 * Fetch bytes, but no more than we would need to complete
+	 * the current dsm frame.
+	 */
+	ret = read(dsm_fd, &dsm_frame[dsm_partial_frame_count], DSM_FRAME_SIZE - dsm_partial_frame_count);
+
+	/* if the read failed for any reason, just give up here */
+	if (ret < 1)
+		return false;
+
+	dsm_last_rx_time = now;
+
+	/*
+	 * Add bytes to the current dsm frame
+	 */
+	dsm_partial_frame_count += ret;
+
+	/*
+	 * If we don't have a full dsm frame, return
+	 */
+	if (dsm_partial_frame_count < DSM_FRAME_SIZE)
+		return false;
+
+	/*
+	 * Great, it looks like we might have a dsm frame.  Go ahead and
+	 * decode it.
+	 */
+	dsm_partial_frame_count = 0;
+	return dsm_decode(now, values, num_values);
+}

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		2
+#define PX4IO_PROTOCOL_VERSION		3
 
 /* static configuration page */
 #define PX4IO_PAGE_CONFIG		0

src/modules/px4iofirmware/px4io.h

@@ -198,7 +198,7 @@ extern void	controls_init(void);
 extern void	controls_tick(void);
 extern int	dsm_init(const char *device);
 extern bool	dsm_input(uint16_t *values, uint16_t *num_values);
-extern void     dsm_bind(uint16_t cmd, int pulses);
+extern void	dsm_bind(uint16_t cmd, int pulses);
 extern int	sbus_init(const char *device);
 extern bool	sbus_input(uint16_t *values, uint16_t *num_values);
 

src/modules/sensors/sensor_params.c

@@ -70,6 +70,9 @@ PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f);
 
 PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0);
 
+PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0);
+PARAM_DEFINE_INT32(SENS_EXT_MAG_ROT, 0);
+
 PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f);
 PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f);
 PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f);

src/modules/sensors/sensors.cpp

@@ -50,6 +50,7 @@
 #include <stdio.h>
 #include <errno.h>
 #include <math.h>
+#include <mathlib/mathlib.h>
 
 #include <nuttx/analog/adc.h>
 
@@ -137,6 +138,77 @@
 
 #define limit_minus_one_to_one(arg) (arg < -1.0f) ? -1.0f : ((arg > 1.0f) ? 1.0f : arg)
 
+/**
+ * Enum for board and external compass rotations.
+ * This enum maps from board attitude to airframe attitude.
+ */
+enum Rotation {
+    ROTATION_NONE                = 0,
+    ROTATION_YAW_45              = 1,
+    ROTATION_YAW_90              = 2,
+    ROTATION_YAW_135             = 3,
+    ROTATION_YAW_180             = 4,
+    ROTATION_YAW_225             = 5,
+    ROTATION_YAW_270             = 6,
+    ROTATION_YAW_315             = 7,
+    ROTATION_ROLL_180            = 8,
+    ROTATION_ROLL_180_YAW_45     = 9,
+    ROTATION_ROLL_180_YAW_90     = 10,
+    ROTATION_ROLL_180_YAW_135    = 11,
+    ROTATION_PITCH_180           = 12,
+    ROTATION_ROLL_180_YAW_225    = 13,
+    ROTATION_ROLL_180_YAW_270    = 14,
+    ROTATION_ROLL_180_YAW_315    = 15,
+    ROTATION_ROLL_90             = 16,
+    ROTATION_ROLL_90_YAW_45      = 17,
+    ROTATION_ROLL_90_YAW_90      = 18,
+    ROTATION_ROLL_90_YAW_135     = 19,
+    ROTATION_ROLL_270            = 20,
+    ROTATION_ROLL_270_YAW_45     = 21,
+    ROTATION_ROLL_270_YAW_90     = 22,
+    ROTATION_ROLL_270_YAW_135    = 23,
+    ROTATION_PITCH_90            = 24,
+    ROTATION_PITCH_270           = 25,
+    ROTATION_MAX
+};
+
+typedef struct
+{
+    uint16_t roll;
+    uint16_t pitch;
+    uint16_t yaw;
+} rot_lookup_t;
+
+const rot_lookup_t rot_lookup[] =
+{
+    {  0,   0,   0 },
+    {  0,   0,  45 },
+    {  0,   0,  90 },
+    {  0,   0, 135 },
+    {  0,   0, 180 },
+    {  0,   0, 225 },
+    {  0,   0, 270 },
+    {  0,   0, 315 },
+    {180,   0,   0 },
+    {180,   0,  45 },
+    {180,   0,  90 },
+    {180,   0, 135 },
+    {  0, 180,   0 },
+    {180,   0, 225 },
+    {180,   0, 270 },
+    {180,   0, 315 },
+    { 90,   0,   0 },
+    { 90,   0,  45 },
+    { 90,   0,  90 },
+    { 90,   0, 135 },
+    {270,   0,   0 },
+    {270,   0,  45 },
+    {270,   0,  90 },
+    {270,   0, 135 },
+    {  0,  90,   0 },
+    {  0, 270,   0 }
+};
+
 /**
  * Sensor app start / stop handling function
  *
@@ -189,8 +261,8 @@ private:
 	int		_mag_sub;			/**< raw mag data subscription */
 	int 		_rc_sub;			/**< raw rc channels data subscription */
 	int		_baro_sub;			/**< raw baro data subscription */
-	int     _airspeed_sub;		/**< airspeed subscription */
+	int		_airspeed_sub;			/**< airspeed subscription */
-	int		_diff_pres_sub;		/**< raw differential pressure subscription */
+	int		_diff_pres_sub;			/**< raw differential pressure subscription */
 	int		_vcontrol_mode_sub;			/**< vehicle control mode subscription */
 	int 		_params_sub;			/**< notification of parameter updates */
 	int 		_manual_control_sub;			/**< notification of manual control updates */
@@ -210,13 +282,15 @@ private:
 	struct differential_pressure_s _diff_pres;
 	struct airspeed_s _airspeed;
 
+	math::Matrix	_board_rotation;		/**< rotation matrix for the orientation that the board is mounted */
+	math::Matrix	_external_mag_rotation;		/**< rotation matrix for the orientation that an external mag is mounted */
+
 	struct {
 		float min[_rc_max_chan_count];
 		float trim[_rc_max_chan_count];
 		float max[_rc_max_chan_count];
 		float rev[_rc_max_chan_count];
 		float dz[_rc_max_chan_count];
-		// float ex[_rc_max_chan_count];
 		float scaling_factor[_rc_max_chan_count];
 
 		float gyro_offset[3];
@@ -227,6 +301,9 @@ private:
 		float accel_scale[3];
 		float diff_pres_offset_pa;
 
+		int board_rotation;
+		int external_mag_rotation;
+
 		int rc_type;
 
 		int rc_map_roll;
@@ -264,7 +341,6 @@ private:
 		param_t max[_rc_max_chan_count];
 		param_t rev[_rc_max_chan_count];
 		param_t dz[_rc_max_chan_count];
-		// param_t ex[_rc_max_chan_count];
 		param_t rc_type;
 
 		param_t rc_demix;
@@ -304,6 +380,9 @@ private:
 
 		param_t battery_voltage_scaling;
 
+		param_t board_rotation;
+		param_t external_mag_rotation;
+
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
 
@@ -312,6 +391,11 @@ private:
 	 */
 	int		parameters_update();
 
+	/**
+	 * Get the rotation matrices
+	 */
+	void		get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix);
+
 	/**
 	 * Do accel-related initialisation.
 	 */
@@ -448,7 +532,10 @@ Sensors::Sensors() :
 	_diff_pres_pub(-1),
 
 /* performance counters */
-	_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update"))
+	_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")),
+
+	_board_rotation(3,3),
+	_external_mag_rotation(3,3)
 {
 
 	/* basic r/c parameters */
@@ -538,6 +625,10 @@ Sensors::Sensors() :
 
 	_parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING");
 
+	/* rotations */
+	_parameter_handles.board_rotation = param_find("SENS_BOARD_ROT");
+	_parameter_handles.external_mag_rotation = param_find("SENS_EXT_MAG_ROT");
+
 	/* fetch initial parameter values */
 	parameters_update();
 }
@@ -724,9 +815,33 @@ Sensors::parameters_update()
 		warnx("Failed updating voltage scaling param");
 	}
 
+	param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation));
+	param_get(_parameter_handles.external_mag_rotation, &(_parameters.external_mag_rotation));
+
+	get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation);
+	get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation);
+
 	return OK;
 }
 
+void
+Sensors::get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix)
+{
+	/* first set to zero */
+	rot_matrix->Matrix::zero(3,3);
+
+	float roll  = M_DEG_TO_RAD_F * (float)rot_lookup[rot].roll;
+	float pitch = M_DEG_TO_RAD_F * (float)rot_lookup[rot].pitch;
+	float yaw   = M_DEG_TO_RAD_F * (float)rot_lookup[rot].yaw;
+
+	math::EulerAngles euler(roll, pitch, yaw);
+
+	math::Dcm R(euler);
+
+	for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
+		(*rot_matrix)(i,j) = R(i, j);
+}
+
 void
 Sensors::accel_init()
 {
@@ -750,7 +865,7 @@ Sensors::accel_init()
 		/* set the driver to poll at 1000Hz */
 		ioctl(fd, SENSORIOCSPOLLRATE, 1000);
 
-		#else
+		#elif CONFIG_ARCH_BOARD_PX4FMU_V2
 
 		/* set the accel internal sampling rate up to at leat 800Hz */
 		ioctl(fd, ACCELIOCSSAMPLERATE, 800);
@@ -758,6 +873,9 @@ Sensors::accel_init()
 		/* set the driver to poll at 800Hz */
 		ioctl(fd, SENSORIOCSPOLLRATE, 800);
 
+		#else
+			#error Need a board configuration, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2
+
 		#endif
 
 		warnx("using system accel");
@@ -867,9 +985,12 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
 
 		orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report);
 
-		raw.accelerometer_m_s2[0] = accel_report.x;
+		math::Vector3 vect = {accel_report.x, accel_report.y, accel_report.z};
-		raw.accelerometer_m_s2[1] = accel_report.y;
+		vect = _board_rotation*vect;
-		raw.accelerometer_m_s2[2] = accel_report.z;
+
+		raw.accelerometer_m_s2[0] = vect(0);
+		raw.accelerometer_m_s2[1] = vect(1);
+		raw.accelerometer_m_s2[2] = vect(2);
 
 		raw.accelerometer_raw[0] = accel_report.x_raw;
 		raw.accelerometer_raw[1] = accel_report.y_raw;
@@ -890,9 +1011,12 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
 
 		orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report);
 
-		raw.gyro_rad_s[0] = gyro_report.x;
+		math::Vector3 vect = {gyro_report.x, gyro_report.y, gyro_report.z};
-		raw.gyro_rad_s[1] = gyro_report.y;
+		vect = _board_rotation*vect;
-		raw.gyro_rad_s[2] = gyro_report.z;
+
+		raw.gyro_rad_s[0] = vect(0);
+		raw.gyro_rad_s[1] = vect(1);
+		raw.gyro_rad_s[2] = vect(2);
 
 		raw.gyro_raw[0] = gyro_report.x_raw;
 		raw.gyro_raw[1] = gyro_report.y_raw;
@@ -913,9 +1037,14 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
 
 		orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report);
 
-		raw.magnetometer_ga[0] = mag_report.x;
+		// XXX TODO add support for external mag orientation
-		raw.magnetometer_ga[1] = mag_report.y;
+
-		raw.magnetometer_ga[2] = mag_report.z;
+		math::Vector3 vect = {mag_report.x, mag_report.y, mag_report.z};
+		vect = _board_rotation*vect;
+
+		raw.magnetometer_ga[0] = vect(0);
+		raw.magnetometer_ga[1] = vect(1);
+		raw.magnetometer_ga[2] = vect(2);
 
 		raw.magnetometer_raw[0] = mag_report.x_raw;
 		raw.magnetometer_raw[1] = mag_report.y_raw;

src/modules/uORB/objects_common.cpp

@@ -37,6 +37,10 @@
  * Common object definitions without a better home.
  */
 
+/**
+ * @defgroup topics List of all uORB topics.
+ */
+
 #include <nuttx/config.h>
 
 #include <drivers/drv_orb_dev.h>

src/modules/uORB/topics/actuator_controls.h

@@ -55,11 +55,21 @@
 /* control sets with pre-defined applications */
 #define ORB_ID_VEHICLE_ATTITUDE_CONTROLS	ORB_ID(actuator_controls_0)
 
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
 struct actuator_controls_s {
 	uint64_t timestamp;
 	float	control[NUM_ACTUATOR_CONTROLS];
 };
 
+/**
+ * @}
+ */
+
 /* actuator control sets; this list can be expanded as more controllers emerge */
 ORB_DECLARE(actuator_controls_0);
 ORB_DECLARE(actuator_controls_1);

src/modules/uORB/topics/actuator_controls_effective.h

@@ -53,11 +53,20 @@
 #define NUM_ACTUATOR_CONTROLS_EFFECTIVE		NUM_ACTUATOR_CONTROLS
 #define NUM_ACTUATOR_CONTROL_GROUPS_EFFECTIVE	NUM_ACTUATOR_CONTROL_GROUPS	/**< for sanity checking */
 
+/**
+ * @addtogroup topics
+ * @{
+ */
+
 struct actuator_controls_effective_s {
 	uint64_t timestamp;
 	float	control_effective[NUM_ACTUATOR_CONTROLS_EFFECTIVE];
 };
 
+/**
+ * @}
+ */
+
 /* actuator control sets; this list can be expanded as more controllers emerge */
 ORB_DECLARE(actuator_controls_effective_0);
 ORB_DECLARE(actuator_controls_effective_1);

src/modules/uORB/topics/actuator_outputs.h

@@ -52,12 +52,21 @@
 #define NUM_ACTUATOR_OUTPUTS		16
 #define NUM_ACTUATOR_OUTPUT_GROUPS	4	/**< for sanity checking */
 
+/**
+ * @addtogroup topics
+ * @{
+ */
+
 struct actuator_outputs_s {
 	uint64_t timestamp;				/**< output timestamp in us since system boot */
 	float	output[NUM_ACTUATOR_OUTPUTS];		/**< output data, in natural output units */
 	int noutputs;					/**< valid outputs */
 };
 
+/**
+ * @}
+ */
+
 /* actuator output sets; this list can be expanded as more drivers emerge */
 ORB_DECLARE(actuator_outputs_0);
 ORB_DECLARE(actuator_outputs_1);

src/modules/uORB/topics/debug_key_value.h

@@ -47,6 +47,7 @@
 
 /**
  * @addtogroup topics
+ * @{
  */
 
 /**

src/modules/uORB/topics/esc_status.h

@@ -51,10 +51,6 @@
 #include <stdbool.h>
 #include "../uORB.h"
 
-/**
- * @addtogroup topics @{
- */
-
 /**
  * The number of ESCs supported.
  * Current (Q2/2013) we support 8 ESCs,
@@ -76,7 +72,12 @@ enum ESC_CONNECTION_TYPE {
 };
 
 /**
- *
+ * @addtogroup topics
+ * @{
+ */
+
+/**
+ * Electronic speed controller status.
  */
 struct esc_status_s
 {

src/modules/uORB/topics/mission.h

@@ -46,11 +46,6 @@
 #include <stdbool.h>
 #include "../uORB.h"
 
-/**
- * @addtogroup topics
- * @{
- */
-
 enum NAV_CMD {
 	NAV_CMD_WAYPOINT = 0,
 	NAV_CMD_LOITER_TURN_COUNT,
@@ -61,6 +56,11 @@ enum NAV_CMD {
 	NAV_CMD_TAKEOFF
 };
 
+/**
+ * @addtogroup topics
+ * @{
+ */
+
 /**
  * Global position setpoint in WGS84 coordinates.
  *

src/modules/uORB/topics/offboard_control_setpoint.h

@@ -43,11 +43,6 @@
 #include <stdint.h>
 #include "../uORB.h"
 
-/**
- * @addtogroup topics
- * @{
- */
-
 /**
  * Off-board control inputs.
  * 
@@ -66,6 +61,11 @@ enum OFFBOARD_CONTROL_MODE
 	OFFBOARD_CONTROL_MODE_MULTIROTOR_SIMPLE = 7, /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */
 };
 
+/**
+ * @addtogroup topics
+ * @{
+ */
+
 struct offboard_control_setpoint_s {
 	uint64_t timestamp;
 

src/modules/uORB/topics/omnidirectional_flow.h

@@ -46,6 +46,7 @@
 
 /**
  * @addtogroup topics
+ * @{
  */
 
 /**

src/modules/uORB/topics/parameter_update.h

@@ -42,11 +42,20 @@
 #include <stdint.h>
 #include "../uORB.h"
 
+/**
+ * @addtogroup topics
+ * @{
+ */
+
 struct parameter_update_s {
 	/** time at which the latest parameter was updated */
 	uint64_t	timestamp;
 };
 
+/**
+ * @}
+ */
+
 ORB_DECLARE(parameter_update);
 
 #endif

src/modules/uORB/topics/rc_channels.h

@@ -45,11 +45,6 @@
 #include <stdint.h>
 #include "../uORB.h"
 
-/**
- * @addtogroup topics
- * @{
- */
-
 /**
  * The number of RC channel inputs supported.
  * Current (Q1/2013) radios support up to 18 channels,
@@ -85,6 +80,11 @@ enum RC_CHANNELS_FUNCTION
   RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */
 };
 
+/**
+ * @addtogroup topics
+ * @{
+ */
+
 struct rc_channels_s {
 
 	uint64_t timestamp;                 /**< In microseconds since boot time. */

src/modules/uORB/topics/sensor_combined.h

@@ -46,17 +46,17 @@
 #include <stdbool.h>
 #include "../uORB.h"
 
-/**
- * @addtogroup topics
- * @{
- */
-
 enum MAGNETOMETER_MODE {
 	MAGNETOMETER_MODE_NORMAL = 0,
 	MAGNETOMETER_MODE_POSITIVE_BIAS,
 	MAGNETOMETER_MODE_NEGATIVE_BIAS
 };
 
+/**
+ * @addtogroup topics
+ * @{
+ */
+
 /**
  * Sensor readings in raw and SI-unit form.
  *

src/modules/uORB/topics/subsystem_info.h

@@ -50,10 +50,6 @@
 #include <stdbool.h>
 #include "../uORB.h"
 
-/**
- * @addtogroup topics
- */
-
 enum SUBSYSTEM_TYPE
 {
 	SUBSYSTEM_TYPE_GYRO = 1,
@@ -75,6 +71,10 @@ enum SUBSYSTEM_TYPE
 	SUBSYSTEM_TYPE_RANGEFINDER = 131072
 };
 
+/**
+ * @addtogroup topics
+ */
+
 /**
  * State of individual sub systems
  */

src/modules/uORB/topics/telemetry_status.h

@@ -50,6 +50,11 @@ enum TELEMETRY_STATUS_RADIO_TYPE {
     TELEMETRY_STATUS_RADIO_TYPE_WIRE
 };
 
+/**
+ * @addtogroup topics
+ * @{
+ */
+
 struct telemetry_status_s {
 	uint64_t timestamp;
     enum TELEMETRY_STATUS_RADIO_TYPE type;  /**< type of the radio hardware     */
@@ -62,6 +67,10 @@ struct telemetry_status_s {
     uint8_t txbuf;              /**< how full the tx buffer is as a percentage  */
 };
 
+/**
+ * @}
+ */
+
 ORB_DECLARE(telemetry_status);
 
 #endif /* TOPIC_TELEMETRY_STATUS_H */

src/modules/uORB/topics/vehicle_attitude.h

@@ -48,6 +48,7 @@
 
 /**
  * @addtogroup topics
+ * @{
  */
 
 /**

src/modules/uORB/topics/vehicle_command.h

@@ -45,11 +45,6 @@
 #include <stdint.h>
 #include "../uORB.h"
 
-/**
- * @addtogroup topics
- * @{
- */
-
 /**
  * Commands for commander app.
  *
@@ -110,6 +105,10 @@ enum VEHICLE_CMD_RESULT
 	VEHICLE_CMD_RESULT_ENUM_END=5, /*  | */
 };
 
+/**
+ * @addtogroup topics
+ * @{
+ */
 
 struct vehicle_command_s
 {

src/modules/uORB/topics/vehicle_global_position_set_triplet.h

@@ -60,8 +60,8 @@
  */
 struct vehicle_global_position_set_triplet_s
 {
-	bool previous_valid;
+	bool previous_valid;					/**< flag indicating previous position is valid */
-	bool next_valid;
+	bool next_valid;					/**< flag indicating next position is valid */
 
 	struct vehicle_global_position_setpoint_s previous;
 	struct vehicle_global_position_setpoint_s current;

src/modules/uORB/topics/vehicle_status.h

@@ -159,6 +159,10 @@ enum VEHICLE_BATTERY_WARNING {
     VEHICLE_BATTERY_WARNING_ALERT            /**< alerting of low voltage 2. stage */
 };
 
+/**
+ * @addtogroup topics
+ * @{
+ */
 
 /**
  * state machine / state of vehicle.